// *************************
// RemoteXY select connection mode and include library 
#define REMOTEXY_MODE__ESP32CORE_BLE
#include <BLEDevice.h>
#include <RemoteXY.h>

// RemoteXY connection settings 
#define REMOTEXY_BLUETOOTH_NAME "ESP32-C3_Remote"

// RemoteXY configurate  
#pragma pack(push, 1)
uint8_t RemoteXY_CONF[] =   // 43 bytes
  { 255,3,0,0,0,36,0,16,202,1,5,32,17,37,30,30,2,26,24,4,
  128,13,23,37,6,2,78,129,0,3,3,24,6,165,66,108,117,101,32,65,
  110,116,0 };
// this structure defines all the variables and events of your control interface 
struct {
  
    // input variables
  int8_t joystick_1_x; // from -100 to 100  
  int8_t joystick_1_y; // from -100 to 100  
  int8_t slider_1; // =0..100 slider position 
    // other variable
  uint8_t connect_flag;  // =1 if wire connected, else =0 
} RemoteXY;
#pragma pack(pop)
// *************************

// ************************* Servos
#include <ESP32C3_Servo.h>	//for ESP32-C3
//#include <Servo.h>		//for ESP8266
//#include <ESP32Servo.h>	//for ESP32-Wroom
Servo servo1;  Servo servo2;  Servo servo3;  // Servo3 is attacheched to same pin as Servo1 !!!
Servo Jaws;
int pos1, pos2;    // variable to store the servo position
int centerpos = 90;
int minpos = centerpos-12;
int maxpos = centerpos+12;
int gripper;
//int speed1; 


void setup() {
  Serial.begin(115200);
  servo1.attach(2); servo2.attach(3); servo3.attach(4);  //Servo3 can be attached to same pin as Servo1 //GPIOs on ESP32-C3!
  Jaws.attach(5);
  delay(2);
  servo1.write(centerpos);servo2.write(centerpos-7);servo3.write(centerpos);  //center all servos
  Jaws.write(centerpos);
  
 delay(3000);
 
 RemoteXY_Init (); 
}


void loop() {
   RemoteXY_Handler ();
   
 
    if ((RemoteXY.joystick_1_x) < -30) {
      //Serial.println("<-- left   ");
      left(16);
    }
    if ((RemoteXY.joystick_1_x) > 30) {
      //Serial.println("  right -->");
      right(16);
    }
    if ((RemoteXY.joystick_1_y) < -30) {
      //Serial.println(" backwards ");
      // not yet installed
    }
    if ((RemoteXY.joystick_1_y) > 30) {
      //Serial.println(" ^forward^ ");
      forward(16);
    }
    gripper = map((RemoteXY.slider_1), 0, 100, 75,105);
    Jaws.write(gripper);
    delay(1);
}


void forward(int speed1) {               //speed1 in ms as delay between steps
    
  for (pos1 = minpos; pos1 <= maxpos; pos1 += 2) { 
    pos2 = map(pos1, minpos, maxpos, maxpos, minpos);
    
    servo1.write(pos1);servo3.write(pos1);Serial.print(pos1);Serial.print("  ");  // front/rear legs
    servo2.write(pos2);  Serial.print(pos2); Serial.println("");                  // center legs
      
    delay(speed1); 
  }
  for (pos1 = maxpos; pos1 >= minpos; pos1 -= 2) {
    pos2 = map(pos1, minpos, maxpos, maxpos, minpos);
    
    servo1.write(pos1);servo3.write(pos1);      // front/rear legs
    servo2.write(pos2);
    
    delay(speed1);
  }
}


void right(int speed1) {               //speed1 in ms as delay between steps
    
  for (pos1 = minpos; pos1 <= maxpos; pos1 += 2) { 
    pos2 = map(pos1, minpos, maxpos, maxpos, minpos);
    
    servo1.write(pos1+10);servo3.write(pos1+10);  // front/rear legs
    servo2.write(pos2+10);                           // center legs
      
    delay(speed1); 
  }
  for (pos1 = maxpos; pos1 >= minpos; pos1 -= 1) {
    pos2 = map(pos1, minpos, maxpos, maxpos, minpos);
    
    servo1.write(pos1+10);servo3.write(pos1+10);      // front/rear legs
    servo2.write(pos2+10);            

    
    delay(speed1);
  }
}

void left(int speed1) {               //speed1 in ms as delay between steps
    
    //pos2 = maxpos + 1 -7; //-8 is to correct center position
  for (pos1 = minpos; pos1 <= maxpos; pos1 += 1) { 
    pos2 = map(pos1, minpos, maxpos, maxpos, minpos);
    
    servo1.write(pos1-10);servo3.write(pos1-10);  // front/rear legs
    servo2.write(pos2-10);                           // center legs
      
    delay(speed1); 
  }
  for (pos1 = maxpos; pos1 >= minpos; pos1 -= 2) {
    pos2 = map(pos1, minpos, maxpos, maxpos, minpos);
    
    servo1.write(pos1-10);servo3.write(pos1-10);      // front/rear legs
    servo2.write(pos2-10);            
    
    delay(speed1);
  }
}