#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

long xPosMM=0, yPosMM=0;   // units: millimeters | range: +- 2000 km
long xPosMMTrip,yPosMMTrip;

long angle=0,angleCounter=0;  // both in millidegrees

inline void resetAngleCounter() {
  angleCounter = 0;
}

inline void setAngleCounter(long amount) {
  angleCounter = amount;
}

inline long getAngleCounter() {
  return angleCounter;
}

void resetDistanceCounter() {
  xPosMMTrip = xPosMM;
  yPosMMTrip = yPosMM;
}

void turnEx(long turnAmount, byte useB, byte useC) {
  char direction = sign(turnAmount);
  resetAngleCounter();

  OnFwdRegEx(OUT_B,direction*10*useB,OUT_REGMODE_SPEED,RESET_NONE);
  OnRevRegEx(OUT_C,direction*10*useC,OUT_REGMODE_SPEED,RESET_NONE);

  while (direction*getAngleCounter() < 10000 && direction*getAngleCounter() < direction*turnAmount);

  OnFwdRegEx(OUT_B,direction*15*useB,OUT_REGMODE_SPEED,RESET_NONE);
  OnRevRegEx(OUT_C,direction*15*useC,OUT_REGMODE_SPEED,RESET_NONE);

  while (direction*turnAmount - direction*getAngleCounter() > 20000);

  OnFwdRegEx(OUT_B,direction*5*useB,OUT_REGMODE_SPEED,RESET_NONE);
  OnRevRegEx(OUT_C,direction*5*useC,OUT_REGMODE_SPEED,RESET_NONE);

  /*while (direction*turnAmount - direction*getAngleCounter() > 10000);

  OnFwdEx(OUT_B,direction*10,RESET_NONE);
  OnRevEx(OUT_C,direction*10,RESET_NONE); */

  while (direction*getAngleCounter() < direction*turnAmount);

  OffEx(OUT_BC,RESET_NONE);
}

void turn(long turnAmount) {
  turnEx(turnAmount, 1, 1);
}

void turnToEx(long targetHeading, byte useB, byte useC) {

  while (targetHeading < 0)
    targetHeading += 360000;
  while (targetHeading >= 360000)
    targetHeading -= 360000;


  long diff = targetHeading - angle;

  // we want to find which direction to turn...(for shortest turn)

  long turnAmount;
  if (diff >= 0 && diff <= 180000)
    turnEx(diff, useB, useC);
  else if(diff <= -180000)
    turnEx(diff+360000, useB, useC);
  else if (diff > -180000 && diff < 0)
    turnEx(diff, useB, useC);
  else // diff > 180000
    turnEx(diff-360000, useB, useC);
}

void turnTo(long turnAmount) {
  turnToEx(turnAmount, 1, 1);
}

/*void xgoTo(long x, long y) {

  long delX, delY;
  long dSquared,d,oldDist,newDist;
  
  bool firstMove = true;
  
  while (true) {
    
    delX = x-xPosMM;
    delY = y-yPosMM;

    dSquared = delX*delX+delY*delY;

    d = Sqrt(dSquared);
    
    oldDist = d;

    if (d < 10)
      break;

    // we need 100*cos in order to use the Acos
    long theCos = delX*100/d;
    long trajAngle = Acos(theCos);

    if (delY < 0)
      trajAngle = 360 - trajAngle;

    // convert trig angle to heading angle:
    if (trajAngle < 90)
      trajAngle += 270;
    else
      trajAngle -= 90;

    if (firstMove) {
      turnToEx(trajAngle*1000, false);
      Wait(100);
    } else {
      turnToEx(trajAngle*1000, true);
    }
    
    firstMove = false;
    
      
    OnFwdEx(OUT_BC,40,RESET_NONE);

    Wait(200);
  }

    OffEx(OUT_BC,RESET_NONE);

    Wait(50);
} */

task odometry() {

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

  long lastLeftDeg=0,lastRightDeg=0;  // units: degrees
  int deltaLeftDeg, deltaRightDeg;    // units: degrees
  int deltaLeft, deltaRight;          // units: micrometers | range: +- 3cm
  int deltaX, deltaY;                 // units: micrometers | range: +- 3cm

  long deltaAngle;                    // units: millidegrees

  long distance;

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


  ResetTachoCount(OUT_BC);

  TextOut(0,LCD_LINE1,"angle (dg):",true);
  TextOut(0,LCD_LINE3,"y pos (mm):");
  TextOut(0,LCD_LINE4,"y pos (mm):");


  while (true) {
    deltaLeftDeg  = MotorTachoCount(OUT_C)-lastLeftDeg;
    deltaRightDeg = MotorTachoCount(OUT_B)-lastRightDeg;

    lastLeftDeg = MotorTachoCount(OUT_C);
    lastRightDeg = 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;
    angleCounter += 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;

    TextOut(70,LCD_LINE1,"   ");
    TextOut(70,LCD_LINE3,"      ");
    TextOut(70,LCD_LINE4,"      ");

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

    Wait(ODO_POLL_PERIOD);
  }
}



  

    

