Change goal function
double some vectors
comment some functions
Improve style of code