Finished algorithm implementation

Algorithm is implemented, needs tweaking for cars that are around the
same HR