#pragma config(Sensor, S1,     ,                    sensorTouch)
#pragma config(Sensor, S2,     nCompass,            sensorI2CHiTechnicCompass)
#pragma config(Sensor, S3,     ,                    sensorSONAR)
#pragma config(Sensor, S4,     ,                    sensorLightInactive)
#pragma config(Motor,  motorA,          grabber,       tmotorNormal, PIDControl, )
#pragma config(Motor,  motorB,          rightMotor,    tmotorNormal, PIDControl, )
#pragma config(Motor,  motorC,          LeftMotor,     tmotorNormal, PIDControl, )
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "AdvancedSensors.c"    // include library files

void clear_motor_encoders();
void motorizedIt(int cdegree);
task drawDial();
task main();
void moveTo(int rot, int direction);

task drawDial() {
   int nCompassHeading;
   int nLastX     = 31; // out of range to force initial display
   int nLastY     = 31; // out of range to force initial display
  nCompassHeading = SensorValue[nCompass];
   //
   // Clear screen and setup variables
   //
   eraseDisplay();
   //
   // Repeat forever display angle and sine on the NXT LCD display
   //
  nxtDrawEllipse(0, 62, 62, 0);
   nxtDisplayStringAt(29, 58, "N");
   nxtDisplayStringAt(29,  8, "S");
   nxtDisplayStringAt( 3, 34, "W");
   nxtDisplayStringAt(54, 34, "E");
  nxtDrawLine(31, 31, nLastX, nLastY);
  while (true)
   {
      int X;
      int Y;
     //
     // Normalize
     //
     nCompassHeading = SensorValue[nCompass] % 360;
     if (nCompassHeading < 0)
       nCompassHeading += 360;
    X = 31 + (float) (sinDegrees(nCompassHeading) * 31);
    Y = 31 + (float) (cosDegrees(nCompassHeading) * 31);
     if ((nLastX != X)  || (nLastY != Y))
     {
        //
        // Position has changed. Redraw some screen elements
        // If unchanged, skip to avoid screen flicker
        //
        nxtInvertLine(31, 31, X,  Y);
        nxtInvertLine(31, 31, nLastX, nLastY);
        //
        // Numeric display of 'compass on right of screen
        //
        nxtDisplayStringAt(57, 60, "Heading");
        nxtDisplayStringAt(65, 52, "%+5d", nCompassHeading);
        nxtDisplayStringAt(75, 16, "Port");
        nxtDisplayStringAt(80,  8, "S%1d", (short) ((short) nCompass + 1));
        nLastX = X;
        nLastY = Y;
      }
   }
}


task main(){
  StartTask(drawDial);
   while (true){
    motorizedIt(SensorValue[nCompass]);
  }
}

void motorizedIt(int cdegree) {
  int rot =cdegree % 360;
  int mot = 1;
  //stop when the NXT facing North
  if (cdegree == 0){
     return;
  }
  //reset the encoders value to avoid overflaow
   clear_motor_encoders();

   if (cdegree > 180 && cdegree < 360){
      rot = 360 - rot;
      mot = 0;
   }

   rot *= 5;  // ratio between the circumference of the tire to the circumference of the rotation circle around itself
   switch (mot){
      case 1:
      moveTo(rot/2,1);
     break;
     case 0:
     moveTo(rot/2,-1);
     break;
     case -1:
     moveTo(rot,1);
     break;
   }
}


void clear_motor_encoders(){
   nMotorEncoder[motorA] = 0;
   nMotorEncoder[motorB] = 0;
}

void moveTo(int rot, int direction){
   nSyncedMotors = synchBC;
   nSyncedTurnRatio = -100;
   nMotorEncoderTarget[motorB] = rot;
   motor[motorB] = direction * 50;
   while (nMotorRunState[motorB] != runStateIdle) ;
   motor[motorB] = 0;

}