#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,           frtrtdrivee,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,           backrtdrivee,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_3,           frtlftdrivee,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_4,           backlftdrivee,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_5,           toprtarme,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_6,           btlftarme,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_7,           toplftarme,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           toprtarm,      tmotorVex393, openLoop, reversed, encoder, encoderPort, I2C_5, 1000)
#pragma config(Motor,  port2,           btrtarm,       tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port3,           frtrtdrive,    tmotorVex393, openLoop, encoder, encoderPort, I2C_1, 1000)
#pragma config(Motor,  port4,           backrtdrive,   tmotorVex393, openLoop, encoder, encoderPort, I2C_2, 1000)
#pragma config(Motor,  port5,           frtlftdrive,   tmotorVex393, openLoop, reversed, encoder, encoderPort, I2C_3, 1000)
#pragma config(Motor,  port6,           backlftdrive,  tmotorVex393, openLoop, reversed, encoder, encoderPort, I2C_4, 1000)
#pragma config(Motor,  port7,           btlftarm,      tmotorVex393, openLoop, encoder, encoderPort, I2C_6, 1000)
#pragma config(Motor,  port8,           toplftarm,     tmotorVex393, openLoop, encoder, encoderPort, I2C_7, 1000)
#pragma config(Motor,  port9,           lftin,         tmotorVex269, openLoop)
#pragma config(Motor,  port10,          rtin,          tmotorVex269, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

#define TPR 627.2 //Ticks per revolution

/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

float kp = 0.625;
float ki = 0;
float kd = 0.0;
int previousError = 0;
float errorSum = 0;
int motPower = 0;
void armPID(int target)
{
  int error = target - nMotorEncoder[toprtarme];
  int derivative = error-previousError;
  motPower = (error*kp+errorSum*ki+derivative*kd);
  motor[toprtarm] = motPower;
  motor[toplftarm] = -motPower;
  motor[btrtarm] = motPower;
  motor[btlftarm] = -motPower;
  previousError = error;
  errorSum += error/1024.0;
}
void PIDreset(){
  previousError = 0;
  errorSum = 0;
}
void pre_auton()
{
  PIDreset();
}
void fdrive(int power)
{
    motor[frtrtdrive]=power;
    motor[frtlftdrive]=power;
    motor[backlftdrive]=power;
    motor[backrtdrive]=power;
}
float leftdistance()
{
    return nMotorEncoder[frtlftdrivee] * (TPR/2) * (4*PI);
}
float rightdistance()
{
    return nMotorEncoder[frtrtdrivee] * (TPR/2) * (4*PI);
}
void distancedrive(float inches)
{
    nMotorEncoder[frtrtdrivee]=0;
    while(true)
    {
        float dist=abs(leftdistance());
    if (dist<inches)
        fdrive(127);
  else
    fdrive(0);
        break;
  }
}
void turn(float degrees,int direction)//turn; degrees of rotation, and direction of rotation
{
    float inches = (degrees/20)*PI;
  nMotorEncoder[frtrtdrivee]=0;
  nMotorEncoder[frtlftdrivee]=0;
  if(direction==1){
    while(true){
      float leftdist=abs(leftdistance());
        float rightdist=abs(rightdistance());
          if (leftdist<inches){
          //motor[frtrtdrive]=power1;
        //motor[backrtdrive]=power1;
        motor[frtlftdrive]=127;
            motor[backlftdrive]=127;
          }
          if(rightdist<inches){
            motor[frtrtdrive]=-127;
            motor[backrtdrive]=-127;
          }
          else{
            fdrive(0);
          }
      }
  }
  if(direction==0){
      while(true)
    {
      float leftdist=abs(leftdistance());
        float rightdist=abs(rightdistance());
        if (leftdist<inches){
          //motor[frtrtdrive]=power1;
        //motor[backrtdrive]=power1;
        motor[frtlftdrive]=-127;
            motor[backlftdrive]=-127;
          }
        if(rightdist<inches){
          motor[frtrtdrive]=127;
          motor[backrtdrive]=127;
        }
        else{
          fdrive(0);
        }
    }
  }
}
void intake(int power)
{
    motor[rtin]=power;
    motor[lftin]=power;
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task autonomous()
{
    PIDreset();
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
    // User control code here, inside the loop

    while (true)
    {
      //The first four statements are for the drivebase.
        //The up and down controls on the left and right joysticks control the drive motors.
        //Moving the left joystick forward will make the left motors go forward at full power (Same for right side).
        //Thus turning the right joystick forward and the left joystick backward will turn the robot to the left.
        //*Needs confirmation* Turning the right joystick halfway forward SHOULD make the right motors move at half power.
        motor[frtrtdrive]=vexRT[Ch2];
        motor[backrtdrive]=vexRT[Ch2];
        motor[frtlftdrive]=vexRT[Ch3];
        motor[backlftdrive]=vexRT[Ch3];
        //The next two "if" are for the lift.
        //The right shoulder buttons will move the lift up and down at full power.
        //The closer button will move the lift up, and the further back button will move the lift down.
        if (vexRT[Btn6U]==1)
        {
          motor(toplftarm)=127;
          motor(btrtarm)=127;
          motor(toprtarm)=127;
          motor(btlftarm)=127;
      }
        if (vexRT[Btn6D]==1)
        {
          motor(toplftarm)=-127;
          motor(btrtarm)=-127;
          motor(toprtarm)=-127;
          motor(btlftarm)=-127;
        }
        if (vexRT[Btn6D]==0&&vexRT[Btn6U]==0)
        {
          motor(toplftarm)=0;
          motor(btrtarm)=0;
          motor(toprtarm)=0;
          motor(btlftarm)=0;
        }

        //The final two "if" statements are for controlling  intake.
        //The left shoulder buttons will turn the intake.
        //The closer left button will make the robot intake, and the button further back will make the robot outtake.
        if (vexRT[Btn5U]==1)
        {
          motor(rtin)=127;
          motor(lftin)=127;
        }
        if (vexRT[Btn5U]==0&&vexRT[Btn5D]==0){
          motor(rtin)=0;
          motor(lftin)=0;
        }
        if (vexRT[Btn5D]==1)
        {
          motor(rtin)=-127;
          motor(lftin)=-127;
        }
    }
}