#pragma config(Sensor, in1, armEncoder, sensorPotentiometer)
#pragma config(Motor, port1, rightDrive, tmotorVex393_HBridge, openLoop, reversed, driveRight)
#pragma config(Motor, port2, limitSwitch, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port6, claw, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port7, armMotor, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port10, leftDrive, tmotorVex393_HBridge, openLoop, driveLeft)
//!!Code automatically generated by 'ROBOTC' configuration wizard !!//

//gain = skim threshold = driving
float gain = .8;
int threshold = 5;

//PID initial values/constants
float Kp = .12;
float Kd = 0;
float Ki = .000001; // ki = .00001 and kp = .1 is good
float errorSum = 0;
float lastError = 0;
int setpoint = 1810;

//PID variables
float curPos;
float diff;
int errorTerm;
int mVal;

//driving
int throttle, turn;

//driving
int leftMotor, tempLeft;
int rightMotor, tempRight;

//arm and claw
int clawUp, clawDown,clawFinal;
int armUp, armDown,armFinal;

//button Presets right side
int enablePID, disablePID, incPID_Setpoint, decPID_Setpoint;
bool isPIDActive = false;

//inverses the value (so must use opposite to skim)
float skim( float f ) {

if( f >127 ){
	return - (f - 127) * gain;
}

else if( f < -127 ) {
	return - (f + 127) * gain;
}

else {
	return 0;
}
}

void codePID(){

curPos = SensorValue(armEncoder);    // Display the reading of the Potentiometer to current cursor position
errorTerm = setpoint - curPos;
mVal = Kp*errorTerm;  //.1 kp is most precise

if(errorTerm < 400 && errorTerm > -400) {
	errorSum += errorTerm;
}

diff = errorTerm - lastError;

lastError = errorTerm;

mVal += Ki*errorSum;
mVal += Kd*diff;

if(mVal > 127) {
	mVal = 127;
}
else if(mVal < -127) {
	mVal = -127;
}
}

void autoPID(){

enablePID = vexRT[Btn8R];
disablePID = vexRT[Btn8L];
incPID_Setpoint = vexRT[Btn8U];
decPID_Setpoint = vexRT[Btn8D];

	if(incPID_Setpoint == 1){
	setpoint += 1;
	}

	//1 * 16 encoder

	if(decPID_Setpoint == 1){
	setpoint -= 1;
	}

	if(enablePID == 1 && !isPIDActive){
		isPIDActive = true;
		setpoint = SensorValue(armEncoder);
	}else if(disablePID == 1 ){
		isPIDActive = false;
	}

	if(isPIDActive){
		codePID();
	}
}

task armMovement(){

	while(true){
			armUp = vexRT[Btn6U];
			armDown = vexRT[Btn6D];

			if(armUp == 1){
			armFinal = 50;
			}else if(armDown == 1){
			armFinal = -50;
			}else if(SensorValue(armEncoder) > 1810){
			armFinal = -5;
			}else{
			armFinal = 5;
			}

			if( armFinal < -20 && SensorValue[limitSwitch]  == 1){
				armFinal = 0;
			}

		//	autoPID();

		if(isPIDActive){
		motor[armMotor] = mVal;
		}else{
		motor[armMotor] = armFinal;
		}

			wait1Msec(10);
	}
}

task driving(){

		while( true ) {

			throttle = vexRT[Ch4];
			turn = vexRT[Ch2];

							if(throttle < threshold && throttle > -threshold){
				throttle = 0;
			}
			if(turn < threshold && turn > -threshold){
				turn = 0;
			}

			tempRight = throttle + turn;
			tempLeft = throttle - turn;

			leftMotor = skim(tempRight) + tempLeft;
			rightMotor = tempRight + skim(tempLeft);

			motor[ rightDrive ] = rightMotor;
			motor[ leftDrive] = leftMotor;

			wait1Msec(10);
			}
}

task clawMovement(){

	while(true){

			clawUp = vexRT[Btn5U];
			clawDown = vexRT[Btn5D];


			if(clawUp == 1){
			clawFinal = 70;
			}else if(clawDown ==  1){
			clawFinal = -70;
			}else{
			clawFinal = 0;
			}
	 			motor[claw] = clawFinal;


			wait1Msec(10);
	}
}

task main()
{
wait1Msec(20);

			startTask(driving);
			startTask(armMovement);
			startTask(clawMovement);
}