#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S1,     ,                    sensorI2CMuxController)
#pragma config(Sensor, S2,     Magnet,              sensorAnalogInactive)
#pragma config(Sensor, S3,     touch,               sensorTouch)
#pragma config(Sensor, S4,     gyro,                sensorNone)
#pragma config(Motor,  mtr_S1_C1_1,     LeftD,         tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     RightD,        tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_1,     Collector,     tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     Arm1,          tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     Arm2,          tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     Lift,          tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    SpinGrip,             tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C4_2,    GangR,                tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    GangL,                tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    MagArm,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    Bal1,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C4_6,    Bal2,                 tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                        Tele-Operation Mode Code Template
//
// This file contains a template for simplified creation of an tele-op program for an FTC
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////


#include "JoystickDriver.c"    //Include file to "handle" the Bluetooth messages.
//#include "drivers/HTMAG-driver.h" //include file to operate magnet sensor

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                                      initializeRobot
//
// Prior to the start of tele-op mode, you may want to perform some initialization on your robot
// and the variables within your program.
//
// In most cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

void initializeRobot()
{
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

  return ;
}


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                                             Main Task
//
// The following is the main code for the tele-op robot operation. Customize as appropriate for
// your specific robot.
//
// Game controller / joystick information is sent periodically (about every 50 milliseconds) from
// the FMS (Field Management System) to the robot. Most tele-op programs will follow the following
// logic:
//    1. Loop forever repeating the following actions:
//    2. Get the latest game controller / joystick settings that have been received from the PC.
//    3. Perform appropriate actions based on the joystick + buttons settings. This is usually a
//         simple action:
//         *    Joystick values are usually directly translated into power levels for a motor or
//             position of a servo.
//         *    Buttons are usually used to start/stop a motor or cause a servo to move to a specific
//             position.
//    4. Repeat the loop.
//
// Your program needs to continuously loop because you need to continuously respond to changes in
// the game controller settings.
//
// At the end of the tele-op period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

int tweekArm2(int joyValue){
  const int TWEEK =0;
  if(abs(joyValue)<5){
    return 0;
  }

  return 0;
}
int scaleMotor(int joyValue){
  const int MAX_MOTOR= 100;
  const float MAX_JOY = 127.0;
  /*deadzone*/
  if(abs(joyValue)<5){
    return 0;
  }
  //calculate scale value easier to control robot
  int dir=joyValue/abs(joyValue); // gets neg or pos.
  float ratio = (joyValue*joyValue)/(MAX_JOY*MAX_JOY);
  int scale=(ratio*MAX_MOTOR)*dir;
  return scale;
}


task drive()
{
  while(true)
  {
    getJoystickSettings(joystick);
    //controls tank drive
    motor[LeftD]=scaleMotor(joystick.joy1_y1);
    motor[RightD]=scaleMotor(joystick.joy1_y2);
  }
}
//needs adjustmemts senses magnets and auto separates
task magnet(){
  while(true){
   nxtDisplayTextLine(2, "Bias: %4d",SensorValue[Magnet]);
    if(SensorValue[Magnet]<508||/*SensorValue[Magnet]>700||*/SensorValue[Magnet]>515){
      servo[MagArm]=250;
    motor[Collector]=25;
      wait1Msec(1050);
      motor[Collector]=0;
      wait1Msec(1500);
      servo[MagArm]=0;
    }
  }
}
task main()
{
  initializeRobot();
  waitForStart();      // wait for start of tele-op phase
  StartTask(drive);
  StartTask(magnet);
  while (true)
  {
    getJoystickSettings(joystick);
    motor[Arm1]=joystick.joy2_y1;
    motor[Arm2]=joystick.joy2_y1;

    //collector
    if(joy1Btn(4)==1){
      motor[Collector]=25;
    }
    else if(joy1Btn(3)){
      motor[Collector]=0;
    }
    else if(joy1Btn(2)){
      motor[Collector]=-100;
    }
    //normal ball depositor arm
    else if(joy2Btn(1)){
      servo[Bal2]=250;
      wait1Msec(5000);
      servo[Bal2]=0;
    }
    //servos mag depositor arm
    else if(joy1Btn(1)){
      servo[Bal1]=25;
      wait1Msec(5000);
      servo[Bal1]=0;
    }
    //lift
    else if(joy2Btn(2)){
      while(SensorValue(touch)!=1){
        motor[Lift]=100;
      }

    }
    else if(joy2Btn(2)==0){
      motor[Lift]=0;
    }
    else if (joy2Btn(5)){
      motor[Lift]=-100;
    }
    //gripper servos
    else if(joy2Btn(8)){
      servo[GangR]=200;
      servo[GangL]=-200;
    }
    //flipper clock rotate joy2 ****tweeking needed
    else if(joy2Btn(6)){
      servo[SpinGrip]=255;
    }
    //flipper stops joy2 *****
    else if(joy2Btn(6)==0){
      servo[SpinGrip]=127;
    }
    //flipper rotates Counter joy2 ****
    else if(joy2Btn(9)){
      servo[SpinGrip]=0;
    }
    else if(joy2Btn(9)==0){
      servo[SpinGrip]=127;
    }

    ///////////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////
    ////                                                                            ////
    ////         Add your robot specific tele-op code here.    ////
    ////                                                                            ////
    ///////////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////

    // Insert code to have servos and motors respond to joystick and button values.

    // Look in the ROBOTC samples folder for programs that may be similar to what you want to perform.
    // You may be able to find "snippets" of code that are similar to the functions that you want to
    // perform.
  }
}