//This code measures how quickly a battery will drain (please start as fully charged)//
task main()
{
	waitForStart();
	nMotorEncoder(Right) = 0;
	nMotorEncoder(Left) = 0;
	wait1Msec(50);

	ClearTimer(T2);

	motor[Right] = speed;
	motor[Left] = -speed;
	AddToDatalog(speedDat, speed);
	wait1Msec(8000);
	//Get up to speed//
	ClearTimer(T1);
	startVolts = nAvgBatteryLevel;
	while(time1(T1) < 10000){
	}
	//Starting encoder ticks per second//
	encSpeed = ((nMotorEncoder(Right) / 10) + (abs(nMotorEncoder(Left)) / 10)) / 2;
	AddToDatalog(encSpeedDat, encSpeed);
	AddToDatalog(startVoltsDat, startVolts);

	newSpeed = encSpeed;

	//Run while at 80% of original speed or greater//
	while(newSpeed >= (encSpeed * 80 / 100)){
		ClearTimer(T1);

		nMotorEncoder(Right) = 0;
		nMotorEncoder(Left) = 0;
		wait1Msec(50);

		while(time1(T1) < 10000){
		}
		//Current encoder ticks per second//
		newSpeed = ((nMotorEncoder(Right) / 10) + (abs(nMotorEncoder(Left)) / 10)) / 2;
		AddToDatalog(newSpeedDat, newSpeed);
		AddToDatalog(newVoltsDat, nAvgBatteryLevel);
	}

  time = time100(T2) / 10;
	AddToDatalog(timeDat, time);
	SaveNxtDatalog();
}

// Moves the arm to base height
void ArmBase()
{
    ClearTimer(T1);
    while(SensorValue[P1] + SensorValue[P2] < 2212 && time1[T1] < 3000)
    {
        Arm(127);
    }
    Trim();
}

// Moves the arm to wall height
void ArmWall()
{
    ClearTimer(T1);
    while(SensorValue[P1] + SensorValue[P2] < 2700 && time1[T1] < 4500)
    {
        Arm(127);
    }
    Trim();
}

task Foreground(){
  int timeLeft;
	while(true){
		ClearTimer(T1);
		hogCPU();
		//--------------------Robot Code---------------------------//

		long robotDist = nMotorEncoder[rtWheelMotor] + nMotorEncoder[ltWheelMotor];
		int robotDir  = (int)(nMotorEncoder[ltWheelMotor] - nMotorEncoder[rtWheelMotor]);

		// Calculate the speed and direction commands
//    int speedCmd = ForwardDist(path[pathIdx][DIST_IDX], robotDist, path[pathIdx][SPD_IDX]);
//		int dirCmd=Direction(path[pathIdx][DIR_IDX], robotDir, path[pathIdx][DIRRL_IDX]);

//		DebugInt("spd",speedCmd);
//		DebugInt("dir",dirCmd);

		#define FWDa 1
		#define	RT90a 2
		int sm=FWD1;
		int speedCmd=0;
		int dirCmd=0;

		switch sm {
		case FWDa: //Drive Forward
    	speedCmd = ForwardDist(10, robotDist, 50);// CmdStopDist,,CmdSpeed
			dirCmd=Direction(0, robotDir, 50);// CmdDir,,RateLimitValue
			break;
		default:
		}
		// Calculate the motor commands given the speed and direction commands.
//		motor[ltWheelMotor]=speedCmd+dirCmd;
//		motor[rtWheelMotor]=speedCmd-dirCmd;

		//--------------------------Robot Code--------------------------//
		// Wait for next itteration
	  timeLeft=FOREGROUND_MS-time1[T1];
	  releaseCPU();
	  wait1Msec(timeLeft);
	}// While
}//Foreground
task main()
{
	//--------------------INIT Code---------------------------//
  ForwardDistReset((tMotor)rtWheelMotor, (tMotor)ltWheelMotor);
	DirectionReset();
	//--------------------End INIT Code--------------------------//

  StartTask(Foreground, 255);

  while(true){

  	//-----------------Print the debug statements out------------------//
		DebugPrint();

	}// While
}// Main

int CenterGoal() {
	ClearTimer(T4);
	retractKnocker();
	turnUltra(0, 90);
	StartTask(raiseLift);
	moveDistanceRamp(40, 20);
	pause(1);
	int position = detectPosition();
	switch(position) {
		case 1:
			sound(1, 0.2);
			CenterPosition1();
			break;
		case 2:
			sound(2, 0.2);
			CenterPosition2();
			break;
		case 3:
			sound(3, 0.2);
			CenterPosition3();
			break;
		default:
			return -1;
	}

	deployClamp();
	while(!lifted);
	//int position = 2;
	readAllSwitches();
	//scoring commences
	bool anything_pressed = false;
	bool called_already = true;
	translateRTHeading(100, 90);
	while(!sideSwitch || !armSwitch) {
		readAllSwitches();
		if(tipSwitch) {
			translating = false;
			called_already = false;
			move(-20);
		}
		else if(sideSwitch) {
			translating = false;
			called_already = false;
			move(20);
		}
		else if(!called_already) {
			translateRTHeading(100, 90);
			called_already = true;
		}
		pause(0.2);
	}
	PlaySound(soundBeepBeep);
	scoreBall();
	move(0);
	translating = false;
	return position;
}

task main()
{
	while (true)
	{
		if(joystick.joy1_TopHat == 0)
		{
			ClearTimer(T1);
			int total = 0;
			while(time1[T1] < 3000)
			{
				if(joystick.joy1_TopHat > -1)
				{
					total = total + joystick.joy1_TopHat;
				}
			}
			if(total == 24)
			{
				ClearTimer(T1);
				while(time1[T1] < 1000)
				{
					if(joy1Btn(3) == 1)
					{
						ClearTimer(T1);
						while(time1[T1] < 1000)
						{
							if(joy1Btn(2) == 1)
							{
								ClearTimer(T1);
								while(time1[T1] < 1000)
								{
									if(joy1Btn(9) == 1)
									{
										motor[leftMotor] = 50;
										motor[rightMotor] = -50;
									}
								}
							}
						}
					}
				}
			}
		}
	}
}

void turnPointLeft (float mRot, float mRotPerSec)
{
	nxtDisplayCenteredTextLine(5, "TPL(%.2f,%.2f)",
		mRot, mRotPerSec);

	if (moveModeType != MMAllMoveTypes
	      && moveModeType != MMTurnsOnly
	      && moveModeType != MMPointTurnsOnly) {
		return;
	}

	if (moveModeTiming == MMOneMoveAtATime) {
		waitForTouch();
	}


	if (stepThroughMode == stepThroughModeOn) {
		waitForTouch();
	}

	checkParameterRange(mRot, mRotPerSec);

	int leftWheelInitial = nMotorEncoder[leftWheelMotor];
	int rightWheelInitial = nMotorEncoder[rightWheelMotor];
	int leftWheelTarget = nMotorEncoder[leftWheelMotor] - 360 * mRot;
	int rightWheelTarget = nMotorEncoder[rightWheelMotor] + 360 * mRot;

	ClearTimer(T1);

	float motorPower = revolutionsPerSecondToMotorPower(mRotPerSec);
	motor[leftWheelMotor] = -1 * motorPower;
	motor[rightWheelMotor] = motorPower;

	while ((nMotorEncoder[leftWheelMotor] > leftWheelTarget)
		&& (nMotorEncoder[rightWheelMotor] < rightWheelTarget))
	{
		nxtDisplayCenteredTextLine(7, "%d", nMotorEncoder[leftWheelMotor]);
	}

	motor[leftWheelMotor] = 0;
	motor[rightWheelMotor] = 0;

	int leftWheelChange = nMotorEncoder[leftWheelMotor] - leftWheelInitial;
	int rightWheelChange = nMotorEncoder[rightWheelMotor] - rightWheelInitial;
	float revolutionsWheelsRotated =
	  ((float) ( abs(leftWheelChange) > abs(rightWheelChange) ?
	                leftWheelChange : rightWheelChange ))
	        / 360.0;

	nxtDisplayCenteredTextLine(2, "TPL(%.2f,%.2f)",
		mRot, mRotPerSec);
	nxtDisplayCenteredTextLine(3, "%.2frev %.2fsec",
	  (float) revolutionsWheelsRotated, (float) time10(T1) / 100);
	nxtDisplayCenteredTextLine(5, "");
	nxtDisplayCenteredTextLine(7, "");
}

task main()
{
	waitForStart();
	ClearTimer(T1);
	StartTask(firstMove);
	while(true)
	{
		wait1Msec(1);
	}
}

void raise()
{
	nMotorPIDSpeedCtrl[lift] = mtrNoReg;
	ClearTimer(T1);
	while( time1[T1] < 1500 )
		motor[lift] = -30;

	motor[lift] = 0;
	//nMotorPIDSpeedCtrl[lift] = mtrSpeedReg;
}

/*-------------------------------------------------------
  WM_DESTROY message
---------------------------------------------------------*/
void OnDestroy(HWND hwnd)
{
	ClearTimer();
	
	KillTimer(hwnd, IDTIMER_TIMER);
	
	if(g_hfontDialog) DeleteObject(g_hfontDialog);
	
	PostQuitMessage(0);
}

void initRobot() {
	servo[rampLatch] = 20;
	servo[platLatch] = 255;
	ClearTimer(T1);	 //Clear timer 1 for checking to see how long the robot waits before starting
	initDrivetrain();//Initialize systems
	initElevator();
	StartTask(ElevatorControlTask);	//begin elevator control task
	StartTask(SignalLight);
	setMode(IDLE);
}

// =======================================================================
// Function to move the robot by the gyro by time V2
// =======================================================================
void GyroTime_moveV2(int time, int power,bool StopWhenDone, bool adjust, bool ConstOrRel)
{
	relHeading =0;
	Current_Angle=0;   // reset current angle
	long adj_power;
	long adj_deg;
	wait1Msec(200);
	motor[LF_motor] = -power;
	motor[RF_motor] = power;
	motor[LB_motor] = -power;
	motor[RB_motor] = power;
	current_power = power;
	ClearTimer(T1);
	bool Done = false;
	while(!Done)
	{
		/*nxtDisplayTextLine(1, "ad: %3d", adj_deg);
		nxtDisplayTextLine(2, "R: %3d", (current_power + adj_power));
		nxtDisplayTextLine(3, "L: %3d", (current_power - adj_power));
		nxtDisplayTextLine(4, "ap: %3d", adj_power);*/
		nxtDisplayBigTextLine(2, "S: %3d", SensorValue[SONAR]);

		if(time1[T1] > time)
		{
			Done = true;
		}
		//----------------------------
		/*if(ConstOrRel) Current_Speed=constHeading;
		else Current_Speed=relHeading;
		Current_Angle= Current_Angle + (float)(Current_Speed/GCPD);
		wait1Msec(5);*/
		//----------------------------

		if(adjust == true)
		{
			if(ConstOrRel) adj_deg = (long) constHeading;
			else adj_deg = (long) relHeading;
			adj_power = adj_deg*GYRO_PROPORTION;
			/*adj_deg = (long) Current_Angle;
			adj_power = adj_deg*GYRO_PROPORTION;*/

			motor[LF_motor] = -(current_power - adj_power);
			motor[RF_motor] = (current_power + adj_power);
			motor[LB_motor] = -(current_power);
			motor[RB_motor] = (current_power);
		}
	}
	if(StopWhenDone==true)
	{
		motor[LF_motor] = 0;
		motor[RF_motor] = 0;
		motor[LB_motor] = 0;
		motor[RB_motor] = 0;
	}
}

//Atomatic controlling of the basket servo & sweeper
void BasketStateMachine()
{
	static int state=CLOSED;

	if(state==CLOSED &&
		 time1[T1]>=SERVOTIME &&
		(SensorValue[touchSensor]==1 || joy2Btn(BASKETDOWNBUTTON)==true))
	{
		motor[sweeper]=-sweeperOn;
		sweeperEnabled=false;
		servo[basketServo]=basketServoDown;
		ClearTimer(T1);
		armEnabled=false;

		state=OPENING;
	}
	else if(state==OPENING && time1[T1]>=SERVOTIME)
	{
		motor[sweeper]=off;
		sweeperEnabled=true;

		state=OPEN;
	}
	else if(state==OPEN && (ARMJOYSTICK>deadZone || joy2Btn(BASKETUPBUTTON)==true))
	{
		motor[sweeper]=sweeperOn;
		sweeperEnabled=false;
		servo[basketServo]=basketServoUp;
		ClearTimer(T1);

		state=CLOSING;
	}
	else if(state==CLOSING && time1[T1]>=SERVOTIME)
	{
		motor[sweeper]=off;
		sweeperEnabled=true;
		armEnabled=true;
		ClearTimer(T1);

		state=CLOSED;
	}
}

void raiseLift()
{
	ClearTimer(T1);
	while(abs(nMotorEncoder[elevator]) < 8000 && time1[T1] <2500)
	{
		motor[elevator] = 100;
		print(nMotorEncoder[elevator],4);
		PlaySound(soundBlip);
	}
	motor[elevator] = 0;
}

task getaccel()
{
	while(true)
	{
		ClearTimer(T1);
		HTACreadAllAxes(Accel, X_axis, Y_axis, Z_axis);
		X_axis = X_axis - offset_X;
		wait10Msec(1);
		X_axis_old = X_axis;
	}
}

void armHeight(float percent) // uses a PID loop to drive the arm to a given height - used in autonomous 
{ 
    float goal = ( (percent/100) * (LEFT_ARM_UPPER_LIMIT - LEFT_ARM_LOWER_LIMIT) ) + LEFT_ARM_LOWER_LIMIT; 
    int output;                // speed to send to the arm motots, set in the loop 
    InitPidGoal(armPid, goal); // initialize the arm PID with the goal 
    ClearTimer(T3);            // clear the timer 
  
    while( (abs(armPid.error) > armErrorThreshold) 
          || (abs(armPid.derivative) > armDerivativeThreshold) ) // until both the error and speed drop below acceptable thresholds... 
    { 
        if (time1(T3) > PID_UPDATE_TIME) // if enough time has passed since the last PID update... 
        { 
            output = UpdatePid(armPid, SensorValue(leftArmPot));  // ...update the motor speed with the arm PID... 
          setArmMotors(output); // ...and send that speed to the arm motors 
          ClearTimer(T3); 
         } 
    } 
    // don't set the motors back to 0 afterwards, we want them to hold position 
    wait1Msec(TIME_BETWEEN_AUTONOMOUS_ACTIONS); // lets things settle before moving to next action 
} 

void putLiftUpAuto(){
	//put up lift to lower height
  hogCPU();
  motor[M_Lift]=100;
  int safetime=1200; //1.2 seconds
  ClearTimer(T4);
  while(nMotorEncoder[M_Lift]<C_LIFTLOWTARGET-200 && time1[T4]<safetime){};
  motor[M_Lift]=0;
  if(time1[T4]>=safetime) StopAllTasks();
  releaseCPU();
}

task robotHeading(){
	ClearTimer(T1); // sets timer to 0
	while(true)
	{
		int currentReading = HTGYROreadRot(HTGYRO) - initial; // gets the new sensor reading
		heading += (currentReading) * (time1[T1] - lastTime) * .001; // modifies the header
		if(heading>=360)
			heading=heading-360;
		if(heading<=0)
			heading=heading+360;
		lastTime = time1[T1]; // sets the last time for the next reading
		if (time1[T1]>30000)
		{ // this resets the timer after 30 seconds
			ClearTimer(T1);
			lastTime = 0;
		}
		radheading = heading/180*PI; // the heading expressed in radians
		wait10Msec(1); // lets other tasks run
	}
}

task failSafe()
{
	ClearTimer(T1);
	while(1)
	{
		if (time1 [T1] > 3500)
		{
			StopAllTasks();
		}
	}
}

// Moves robot forward for left and right distance
void Forward(int left, int right, int time = 20000)
{
    // Formatting user input values
    if(left < 0)
    {
        left = -left;
    }
    if(right < 0)
    {
        right = -right;
    }

    ClearTimer(T1);

    // Hidden cumulative left and right values...
    QLeft += left;
    QRight += right;

    // Calibrating left and right drives
    // The larger it is, the earlier the drive stops
    // The smaller it is, the later the drive stops
    int leftConstant = 5;
    int rightConstant = 5;

    // Loop until both conditions have been satisfied
    while(time1(T1) < time && (SensorValue[QuadLeft] < QLeft - leftConstant || SensorValue[QuadRight] < QRight - rightConstant))
    {
        // If left needs to move
        if(SensorValue[QuadLeft] < QLeft - leftConstant)
        {
            // Driving left side
            DriveLeft(40);
        }
        else
        {
            // Stopping left side
            DriveLeft(0);
        }

        if(SensorValue[QuadRight] < QRight - rightConstant)
        {
            // Driving right side
            DriveRight(40);
        }
        else
        {
            // Stopping right side
            DriveLeft(0);
        }
    }
    // Applying safety brake for safety measures
    SafetyBrake();
}