Combined My and Peter's code

Added calculations from Peter's code.