// Robot Bacteria with ESP-NOW
// ===========================
// Markus Opitz 2023
// Instructables.com


#include <esp_now.h>
#include <WiFi.h>

//motor
int motorPin = 2; // == D0
int mspeed = 100;  // 0-255

//stepper motors
#define A1 6    //blue
#define A2 5    //orange
#define B1 21   //green
#define B2 7    //brown

#define C1 8    //blue
#define C2 20   //orange
#define D1 10   //green
#define D2 9    //brown

unsigned int delaytime = 10;
int xflag=0, xtrigger = 0;
int yflag=0, ytrigger = 0;


typedef struct struct_message {
    int xVal;
    int yVal;
    int bVal;
} struct_message;
struct_message myData;


const int DEBOUNCE_DELAY = 50;   // the debounce time; increase if the output flickers

// button debounce
int lastSteadyState = LOW;
int lastState = LOW;
unsigned long lastDebounceTime = 0;
int flag = 0;


void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
  memcpy(&myData, incomingData, sizeof(myData));
  
  commands();
}
 
void setup() {
    Serial.begin(115200);
  //stepper motors
    pinMode(A1, OUTPUT);
    pinMode(A2, OUTPUT);
    pinMode(B1, OUTPUT);
    pinMode(B2, OUTPUT);

    pinMode(C1, OUTPUT);
    pinMode(C2, OUTPUT);
    pinMode(D1, OUTPUT);
    pinMode(D2, OUTPUT);

  //motor
    pinMode(motorPin, OUTPUT);
    delay(10);
  
  Serial.begin(115200);
  delay(100);
  WiFi.mode(WIFI_STA);

  // Init ESP-NOW
  if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }
  esp_now_register_recv_cb(OnDataRecv);
  Serial.println("ESP-NOW ready");
}


void loop() {
}

void commands() {
  
    // convert the values
  int xvalue = map(myData.xVal, 0, 255, -100, 100) +10;
  int yvalue = map(myData.yVal, 0, 255, -100, 100) +9;
  
  // get trigger
  if (yvalue < -70) (xtrigger = 1); //left
  if (yvalue > 70) (xtrigger = 2); //right
  
  if (xvalue < -70) (ytrigger = 2); //up
  if (xvalue > 70) (ytrigger = 1); //down

  //centered?
  if ((xvalue > -10) && (xvalue< 10) &&
      (yvalue > -10) && (yvalue< 10)) {
        xtrigger = 0;
        ytrigger = 0;
    }


  // see if already triggered 
  if ((xtrigger == 1) && (xflag == 0)) {
      Serial.println(" make 1 step left"); 
      clockwise1(1); delay(1);
      xflag = 1; 
  }
  if ((xtrigger == 2) && (xflag == 0)) {
      Serial.println(" make 1 step right"); 
      cclockwise1(1); delay(1);
      xflag = 2; 
  }
  if ((ytrigger == 2) && (yflag == 0)) {
      Serial.println(" make 1 step up"); 
      clockwise2(1); delay(1);
      yflag = 2; 
  }
  if ((ytrigger == 1) && (yflag == 0)) {
      Serial.println(" make 1 step down"); 
      cclockwise2(1); delay(1);
      yflag = 1; 
  }

// back to center when joystick is centered
  if (xtrigger == 0) {
    if (xflag == 1) {
      cclockwise1(1); delay(1); xflag = 0;  // back to center
      Serial.println("    --- centered ---");
    }
    if (xflag == 2) {
      clockwise1(1); delay(1); xflag = 0;  // back to center
      Serial.println("    --- centered ---");
    }
  }
  if (ytrigger == 0) {
    if (yflag == 1) {
      cclockwise2(1); delay(1); yflag = 0;  // back to center
      Serial.println("    --- centered ---");
    }
    if (yflag == 2) {
      clockwise2(1); delay(1); yflag = 0;  // back to center
      Serial.println("    --- centered ---");
    }
  }


  

  //debounce subfunction
  if (myData.bVal != lastState) {
    lastDebounceTime = millis();
    lastState = myData.bVal;
  }
  if ((millis() - lastDebounceTime) > DEBOUNCE_DELAY) {
    if (lastSteadyState == HIGH && myData.bVal == LOW)
      if (flag == 0) {
        Serial.println("Motor on");
        analogWrite(motorPin, mspeed);
        flag = 1;
      }
      else if (flag == 1) {
        Serial.println("Motor off");
        analogWrite(motorPin, 0);
        flag = 0;
      }

    lastSteadyState = myData.bVal;
  }
  
 delay(10);
}






void step11(){
   digitalWrite(A1, 1);
   digitalWrite(A2, 0);
   digitalWrite(B1, 1);
   digitalWrite(B2, 0);
   delay(delaytime);
}

void step12(){
   digitalWrite(A1, 1);
   digitalWrite(A2, 0);
   digitalWrite(B1, 0);
   digitalWrite(B2, 1);
   delay(delaytime);
}

void step13(){
   digitalWrite(A1, 0);
   digitalWrite(A2, 1);
   digitalWrite(B1, 0);
   digitalWrite(B2, 1);
   delay(delaytime);
}

void step14(){
   digitalWrite(A1, 0);
   digitalWrite(A2, 1);
   digitalWrite(B1, 1);
   digitalWrite(B2, 0);
   delay(delaytime);
}



void step21(){
   digitalWrite(C1, 1);
   digitalWrite(C2, 0);
   digitalWrite(D1, 1);
   digitalWrite(D2, 0);
   delay(delaytime);
}

void step22(){
   digitalWrite(C1, 1);
   digitalWrite(C2, 0);
   digitalWrite(D1, 0);
   digitalWrite(D2, 1);
   delay(delaytime);
}

void step23(){
   digitalWrite(C1, 0);
   digitalWrite(C2, 1);
   digitalWrite(D1, 0);
   digitalWrite(D2, 1);
   delay(delaytime);
}

void step24(){
   digitalWrite(C1, 0);
   digitalWrite(C2, 1);
   digitalWrite(D1, 1);
   digitalWrite(D2, 0);
   delay(delaytime);
}


void clockwise1(long steps){  //turn right
 long p = 1;
 while( p < steps ){
   step11();
   step12();
   step13();
   step14();
   p++;
 }
}

void cclockwise1(long steps){  //turn left
 long p = 1;
 while( p < steps ){
   step14();
   step13();
   step12();
   step11();
   p++;
 }
}

void clockwise2(long steps){  //turn right
 long p = 1;
 while( p < steps ){
   step21();
   step22();
   step23();
   step24();
   p++;
 }
}

void cclockwise2(long steps){  //turn left
 long p = 1;
 while( p < steps ){
   step24();
   step23();
   step22();
   step21();
   p++;
 }
}