#include        "AccelStepper.h"
#include        "ESP32Servo.h"
#define XSTEP   15
#define XDIR    4
#define YSTEP   5
#define YDIR    18
#define SERVO   13
#define BTN1    12
#define DRIVER  1

#define CLOSE   0
#define OPEN    180
#define STOP    0
#define REV     1
#define FWD     3

AccelStepper stepperX(DRIVER, XSTEP, XDIR);
AccelStepper stepperY(DRIVER, YSTEP, YDIR);
Servo servo;

struct STRUCT1 {
  byte xData = STOP;
  byte yData = STOP;
  byte servo = OPEN;
} txPacket;

struct STRUCT2 {
  byte xData = STOP;
  byte yData = STOP;
  byte servo = OPEN;
} rxPacket;

byte  xState = STOP, lastXstate = STOP, yState = STOP, lastYstate = STOP, 
      jawState = OPEN, lastJawState = OPEN, lastServoState = OPEN,
      lastRxX = STOP, lastRxY = STOP;
bool  btnState, lastBtnState = true, press = false, moveX = false, moveY = false;
long prevTime = 0;

void setup(){
  Serial.begin(115200);
  pinMode(BTN1, INPUT_PULLUP);
  ESP32PWM::allocateTimer(3);   //Not needed in an AVR setup
  servo.setPeriodHertz(50);    // standard 50 hz servo
	servo.attach(SERVO, 1000, 2000); // attaches the servo on pin 18 to the servo object
  servo.write(CLOSE);              
  delay(500);
  stepperX.setMaxSpeed(1000);
  stepperY.setMaxSpeed(1000);
  stepperX.setAcceleration(50);
  stepperY.setAcceleration(50);
  stepperX.stop();
  stepperY.stop();
}	

void loop() {
  byte xysState = 0; 
  int val1  = analogRead(34);                //Transmitter Logic Starts by reading states
  int val2  = analogRead(35);
  int xAxis = map(val1, 0, 4095, -100, 100);
  int yAxis = map(val2, 0, 4095, -100, 100);
  btnState  = digitalRead(BTN1);

  //First we do our button
  if(btnState != lastBtnState){
    press = true;
    prevTime = millis();
  }
  if(millis() - prevTime >= 50){
    if(press){
      if(btnState == LOW){
        if(jawState == OPEN){
          jawState = CLOSE;
        }else{
          jawState = OPEN;
        }
      }
      press = false;
    }
  }
  
  if(xAxis < 0 || xAxis > 0){    //Creates State from Joystick Input
    if(xAxis > 0){
      xState = FWD;
    }else if(xAxis < 0){
      xState = REV;
    }
  }else{
     xState = STOP;
  }
  if(yAxis < 0 || yAxis > 0){    //Creates State from Joystick Input
    if(yAxis > 0){
      yState = FWD;
    }else if(yAxis < 0){
      yState = REV;
    }
  }else{
     yState = STOP;
  }


  if(xState != lastXstate || yState != lastYstate || jawState != lastJawState){    
    txPacket.xData = xState;
    txPacket.yData = yState;
    txPacket.servo = jawState;
    ////////////////////////////////////////////////////////////////
    /******************* Imaginary WiFi Link **********************/
    ////////////////////////////////////////////////////////////////
    rxPacket.xData = txPacket.xData;
    rxPacket.yData = txPacket.yData;
    rxPacket.servo = txPacket.servo;
      if(rxPacket.xData == FWD){
        Serial.println("X-Axis Moving Reverse");
      }
      if(rxPacket.xData == REV){
        Serial.println("X-Axis Moving Forward");
      }
      if(rxPacket.xData == STOP){
        Serial.println("X-Axis Stopped");
      }
      if(rxPacket.yData == FWD){ 
        Serial.println("Y-Axis Moving Forward");
      }
      if(rxPacket.yData == REV){
        Serial.println("Y-Axis Moving Reverse");
      }
      if(rxPacket.yData == STOP){
        Serial.println("Y-Axis Stopped");
      }
      if(rxPacket.servo == OPEN){
        Serial.println("JAW is Open");
      }else{      
        Serial.println("JAW is Closed");
      }
      Serial.printf("Bytes Received: %d\n\n", sizeof(rxPacket));
  }                                                  

/*                                              RX Logic Begins  */              
  if(rxPacket.xData == FWD){               //Receiver X-Axis Motion Logic
    stepperX.setSpeed(50);
    moveX = true;
  }else if(rxPacket.xData == REV){
    stepperX.setSpeed(-50);
    moveX = true;
  }else if(rxPacket.xData == STOP){
    moveX = false;
    stepperX.stop();
  }
  if(rxPacket.yData == FWD){               //Receiver X-Axis Motion Logic
    stepperY.setSpeed(50);
    moveY = true;
  }else if(rxPacket.yData == REV){
    stepperY.setSpeed(-50);
    moveY = true;
  }else if(rxPacket.yData == STOP){
    moveY = false;
    stepperY.stop();
  }
  if(rxPacket.servo != lastServoState){
    if(rxPacket.servo == OPEN){
      servo.write(CLOSE);
    }else{
      servo.write(OPEN);
    }
  }
  if(moveX){
      stepperX.runSpeed();
  }                                     //End Receiver Logic
    if(moveY){
      stepperY.runSpeed();
  }
  lastXstate = xState;                  //The State engine of TX Logic
  lastYstate = yState;                  //The State engine of TX Logic
  lastBtnState = btnState;              //For Button State in TX
  lastJawState = jawState;              //For Servo State in TX
  lastServoState = rxPacket.servo;      //RX Logic for Servo
  lastRxX = rxPacket.xData;             //RX logic for X-Axis
  lastRxY = rxPacket.yData;             //RX Logic for Y-Axis

  delay(10);
}