#pragma config(Sensor, S1,     ,               sensorI2CCustom)
#pragma config(Sensor, S4,     sonar,          sensorSONAR)
#pragma config(Motor,  motorA,          frontLeftMotor, tmotorEV3_Large, PIDControl,reversed, encoder)
#pragma config(Motor,  motorB,          frontRightMotor, tmotorEV3_Large, PIDControl, reversed, encoder)
#pragma config(Motor,  motorC,          backLeftMotor, tmotorEV3_Large, PIDControl, reversed, encoder)
#pragma config(Motor,  motorD,          backRightMotor, tmotorEV3_Large, PIDControl, reversed, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//Robot:Grasshopper Vers. 1.0 EV3, by Sigtrygg (2015)


#include "drivers/hitechnic-compass.h" //take care for choosing the right path!

//Variables
const int WHEELCIRCUMSTANCE=50;//cm
//const int TRACKWIDTH=22;//cm
const float PI=3.14159265;
float rpmSetPoint [4]={0,1,2,3};
float rpmMotorNow [4]={0,1,2,3};
int rpmCounter=0;
int motorNumber=0;
const int rpm_threshold = 10;      // An RPM below 10% the expected RPM value at a specific speed will be considered a stall
const int rpm_large = 175;      // 175 This is the official RPM an unloaded large EV3/NXT motor has
float rpmComparisionValue=0;

// Create struct to hold sensor data
tHTMC compass;

//local data for calculations
float lon1;
float lat1;

//destination data for calculations
float lon2;
float lat2;

float distance;//distance to destination
short targetangle;//angle to destination

int max_velocity=55;
int Velocity=0;
float curvefactor_ahead=1.3; //higher value => smaller curves, lower value => larger curves

float dx,dy;
int sonarvalue=0;

//Declare functions
void turnLeft(int Velocity,float curvefactor, long ratio);
void turnRight(int Velocity,float curvefactor, long ratio);
void driveBack(int Velocity, int distance);
void Positioning();
void avoidObstacle ();
void setRPMTargetToZero();
void readCompass_and_show();
void stopAllMotors();
void checkMotorStall();


float calculate_distance()
{
   float l_factor=(float)111300*cos(((lat1+lat2)/2)*PI/180);
   dx=l_factor*(float)(lon1-lon2);
   dy=(float)111300*(lat1-lat2);
   distance=sqrt(dx*dx+dy*dy);
   return distance;         //in meters !!!
}

float calculate_targetangle()//angle for destination
{
   if (lon2>lon1&&lat2>lat1)
      targetangle=90-atan((dy/dx))*180/PI;//acos(distance_L/distance)*180/PI;
   if(lon2>lon1&&lat1>lat2)
      targetangle=180-(90+atan(dy/dx)*180/PI);//(distance_L/distance)*180/PI+90;
   if (lon1>lon2&&lat1>lat2)
      targetangle=270-atan(dy/dx)*180/PI;//(distance_L/distance)*180/PI+180;
   if (lon1>lon2&&lat2>lat1)
      targetangle=360-(90+atan(dy/dx)*180/PI);//(distance_L/distance)*180/PI+270;
   return targetangle;
}

void avoidObstacle () //drive back for 50cm and turn left for 3 seconds
{
   driveBack(30,50); //speed of 30% and distance of 50 cm
   clearTimer (timer1);
   while (time1[T1] <2000)
   {
      turnLeft(40, 1.6, 25); //speed of 40% and curvefactor of 1.6
   }
   stopAllMotors();
}

//======================== DRIVING FUNCTIONS =======================================

//With this function the robot drives ahead to the waypoint in curves.
//It compares continuously its currently heading with the target heading and corrects its heading, if necessary.

void driveAhead()
{
   playSoundFile("Forward");

   float Encoderdegrees=(float)(distance*100*360)/WHEELCIRCUMSTANCE; //*100 because of meters!!
   resetMotorEncoder(backLeftMotor);

   clearTimer(T2);

   rpmCounter=0;

   while (nMotorEncoder[backLeftMotor] < Encoderdegrees)
   {
      readCompass_and_show(); //read compass values every 200msec

      if (time1[T2]<2000)
      {
         Velocity=max_velocity/2;
      }
      if (time1[T2]>=2000)
      {
         Velocity=max_velocity;
      }
      if (compass.relativeHeading>0)//turn left
      {
         turnLeft(Velocity, curvefactor_ahead, 15);
      }
      if (compass.relativeHeading<0) //turn right
      {
         turnRight(Velocity, curvefactor_ahead,15);
      }

      if (compass.relativeHeading==0) //straight ahead
      {
         setMotorSync(frontLeftMotor, frontRightMotor, 0, -Velocity);
         setMotorSpeed(backRightMotor, Velocity);
         setMotorSpeed(backLeftMotor, Velocity);
         rpmSetPoint [0]=Velocity*rpm_large/100;
         rpmSetPoint [1]=Velocity*rpm_large/100;
         rpmSetPoint [2]=Velocity*rpm_large/100;
         rpmSetPoint [3]=Velocity*rpm_large/100;
      }

      //monitoring stall of motors
      checkMotorStall();

      //check for obstacles!!
      sonarvalue = SensorValue[sonar];
      if (sonarvalue<30 && sonarvalue>0)//>0 because of avoiding inital error
      {
         playSoundFile("Ouch");
         avoidObstacle ();
         wait1Msec(250);
      }
   }
   setRPMTargetToZero(); //Reset stall detection

   stopAllMotors();

   wait1Msec(500);
}


void driveBack(int Velocity, int distance)//distance in cm
{
   float Encoderdegrees=(float)(distance)*360/WHEELCIRCUMSTANCE;
   setMotorSyncEncoder(backLeftMotor, backRightMotor, 0, Encoderdegrees, Velocity);
   moveMotorTarget(frontLeftMotor, -Encoderdegrees, -Velocity);
   moveMotorTarget(frontRightMotor, -Encoderdegrees, -Velocity);
   waitUntilMotorStop(backLeftMotor);
}


void turnRight(int Velocity, float curvefactor, long ratio)
{
   setMotorSync(frontLeftMotor,frontRightMotor, ratio, -Velocity);
   setMotorSpeed(backLeftMotor, Velocity);
   setMotorSpeed(backRightMotor, Velocity/(curvefactor*1.1)); //1.1 = correction because of different track width of front and back wheels
   rpmSetPoint [0]=Velocity*rpm_large/100;
   rpmSetPoint [1]=(Velocity/curvefactor)*rpm_large/100;
   rpmSetPoint [2]=Velocity*rpm_large/100;
   rpmSetPoint [3]=(Velocity/(curvefactor*1.1))*rpm_large/100;

}

void turnLeft(int Velocity, float curvefactor, long ratio)
{
   setMotorSync(frontRightMotor, frontLeftMotor, -ratio, -Velocity);
   setMotorSpeed(backRightMotor, Velocity);
   setMotorSpeed(backLeftMotor, Velocity/(curvefactor*1.1)); //1.1 = correction
   rpmSetPoint [0]=(Velocity/curvefactor)*rpm_large/100;
   rpmSetPoint [1]=Velocity*rpm_large/100;
   rpmSetPoint [2]=(Velocity/(curvefactor*1.1))*rpm_large/100;
   rpmSetPoint [3]=Velocity*rpm_large/100;

}

void stopAllMotors()
{
   setMotorSpeed(frontLeftMotor,0);
   setMotorSpeed(frontRightMotor,0);
   setMotorSpeed(backLeftMotor,0);
   setMotorSpeed(backRightMotor,0);
}

void setRPMTargetToZero() //reset stall detection
{
   for (int i=0; i<4; i++)
   {
      rpmSetPoint [i]=0;
   }
}

void readCompass_and_show()
{
   sleep (200);
   readSensor(&compass);
   eraseDisplay();
   displayBigTextLine(2, "Target: %4d", compass.offset);
   displayBigTextLine(6, "Abs:   %4d", compass.heading);
   displayBigTextLine(10, "Rel:   %4d", compass.relativeHeading);
}


void checkMotorStall()
{
   if (   rpmCounter>15)
   {
      writeDebugStreamLine("motor %d is stalled    rpmMotorNow %f    target %f", motorNumber, rpmMotorNow [motorNumber],rpmComparisionValue);
      stopAllMotors();
      driveBack(30,50);
      rpmCounter=0;
   }
}


//================= Positioning ====================================================================

//With the function "Positioning" the robot should set to the target angle as quick as possible.
//After setting to the right heading, the robot has to adjust itself continuously during its journey using the function "driveAhead".

void Positioning()
{
   setRPMTargetToZero(); //reset stall detection, set rpmSetPoint to zero

   compass.offset=targetangle;

   readCompass_and_show();

   while (compass.relativeHeading < -2 || compass.relativeHeading >2)
   {
      readCompass_and_show();
      if (compass.relativeHeading>0)
      {
         turnLeft(20,1.5,25);
      }
      if (compass.relativeHeading<0)
      {
         turnRight(20,1.5,25);
      }
      checkMotorStall();
   }
   playSoundFile("Okay");
   stopAllMotors();
   sleep(250);
}

//====================================================================================================


//===================== COMPASS CALIBRATION (Code by Xander Soldaat) ==================================
// Start the calibration and complain loudly if something goes wrong
void startCalibration() {
   if (!sensorCalibrate(&compass)) {
      eraseDisplay();
      displayTextLine(1, "ERROR: Couldn't");
      displayTextLine(2, "calibrate sensor.");
      displayTextLine(4, "Check connection");
      displayTextLine(5, "and try again.");
      playSound(soundException);
      while(bSoundActive) sleep(1);
      sleep(5000);
      stopAllTasks();
   }
}

// Stop the calibration and complain loudly if somethign goes wrong
void stopCalibration() {
   if (!sensorStopCalibrate(&compass)) {
      eraseDisplay();
      displayTextLine(1, "ERROR: Calibration");
      displayTextLine(2, "has failed.");
      displayTextLine(4, "Check connection");
      displayTextLine(5, "and try again.");
      playSound(soundException);
      while(bSoundActive) sleep(1);
      sleep(5000);
      stopAllTasks();
      } else {
      displayTextLine(1, "SUCCESS: ");
      displayTextLine(2, "Calibr. done.");
      playSound(soundUpwardTones);
      while(bSoundActive) sleep(1);
      sleep(5000);
   }
}


///============================ HELP-TASKS =================================

task checkBattery()
{
   while (true)
   {
      //eraseDisplay();
      //displayBigTextLine(2, "Voltage: %2f", getBatteryVoltage());
      if (getBatteryVoltage()<7.00)
      {
         clearSounds();
         playSoundFile("Uh-oh");
      }
      sleep(5000);
   }
}


task stallMonitoringTask()
{

   setRPMTargetToZero(); //reset stall detection, set rpmSetPoint to zero

   while (true)
   {
      rpmMotorNow [0]=getMotorRPM(frontLeftMotor);
      rpmMotorNow [1]=getMotorRPM(frontRightMotor);
      rpmMotorNow [2]=getMotorRPM(backLeftMotor);
      rpmMotorNow [3]=getMotorRPM(backRightMotor);

      // If motors RPM fall below 10 percent of target RPM the motors stalled. Then call function "avoid obstacle"
      for (int i = 0; i < 4; i++)
      {
         rpmComparisionValue=rpm_threshold*rpm_large*rpmSetPoint [i]/10000;

         if (rpmMotorNow [i]< rpmComparisionValue)
         {
            rpmCounter++;
            motorNumber=i;
         }
      }
      sleep(100);
   }
}

task EnterButtonPressed()
{
   while(true)
   {
      //While the left button (5) is pressed
      if (getButtonPress(2) == 1)
      {
         stopAllTasks();
      }
   }
}


//===========================================================================

task main()

{
   // Initialise and configure struct and port
   initSensor(&compass, S1);

   startTask (EnterButtonPressed);

   startCalibration();
   clearTimer (timer1);
   while (time1[T1] <12000)
   {
      turnLeft(20, 2.0, 25); //with a speed of 20% and a curvefactor of 2.0 and a motor power ratio of 25
   }
   stopAllMotors();

   stopCalibration();

   resetSensorConn(S1); //important!! Otherwise the EV3 will freeze!


   startTask (checkBattery);

   startTask(stallMonitoringTask);


   //================ DRIVING program The coordinates are not these ones I used in the video!!! =====================================
   //Coordinates for the starting point
   lon1= 9.574336;
   lat1= 54.484920;

   //1. waypoint
   lon2 = 9.574256;
   lat2 = 54.485194;
   calculate_distance();
   calculate_targetangle();
   Positioning();         
   wait1Msec(250);
   driveAhead();
   playSoundFile("One");

   //2. waypoint
   lon1=lon2;
   lat1=lat2;
   lon2 =  9.574743;
   lat2 =  54.485235;
   calculate_distance();
   calculate_targetangle();
   Positioning();         
   wait1Msec(250);
   driveAhead();
   playSoundFile("Two");

   //3. waypoint
   lon1=lon2;
   lat1=lat2;
   lon2 =  9.574801;
   lat2 =  54.484959;
   calculate_distance();
   calculate_targetangle();
   Positioning();      
   wait1Msec(250);
   driveAhead();
   playSoundFile("Three");

   //4. waypoint = starting point
   lon1=lon2;
   lat1=lat2;
   lon2 =  9.574336;
   lat2 =  54.484920;
   calculate_distance();
   calculate_targetangle();
   Positioning();      
   wait1Msec(250);
   driveAhead();
   playSoundFile("Four");


   wait1Msec (750);
   playSoundFile("Ready");

   wait1Msec(1000);

   resetSensorConn(S1); //important!! Otherwise the EV3 will freeze!
   stopAllTasks();
}