/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       VEX                                                       */
/*    Created:      Thu Sep 26 2019                                           */
/*    Description:  Competition Template                                      */
/*                                                                            */
/*----------------------------------------------------------------------------*/

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name]               [Type]        [Port(s)]
// LeftMotorF           motor         1
// LeftMotorB           motor         17
// RightMotorB          motor         12
// RightMotorF          motor         19
// Scooper2             motor         2
// Scooper              motor         6
// Lift                 motor         8
// Arm2000              motor         16
// Controller1          controller
// Controller2          controller
// P1                   pot           D
// P2                   pot           G
// Inertial11           inertial      11
// LeftEncoder          encoder       A, B
// RightEncoder         encoder       E, F
// ---- END VEXCODE CONFIGURED DEVICES ----

#include "vex.h"


using namespace vex;


int count =0;
int target = 0;
bool driveMode = false;
int PreviousErrorLeft =0;
int PreviousErrorRight =0;
int PreviousErrorArm=0;

////This is problem
// A global instance of competition
competition Competition;

// define your global instances of motors and other devices here

/*---------------------------------------------------------------------------*/
/*                          Pre-Autonomous Functions                         */
/*                                                                           */
/*  You may want to perform some actions before the competition starts.      */
/*  Do them in the following function.  You must return from this function   */
/*  or the autonomous and usercontrol tasks will not be started.  This       */
/*  function is only called once after the V5 has been powered on and        */
/*  not every time that the robot is disabled.                               */
/*---------------------------------------------------------------------------*/

void pre_auton(void) {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // All activities that occur before the competition starts
  // Example: clearing encoders, setting servo positions, ...
}


/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              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.   */
/*---------------------------------------------------------------------------*/
void RightMotorFB(double speed) {
  RightMotorF.spin(directionType::fwd,speed,velocityUnits::rpm);
  RightMotorB.spin(directionType::fwd,speed,velocityUnits::rpm);
}

void LeftMotorFB(double speed) {
  LeftMotorF.spin(directionType::fwd,speed,velocityUnits::rpm);
  LeftMotorB.spin(directionType::fwd,speed,velocityUnits::rpm);
}

void slop(int distance) {
  if(distance <0) {
    LeftMotorFB(-40);
    vex::task::sleep(35);
  }
}

const int accel_step = 9; ////9
const int deccel_step =256; ///100
static int leftSpeed =0;
static int rightSpeed =0;

void LeftMotorFBS(int leftTarget){
  int step;

  if(abs(leftSpeed) < abs(leftTarget))
    step= accel_step;
  else
    step= deccel_step;

  if(leftTarget > leftSpeed + step)
    leftSpeed +=step;
  else if(leftTarget < leftSpeed - step)
    leftSpeed -= step;
  else
    leftSpeed = leftTarget;

  LeftMotorFB(leftSpeed);

}

void RightMotorFBS(int rightTarget){
  int step;

  if(abs(rightSpeed) < abs(rightTarget))
    step= accel_step;
  else
    step= deccel_step;

  if(rightTarget > rightSpeed + step)
    rightSpeed +=step;
  else if(rightTarget < rightSpeed - step)
    leftSpeed -= step;
  else
    rightSpeed = rightTarget;

  RightMotorFB(rightSpeed);

}


int drivePos() {
return (LeftMotorF.rotation(rotationUnits::deg) + RightMotorF.rotation(rotationUnits::deg))/2;
}
bool isDriving() {
    return(LeftMotorF.isSpinning() || LeftMotorB.isSpinning() || RightMotorF.isSpinning() || RightMotorB.isSpinning());
}

void PidBasicAsync(int distance) {
    slop(distance);
    count = 0;
    target = distance;
    driveMode = true;
    LeftMotorF.resetRotation();
    RightMotorF.resetRotation();
    LeftMotorB.resetRotation();
    RightMotorB.resetRotation();
    PreviousErrorLeft = 0;
    PreviousErrorRight = 0;

}

void BigArm (double degrees) {
    double LiftPos= P2.value(rotationUnits::deg);
    double kp = 2.20;

    while (LiftPos >= degrees) {
        LiftPos= P2.value(rotationUnits::deg);
        int power = (LiftPos - degrees) * kp + 10;
        Arm2000.spin(directionType::rev, power, velocityUnits::pct);
        Brain.Screen.printAt(2, 40, "LiftPos: %f", LiftPos);
         wait(0.01, seconds);
    }

    Arm2000.stop(brakeType::brake);
}

    void PidBasic (int distance) {
    if (driveMode) {

    double Kp = 0.18;///0.2 or .215 .162
    double Kd = 0.495;//465 or .52  or .45

    int Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
    int Rightdegrees=RightMotorF.rotation(rotationUnits::deg);

    int ErrorLeft = distance - Leftdegrees;
    int ErrorRight = distance - Rightdegrees;

    Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
    Rightdegrees=RightMotorF.rotation(rotationUnits::deg);

    ErrorLeft = distance - Leftdegrees;
    ErrorRight = distance - Rightdegrees;

    int LeftsideProportionalError = Kp * ErrorLeft;
    int RightsideProportionalError = Kp * ErrorRight;

    double LeftsidedDerivativeError = Kd * (ErrorLeft - PreviousErrorLeft);
    double RightsidedDerivativeError = Kd * (ErrorRight - PreviousErrorRight);

    LeftMotorFBS(LeftsideProportionalError + LeftsidedDerivativeError);
    RightMotorFBS(RightsideProportionalError + RightsidedDerivativeError);

    PreviousErrorLeft = ErrorLeft;
    PreviousErrorRight = ErrorRight;

        /*
        if (abs(ErrorLeft) < 6 && abs(ErrorRight) < 6) {
            break;
        }
        */

        if (!isDriving()) {
            count++;
        }
        if (count > 30) {
            driveMode = false;

        }



        vex::task::sleep(30);
    }
}

void Turning (int distance) {
    LeftMotorF.resetRotation();
    RightMotorF.resetRotation();
      LeftMotorB.resetRotation();
    RightMotorB.resetRotation();

    LeftMotorF.setStopping(brakeType::brake);
    RightMotorF.setStopping(brakeType::brake);
    LeftMotorB.setStopping(brakeType::brake);
    RightMotorB.setStopping(brakeType::brake);

    float Kp = .18; //// best.17 //// ..30 ///4
    float Kd = 0.5;/// best .35 //// .34 ////next best .32 next best// 25 //smallest movement.1 ///.395

    int Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
    int Rightdegrees=RightMotorF.rotation(rotationUnits::deg);

    int ErrorLeft = -1*Leftdegrees - distance;
    int ErrorRight = distance - Rightdegrees;

    int PreviousErrorLeft = 0;
    int PreviousErrorRight = 0;

    while(1) {
    Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
    Rightdegrees=RightMotorF.rotation(rotationUnits::deg);

     ErrorLeft = -1*Leftdegrees - distance;
     ErrorRight = distance - Rightdegrees;

        int LeftsideProportionalError = Kp * ErrorLeft;
        int RightsideProportionalError = Kp * ErrorRight;

        double LeftsidedDerivativeError = Kd * (ErrorLeft - PreviousErrorLeft);
        double RightsidedDerivativeError = Kd * (ErrorRight - PreviousErrorRight);

        LeftMotorFB(LeftsideProportionalError + LeftsidedDerivativeError);
        RightMotorFB(RightsideProportionalError + RightsidedDerivativeError);

        PreviousErrorLeft = ErrorLeft;
        PreviousErrorRight = ErrorRight;

        if (abs(ErrorLeft) < 10 && abs(ErrorRight) < 10) {
        LeftMotorFBS(0);
        RightMotorFBS(0);

            break;
    }
        vex::task::sleep(30);
    }
    }

   void PidFoward (int distance) {
   LeftMotorF.resetRotation();
   RightMotorF.resetRotation();
   LeftMotorB.resetRotation();
   RightMotorB.resetRotation();

   double Kp = 0.3;///0.2 or .215 //.3  ////0.25 ///0.4
   double Kd = 0.4; ////.275 or .0325 //.5 ///4375 //////0.438109 ///.7 .54 ////290 /////0.255
    ////0.8 ////0.6

   int Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
   int Rightdegrees=RightMotorF.rotation(rotationUnits::deg);

   int ErrorLeft = distance - Leftdegrees;
   int ErrorRight = distance - Rightdegrees;

   int PreviousErrorLeft = 0;
   int PreviousErrorRight = 0;

    while(1) {

   Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
   Rightdegrees=RightMotorF.rotation(rotationUnits::deg);

   ErrorLeft = distance - Leftdegrees;
   ErrorRight = distance - Rightdegrees;

   int LeftsideProportionalError = Kp * ErrorLeft;
   int RightsideProportionalError = Kp * ErrorRight;

   double LeftsidedDerivativeError = Kd * (ErrorLeft - PreviousErrorLeft);
   double RightsidedDerivativeError = Kd * (ErrorRight - PreviousErrorRight);

   int leftPower = LeftsideProportionalError + LeftsidedDerivativeError;
   int rightPower = RightsideProportionalError + RightsidedDerivativeError;

   //////0.7 ////0.6 is nice////.62955///.62955 good/// 0.068

    int velocitydiff2 = Leftdegrees - Rightdegrees;
   rightPower = leftPower;
   leftPower -= (velocitydiff2*0.10);
    LeftMotorFBS(leftPower);
   RightMotorFBS(rightPower);



   if (abs(ErrorLeft) < 10 && abs(ErrorRight) < 10) {
     LeftMotorFBS(0);
        RightMotorFBS(0);

        break;
    }
        vex::task::sleep(20);

    }
}

void DriveInInches(double distanceinInches) {

  double in = 3;
  double out = 2;
  double wheelDiameter = 4; // in inches
  double gearratio = (360 * out) / (wheelDiameter * 3.14 * in);
  double targetDistance = distanceinInches * gearratio;
  PidFoward(targetDistance);
}

/////////////////////////////////////////////////////////////////////////////////////////////

void StopMotor(vex::brakeType type = brakeType::coast){
    LeftMotorF.stop(type);
    RightMotorF.stop(type);
    LeftMotorB.stop(type);
    RightMotorB.stop(type);
}
void SetMotorBrakingType(brakeType type = brakeType::coast) {
    LeftMotorF.setStopping(type);
    RightMotorF.setStopping(type);
    LeftMotorB.setStopping(type);
    RightMotorB.setStopping(type);
}
int CubeIntake2() {
    int timeout = 4000;
    Scooper.setStopping(brakeType::coast);
    int sleepTimeout = 20;
    Scooper.spin(directionType::rev, 100, velocityUnits::pct);
     Scooper.spin(directionType::rev, 100, velocityUnits::pct);

    while (timeout > 0) {
        task::sleep(sleepTimeout);
        timeout = timeout - sleepTimeout;
    }

    Scooper.stop(brakeType::coast);

    return(0);
}
int CubeIntake() {
    int timeout = 4000;
    Scooper.setStopping(brakeType::coast);
     Scooper2.setStopping(brakeType::coast);
    int sleepTimeout = 20;
    Scooper.spin(directionType::fwd, 100, velocityUnits::pct);
     Scooper2.spin(directionType::fwd, 100, velocityUnits::pct);

    while (timeout > 0) {
        task::sleep(sleepTimeout);
        timeout = timeout - sleepTimeout;
    }
    Scooper.stop(brakeType::coast);
        Scooper2.stop(brakeType::coast);
    return(0);
}
int AutoBallIntakeTakeBall(int timeout = 5000) {

    Scooper.setStopping(brakeType::coast);
    int sleepTimeout = 20;
    Scooper.spin(directionType::fwd, 60, velocityUnits::pct);
    while (timeout > 0) {
        task::sleep(sleepTimeout);
        timeout = timeout - sleepTimeout;
    }

    Scooper.stop(brakeType::coast);

    return(0);
}
void Drive(double numOfRevs, int velocity = 50, bool fwd = true, int timeout = 500)
{
    Brain.Screen.printAt(1, 40, "numofrevs: %f, vel: %f", numOfRevs, velocity);
    SetMotorBrakingType();

    int sleepTimeout = 20;

    if (fwd == false) {
        numOfRevs = numOfRevs * -1;
    }


    LeftMotorF.startRotateFor(numOfRevs, rotationUnits::rev, velocity, velocityUnits::pct);
    RightMotorF.startRotateFor(numOfRevs, rotationUnits::rev, velocity, velocityUnits::pct);
    LeftMotorB.startRotateFor(numOfRevs, rotationUnits::rev, velocity, velocityUnits::pct);
    RightMotorB.startRotateFor(numOfRevs, rotationUnits::rev, velocity, velocityUnits::pct);

    while(LeftMotorF.isSpinning()||RightMotorF.isSpinning()||
          LeftMotorB.isSpinning()||RightMotorB.isSpinning()) {

        if (timeout <= 0) {
            break;
        }
        task::sleep(sleepTimeout);
        timeout = timeout - sleepTimeout;
    }
    StopMotor();
}
  void DriveInCM(double distanceInCM, int velocity, bool fwd = true, int timeout = 5000) {
  //  double gearRatio = 1.67;
  //  double wheelDiameter = 10.16;
  //  double circumference = wheelDiameter * M_PI;
  //  double numOfRevs = (distanceInCM)/ (circumference * gearRatio);
  double numOfRevs = distanceInCM/47.9;////old 53.2 47.9
  Drive(numOfRevs, velocity, fwd, timeout);
}

/////////////////////////////////////////////////////////////////////////////////
void LiftPosition1 () {

    int timeout = 5000;

    Arm2000.setStopping(brakeType::coast);

    int sleepTimeout = 30;
    while (timeout > 0) {

    Brain.Screen.clearScreen();

    float LiftPos = P2.value(rotationUnits::deg);

   Brain.Screen.printAt(2, 40, "LiftPos: %f", LiftPos);

        if (LiftPos == 0)
        {
            task::sleep(sleepTimeout);
            continue;
        }

        // Lowest position for launch
        if (LiftPos >= 108)
        {
            Brain.Screen.printAt(2, 80, "Stopped: %f", LiftPos);
            break;
        }

        Arm2000.spin(directionType::fwd, 100, velocityUnits::pct);

        task::sleep(sleepTimeout);

        timeout = timeout - sleepTimeout;

        Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
    }

    Arm2000.stop(brakeType::coast);
}

void LiftPosition2() {

    int timeout = 5000;

    Arm2000.setStopping(brakeType::coast);

    int sleepTimeout = 30;

    while (timeout > 0) {

        Brain.Screen.clearScreen();

        float LiftPos = P2.value(rotationUnits::deg);

        Brain.Screen.printAt(2, 40, "LiftPos: %f", LiftPos);

        if (LiftPos == 0)
        {
            task::sleep(sleepTimeout);
            continue;
        }

        // load position
        if (LiftPos <= 44)
        {
            break;
        }


        Arm2000.spin(directionType::rev, 45, velocityUnits::pct);

        task::sleep(sleepTimeout);

        timeout = timeout - sleepTimeout;
        Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
    }

    Arm2000.stop(brakeType::hold);
}

void LiftPosition3() {
    int timeout = 2500;

    Arm2000.setStopping(brakeType::coast);

    int sleepTimeout = 30;

    while (timeout > 0) {

        Brain.Screen.clearScreen();

        float LiftPos = P2.value(rotationUnits::deg);

        Brain.Screen.printAt(2, 40, "LiftPos: %f", LiftPos);


         if(P2.value(rotationUnits::deg) <44) {
             while(P2.value(rotationUnits::deg) <44) { ///////////13
                 Arm2000.spin(vex::directionType::fwd, 45, vex::velocityUnits::pct);

         }

      break;

        }
            else if
               (P2.value(rotationUnits::deg) > 45) {
                  while(P2.value(rotationUnits::deg) > 45) { /////////16
                 Arm2000.spin(vex::directionType::rev, 45, vex::velocityUnits::pct);
                         }

          break;

        }

        Arm2000.spin(directionType::fwd, 40, velocityUnits::pct);

        task::sleep(sleepTimeout);

        timeout = timeout - sleepTimeout;
        Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
    }

    Arm2000.stop(brakeType::coast);
}
///////////////////////////////////////////////??///////////////////////

void Lift10() {
    int timeout = 2500;

    Lift.setStopping(brakeType::hold);


    int sleepTimeout = 30;

    while (timeout > 0) {

        Brain.Screen.clearScreen();

        float LiftCube = P1.value(rotationUnits::deg);


        Brain.Screen.printAt(2, 40, "LiftCube %f", LiftCube);


   if (LiftCube >= 87)
        {

        Brain.Screen.printAt(2, 80, "Stopped: %f", LiftCube);
            break;
        }

            Lift.spin(directionType::rev, 127, velocityUnits::pct);

        task::sleep(sleepTimeout);

        timeout = timeout - sleepTimeout;
        Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
    }

    Lift.stop(brakeType::hold);

}
void Lift11() {
    int timeout = 2500;

    Lift.setStopping(brakeType::hold);


    int sleepTimeout = 30;

    while (timeout > 0) {

        Brain.Screen.clearScreen();

        float LiftCube = P1.value(rotationUnits::deg);


        Brain.Screen.printAt(2, 40, "LiftCube %f", LiftCube);


   if (LiftCube >= 67)
        {

        Brain.Screen.printAt(2, 80, "Stopped: %f", LiftCube);
            break;
        }


            Lift.spin(directionType::rev, 127, velocityUnits::pct);

        task::sleep(sleepTimeout);

        timeout = timeout - sleepTimeout;
        Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
    }

    Lift.stop(brakeType::hold);

}
void Lift12() {
    int timeout = 2500;

    Lift.setStopping(brakeType::hold);


    int sleepTimeout = 30;

    while (timeout > 0) {

        Brain.Screen.clearScreen();

        float LiftCube = P1.value(rotationUnits::deg);


        Brain.Screen.printAt(2, 40, "LiftCube %f", LiftCube);


   if (LiftCube <= 10)
        {

        Brain.Screen.printAt(2, 80, "Stopped: %f", LiftCube);
            break;
        }


            Lift.spin(directionType::fwd, 127, velocityUnits::pct);

        task::sleep(sleepTimeout);

        timeout = timeout - sleepTimeout;
        Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
    }

    Lift.stop(brakeType::brake);

}
/*
LiftPos >= 108*/

void CheckLiftPotValues() {

    while (1)
    {
        Brain.Screen.clearScreen();
        Brain.Screen.printAt(1, 30, "CheckLiftPot");
        float LiftPos = P2.value(rotationUnits::deg);
        Brain.Screen.printAt(1, 20, "rotation: %f degrees", LiftPos);

        //Sleep the task for a short amount of time to prevent wasted resources.
        task::sleep(20);
    }
}

int DriveTask() {
    while (true) {
        if (driveMode) {
            PidBasic(target);
        }
    }
}

int LiftTask()
{
    while (true) {

           Brain.Screen.clearScreen();
        float LiftPos = P2.value(rotationUnits::deg);

        Brain.Screen.printAt(1, 40, "LiftPos: %f", LiftPos);

        if (Controller1.ButtonY.pressing()) {
            // Load
           LiftPosition1();
        }

         else if (Controller1.ButtonB.pressing()) {
            // Launch
             BigArm(41);
        } else {
Arm2000.stop(brakeType::brake);
         }

           if (Controller2.ButtonL1.pressing()) {
            // Launch
            Lift10();
        }

             else if (Controller2.ButtonL2.pressing()) {
            // Launch
            Lift11();
        }

              else if (Controller2.ButtonDown.pressing()) {
            // Launch
            Lift12();
        }

        task::sleep(30);
    }
    return (0);
}

   void Turn() {

 Inertial11.calibrate();
  // waits for the Inertial Sensor to calibrate
  while (Inertial11.isCalibrating()) {
    wait(100, msec);
  }
  // Turns the robot to the right
  LeftMotorF.spin(forward);
  RightMotorF.spin(reverse);
  RightMotorB.spin(reverse);
  LeftMotorB.spin(forward);

  // Waits until the motor reaches a 90 degree turn and stops the Left and
  // Right Motors.
  waitUntil((Inertial11.rotation(degrees) >= 90.0));
  LeftMotorF.stop();
  RightMotorB.stop();
  LeftMotorB.stop();
  RightMotorF.stop();
  wait(1, seconds);
}

void autonomous(void) {


PidBasicAsync(1000);

 while(driveMode) {
            PidBasic(target);
          if(drivePos() < 100 )
          {
            Scooper.spin(directionType::fwd,100, velocityUnits::pct);
              Scooper2.spin(directionType::fwd,100, velocityUnits::pct);

          }
        }


return;


}

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

void usercontrol(void) {

  task CalliftTask(LiftTask);
  // User control code here, inside the loop
  while (true) {

RightMotorB.spin(directionType::fwd, Controller1.Axis2.position(percentUnits::pct),velocityUnits::pct);
RightMotorF.spin(directionType::fwd, Controller1.Axis2.position(percentUnits::pct),velocityUnits::pct);
LeftMotorF.spin(directionType::fwd, Controller1.Axis3.position(percentUnits::pct),velocityUnits::pct);
LeftMotorB.spin(directionType::fwd, Controller1.Axis3.position(percentUnits::pct),velocityUnits::pct);

if(Controller1.ButtonL2.pressing()) {
  Scooper.spin(vex::directionType::fwd, 100, velocityUnits::pct);
    Scooper2.spin(vex::directionType::fwd, 100, velocityUnits::pct);
}
else if (Controller1.ButtonL1.pressing()) {
   Scooper.spin(vex::directionType::rev, 100, velocityUnits::pct);
    Scooper2.spin(vex::directionType::rev, 100, velocityUnits::pct);
}
else {
  Scooper.stop(brakeType::brake);
  Scooper2.stop(brakeType::brake);
}


    wait(20, msec); // Sleep the task for a short amount of time to
                    // prevent wasted resources.
  }
}

//
// Main will set up the competition functions and callbacks.
//
int main() {
//Set up callbacks for autonomous and driver control periods.

  Competition.autonomous(autonomous);

  Competition.drivercontrol(usercontrol);

  // Run the pre-autonomous function.
  pre_auton();

  // Prevent main from exiting with an infinite loop.
  while (true) {
    wait(100, msec);
  }
}