//*!!Sensor,    S1,          TouchSensor, sensorTouch,      ,                    !!*//
//*!!Sensor,    S2,          SonarSensor, sensorSONAR,      ,                    !!*//
//*!!Sensor,    S3,          LightSensor, sensorLightActive,      ,              !!*//
//*!!                                                                            !!*//
//*!!Start automatically generated configuration code.                           !!*//
const tSensors TouchSensor          = (tSensors) S1;   //sensorTouch        //*!!!!*//
const tSensors SonarSensor          = (tSensors) S2;   //sensorSONAR        //*!!!!*//
const tSensors LightSensor          = (tSensors) S3;   //sensorLightActive  //*!!!!*//
//*!!CLICK to edit 'wizard' created sensor & motor configuration.                !!*//

// Type Definitions
typedef enum
{
	Fastest = 95,
	Fast = 75,
	Normal = 60,
	Slow = 40,
	Slowest = 35,
	Stop = 0,
	Reverse = -100
} TSpeed;

typedef enum
{
	Up,
	Down
} TCraneDirection;



typedef enum
{
	Left,
	Right
} TDirection;

int Seconds;
bool EndProgram = false;
bool EndSyncMovement = false;
bool isSyncMovementRunning = false;
bool StopTimer = false;
TSpeed gSpeed = Stop;

inline SetSpeed(TSpeed Speed);
inline moveCrane(TCraneDirection craneDirection, int aLiftHeight);
inline Rotate(int lenght, TDirection direction);

// ############################################################

typedef struct
{
	float  X;
	float  Y;
	float  heading;    // Radians
} tLocation;

//tLocation target;
tLocation currPos;
tLocation maxDeviation;

//int motorPower   = 50;
const int k_h    = 5; //Kludege


void zeroLocation(tLocation &theLocation)
{
	theLocation.X = 0;
	theLocation.Y = 0;
	theLocation.heading = 0;
}

void calculateMaxDeviation()
{
	if (maxDeviation.X < abs(currPos.X))
		maxDeviation.X = abs(currPos.X);

	if (maxDeviation.Y < abs(currPos.Y))
		maxDeviation.Y = abs(currPos.Y);

	if (maxDeviation.heading < abs(currPos.heading))
		maxDeviation.heading = abs(currPos.heading);
	return;
}

task calculateCurrentPosition()
{
	float distance;
	static int lastLeft = 0;
	static int lastRight = 0;
	int currLeft;
	int currRight;
	int deltaLeft;
	int deltaRight;

	//
	// One-time initialization on startup
	//
	const float kWheelDiameterInInches = 2.25;
	const float kInchesPerEncoderCount = PI * kWheelDiameterInInches / 360;
	const float kWheelBaseInInches     = 4 + 7/(float)16;

	zeroLocation(maxDeviation);

	//
	// Loop every 25 msec
	//
	while (!EndProgram)
	{
		currLeft  = nMotorEncoder[motorA];
		currRight = nMotorEncoder[motorC];

		deltaLeft  = currLeft  - lastLeft;
		deltaRight = currRight - lastRight;

		lastLeft  = currLeft;
		lastRight = currRight;

		distance = (deltaLeft + deltaRight) / (float) 2.0;
		distance *= kInchesPerEncoderCount;

		currPos.heading += ((float) (deltaLeft - deltaRight)) * (kInchesPerEncoderCount / kWheelBaseInInches);
		if (currPos.heading > (2 * PI))
		  currPos.heading -= 2 * PI;
		else if (currPos.heading < -(2 * PI))
		  currPos.heading += 2 * PI;

		currPos.Y 			+= distance * sin(currPos.heading);
		currPos.X 			+= distance * cos(currPos.heading);

		calculateMaxDeviation();
		wait1Msec(50);
  }
}

task SyncMovement(){
  int nMoveSize     = 360;    // Ten rotation
  int nSpeed        = gSpeed;
  int nTurnRatio    = 100;  //C has same speed as A, but opposite direction
  //int nCount;

  const TSynchedMotors kSyncType     = synchAB;
  const tMotor         kPrimaryMotor = motorA;

  bFloatDuringInactiveMotorPWM = false;

  nMotorEncoder[kPrimaryMotor]       = 0;
  nMotorEncoderTarget[kPrimaryMotor] = 0;

  nSyncedMotors            = kSyncType;  // "C" will be synchronized to "A".
  nSyncedTurnRatio         = nTurnRatio;
  nPidUpdateInterval       = 10;

  while(!EndSyncMovement){
  	isSyncMovementRunning = true;

	    while (    (nMotorRunState[kPrimaryMotor] != runStateIdle)
	    				&& (nMotorRunState[kPrimaryMotor] != runStateHoldPosition))
	    {/*if(EndSyncMovement){break;}*/}

	    nMotorEncoderTarget[kPrimaryMotor] = nMoveSize; // incremental amount to move motor
	    motor[kPrimaryMotor]               = nSpeed;    // motor speed
	    while (    (nMotorRunState[kPrimaryMotor] != runStateIdle)
	    				&& (nMotorRunState[kPrimaryMotor] != runStateHoldPosition) && !EndSyncMovement)
	    {/*if(EndSyncMovement){break;}*/}

 	}
 	nSyncedMotors = synchNone;
  isSyncMovementRunning = false;
}

// ############################################################


task SensorMonitor(){
	while(!EndProgram){
		nxtDisplayTextLine(1, "Light Value = %d",SensorValue(LightSensor));
		nxtDisplayTextLine(2, "Sonar Value = %d",SensorValue(SonarSensor));
		nxtDisplayTextLine(4, "Time = %d:%d",(Seconds/60), Seconds);
		wait10Msec(10);
	}
}

task Timer(){
	PlayTone(1500,50);
	wait10Msec(10);
	Seconds = 0;
	while(!StopTimer){
		wait10Msec(100);
		Seconds++;
	}
	PlayTone(1500,50);
	wait10Msec(10);
}


task main()
{


  StartTask(calculateCurrentPosition);
	StartTask(SensorMonitor);
	nMotorEncoder[motorA] = 0;

		/* 1.	 Run
		 * 2.  Cross first line
		 * 3.  Cross second line
		 * 4.  Find object
		 * 5.  Take liftposition
		 * 6.  Lift object and start timer
		 * 7.  Go backward
		 * 8.  Cros second line
		 * 9.  Cros first line
		 * 10. Make distance to line
		 * 11. Place object and stop timer
		 * 12. Find home
		 */


	// BEGIN COUNT
	wait10Msec(100);
	PlaySound(soundBlip);
	wait10Msec(100);
	PlaySound(soundBlip);
	wait10Msec(100);
	PlaySound(soundBlip);
	wait10Msec(100);
	PlaySound(soundBlip);
	wait10Msec(100);
	PlaySound(soundBlip);
	wait10Msec(100);
	PlaySound(soundBlip);
	wait10Msec(200);

	// GO TO OBJECT
	SetSpeed(Slowest);
	wait10Msec(20);
	SetSpeed(Normal);
	wait10Msec(20);
	SetSpeed(Fast);

	while(SensorValue(LightSensor) > 45){wait10Msec(5);}
	wait10Msec(200);
	moveCrane(Down, 880);
	while(SensorValue(LightSensor) > 45){wait10Msec(5);}
	SetSpeed(Normal);
	while(SensorValue(SonarSensor) > 11 || SensorValue(SonarSensor) < 9){
		wait10Msec(1);
	}
	SetSpeed(Stop);

	// LIFT OBJECT AND START TIMER
	StartTask(Timer);
	moveCrane(Up, 400);
	//Rotate(500, Left);
	wait10Msec(25);
	SetSpeed(Reverse);

	while(SensorValue(LightSensor) > 45){wait10Msec(5);}
	wait10Msec(200);
	while(SensorValue(LightSensor) > 45){wait10Msec(5);}
	wait10Msec(200);
	SetSpeed(Stop);
	wait10Msec(50);

	// PLACE OBJECT AND STOP TIMER
	moveCrane(Down, 400);
	StopTimer = true;
	wait10Msec(50);

	// AVOID THE OBJECT
	SetSpeed(Reverse);
	wait10Msec(50);
	SetSpeed(Stop);
	wait10Msec(10);
	Rotate(260, Left);
	wait10Msec(10);
	SetSpeed(Slowest);
	wait10Msec(20);
	SetSpeed(Slow);
	wait10Msec(180);
	SetSpeed(Stop);
	wait10Msec(25);
	Rotate(270, Right);
	wait10Msec(10);

	// GO HOME
	SetSpeed(Slowest);
	SetSpeed(Slow);
	SetSpeed(Normal);
	SetSpeed(Fast);
	while(SensorValue(LightSensor) > 45){wait10Msec(5);}
	wait10Msec(200);
	moveCrane(Up, 880);
	while(SensorValue(LightSensor) > 45){wait10Msec(5);}
	wait10Msec(150);
	SetSpeed(Stop);


	wait10Msec(120000);
	EndProgram = true;
}




inline moveCrane(TCraneDirection craneDirection, int aLiftHeight)
{
	int speed = 0;
	int liftHeight = aLiftHeight;

	nMotorEncoder[motorC] = 0;

	if(craneDirection == Up){
		speed = 100;
	}else{
		speed = -50;
	}

	nMotorEncoderTarget[motorC] = liftHeight;
	motor[motorC] = speed;
	while(nMotorRunState[motorC] != runStateIdle){}
	PlaySound(soundBlip);

}

inline SetSpeed(TSpeed Speed){
	/*motor[motorA] = Speed;
	motor[motorB] = Speed;
*/
	//EndSyncMovement = true;
	if(Speed == 0){
		EndSyncMovement = true;
	}else{
		EndSyncMovement = false;
		gSpeed = Speed;
		StartTask(SyncMovement);
	}
}

inline Rotate(int lenght, TDirection direction){
	while(isSyncMovementRunning){
		wait10Msec(25);
		PlaySound(soundException);
	}
	int speedA = 0;
	int speedB = 0;
	moveCrane(Down, 110);
	if(direction == Left){
		speedA = 100;
		speedB = -100;
	}else{
		speedB = 100;
		speedA = -100;
	}

	int rotationLenght = lenght;

	nMotorEncoder[motorA] = 0;
	nMotorEncoder[motorB] = 0;

	nMotorEncoderTarget[motorA] = rotationLenght;
	nMotorEncoderTarget[motorB] = rotationLenght;
	motor[motorA] = speedA;
	motor[motorB] = speedB;
	while(nMotorRunState[motorA] != runStateIdle){
		PlaySound(soundBlip);
		wait10Msec(15);
	}
	moveCrane(Up, 110);


	//CurrentSpeed = Speed;
}