#include "Navigation.h"
#include "hardware.h"

#include <math.h>


Navigation::Navigation(Motor *m1, float p1, Motor *m2, float p2, Motor *m3, float p3) {
  motors[0] = m1;
  motorPos[0] = p1;
  motors[1] = m2;
  motorPos[1] = p2;
  motors[2] = m3;
  motorPos[2] = p3;
  
  speed = direction = spin = 0.0;
  
  update();
}

void Navigation::update() {
  float s1, s2, s3;
  
  s1 = s2 = s3 = spin;
  
  s1 += speed*sin((direction-motorPos[0])*M_PI/180);
  s2 += speed*sin((direction-motorPos[1])*M_PI/180);
  s3 += speed*sin((direction-motorPos[2])*M_PI/180);
  
  if(ABS(s1) > 1.0) {
    s1 /= ABS(s1);
    s2 /= ABS(s1);
    s3 /= ABS(s1);
  }
  if(ABS(s2) > 1.0) {
    s1 /= ABS(s2);
    s2 /= ABS(s2);
    s3 /= ABS(s2);
  }
  if(ABS(s3) > 1.0) {
    s1 /= ABS(s3);
    s2 /= ABS(s3);
    s3 /= ABS(s3);
  }
  
  motors[0]->setSpeed(int(s1*255));
  motors[1]->setSpeed(int(s2*255));
  motors[2]->setSpeed(int(s3*255));
}