Adding working code.

Added code to the methods that does all the calculations. Next checkin will add more comments.