#pragma config(Sensor, in2,    ArmLF,          sensorLineFollower)
#pragma config(Sensor, in3,    CDLF,           sensorLineFollower)
#pragma config(Sensor, dgtl1,  distance,       sensorSONAR_cm)
#pragma config(Sensor, dgtl3,  eStop,          sensorTouch)
#pragma config(Sensor, dgtl4,  en,             sensorQuadEncoder)
#pragma config(Motor,  port2,           mGrip,         tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           mSlider,       tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,           mRotate,       tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port5,           mm,            tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           lidStamper,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           pmCyl,         tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           chestClose,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port9,           coinStamper,   tmotorVex393_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//mm is the cd racks motor
//cm is the counters motor
//km is the lid closer motor

int counter = 0;
int coinsPassed = 0;
int cdRotations = 0;

task e_stop()
{
  while(true)
  {
    if(SensorValue(eStop) == 1)
    {
    	stopAllTasks();      // ends the program and all tasks including task main.
    }
    	wait1Msec(10);  	// prevents the current task from using majority of available CPU capacity
  }
}
task cdCounter()
{
	while(true) {
		if(SensorValue(CDLF) < 1800 && SensorValue(CDLF) > 1400) {
			coinsPassed = coinsPassed + 1;
			waitUntil(SensorValue(CDLF) > 1800); //Wait until coin passes
		}

		if(coinsPassed == 4) {
			coinsPassed = 0;
		}

		wait1Msec(5);
	}
}
bool goTo(tMotor m, float point, int speed)
{
	while(SensorValue(en) != point)
	{
		if(SensorValue(en) > point)
		{
			startMotor(m, -1 * speed);
		}
		else
		{
			startMotor(m, speed);
		}
	}
	stopMotor(m);
	return true;
}

/* Will attempt to go to an exact point 6 times
If after 6 attempts it can't get the exact value requested it will
accept a value that is +- 1 of the requested value */
//Update now will accept anyting within 3 degrees of requested value

bool goToExactValue(tMotor m, float point)
{
	int completed = false;
	//int fixAttempts = 0; //Here so robot doesn't get stuck and try to correct itself forever

	while(!completed) {
		waitUntil(goTo(m, point, 19));
		wait(.25);  //Allow the motor to fully stop before correction
		completed = SensorValue(en) < (point + 3) && SensorValue(en) > (point - 3);
		//if(SensorValue(en) != point  && fixAttempts <= 0) {
			//waitUntil(goTo(m, point, 19));
			//fixAttempts = fixAttempts + 1;
		//} else {
			//completed = SensorValue(en) < (point + 3) && SensorValue(en) > (point + 3);
			//completed = (SensorValue(en) == point || SensorValue(en) == point + 1 || SensorValue(en) == point - 1);
		//}
	}

	return true;
}

bool manipulateGrip(bool open) {
	if(open) {
		startMotor(mGrip, 40);
		} else {
		startMotor(mGrip, -40);
	}

	wait(.5);
	stopMotor(mGrip);
	return true;
}

bool moveSlider(float point)
{
	if(SensorValue(distance) > point) {
		startMotor(mSlider, -25);
		waitUntil(SensorValue(distance) <= point && SensorValue(distance) != -1);
		startMotor(mSlider, 25);
		waitUntil(SensorValue(distance) <= point && SensorValue(distance) != -1);
	} else {
		startMotor(mSlider, 25);
		waitUntil(SensorValue(distance) >= point && SensorValue(distance) != -1);
		startMotor(mSlider, -25);
		waitUntil(SensorValue(distance) >= point && SensorValue(distance) != -1);
	}
	stopMotor(mSlider);

	return true;
}

bool rotateArm(bool home) {
	if (home) {
		setServo(mRotate, -15);
		} else {
		setServo(mRotate, -120);
	}

	wait(1);

	return true;
}

task cdRack()
{
	SensorValue(en) = 0;
	//startMotor(cm, 20);
	startTask(cdCounter);
	while(true) {
		int rotationEffect = cdRotations * 360;

		waitUntil(goToExactValue(mm, rotationEffect + 65));//Pick up a new container
		waitUntil(goToExactValue(mm, rotationEffect + 50)); //Move back to ensure the container is grabbed
		waitUntil(goToExactValue(mm, rotationEffect + 180)); //Move to coin dropper
		waitUntil(coinsPassed == 4); //Count coins
		wait(5); //Wait for all coins to slide down
		//Wiggle to ensure coins are fully in the container
		waitUntil(goToExactValue(mm, rotationEffect + 170)); //Wiggle
		waitUntil(goToExactValue(mm, rotationEffect + 190)); //Wiggle
		waitUntil(goToExactValue(mm, rotationEffect + 180)); //Wiggle

		wait(goTo(mm, rotationEffect + 280, 20)); //Go to lid stop
	 	//Close the lid
		startMotor(lidStamper, 30);
		wait(.5);
		startMotor(lidStamper, -30);
		wait(.5);
		stopMotor(lidStamper);
		wait(goToExactValue(mm, rotationEffect + 360)); //Go to start
		wait(goToExactValue(mm, rotationEffect + 365)); //Go to start
		wait(goToExactValue(mm, rotationEffect + 355)); //Go to start

		cdRotations = cdRotations + 1;
	}


	//while(true) {
		//if(coinsPassed == 4) {
			//wait(15);
			//Wiggle before moving
			//waitUntil(goTo(mm, (90 * cdRotations) + 10, 100));
			//wait(.5);
			//waitUntil(goTo(mm, (90 * cdRotations) - 10, 100));
			//wait(.5);
			//cdRotations = cdRotations + 1;
			//goToExactValue(mm, 60);
			//waitUntil(goToExactValue(mm, 90 * cdRotations));// No longer using the gear makes corrections not needed
			//waitUntil(goTo(mm, 90 * cdRotations, 30));
		//}
	//}
}

task main()
{
	startTask(e_stop);
	startTask(cdRack);

	waitUntil(manipulateGrip(true)); //open gripper
	waitUntil(rotateArm(true)); //Move the arm to the home position

	while(true) {
		if(SensorValue(ArmLF) < 2800 && counter <= 3) {
			waitUntil(manipulateGrip(false)); //close gripper
			int position = (counter * 3) + 6;
			waitUntil(rotateArm(false));//Rotate the arm
			waitUntil(moveSlider(position)); //Move slider to position
			counter = counter + 1;

			waitUntil(manipulateGrip(true)); //open the gripper
			wait(2);
			waitUntil(moveSlider(6)); //Move slider back home
			waitUntil(rotateArm(true)); //Move arm back to home

			if(counter == 3) {
				startMotor(chestClose, 25);
				wait(.5);
				startMotor(chestClose, -25);
				wait(.5);
				stopMotor(chestClose);
				counter = 0;
			}
		}
		waitInMilliseconds(10);
	}

}