//add the sevo library
#include <Servo.h>
//#include <ezbutton>
//define servos
/*Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;


//define joystick pins (analog)
int joy1X = A0;
int joy1Y = A1;

int joy2X = A2;
int joy2Y = A3;

int StickL = 6;
int StickR = 7;

//variable to read the values from the analog pins
int joyval;

void setup()
{
  servo1.attach(2);
  servo2.attach(3);
  servo3.attach(4);
  servo4.attach(5);
  servo5.attach(45);
  servo6.attach(46);
  Serial.begin(9600);
}

void loop()
{
  //read the value of joustic (between 0-1023)
  int joyval = analogRead(joy1X);
  joyval = map(joyval,0,1023,0,180);
  servo1.write(joyval);
  Serial.println(joyval);
  
  int joyval1 = analogRead(joy1Y);
  joyval1 = map(joyval1,0,1023,0,180);
  servo2.write(joyval1);
  Serial.println(joyval1);

  int joyval2 = analogRead(joy2X);
  joyval2 = map (joyval2,0,1023,0,180);
  servo3.write(joyval2);
  Serial.println(joyval2);
  
  int joyval3 = analogRead(joy2Y);
  joyval3 = map (joyval3,0,1023,0,180);
  servo4.write(joyval3);
  Serial.println(joyval3);
  
  int joyval4 = digitalRead(StickR);
    if (joyval4 == 1)
    {
      servo5.write(0);
    }
    else
    {
      servo5.write(90);
    }
  int joyval5 = digitalRead(StickL);
    if (joyval5 == 1)
    {
      servo6.write(0);
    }
    else
    {
      servo6.write(90);
    }
delay(100);
}*/




/*int xValue = analogRead(A0);
int yValue = analogRead(A1);


/*
 * Created by ArduinoGetStarted.com
 *
 * This example code is in the public domain
 *
 * Tutorial page: https://arduinogetstarted.com/tutorials/arduino-joystick
 

#define VRX_PIN  A0 // Arduino pin connected to VRX pin
#define VRY_PIN  A1 // Arduino pin connected to VRY pin

int xValue = 0; // To store value of the X axis
int yValue = 0; // To store value of the Y axis

void setup() {
  Serial.begin(9600) ;
}

void loop() {
  // read analog X and Y analog values
  xValue = analogRead(VRX_PIN);
  yValue = analogRead(VRY_PIN);

  // print data to Serial Monitor on Arduino IDE
  Serial.print("x = ");
  Serial.print(xValue);
  Serial.print(", y = ");
  Serial.println(yValue);
  delay(200);
}*/
/*
 * Created by ArduinoGetStarted.com
 *
 * This example code is in the public domain
 *
 * Tutorial page: https://arduinogetstarted.com/tutorials/arduino-joystick
 */

#include <ezButton.h>

#define VRX_PIN  A0 // Arduino pin connected to VRX pin
#define VRY_PIN  A1 // Arduino pin connected to VRY pin
#define SW_PIN   6  // Arduino pin connected to SW  pin
#define SW_PIN1   7  // Arduino pin connected to SW  pin


ezButton button(SW_PIN);

int xValue = 0; // To store value of the X axis
int yValue = 0; // To store value of the Y axis
int bValue = 0; // To store value of the button


**************************************************************
#include <Ps3Controller.h>
#include <ESP32Servo.h>

#define SERVO_FORWARD_STEP_ANGLE 1
#define SERVO_BACKWARD_STEP_ANGLE -1

struct ServoPins
{
  Servo servo;
  int servoPin;
  String servoName;
  int initialPosition;  
};
std::vector<ServoPins> servoPins = 
{
  { Servo(), 27 , "Base", 90},
  { Servo(), 26 , "Shoulder", 90},
  { Servo(), 25 , "Elbow", 90},
  { Servo(), 33 , "Gripper", 90},
};

bool gripperSwitch = false;

const int PWMFreq = 1000; /* 1 KHz */
const int PWMResolution = 8;
const int PWMSpeedChannel = 4;

void writeServoValues(int servoIndex, int servoMoveStepSize, bool servoStepSizeIsActualServoPosition = false)
{
  int servoPosition;
  if (servoStepSizeIsActualServoPosition)
  {
    servoPosition = servoMoveStepSize; 
  }
  else
  {
    servoPosition = servoPins[servoIndex].servo.read();
    servoPosition = servoPosition + servoMoveStepSize;
  }
  if (servoPosition > 180 || servoPosition < 0)
  {
    return;
  }

  servoPins[servoIndex].servo.write(servoPosition);   
}


void notify()
{
  int rx =(Ps3.data.analog.stick.rx);  //Base       =>  Right stick - x axis
  int ry =(Ps3.data.analog.stick.ry);  //Shoulder   =>  Right stick  - y axis
  int ly =(Ps3.data.analog.stick.ly);  //Elbow      =>  Left stick  - y axis 
  int lx =(Ps3.data.analog.stick.lx);  //Gripper    =>  Left stick - x axis

  if (rx > 50)
  {
    writeServoValues(0, SERVO_BACKWARD_STEP_ANGLE);  
  }
  else if (rx < -50)
  {
    writeServoValues(0, SERVO_FORWARD_STEP_ANGLE);  
  }

  if (ry > 50)
  {
    writeServoValues(1, SERVO_BACKWARD_STEP_ANGLE);  
  }
  else if (ry < -50)
  {
    writeServoValues(1, SERVO_FORWARD_STEP_ANGLE);  
  }

  if (ly > 50)
  {
    writeServoValues(2, SERVO_FORWARD_STEP_ANGLE);  
  }
  else if (ly < -50)
  {
    writeServoValues(2, SERVO_BACKWARD_STEP_ANGLE);  
  }

  if (lx > 50)
  {
    writeServoValues(3, SERVO_BACKWARD_STEP_ANGLE);  
  }
  else if (lx < -50)
  {
    writeServoValues(3, SERVO_FORWARD_STEP_ANGLE);  
  }

  if (Ps3.event.button_down.r2)
  {
    gripperSwitch = !gripperSwitch;  //Toggle gripper close / open
    gripperSwitch ? writeServoValues(3, 170, true) :  writeServoValues(3, 100, true) ;
  }
    
  delay(10);
  
}

void onConnect()
{
  Serial.println("Connected!.");
}

void onDisConnect()
{
  Serial.println("Disconnected!.");    
}

void setUpPinModes()
{
  for (int i = 0; i < servoPins.size(); i++)
  {
    servoPins[i].servo.attach(servoPins[i].servoPin);
    servoPins[i].servo.write(servoPins[i].initialPosition);    
  }

  //Set up PWM for motor speed
  ledcSetup(PWMSpeedChannel, PWMFreq, PWMResolution);
  ledcAttachPin(enableRightMotor, PWMSpeedChannel);
  ledcAttachPin(enableLeftMotor, PWMSpeedChannel);  
  ledcWrite(PWMSpeedChannel, MAX_MOTOR_SPEED);
  
  rotateMotor(0, 0);  
}


void setup()
{
  setUpPinModes();
  Serial.begin(115200);
  Ps3.attach(notify);
  Ps3.attachOnConnect(onConnect);
  Ps3.attachOnDisconnect(onDisConnect);
  Ps3.begin();
  Serial.println("Ready.");
}

void loop()
{
}