#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


//The following defines are the constraints imposed by the TA
#define STRAIGHT1   50   //Straight distance 1 (meters)
#define ROTATION1   30   //First rotation  (degrees)

#define STRAIGHT2   30   //Straight Distance 2  (meters)
#define ROTATION2   170  //Second rotation (absolute value in degrees)

#define STRAIGHT3   20   //Third forward motion



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);
  }
}



task main() {



        ResetAllTachoCounts(OUT_AC);
        ClearScreen();

        start odometry;
        long rotation, target, total;

        //First forward motion
        //We figure out the tracho rotation as per calibration based on required path
        //STRAIGHT in meters * (1000um / meter) / (480 um / degrees) = degrees rotation

        target = (STRAIGHT1 * 1000 / uM_PER_DEGREE) + MotorTachoCount(OUT_B);
        while (MotorTachoCount(OUT_B) < target);
              {OnFwdReg(OUT_BC,40, 2);}
        Off(OUT_AC);
        Wait(200);


       //First Rotation
       
       //from odometer; angle/1000 = heading in degrees
       //TURN_PER_DIFF_DEG (as calibrated) 240 millidegrees per degree
       //(ROTTATION1 in degrees - Heading = Required robot rotation in degrees)
       // total = (Req. robot rotation) * (240 mdegrees wheel rotation/degree)/1000
       //Gives total wheel rotation in degrees
       total = (ROTATION1 - (angle/1000)) * TURN_PER_DIFF_DEG/1000;

       if (total > 0){  //Left rotaiton  (B is the right wheel)
       target = MotorTachoCount(OUT_B) + total;
       while (MotorTachoCount(OUT_B) < target);
                    {OnFwd(OUT_B, 40);}
              Off(OUT_A);
              Wait(500);
         }

       if (total < 0){  //Right rotation  (C is the left wheel)
       target = MotorTachoCount(OUT_C) + total;
       while (MotorTachoCount(OUT_C) < target);
                    {OnFwd(OUT_C, 40);}
              Off(OUT_C);
              Wait(500);
         }


           //Second forward motion
        target = (STRAIGHT2 * 1000 / uM_PER_DEGREE) + MotorTachoCount(OUT_B);
        while (MotorTachoCount(OUT_B) < target);
              {OnFwdReg(OUT_BC,40, 2);}
        Off(OUT_AC);
        Wait(200);




               //Second Rotation
       total = (ROTATION2 - (angle/1000)) * TURN_PER_DIFF_DEG/1000;

       if (total > 0){  //Left rotaiton  (B is the right wheel)
       target = MotorTachoCount(OUT_B) + total;
       while (MotorTachoCount(OUT_B) < target);
                    {OnFwd(OUT_B, 40);}
              Off(OUT_A);
              Wait(500);
         }

       if (total < 0){  //Right rotation  (C is the left wheel)
       target = MotorTachoCount(OUT_C) + total;
       while (MotorTachoCount(OUT_C) < target);
                    {OnFwd(OUT_C, 40);}
              Off(OUT_C);
              Wait(500);
         }
           //Third forward motion
        target = (STRAIGHT3 * 1000 / uM_PER_DEGREE) + MotorTachoCount(OUT_B);
        while (MotorTachoCount(OUT_B) < target);
              {OnFwdReg(OUT_BC,40, 2);}
        Off(OUT_AC);
        Wait(200);
}


