#pragma config(Sensor, S1,       touchSensor,       sensorTouch)
#pragma config(Sensor, S2,       HTCOMPASS,          sensorI2CCustom)
#pragma config(Sensor, S3,       HTGYRO,          sensorAnalogInactive)
#pragma config(Sensor, S4,       sonarSensor,       sensorSONAR)
#pragma config(Motor,    motorA,               sonarMotor,       tmotorNXT, PIDControl, encoder)
#pragma config(Motor,    motorB,               rightMotor,       tmotorNXT, PIDControl, encoder)
#pragma config(Motor,    motorC,               leftMotor,       tmotorNXT, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard                      !!*//

//Roomexplorer and mapping robot by Sigtrygg, December 2013

/* Credits :
* Special thanks to to Xander Soldaat for gyro and compass-drivers and gyro/compass example codes!
*
* License: You may use this code as you wish.
*/

#include "REM-Bot_Roomexplorer_functions_1.3.h" //all necessary functions are written in the header-file

//*************************************************************************************************

task mapping()
{
   eraseDisplay();
   time10[T2]=0;
   maxtime=(long)(nFileSize)*interval/(60*17);//in minutes
   //Prepare file-handling
   hFileHandle = 0;
   Delete(sFileName, nIoResult);
   OpenWrite(hFileHandle, nIoResult, sFileName, nFileSize);

   while (mode_exit !=1)
   {
      odometryValueOld=nMotorEncoder[rightMotor];
      //eraseDisplay();
      valTime10 = time10[T2];
      recordtime=(recordtime+valTime10/100)/60;
      //nxtDisplayStringAt(0, 60,"Recordtime: %d",recordtime);
      //nxtDisplayStringAt(0, 50,"Max.time: %d",maxtime);
      //nxtDisplayStringAt(0, 40,"Sonar: %d",SensorValue(sonarSensor));
      //nxtDisplayStringAt(0, 30,"Heading: %d",compassValue);
      wait10Msec(interval*100);//write data according interval!
      compassValue=HTMCreadHeading(HTCOMPASS);//get current heading

      if (recordtime==maxtime || nNxtButtonPressed==3)//if recordtime is reached or the orange Key is pressed stop!
      {
         wait1Msec(500);
         Close(hFileHandle, nIoResult);

         mode_exit=1;//when reaching the max. recordtime, exit with closing file
      }
      if (touchflag==0)//only write coordinates if touch sensor is 0 and after robot has repositioned
      {
         odometryValueCurrent=nMotorEncoder[rightMotor];
         odometryValueRadius=(odometryValueCurrent-odometryValueOld)*WHEELCIRCUMFERENCE/360;//radius in cm
         //calculate cartesian coordinates according to start point
         cartesX+=-(cosDegrees(compassValue)*odometryValueRadius);//calculating with float!!
         cartesY+=(sinDegrees(compassValue)*odometryValueRadius);
         //convert int values to string values for file-writing
         StringFormat(scartesX, "%d", round(cartesX));//show only rounded integer
         StringFormat(scartesY, "%d", round (cartesY));
         //convert int values to string values for file-writing (Polar)
         StringFormat(sodometryValueRadius, "%d", round(odometryValueRadius));
         StringFormat(scompassValue, "%d", round(compassValue));
         //write cartesian coordinates to file
         WriteText(hFileHandle, nIoResult, "\r\n");
         WriteText(hFileHandle, nIoResult, sodometryValueRadius);
         WriteText(hFileHandle, nIoResult, ",");
         WriteText(hFileHandle, nIoResult, scompassValue);
         WriteText(hFileHandle, nIoResult, ",");
         WriteText(hFileHandle, nIoResult, scartesX);
         WriteText(hFileHandle, nIoResult, ",");
         WriteText(hFileHandle, nIoResult, scartesY);
         //Draw Map-Pixels on screen
         Pixel_x=round((cartesX+PixelOffset_x)*DRAW_SCALE);
         Pixel_y=round((cartesY+PixelOffset_y)*DRAW_SCALE);
         nxtSetPixel(Pixel_x, Pixel_y);
      }
   }
   sonarLeftPosition();
   wait1Msec(500);
   Close(hFileHandle, nIoResult);

   while(nNxtButtonPressed!=0)//wait until escape button is pressed to take a look at the map
   {
      motor[rightMotor]=0;
      motor[leftMotor]=0;
   }
}
//*****************************************************************************************************************

task main()
{
   //Calibrate the gyro, make sure you hold the sensor still
   wait1Msec(2000);
   HTGYROstartCal(HTGYRO); //returns offset of Gyro (e.g. 610), it depends on temperature and other environmental influences.
   wait1Msec(1000);

   //Calibrate the compass
   compass_calibration();

   //----------------------------- behavior ---------------------------------------------------------

   //check in which direction the nearest wall is and drive to it. Then turn 90° left
   for (int i=0; i<3; i++)
   {
      sonarangle[0]=0;
      sonarturnvelocity[0]=0;
      sonarangle[1]=92;
      sonarturnvelocity[1]=15;
      sonarangle[2]=-180;
      sonarturnvelocity[2]=-15;
      nMotorEncoder[sonarMotor] = 0;
      nMotorEncoderTarget[sonarMotor] = sonarangle[i];
      motor[sonarMotor] = sonarturnvelocity[i];
      while (nMotorRunState[sonarMotor] != runStateIdle)
      {
         // do not continue
      }
      motor[sonarMotor] = 0;
      //get the average of 10 measures
      for (int t=0;t<10;t++)
      {
         wait1Msec(50);
         distancevalue[i]=SensorValue(sonarSensor);
         distancevaluesum+=distancevalue[i];
      }

      distancevalue[i]=distancevaluesum/10;
      nxtDisplayCenteredBigTextLine(4, "%2d",distancevalue[i]);
      wait1Msec(1500);
      distancevaluesum=0;
      eraseDisplay();
      // measure end
   }
   //get minimum distance and its angle
   minimumdistance=256;
   for (int i=0; i<3; i++)
   {
      if (distancevalue[i]<minimumdistance)
      {
         minimumdistance=distancevalue[i];
         firstgodirection=sonarangle[i];
      }
   }
   //get sonar motor back to 0 degree position
   nMotorEncoder[sonarMotor] = 0;
   nMotorEncoderTarget[sonarMotor] =92;
   motor[sonarMotor] =15;
   while (nMotorRunState[sonarMotor] != runStateIdle)
   {
      // do not continue
   }
   motor[sonarMotor] = 0;
   eraseDisplay();
   nxtDisplayCenteredBigTextLine(2, "%3d",firstgodirection);
   wait1Msec(2000);

   //turn to direction of minimum distance
   switch(firstgodirection)
   {
   case 92:
      turn(90,'r',TURN_VELOCITY);
      break;
   case -180:
      turn(90, 'l',TURN_VELOCITY);
      break;
   }

   //start mapping
   StartTask(mapping);

   //drive ahead to the wall until sonar distance=20cm
   wait1Msec(500);
   forward_until_sonardetection(20);
   wait1Msec(500);
   turn(90,'l',TURN_VELOCITY);

   //get sonar back to front position (90 degrees)
   sonarRightPosition();

   /*drive along the wall and hold distance to it for 25cm and stop, if touchSensor detected bumping, then drive back, turn 90°
   and continue driving beneath the wall until the orange button is pressed or record-time is reached*/

   driveBySonarSideCheck();
}

//REM-Bot with hitechnic gyro, hitechnic compass and touch sensor
//Roomexplorer and mapping robot by Sigtrygg, December 2013

/* Credits :
* Special thanks to to Xander Soldaat for gyro and compass-drivers and gyro/compass example codes!
*
* License: You may use this code as you wish.
*/


#include "drivers/hitechnic-gyro.h"
#include "drivers/hitechnic-compass.h"

//Constants REM-Bot
const float WHEELCIRCUMFERENCE = 17.7;   //(cm)
const float WHEELTRACK = 12.06;   //Trackwidth (cm)
const int TURN_VELOCITY = 20;
const int EXPLORE_VELOCITY=15;

//globals
float Geschw_verh=0;
//int Geschw=0;
//int Drehradius=0;
int sonarangle[3];//angle of sonarhead (0, 90, 180)
int distancevalue[3];//array for the distances in three directions (0, 90, 180)
int sonarturnvelocity[3];//speed of sonarhead-motor
int distancevaluesum=0;//sum of measured distances
int minimumdistance=0;//nearest distance to next wall
int firstgodirection=0;//direction to the nearest wall
int minimumSonarDistance=28; //minimum distance to wall = 28cm
int touchflag=0;//if touch sensor is actiive don't mapping

//variable for map drawing on screen (according to livingroom)
int Pixel_x=0;
int Pixel_y=0;
const float DRAW_SCALE=0.15;//0.1767;
const float PixelOffset_x=345.0;
const float PixelOffset_y=190.0;


int interval=1; //continous mode writes data every 1 seconds
int maxtime=0;
int recordtime=0;
long valTime10=0;
int mode_exit=0;

// define values for polar coordinates
int compassValue=0; //angle of current heading
int odometryValueOld=0; //for determine radius for polar coordinates
int odometryValueCurrent=0; //for determine radius for polar coordinates
float odometryValueRadius=0; //has to be float, because of needing precise values for calculating cartesian coordinates

// define values for cartesian coordinates
float cartesX=0;
float cartesY=0;
string scartesX = "";
string scartesY = "";

string sodometryValueRadius="";
string scompassValue="";

// File handling
TFileIOResult nIoResult;
TFileHandle hFileHandle;
// NOTE ON FILESIZE:  Every data point added is about 17 bytes in size.
int nFileSize = 10000; //about 10 min datalogging with an interval of 1s
const string sFileName = "map.txt";




//=========== DECLARE FUNCTIONS ================================================
//******* FUNCTION drive ahead *************************************************
void driveAhead(int velocity, float distance);

//******* FUNCTION drive back **************************************************
void driveBack(int velocity, float distance);

//******* FUNCTION turn left or right for specific angle while using a GYRO-sensor ***
void turn(int degrees, char direction , int velocity);

//******* FUNCTION turn left  **************************************************
void turnLeft(int Turnvelocity, int Turnangle);

//******* FUNCTION turn right  *************************************************
void turnRight(int Turnvelocity, int Turnangle);

//******* FUNCTION forward_until_touch *****************************************
void forward_until_touch();

//******* FUNCTION forward_until_sonar_detection *******************************
void forward_until_sonardetection(int sonar_distance);

//******* FUNCTION turn right with radius **************************************
void rechtsDrehung(int Geschw,int Drehradius);

//******* FUNCTION turn left with radius ***************************************
void linksDrehung(int Geschw, int Drehradius);

//******* FUNCTION turn left with radius for a specific angle ******************
void linksDrehungWinkel(int Geschw, int Drehradius, int angle);

//******* FUNCTION COMPASS CALIBRATION *****************************************
void compass_calibration();

//******* FUNCTION Drive by sonar side check ***********************************
void driveBySonarSideCheck();

//******* FUNCTION Sonar-head to right position ********************************
void sonarRightPosition();

//******* FUNCTION Sonar-head to left position ********************************
void sonarLeftPosition();

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



//******* FUNCTION drive ahead **************************************
//void driveAhead(int velocity, float distance)
//{
//   nSyncedMotors = synchBC;   // sync motors B and C so that B is the master and C is the slave
//   nSyncedTurnRatio = 100;

//   float Angle_sum = distance * 360 / WHEELCIRCUMFERENCE;
//   nMotorEncoder[rightMotor] = 0;
//   nMotorEncoderTarget[rightMotor] = Angle_sum;
//   motor[rightMotor] = velocity;
//   while (nMotorRunState[rightMotor] != runStateIdle)
//   {
//      // do not continue
//   }
//   motor[rightMotor] = 0;
//}

//******* FUNCTION drive ahead **************************************
//void driveAhead(int velocity, float distance)
//{
//   nSyncedMotors = synchBC;   // sync motors B and C so that B is the master and C is the slave
//   nSyncedTurnRatio = 100;

//   float Angle_sum = distance * 360 / WHEELCIRCUMFERENCE;
//   nMotorEncoder[rightMotor] = 0;

//   while (nMotorEncoder[rightMotor] < Angle_sum)
//   {
//      motor[rightMotor] = velocity;
//   }
//   motor[rightMotor] = 0;
//}

//******* FUNCTION drive ahead **************************************
void driveAhead(int velocity, float distance)
{

   float Angle_sum = distance * 360 / WHEELCIRCUMFERENCE;
   nMotorEncoder[rightMotor] = 0;
   nMotorEncoder[leftMotor] = 0;
   nMotorEncoderTarget[rightMotor] = Angle_sum;
   nMotorEncoderTarget[leftMotor] = Angle_sum;
   motor[rightMotor] = velocity;
   motor[leftMotor] = velocity;
   while (nMotorRunState[rightMotor] != runStateIdle || nMotorRunState[leftMotor] != runStateIdle )
   {
      // do not continue
   }
   motor[rightMotor] = 0;
   motor[leftMotor] = 0;
}


//******* FUNCTION drive back **************************************
void driveBack(int velocity, float distance)
{
   float Angle_sum = -distance * 360 / WHEELCIRCUMFERENCE;
   nMotorEncoder[rightMotor] = 0;
   nMotorEncoder[leftMotor] = 0;
   nMotorEncoderTarget[rightMotor] = Angle_sum;
   nMotorEncoderTarget[leftMotor] = Angle_sum;
   motor[rightMotor] = -velocity;
   motor[leftMotor] = -velocity;
   while (nMotorRunState[rightMotor] != runStateIdle || nMotorRunState[leftMotor] != runStateIdle )
   {
      // do not continue
   }
   motor[rightMotor] = 0;
   motor[leftMotor] = 0;
}

//******* FUNCTION turn left or right for specific angle while using a GYRO-sensor ***
void turn(int degrees, char direction, int velocity)
{

   //correction is necessary because of drifting !!
   int correction=0;

   if (degrees==90)
      correction=3; //three degrees correction

   float rotSpeed = 0;
   float heading = 0;
   nSyncedMotors = synchNone;

   // Reset the timer.
   ClearTimer(T1);

   while (abs(heading) < (degrees-correction))
   {
      // Wait until 20ms has passed
      while (time1[T1] < 20)
         wait1Msec(1);

      // Reset the timer
      ClearTimer(T1);

      // Read the current rotation speed
      rotSpeed = HTGYROreadRot(HTGYRO);

      // Calculate the new heading by adding the amount of degrees
      // we've turned in the last 20ms
      // If our current rate of rotation is 100 degrees/second,
      // then we will have turned 100 * (20/1000) = 2 degrees since
      // the last time we measured.
      heading += rotSpeed * 0.02;

      // Display our current heading on the screen
      //nxtDisplayCenteredBigTextLine(3, "%2.0f", heading);

      if (direction == 'r')
      {
         motor[rightMotor] = -velocity;
         motor[leftMotor] = velocity;
      }
      else if (direction == 'l')
      {
         motor[rightMotor] = velocity;
         motor[leftMotor] = -velocity;
      }
   }

   //stop
   motor[rightMotor] = 0;
   motor[leftMotor] = 0;
}

//******* FUNCTION turn left  ********************
void turnLeft(int Turnvelocity, int Turnangle)
{
   nMotorEncoder[leftMotor] = 0;
   while (nMotorEncoder[leftMotor] < Turnangle)
   {
      motor[rightMotor] = Turnvelocity;
      motor[leftMotor] = -Turnvelocity;

   }
   motor[rightMotor] = 0;
   motor[leftMotor] = 0;
}

//******* FUNCTION turn right  ********************
void turnRight(int Turnvelocity, int Turnangle)
{
   nMotorEncoder[leftMotor] = 0;
   while (nMotorEncoder[leftMotor] < Turnangle)
   {
      motor[rightMotor] = -Turnvelocity;
      motor[leftMotor] = Turnvelocity;

   }
   motor[rightMotor] = 0;
   motor[leftMotor] = 0;
}

//******* FUNCTION forward_until_touch **************************************
void forward_until_touch()
{
   nSyncedMotors = synchBC;
   nSyncedTurnRatio = 100;
   while (SensorValue(touchSensor) != 1)
   {
      motor[rightMotor] = 30;
   }
   motor[rightMotor] = 0;
}

//******* FUNCTION forward_until_sonar_detection *****************************
void forward_until_sonardetection(int sonar_distance)
{
   nSyncedMotors = synchBC;
   nSyncedTurnRatio = 100;
   while (SensorValue(sonarSensor) > sonar_distance)
   {
      motor[rightMotor] = EXPLORE_VELOCITY;
   }
   motor[rightMotor] = 0;
}

//******* FUNCTION turn right with radius ************************************
void rechtsDrehung(int Geschw,int Drehradius)
{
   Geschw_verh=(Drehradius+WHEELTRACK/2)/(Drehradius-WHEELTRACK/2);
   motor[rightMotor] = Geschw/Geschw_verh;
   motor[leftMotor] = Geschw;
}

//******* FUNCTION turn left with radius ************************************
void linksDrehung(int Geschw, int Drehradius)
{
   Geschw_verh=(Drehradius+WHEELTRACK/2)/(Drehradius-WHEELTRACK/2);
   motor[rightMotor] = Geschw;
   motor[leftMotor] = Geschw/Geschw_verh;
}

//******* FUNCTION turn left with radius for a specific angle ************************************
void linksDrehungWinkel(int Geschw, int Drehradius, int angle)
{
   Geschw_verh=(Drehradius+WHEELTRACK/2)/(Drehradius-WHEELTRACK/2);
   nMotorEncoder[rightMotor] = 0;

   int Kreisumfang=2*3.141*(Drehradius); //den das rechte Rad beschreibt in cm
   int Turnangle=(Kreisumfang/WHEELCIRCUMFERENCE)*angle;

   while (nMotorEncoder[rightMotor] < Turnangle)
   {
      motor[rightMotor] = Geschw;
      motor[leftMotor] = Geschw/Geschw_verh;
   }
}

//******* FUNCTION COMPASS CALIBRATION ************************************
void compass_calibration()
{

   wait1Msec(2000);

   if (!HTMCstartCal(HTCOMPASS)) {
      eraseDisplay();
      nxtDisplayTextLine(1, "ERROR: Couldn't");
      nxtDisplayTextLine(2, "calibrate sensor.");
      nxtDisplayTextLine(4, "Check connection");
      nxtDisplayTextLine(5, "and try again.");
      PlaySound(soundException);
      while(bSoundActive) EndTimeSlice();
      wait1Msec(5000);
      StopAllTasks();
   }

   turn(365, 'r', 5);//turn right a little bit over 360 degree at 5% speed using gyro

   motor[rightMotor] = 0;
   motor[leftMotor] = 0;

   if (!HTMCstopCal(HTCOMPASS)) {
      eraseDisplay();
      nxtDisplayTextLine(1, "ERROR: Calibration");
      nxtDisplayTextLine(2, "has failed.");
      nxtDisplayTextLine(4, "Check connection");
      nxtDisplayTextLine(5, "and try again.");
      PlaySound(soundException);
      while(bSoundActive) EndTimeSlice();
      wait1Msec(5000);
      StopAllTasks();
      } else {
      nxtDisplayTextLine(1, "SUCCESS: ");
      nxtDisplayTextLine(2, "Calibr. done.");
      PlaySound(soundUpwardTones);
      while(bSoundActive) EndTimeSlice();
      wait1Msec(5000);
   }
   eraseDisplay();

}

//******* FUNCTION Drive by sonar side check ************************************************

void driveBySonarSideCheck()
{
   while (true)
   {
      while (SensorValue(touchSensor) < 1)
      {
         wait1Msec(50);
         if (SensorValue(sonarSensor) > minimumSonarDistance)
         {
            rechtsDrehung(EXPLORE_VELOCITY,30);
         }
         else if (SensorValue(sonarSensor) < minimumSonarDistance)
         {
            linksDrehung(EXPLORE_VELOCITY,30);
         }
         else
         {
            motor[rightMotor]=EXPLORE_VELOCITY;
            motor[leftMotor]=EXPLORE_VELOCITY;
         }
      }
      touchflag=1;
      motor[rightMotor]=0;
      motor[leftMotor]=0;
      driveBack(EXPLORE_VELOCITY, 10);
      wait1Msec(250);
      turn(90,'l',TURN_VELOCITY);
      touchflag=0;
   }
   mode_exit=1;//end mapping and close data-file
}
//******* FUNCTION Sonar-head to right position *******************
void sonarRightPosition()
{
   //get sonar motor to 90 degree position
   nMotorEncoder[sonarMotor] = 0;
   nMotorEncoderTarget[sonarMotor] = 92;
   motor[sonarMotor] =25;
   while (nMotorRunState[sonarMotor] != runStateIdle)
   {
      // do not continue
   }
   motor[sonarMotor] = 0;
   wait1Msec(150);
}

//******* FUNCTION Sonar-head to left position *******************
void sonarLeftPosition()
{
   //get sonar motor to 90 degree position
   nMotorEncoder[sonarMotor] = 0;
   nMotorEncoderTarget[sonarMotor] = -92;
   motor[sonarMotor] =-25;
   while (nMotorRunState[sonarMotor] != runStateIdle)
   {
      // do not continue
   }
   motor[sonarMotor] = 0;
   wait1Msec(150);
}