//////////////////////////////////////////////
//        RemoteXY include library          //
//////////////////////////////////////////////

// RemoteXY select connection mode and include library
#define REMOTEXY_MODE__SOFTSERIAL
#include <SoftwareSerial.h>

#include <RemoteXY.h>

// RemoteXY connection settings
#define REMOTEXY_SERIAL_RX 3
#define REMOTEXY_SERIAL_TX 2
#define REMOTEXY_SERIAL_SPEED 9600
#define REMOTEXY_ACCESS_PASSWORD "TOPSECRETPASSWORD"


// RemoteXY configurate
#pragma pack(push, 1)
uint8_t RemoteXY_CONF[] =   // 52 bytes
{ 255, 5, 0, 0, 0, 45, 0, 16, 248, 0, 10, 53, 69, 3, 15, 15, 4, 26, 31, 79,
  78, 0, 31, 79, 70, 70, 0, 5, 0, 9, 15, 33, 33, 120, 26, 31, 4, 0, 45, 14,
  9, 35, 78, 26, 4, 128, 61, 28, 35, 8, 162, 26
};

// this structure defines all the variables of your control interface
struct {

  // input variable
  uint8_t pushSwitch_1; // =1 if state is ON, else =0
  int8_t joystick_1_x; // =-100..100 x-coordinate joystick position
  int8_t joystick_1_y; // =-100..100 y-coordinate joystick position
  int8_t slider_1; // =0..100 slider position
  int8_t slider_2; // =0..100 slider position

  // other variable
  uint8_t connect_flag;  // =1 if wire connected, else =0

} RemoteXY;
#pragma pack(pop)

/////////////////////////////////////////////
//           END RemoteXY include          //
/////////////////////////////////////////////

//////////////////////////////////////////////
//         START Motor Shield code          //
//////////////////////////////////////////////

// Include for Adafruit Motor Shield
#include <Wire.h>
#include <Adafruit_MotorShield.h>

#include "utility/Adafruit_MS_PWMServoDriver.h"

#define PIN_PUSHSWITCH_1 12

// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
// Or, create it with a different I2C address (say for stacking)
// Adafruit_MotorShield AFMS = Adafruit_MotorShield(0x61);

// Select which 'port' M1, M2, M3 or M4. In this case, M1
Adafruit_DCMotor *motor1 = AFMS.getMotor(1);
Adafruit_DCMotor *motor2 = AFMS.getMotor(2);
Adafruit_DCMotor *motor3 = AFMS.getMotor(3);
Adafruit_DCMotor *motor4 = AFMS.getMotor(4);

void runMotor(Adafruit_DCMotor *myMotor, int speed) {
  if (speed > 0) {
    myMotor->run(FORWARD);
    myMotor->setSpeed(speed);

  } else if (speed < 0) {
    myMotor->run(BACKWARD);
    myMotor->setSpeed(-speed);

  } else {
    myMotor->run(BRAKE);
    myMotor->setSpeed(0);

  }
}


//////////////////////////////////////////////
//          END Motor Shield code           //
//////////////////////////////////////////////

/////////////////////////////////////////////
//           START Servo include           //
/////////////////////////////////////////////

#include <Servo.h>
Servo servo_1;
Servo servo_2;



/////////////////////////////////////////////
//           END Servo include             //
/////////////////////////////////////////////


void setup()
{
  RemoteXY_Init ();
  Serial.begin(9600);
  servo_1.attach(9);
  servo_2.attach(10);
  AFMS.begin();  // create with the default frequency 1.6KHz

}

const int max_speed = 150; // value from 0-255. For 9V batteries, speed over 150 draws too much power.
// Use bigger batteries for larger speeds.
void loop()
{
  RemoteXY_Handler ();
  motor_drive(RemoteXY.joystick_1_x, RemoteXY.joystick_1_y);
  servo_1.write(map(RemoteXY.slider_1, 0, 100, 90, 180));
  servo_2.write(map(RemoteXY.slider_2, -100, 100, 0, 180)); //Uncomment this line to use second servo
  //digitalWrite(PIN_PUSHSWITCH_1, (RemoteXY.pushSwitch_1 == 0) ? LOW : HIGH); //uncomment if you're using an LED on pin 12


  ///////////run the third motor off of button

  if (RemoteXY.pushSwitch_1 == 0) {
    runMotor(motor2, max_speed);
  }
  else {
    runMotor(motor2, 0);
  }


}

void motor_drive(int x, int y)
{
  int v = (100 - abs(x)) * (y / 100) + y;
  int w = (100 - abs(y)) * (x / 100) + x;
  int R = (v + w) / 2;
  int L = (v - w) / 2;
  runMotor(motor1, map(L, -100, 100, max_speed, -max_speed));
  runMotor(motor3, map(R, -100, 100, -max_speed, max_speed));
}