#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)
#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 = 3350;

//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

//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 autoPID(){

	while(true){

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

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

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

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

		if(isPIDActive){
			PID();
		}

	}


}

void PID(){
	while(true)
  {
    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;
    }

  }
}


task armMovement(){

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

				if(armUp == 1){
				armFinal = 70;
				}else if(armDown == 1){
				armFinal = -70;
				}else if(SensorValue(armEncoder) > 3350){
				armFinal = 10;
				}else{
				armFinal = -10;
				}

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

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

				wait1Msec(10);
		}
}

task driving(){

			while( true ) {

				throttle = vexRT[Ch3];
				turn = vexRT[Ch1];

								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);



		while(true)
		{
				//apply signal to motor

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

				wait1Msec(10);
		}


}