#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  HTServo)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     HTMAG,          sensorHiTechnicMagnetic)
#pragma config(Sensor, S3,     ,               sensorLightActive)
#pragma config(Motor,  motorA,          motorA,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorB,          motorB,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          motorC,        tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     motorRight,    tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorLeft,     tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_1,     motorLift,     tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     motorFork,     tmotorNormal, openLoop, reversed)
#pragma config(Servo,  srvo_S1_C3_1,    Mag1,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    leftflap2,            tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    rightflap3,           tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    MagG4,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    MagR5,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    crate6,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_1,    forkL7,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    forkR8,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    AlignL9,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    AlignR10,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    FlipL11,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_6,    FlipR12,              tServoStandard)
#pragma config(SrvoPosition,  Position01,            127,  45, 255, 175,  60, 128,  83,  83)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.

bool btn2Pressed = false;
bool MunIsUp = false;
bool btn3Pressed = false;
bool MagnetGuardIsUp = false;
bool btn5Pressed = false;
bool LeftForkIsUp = false;
bool btn6Pressed = false;
bool RightForkIsUp = false;
bool btn8Pressed = false;
bool CrateIsUp = false;

void initializeRobot()
{
  servoTarget[Mag1] = 127;
  servoTarget[leftflap2] = 45;
  servoTarget[rightflap3] = 255;
  servoTarget[MagG4] = 175;
  servoTarget[MagR5] = 60;
  servoTarget[crate6] = 255;
  servoTarget[forkL7] = 158;
  servoTarget[forkR8] = 50;
  servoTarget[AlignL9] = 107;
  servoTarget[AlignR10] = 137;
  servoTarget[FlipL11] = 35;
  servoTarget[FlipR12] = 225;
  return;
}

void processMagnet()
{
    if(SensorValue[HTMAG] > 680) // Magnetic Ball
    {
      servoTarget[Mag1] = 230;
      wait1Msec(1500);
    }
    else if(SensorValue[HTMAG] < 640)// Magnetic Ball
    {
      servoTarget[Mag1] = 230;
      wait1Msec(1500);
    }
    else
    {
      servoTarget[Mag1] = 127;
    }
}

task main()
{
  initializeRobot();

  waitForStart();   // wait for start of tele-op phase

  while (true)
  {

    getJoystickSettings(joystick);
    processMagnet();
    motor[motorLeft] = joystick.joy1_y1;
    motor[motorRight] = joystick.joy1_y2;
    motor[motorLift] = joystick.joy2_y1;// Ball Lift
    motor[motorFork] = joystick.joy2_y2;// Fork Lift

    //_____________________________________________________________

    //******************Crate Munipulators ******************
    if(!btn2Pressed && joy1Btn(2))
    {
      MunIsUp = !MunIsUp;
      btn2Pressed = true;
    }
    else if(!joy1Btn(2))
    {
      btn2Pressed = false;
    }
    if(MunIsUp)//Closed Position
    {
    servoTarget[FlipL11] = 75;
    servoTarget[FlipR12] = 185;
    }
    else if(joy1Btn(7))//After Button Pressed
    {
    servoTarget[FlipL11] = 245;
    servoTarget[FlipR12] = 25;
    }
    else//Open, Working
    {
    servoTarget[FlipL11] = 127;
    servoTarget[FlipR12] = 127;
    }

    //----------------------
    if(!btn2Pressed && joy1Btn(2))
    {
      MunIsUp = !MunIsUp;
      btn2Pressed = true;
    }
    else if(!joy1Btn(2))
    {
      btn2Pressed = false;
    }
    if(MunIsUp)//Closed Position
    {
    servoTarget[AlignL9] = 107;
    servoTarget[AlignR10] = 137;
    }
    else if(joy1Btn(8))//After Button Pressed
    {
    servoTarget[AlignL9] = 177;
    servoTarget[AlignR10] = 77;
    }
    else if(joy2Btn(7))//After Button Pressed
    {
    servoTarget[AlignL9] = 197;
    servoTarget[AlignR10] = 57;
    }
     else//Open, Working
    {
    servoTarget[AlignL9] = 77;
    servoTarget[AlignR10] = 167;
    }

    //***********Fork Lift Left Servos************
   if(!btn5Pressed && joy2Btn(5))
    {
      LeftForkIsUp = !LeftForkIsUp;
      btn5Pressed = true;
    }
    else if(!joy2Btn(5))
    {
      btn5Pressed = false;
    }
    if(LeftForkIsUp)
    {
    servoTarget[forkL7] = 8;//Open
    }
   else
    {
     servoTarget[forkL7] = 79; //Closed (Locked on to basket)
    }

   //**********Fork Lift Right Servos***********
   if(!btn6Pressed && joy2Btn(6))
    {
      RightForkIsUp = !RightForkIsUp;
      btn6Pressed = true;
    }
    else if(!joy2Btn(6))
    {
      btn6Pressed = false;
    }
    if(RightForkIsUp)
    {
    servoTarget[forkR8] = 216; // Open
    }
   else
    {
     servoTarget[forkR8] = 138; // Closed (Locked on to basket)
   }

   //**********Crate Servos (Nub) ***********
   if(!btn8Pressed && joy2Btn(8))
    {
      CrateIsUp = !CrateIsUp;
      btn8Pressed = true;
    }
    else if(!joy2Btn(8))
    {
      btn8Pressed = false;
    }
    if(CrateIsUp)
    {
    servoTarget[crate6] = 193; // Down, flipping crate
    }
   else
    {
     servoTarget[crate6] = 255; // Up, Behind lift
   }
   //_______________________________________________________________

   //******************Left Flap******************
   if(joy1Btn(5))
    {
    servoTarget[leftflap2] = 195;
    }
  else
    {
    servoTarget[leftflap2] = 75;
    }

   //******************Right Flap******************
   if(joy1Btn(6))
    {
    servoTarget[rightflap3] = 132;
    }
  else
    {
    servoTarget[rightflap3] = 243;
    }

   //******************Magnetic Release******************
   if(joy2Btn(2))
    {
    servoTarget[MagR5] = 187;
    }
  else
    {
    servoTarget[MagR5] = 41;
    }

   //******************Magnetic Ball Guard******************
   if(!btn3Pressed && joy1Btn(3))
    {
      MagnetGuardIsUp = !MagnetGuardIsUp;
      btn3Pressed = true;
    }
    else if(!joy1Btn(3))
    {
      btn3Pressed = false;
    }
    if(MagnetGuardIsUp)
    {
    servoTarget[MagG4] = 50;//Down
    }
   else
    {
     servoTarget[MagG4] = 175; //Up
   }

 }
}