//*!!Sensor,    S1,      SortStartSensor, sensorTouch,      ,                    !!*//
//*!!Sensor,    S2,         colourSensor, sensorI2CCustomStd,      ,             !!*//
//*!!Motor,  motorA,       levelSortMotor, tmotorNxtEncoderClosedLoop,           !!*//
//*!!Motor,  motorB,            sortMotor, tmotorNxtEncoderClosedLoop,           !!*//
//*!!Motor,  motorC,            kickMotor, tmotorNxtEncoderClosedLoop,           !!*//
//*!!                                                                            !!*//
//*!!Start automatically generated configuration code.                           !!*//
const tSensors sortStartSensor      = (tSensors) S1;   //sensorTouch        //*!!!!*//
const tSensors colourSensor         = (tSensors) S2;   //sensorI2CCustomStd //*!!!!*//
const tMotor   levelSortMotor       = (tMotor) motorA; //tmotorNxtEncoderClosedLoop //*!!!!*//
const tMotor   sortMotor            = (tMotor) motorB; //tmotorNxtEncoderClosedLoop //*!!!!*//
const tMotor   kickMotor            = (tMotor) motorC; //tmotorNxtEncoderClosedLoop //*!!!!*//
//*!!CLICK to edit 'wizard' created sensor & motor configuration.                !!*//

//********************************************************//
//Brick Sorter  																					//
//																												//
//Robot sorts bricks of varying colours										//
//																		    								//
//Port 2: HiTechnic Colour Sensor					  							//
//********************************************************//


//Assign port
//const tSensors cColourPort			= S2;
// for I2C Messaging - reading the HiTechnic Colour Sensor
#define		cColourPort  				S2    // Connect CMPS sensorto this port!!

//Colour Address
#define kColourAddr		 		0x02

//Memory location
#define kColourNumber			0x42
#define kColourRed		 		0x43
#define kColourGrn		 		0x44
#define kColourBlu		 		0x45

//for motor stall task
#define bMotorAStalled()   (nStallCount > 3)

int turnAngle=0, lastTurnAngle=0, processFlag=0, colour=0, i=0;
int levelSort=0, prevLevelSort=0, nStallCount = 0;



// reads the HiTechnic Colour Sensor colour value returned via I2C
int ReadColour()
{
  int n = 0;
  byte colourRply[1];
  byte colourMsg[3];

  const int kMsgSize 			= 0;
  const int kMsgAddr 			= 1;
  const int kMsgReadAddr 		= 2;

  //Construct I2C message for bearing
  colourMsg[kMsgSize] 			= 2;
  colourMsg[kMsgAddr]			= kColourAddr;

   	colourMsg[kMsgReadAddr] = kColourNumber;

    //Wait
    while (nI2CStatus[cColourPort] == STAT_COMM_PENDING);
    if ( nI2CStatus[cColourPort] != NO_ERR ) {  // probably sensor is missing.
      return (-1);
    }
    //Send I2C message
    sendI2CMsg(cColourPort, colourMsg[0], 1);

    //Wait
    while (nI2CStatus[cColourPort] == STAT_COMM_PENDING);

    //Receive I2C reply
    readI2CReply(cColourPort, colourRply[0],1);



   n = colourRply[0];
    //n = colourRply;
  	nxtDisplayClearTextLine(1);
    nxtDisplayString(1, "IC2 Colour: %d", n);

    if ( n == -1 ) {  // -1 is reserved to report errors, so
      n = 0;          // change it to zero.
    }

    return(n);

}




void kickTheBrick()
{
  //kicks the brick out of the holder for sorting

      if (processFlag == 1)
      {

      do
      {
        	motor[kickMotor] =  50;
        	nxtDisplayString(6, "KickMotor: %d", nMotorEncoder[kickMotor]);
      }
      while (abs(nMotorEncoder[kickMotor]) < 361);



      motor[kickMotor] = 0;
      nMotorEncoder[kickMotor] = 0;		//motor encoder initialized to 0

      //wait10Msec(100);

      //nudge();
      //wait until the motor has stopped moving.
      while (nMotorRunState[kickMotor] != runStateIdle)
      {} // wait for action to complete

       processFlag = 0;
      }


}


void doSort(int turnAngle)
{
  nxtDisplayTextLine(2, "Sorting...");
  nxtDisplayClearTextLine(3);
  nxtDisplayClearTextLine(4);
	nxtDisplayString(3, "LastAngle: %d", lastTurnAngle);
	nxtDisplayString(4, "Angle2Turn: %d", turnAngle);

  //ucomment this to make it pause and wait for the touch sensor to be triggered by hand
	//while (SensorValue[sortStartSensor] != 1);

  //wait10Msec(100);
	//only move the sorter if the brick colour changed and the sorter needs to be moved
	if (turnAngle != lastTurnAngle)
	{

   if (turnAngle > nMotorEncoder[sortMotor])
    {
    	//since the desired turnAngle value is > than the current motor sensor value, we must
    	//move only by the difference of the two.
    	//turnAngle = turnAngle - nMotorEncoder[sortMotor];
    	//move the sorter based on the angle difference between the last angle and the new angle.

	    do
      {
        	motor[sortMotor] =  60;
        	nxtDisplayClearTextLine(5);
        	nxtDisplayString(5, "Motor: %d", nMotorEncoder[sortMotor]);
      }
      while (nMotorEncoder[sortMotor] < turnAngle + 1);

   }

   else

    {
    	//since the desired turnAngle value is < than the current motor sensor value, we must
    	//move backwards by the difference.
    	//turnAngle =  nMotorEncoder[sortMotor]- turnAngle;
    	//move the sorter based on the angle difference between the last angle and the new angle.
    	do
      {
        	motor[sortMotor] =  -60;
     	  	nxtDisplayClearTextLine(5);
        	nxtDisplayString(5, "Motor: %d", nMotorEncoder[sortMotor]);
      }
      while (nMotorEncoder[sortMotor] > turnAngle - 1);

    }
   	motor[sortMotor] =  0;
   }
	//reset the turn angle variable for the next round
	//turnAngle = 0;

  //set another variable so we know what the last turn angle was. Will be used to check next round through
  //to determine if the sorter needs to turn at all.
  lastTurnAngle = turnAngle;


  //wait until motor stops spinning
  while (nMotorRunState[sortMotor] != runStateIdle)
  {} // wait for action to complete



  //shift the levelSort up to reach further tray
  if (levelSort == 1)
  {
  	 //only change the levelSort if it is not already there
  	 if (prevLevelSort == 0)
  	 {
        do
        {
       	  motor[levelSortMotor] =  60;
        }
        while ((nMotorEncoder[levelSortMotor]) <= 10);
        motor[levelSortMotor] =  0;
        prevLevelSort = 1;
   }

  }
  else
  {
  	 //put the levelSort back to default.
     do
      {
        motor[levelSortMotor] =  -60;
      }
      while ((nMotorEncoder[levelSortMotor]) >= 10);
      motor[levelSortMotor] =  0;
      prevLevelSort = 0;

  }

  levelSort = 0;



  nxtDisplayClearTextLine(2);
  nxtDisplayTextLine(2, "Sorting: Done!");


  //once the sorter has turned, we need to kick the brick out.
  kickTheBrick();

}


void checkBrick()
{
	nxtDisplayClearTextLine(2);
  nxtDisplayTextLine(2, "Chk Colour: Yes");

    //starting to process brick. set the flag back so that we dont initiate same task multiple times - has to wait until all are done
    processFlag = 1;
    i = 0;


   	for (i=0; i<=9; ++i)
    {
      colour = colour + ReadColour();
      wait1Msec(20);
    }

    colour = colour / 10;

    //colour = ReadColour();

      // will setup angles so that hoppers are in the following order:
      //0 degrees (base) - all other colours



       switch(colour)
      {
        case 0:
        //nothing detected.
        turnAngle = -999;
        break;

        case 1:
        //not sure yet - might be gray
        turnAngle = 150;
        //shift to outer sort grid
        levelSort = 1;
        break;

        case 2:
        //blue
        turnAngle = 150;
        break;

        case 4:
        //green
        turnAngle = 300;
        break;

        case 5:
        //yellow
        turnAngle = 450;
        break;

        case 7:
        //orange
        turnAngle = 450;
        //shift to outer sort grid
        levelSort = 1;
        break;


        case 9:
        //red
        turnAngle = 580;
        break;

        case 14:
        //white
        turnAngle = 0;
        break;

        default:
        turnAngle = 0;
        break;

       }




    //if an apprriate colour was detected, turn the sorter. The Sorter will then initiate the brick kick.
    if (turnAngle >= 0)
    {
      //PlaySoundFile("! Sonar.rso");
      doSort(turnAngle);
      //kickTheBrick();
      //while (SensorValue[sortStartSensor] != 1);

    }
    else
    {
     processFlag = 0;
     //still need to kick the brick to get it out if not detected correctly.
     //kickTheBrick();
    }

 	nxtDisplayClearTextLine(7);
 	nxtDisplayString(7, "ColourVal = %d", colour);

	//reset the colour value for the next brick reading.
	colour = 0;



}




task stallMonitoringTask()
{
  int nLastEncoder;

  //
  // Check every 50 msec to see if the motor encoder has moved. If not, increment a counter
  //
  nLastEncoder = nMotorEncoder[levelSortMotor];
  while (true)
  {
    int nEncoderDelta;

    wait1Msec(20);
    nEncoderDelta = (nMotorEncoder[levelSortMotor] - nLastEncoder);
    if ((nEncoderDelta > 2) || (nEncoderDelta < -2))
    {
    	nStallCount = 0;
    	nLastEncoder = nMotorEncoder[levelSortMotor];
    }
    else
    {
      if (nStallCount < 1000)
        ++nStallCount;
    }
  }
}




task main()
{

  //Declare sensor as type I2C custom
  //Thanks to Dick Swan for reminding me to do this!!
  nI2CBytesReady[cColourPort] = 0;
  SensorType[cColourPort] = sensorI2CCustomFast9V;

  nMotorEncoder[kickMotor] = 0;		//motor encoder initialized to 0

    //make sure the levelSortMotor is initialized and reset
    StartTask(stallMonitoringTask);   // This will start the stall monitoring task
    do
    {
      motor[levelSortMotor] =  -30;
    }
    while (bMotorAStalled() == false);

    motor[levelSortMotor] =  0;
    nMotorEncoder[levelSortMotor] = 0;		//motor encoder initialized to 0
    StopTask(stallMonitoringTask);
    PlaySound(soundBeepBeep);

  //commented to test ReadSensor() function. Uncomment to return to working.
  if (SensorValue[sortStartSensor] != 1)
  //if (SensorValue[sortStartSensor] == 1)
  {
	  //only move the sorter  back if its not already there.
    do
    {
     //get the sorter to the starting position
      	motor[sortMotor] =  -40;

    }
    while (SensorValue[sortStartSensor] != 1);

    motor[sortMotor] =  0;
        PlaySound(soundFastUpwardTones);
  }
  //initialize after it has returned to 0 position.
  nMotorEncoder[sortMotor] = 0;		//motor encoder initialized to 0

  //initiate the first brick
  //checkBrick();

  //repeat the brick processing forever
  while(true)
  {
   // only initiate processing of all previous tasks are done.
   if (processFlag == 0)

   {
     checkBrick();
   }

  }

}