bool enableDrivePD = false;
bool resetDriveSensors = false;
double kP = 80;
double kD = 250;

double turnkP = 0.2;
double turnkD = 0.07;

double error;
double prevError = 0;
double derivative;

double turnError;
double turnPrevError = 0;
double turnDerivative;

double desiredTarget = 0;
double desiredTurnValue = 0;

int drivePD() {
  while(1) {
    while(enableDrivePD) {
      if(resetDriveSensors) {
        resetDriveSensors = false;
        rightDt.resetRotation();
        leftDt.resetRotation();
      }

      double leftMotorRotation = leftDtBack.rotation(rev);
      double rightMotorRotation = rightDtBack.rotation(rev);
      double averageRotation = (leftMotorRotation + rightMotorRotation) / 2;

      error = desiredTarget - averageRotation;
      derivative = error - prevError;

      double lateralMotorPower = (kP*error) + (kD*derivative);
      ///////////////////////
      ///////////////////////
      turnError = desiredTurnValue - iSensor.rotation(deg);
      turnDerivative = turnError - turnPrevError;

      double turnMotorPower = (turnkP*turnError) + (turnkD*turnDerivative);
      ///////////////////////
      ///////////////////////
      // cout << "averageRotation: " << averageRotation << endl;
      // cout << "error: " << error << endl;
      // cout << "derivative: " << derivative << endl;
      // cout << "motorPower: " << lateralMotorPower << endl;
      cout << "iSensor: " << iSensor.rotation(deg) << endl;
      cout << "turnError: " << turnError << endl;
      cout << "turnDerivative: " << turnDerivative << endl;
      cout << "turnPower: " << turnMotorPower << endl;
      cout << "////" << endl;
      ///////////////////////
      ///////////////////////
      if(lateralMotorPower > 5) {
        lateralMotorPower = 5;
      } else if(lateralMotorPower < -5) {
        lateralMotorPower = -5;
      }

      if(turnMotorPower > 5) {
        turnMotorPower = 5;
      } else if (turnMotorPower < -5) {
        turnMotorPower = -5;
      }

      leftDt.spin(fwd, lateralMotorPower + turnMotorPower, volt);
      rightDt.spin(fwd, lateralMotorPower - turnMotorPower, volt);

      prevError = error;
      this_thread::sleep_for(15);
    }
    this_thread::sleep_for(15);
  }
  return 1;
}


int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  iSensor.calibrate(2000);
  this_thread::sleep_for(2000);
  cout << "Done calibrating" << endl;

  iSensor.setRotation(270, rotationUnits::deg);
  cout << "Rotation: " << iSensor.rotation() << endl;
  ///////////////////////
  ///////////////////////
  drivePDTask = vex::task(drivePD);
  enableDrivePD = true;
  resetDriveSensors = true;
  desiredTarget = inToRev(23.75);
  desiredTurnValue = 90;
  // this_thread::sleep_for(200);

  // resetDriveSensors = true;
  // desiredTarget = inToRev(-23.75);
  // desiredTurnValue = -180;

  while(1) {
    //Sleep task for short amount of time
    this_thread::sleep_for(10);
  }
}