#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in2,    rightside,      sensorNone)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port1,           intake,        tmotorVex393TurboSpeed_HBridge, openLoop)
#pragma config(Motor,  port2,           RightBase2,    tmotorVex393TurboSpeed_MC29, openLoop, driveRight, encoderPort, I2C_1)
#pragma config(Motor,  port3,           Arm,           tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           RightBase,     tmotorVex393TurboSpeed_MC29, openLoop, reversed, driveRight, encoderPort, I2C_1)
#pragma config(Motor,  port5,           RightBase1,    tmotorVex393TurboSpeed_MC29, openLoop, driveRight, encoderPort, I2C_1)
#pragma config(Motor,  port6,           LeftBase,      tmotorVex393TurboSpeed_MC29, openLoop, reversed, driveLeft, encoderPort, I2C_2)
#pragma config(Motor,  port7,           LeftBase1,     tmotorVex393TurboSpeed_MC29, openLoop, reversed, driveLeft, encoderPort, I2C_2)
#pragma config(Motor,  port8,           Arm,           tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port9,           LeftBase2,     tmotorVex393TurboSpeed_MC29, openLoop, reversed, driveLeft, encoderPort, I2C_2)
#pragma config(Motor,  port10,          intake,        tmotorVex393TurboSpeed_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma DebuggerWindows("Globals")
#pragma DebuggerWindows("Motors")

#pragma platform(VEX)

void moveBase (int speed)
{
    motor[LeftBase] = speed;
    motor[RightBase1] = speed;
    motor[RightBase2] = speed;
    motor[LeftBase1] = speed;
    motor[LeftBase2] = speed;
    motor[RightBase] = speed;
}

void turnBase (int speed) // positive is clockwise
{
    motor[LeftBase] = speed;
    motor[RightBase] = -speed;
}


int inchToTicks (float inch)
{
    int ticks;
    ticks = inch*99.82198;
    return ticks;
}

int fixTimerValue (float rawSeconds)
{
    int miliseconds;
    miliseconds = rawSeconds*1000;
    if (miliseconds < 250)
    {
        miliseconds = 250;
    }
    return miliseconds;
}

void PIDBaseControl (float target, float waitTime, float maxPower = 1)
{
    float Kp = 0.1;
    float Ki = 0.04;
    float Kd = 0.4;

    int error;

    float proportion;
    int integralRaw;
    float integral;
    int lastError;
    int derivative;

    float integralActiveZone = inchToTicks(3);
    float integralPowerLimit = 50/Ki;

    int finalPower;

    bool timerBool = true;

    nMotorEncoder[LeftBase] = 0;
    nMotorEncoder[RightBase] = 0;

    clearTimer(T1);

    while (time1[T1] < fixTimerValue(waitTime))
    {
        error = inchToTicks(target)-(nMotorEncoder[LeftBase]+nMotorEncoder[RightBase]);

        proportion = Kp*error;

        if (abs(error) < integralActiveZone && error != 0)
        {
            integralRaw = integralRaw+error;
        }
        else
        {
            integralRaw = 0;
        }

        if (integralRaw > integralPowerLimit)
        {
            integralRaw = integralPowerLimit;
        }
        if (integralRaw < -integralPowerLimit)
        {
            integralRaw = -integralPowerLimit;
        }

        integral = Ki*integralRaw;

        derivative = Kd*(error-lastError);
        lastError = error;

        if (error == 0)
        {
            derivative = 0;
        }

        finalPower = proportion+integral+derivative;

        if (finalPower > maxPower*127)
        {
            finalPower = maxPower*127;
        }
        else if (finalPower < -maxPower*127)
        {
            finalPower = -maxPower*127;
        }

        moveBase(finalPower);

        wait1Msec(40);

        if (error < 30)
        {
            timerBool = false;
        }

        if (timerBool)
        {
            clearTimer(T1);
        }
    }
    moveBase(0);
}

void PIDBaseTurn (int target, float waitTime, float maxPower = 1)
{
    float Kp = 0.2;
    float Ki = 0.05;
    float Kd = 0.5;

    int error;

    float proportion;
    int integralRaw;
    float integral;
    int lastError;
    int derivative;

    float integralActiveZone = inchToTicks(3);
    float integralPowerLimit = 50/Ki;

    int finalPower;

    bool timerBool = true;

    nMotorEncoder[LeftBase] = 0;
    nMotorEncoder[RightBase] = 0;

    clearTimer(T1);

    while (time1[T1] < fixTimerValue(waitTime))
    {
        error = target-(nMotorEncoder[LeftBase]-nMotorEncoder[RightBase]);

        proportion = Kp*error;

        if (abs(error) < integralActiveZone && error != 0)
        {
            integralRaw = integralRaw+error;
        }
        else
        {
            integralRaw = 0;
        }

        if (integralRaw > integralPowerLimit)
        {
            integralRaw = integralPowerLimit;
        }
        if (integralRaw < -integralPowerLimit)
        {
            integralRaw = -integralPowerLimit;
        }

        integral = Ki*integralRaw;

        derivative = Kd*(error-lastError);
        lastError = error;

        if (error == 0)
        {
            derivative = 0;
        }

        finalPower = proportion+integral+derivative;

        if (finalPower > maxPower*127)
        {
            finalPower = maxPower*127;
        }
        else if (finalPower < -maxPower*127)
        {
            finalPower = -maxPower*127;
        }

        turnBase(finalPower);

        wait1Msec(40);

        if (error < 30)
        {
            timerBool = false;
        }

        if (timerBool)
        {
            clearTimer(T1);
        }
    }
    turnBase(0);
}

task main ()
{
    PIDBaseControl(3,1); // move forward 39 inches with 1 sec delay;
    PIDBaseControl(0,10);

    //close claw


    while(true)
    {
        wait1Msec(20);
    }
}