#define ODO_POLL_PERIOD    100
#define uM_PER_DEGREE      480   // calibrated: 480 micrometers per degree
#define TURN_PER_DIFF_DEG  240    // this was calibrated: 240 millidegrees per degree
#define TRAC_ROT           1482  // this was calibrated: trachodegrees per 360 rotation
#define COFFEE_BREAK       1000   //time to rest between motions


//The following defines are the constraints imposed by the TA
#define STRAIGHT1   500   //Straight distance 1 (mm)
#define ROTATION1   35   //First rotation  (degrees)

#define STRAIGHT2   700   //Straight Distance 2  (mm)
#define ROTATION2   100  //Second rotation (absolute value in degrees)



long xPos=0, yPos=0;  // units: micrometers | range: +- 2 km
long xPosMM=0, yPosMM=0;
long angle=0;

task odometry() {

  int deltaLeft, deltaRight;  // units: micrometers | range: +- 3cm
  int deltaLeftDeg, deltaRightDeg;  // units: degrees

  long deltaX, deltaY;         // units: micrometers | range: +- 3cm

  long deltaAngle;       // units: millidegrees

  long distance;
  int sine, cosine, lastSine=0, lastCosine=100;

  long lastLeft=0,lastRight=0;

  ResetTachoCount(OUT_BC);

  while (true) {
    deltaLeftDeg  = MotorTachoCount(OUT_C)-lastLeft;
    deltaRightDeg = MotorTachoCount(OUT_B)-lastRight;
    //ResetTachoCount(OUT_BC);
    lastLeft = MotorTachoCount(OUT_C);
    lastRight = MotorTachoCount(OUT_B);

    deltaLeft = uM_PER_DEGREE*deltaLeftDeg;
    deltaRight = uM_PER_DEGREE*deltaRightDeg;

    distance =  (deltaLeft+deltaRight)/2;

    deltaAngle = (deltaRightDeg-deltaLeftDeg)*TURN_PER_DIFF_DEG;
    long tempAngle = angle + deltaAngle;

    if (tempAngle >= 360000) {
      angle = tempAngle - 360000;
    } else if (tempAngle < 0) {
      angle = tempAngle + 360000;
    } else {
      angle = tempAngle;
    }

    int angleDeg = angle/1000;
    sine = Sin(angleDeg);
    cosine = Cos(angleDeg);

    deltaX = (distance*(-lastSine))/100/2;
    deltaY = (distance*lastCosine)/100/2;

    deltaX += (distance*(-sine))/100/2;
    deltaY += (distance*cosine)/100/2;

    lastSine = sine;
    lastCosine = cosine;

    xPos += deltaX;
    yPos += deltaY;

    xPosMM = xPos/1000;
    yPosMM = yPos/1000;

    NumOut(0,LCD_LINE1,angle/1000,false);
    NumOut(0,LCD_LINE3,xPosMM);
    NumOut(0,LCD_LINE4,yPosMM);

    Wait(ODO_POLL_PERIOD);
  }
}

void Forward(long distance) {
  long target;
  //We figure out the tracho rotation as per calibration based on required path
        //STRAIGHT in meters * (1000000um / meter) / (480 um / degrees) = degrees rotation

        target = (distance * 1000 / uM_PER_DEGREE) + MotorTachoCount(OUT_B);


        while (MotorTachoCount(OUT_B) < target)
              {OnFwdRegEx(OUT_BC,40, OUT_REGMODE_SPEED, RESET_NONE);}
        OffEx(OUT_BC, RESET_NONE);
}

void Rotate (long degrees) {
  long target, total, wheel_rotation;
  

       //from odometer; (angle/1000) = heading in degrees
       //TRAC_ROT  (1482)  calibrated: trachodegrees per 360 rotation
       //Gives total wheel rotation in degrees
       
       
       target = degrees - (angle/1000);                   //Defines turn required in robot degrees
       if (target > 0)                                      //Left rotaiton  (B is the right wheel)
          {
          total = (target * TRAC_ROT)/360;                  //Gives total wheel trachocount rotation needed
          wheel_rotation = MotorTachoCount(OUT_B)-MotorTachoCount(OUT_C) + total;  //Pegs rotation to current trachocount
                    {OnFwdEx(OUT_B, 40, RESET_NONE);}
                    {OnRevEx(OUT_C, 40, RESET_NONE);}
          while (MotorTachoCount(OUT_B)-MotorTachoCount(OUT_C) < wheel_rotation);
          OffEx(OUT_BC, RESET_NONE);
          }

       if (target < 0)  //Right rotation  (C is the left wheel)
          {
          total = (target * TRAC_ROT)/360;          //Gives total wheel rotation in degrees
          wheel_rotation = MotorTachoCount(OUT_B)-(MotorTachoCount(OUT_C) + total);
          OnFwdEx(OUT_C, 40, RESET_NONE);
          OnRevEx(OUT_B, 40, RESET_NONE);
          while (MotorTachoCount(OUT_C)-MotorTachoCount(OUT_B) < wheel_rotation);
          OffEx(OUT_BC, RESET_NONE);
          }
}


task main() {

        start odometry;
        Wait(1000);

        Forward(STRAIGHT1); //First forward motion
        Wait(COFFEE_BREAK);

        Rotate(ROTATION1);  //First Rotation
        Wait(COFFEE_BREAK);

        Forward(STRAIGHT2); //Second forward motion
        Wait(COFFEE_BREAK);
        
        Rotate(ROTATION2);  //Second Rotation
        Wait(COFFEE_BREAK);


}
