//--------------------------------------------
//ESP32 robot via ESP-NOW Protocol
//Transmit fsr - receive joyvals
//--------------------------------------------
#include "AccelStepper.h"
#include <ESP32MX1508.h>
#include <pwmWrite.h>
#include <esp_now.h>
#include <WiFi.h>
#include <Wire.h>

//------------------------------------------------------------


// Yrotation driver pins
#define ROTA 12
#define ROTB 14
#define CH1 0
#define CH2 1
#define RES 12

MX1508 motorA(ROTA, ROTB, CH1, CH2, RES); //object for yrot motor


#define XSTEP   23
#define XDIR    22
#define YSTEP   33
#define YDIR    32
#define ZSTEP   26
#define ZDIR    25

#define DRIVER  1

AccelStepper stepperX(DRIVER, XSTEP, XDIR);
AccelStepper stepperY(DRIVER, YSTEP, YDIR);
AccelStepper stepperZ(DRIVER, ZSTEP, ZDIR);

uint8_t servoPin = 18;
Pwm myservo = Pwm();

//int fsrValue = 0;

//STATES
#define FWD     2
#define REV     1
#define STOP    0
#define OPEN    0
#define CLOSE   1
#define CW      1
#define ACW     2

//------------------------------------------------------------
uint8_t RxMACaddress[] = {0xEC, 0x62, 0x60, 0x9A, 0x62, 0x60};
//------------------------------------------------------------
typedef struct TxStruct
{
  int fsr;

} TxStruct;
TxStruct sentData;
//------------------------------------------------------------
typedef struct RxStruct
{
  byte xData = STOP;
  byte yData = STOP;
  byte zData = STOP;
  bool jaw = CLOSE;
  byte rot = STOP;
} RxStruct;
RxStruct receivedData;


byte lastxState = 0;
byte lastyState = 0;
byte lastzState = 0;
byte lastjawState = 0;
byte lastrotState = 0;

//byte lastfsrValue = 0;


bool xmove = false;
bool ymove = false;
bool zmove = false;

esp_now_peer_info_t peerInfo;
//------------------------------------------------------------
void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len)
{
  memcpy(&receivedData, incomingData, sizeof(receivedData));

  if (lastxState != receivedData.xData || lastyState != receivedData.yData || lastzState != receivedData.zData || lastjawState != receivedData.jaw || lastrotState != receivedData.rot) {

    // moving x
    if (receivedData.xData == FWD) {
      stepperX.setSpeed(500);
      xmove = true;
    } else if (receivedData.xData == REV) {
      stepperX.setSpeed(-500);
      xmove = true;
    } else if (receivedData.xData == STOP) {
      xmove = false;
      stepperX.stop();
    }

    // moving y
    if (receivedData.yData == FWD) {
      stepperY.setSpeed(500);
      ymove = true;
    } else if (receivedData.yData == REV) {
      stepperY.setSpeed(-500);
      ymove = true;
    } else if (receivedData.yData == STOP) {
      ymove = false;
      stepperY.stop();
    }


    //Moving Z
    if (receivedData.zData == FWD) {
      stepperZ.setSpeed(500);
      zmove = true;
    } else if (receivedData.zData == REV) {
      stepperZ.setSpeed(-500);
      zmove = true;
    } else if (receivedData.zData == STOP) {
      zmove = false;
      stepperZ.stop();
    }

    // jaw
    if (receivedData.jaw == OPEN) {
      myservo.writeServo(servoPin, 0);

    } else {
      myservo.writeServo(servoPin, 180);

    }

    // rot
    if (receivedData.rot == CW) {
      motorA.motorGo(4095);
    } else if (receivedData.rot == ACW) {
      motorA.motorRev(4095);
    }
    else if (receivedData.rot == STOP) {
      motorA.motorBrake();
    }

    lastxState = receivedData.xData;
    lastyState = receivedData.yData;
    lastzState = receivedData.zData;
    lastjawState = receivedData.jaw;
    lastrotState = receivedData.rot;

  }

  //  lastxState = receivedData.xData;
  //  lastyState = receivedData.yData;
  //  lastzState = receivedData.zData;
  //  lastjawState = receivedData.jaw;
  //  lastrotState = receivedData.rot;
}

//======================================================================================
void setup()
{
  Serial.begin(115200);
  //myservo.attach(pin, channel, minUs, maxUs); // channel 0-15
  myservo.attach(servoPin, 4, 500, 2400);

  // quick test
  myservo.writeServo(servoPin, 135);
  delay(1000);
  myservo.writeServo(servoPin, 180);
  delay(1000);
  myservo.writeServo(servoPin, 0);
  delay(1000);
  myservo.writeServo(servoPin, 45);
  delay(1000);
  myservo.writeServo(servoPin, 90);

  pinMode(34, INPUT);

  stepperX.setMaxSpeed(1000);
  stepperX.setAcceleration(30);
  stepperX.setSpeed(50);
  stepperX.stop();

  stepperY.setMaxSpeed(1000);
  stepperY.setAcceleration(30);
  stepperY.setSpeed(50);
  stepperY.stop();

  stepperZ.setMaxSpeed(1000);
  stepperZ.setAcceleration(30);
  stepperZ.setSpeed(50);
  stepperZ.stop();

  //----------------------------------------------------------
  WiFi.mode(WIFI_STA);
  if (esp_now_init() != ESP_OK)
  {
    Serial.println("Error initializing ESP-NOW");
    return;
  }

  //----------------------------------------------------------

  memcpy(peerInfo.peer_addr, RxMACaddress, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;
  //----------------------------------------------------------
  if (esp_now_add_peer(&peerInfo) != ESP_OK)
  {
    Serial.println("Failed to add peer");
    return;
  }
  //----------------------------------------------------------
  esp_now_register_recv_cb(OnDataRecv);
}
//======================================================================================
void loop()
{
  // put your main code here, to run repeatedly:
  if (xmove) {
    stepperX.runSpeed();
  }

  if (ymove) {
    stepperY.runSpeed();
  }

  if (zmove) {
    stepperZ.runSpeed();
  }

  //reading fsr
  sentData.fsr = analogRead(34);

  esp_err_t result = esp_now_send(RxMACaddress, (uint8_t *) &sentData, sizeof(sentData));
  //----------------------------------------------------------
  if (result == ESP_OK) Serial.println("Sent with success");
  else Serial.println("Error sending the data");
}