Adding almost final code. Must be better tested but all the main logic is done