#include "WiFi.h"
#include "WiFiUDP.h"
#include "BasicStepperDriver.h"
#include "MultiDriver.h"
#include "SyncDriver.h"
#include "ESP32Servo.h"

#define MOTOR_STEPS 200
#define L_Motor_RPM 7 //speed
#define R_Motor_RPM 7 //speed
#define MICROSTEPS 16

#define ENB_Pin 19
#define L_Motor_DIR 26
#define L_Motor_STEP 33
#define R_Motor_DIR 32
#define R_Motor_STEP 25

BasicStepperDriver L_Motor(MOTOR_STEPS, L_Motor_DIR, L_Motor_STEP);
BasicStepperDriver R_Motor(MOTOR_STEPS, R_Motor_DIR, R_Motor_STEP);
SyncDriver controller(L_Motor, R_Motor);

WiFiUDP UDP;
char recbuff[8];
IPAddress myIP(192, 168, 4, 1);
IPAddress HOSTIP (192, 168, 4, 2);
const char *ssid = "OSTR";
const char *password = "12345678";
int fs = 3; //font size fs=1mm
int servoPin = 23;
int PEN_DOWN = 60; // f-sizengle of servo when pen is down
int PEN_UP = 20;   // f-sizengle of servo when pen is up
Servo penServo;

float wheel_dia = 68.9;// # mm (increase = spiral out)
float wheel_base = 92.13;// # mm (increase = spiral in, ccw)
int steps_rev = 3200; // # 200*16 MICROSTEPS

void setup() {
  WiFi.softAP(ssid, password); // setting SSID and Password
  WiFi.config(myIP, WiFi.gatewayIP(), WiFi.subnetMask()); // setting IP Address
  UDP.begin(8000); // setting UDP port.
  Serial.begin(115200);
  pinMode(ENB_Pin, OUTPUT); digitalWrite(ENB_Pin, HIGH);
  L_Motor.begin(L_Motor_RPM, MICROSTEPS);
  R_Motor.begin(R_Motor_RPM, MICROSTEPS);
  penServo.attach(servoPin);
  penup();
  delay(300);
}

void loop() {
  if (UDP.parsePacket() > 0) {
    UDP.read(recbuff, 8);
    UDP.flush();
  }
  WiFi_receive();
}

void WiFi_receive() {
  switch (recbuff[0]) {
    case 'F':
      forward(1);
      break;

    case 'B':
      backward(1);
      break;

    case 'R':
      right(1);
      break;

    case 'L':
      left(1);
      break;

    case '0': //100mm Square Calibration
      pendown();
      for (int x = 0; x < 12; x++) {
        forward(100);
        left(90);
      }
      penup();
      done();
      break;

    case '1': // Hexagonal
      forward(20);
      pendown();
      for (int x = 0; x < 6; x++) {
        forward(30);
        right(60);
      }
      penup();
      forward(30);
      done();
      break;

    case '2': //Heart
      forward(50);
      right(45);
      pendown();
      forward(14.84);
      right(45);
      forward(14);
      right(45);
      forward(49.49);
      right(90);
      forward(49.49);
      right(45);
      forward(14);
      right(45);
      forward(14.84);
      right(45);
      forward(14);
      right(45);
      forward(14.84);
      left(90);
      forward(14.84);
      right(45);
      forward(14);
      penup();
      forward(15);
      penup();
      done();
      break;

    case '3': // 3 Diamond
      forward(30);
      right(60);
      pendown();
      forward(20);
      right(60);
      forward(40);
      right(60);
      forward(20);
      left(60);
      backward(20);
      right(60);
      backward(40);
      right(60);
      backward(20);
      left(60);
      forward(20);
      right(60);
      forward(40);
      right(60);
      forward(20);
      penup();
      right(60);
      forward(30);
      done();
      break;

    case '4': // 5Stars
      forward(30);
      right(72);
      pendown();
      forward(60);
      right(144);
      forward(60);
      right(144);
      forward(60);
      right(144);
      forward(60);
      right(144);
      forward(60);
      penup();
      right(72);
      forward(30);
      done();
      break;

    case '5': // 7Stars
      forward(30);
      right(77.142);
      pendown();
      forward(60);
      left(25.714);
      backward(60);
      left(25.714);
      forward(60);
      left(25.714);
      backward(60);
      left(25.714);
      forward(60);
      left(25.714);
      backward(60);
      left(25.714);
      forward(60);
      penup();
      right(77.142);
      forward(30);
      done();
      break;

    case '6': // HELLO WORLD
      fontdat_H();
      fontdat_E();
      fontdat_L();
      fontdat_L();
      fontdat_O();
      fontdat_sp(); //space
      fontdat_W();
      fontdat_O();
      fontdat_R();
      fontdat_L();
      fontdat_D();
      penup();
      done();
      break;

    case '7': // by Paradise
      fontdat_0();
      fontdat_1();
      fontdat_2();
      fontdat_3();
      fontdat_4();
      fontdat_5();
      fontdat_6();
      fontdat_7();
      fontdat_8();
      fontdat_9();
      fontdat_sp(); //space
      fontdat_dl(); // /
      fontdat_hy(); // -
      penup();
      done();
      break;

    case '8': // ABCDEFGHIJKLM
      fontdat_A();
      fontdat_B();
      fontdat_C();
      fontdat_D();
      fontdat_E();
      fontdat_F();
      fontdat_G();
      fontdat_H();
      fontdat_I();
      fontdat_J();
      fontdat_K();
      fontdat_L();
      fontdat_M();
      penup();
      done();
      break;

    case '9': // NOPQRSTUVWXYZ
      fontdat_N();
      fontdat_O();
      fontdat_P();
      fontdat_Q();
      fontdat_R();
      fontdat_S();
      fontdat_T();
      fontdat_U();
      fontdat_V();
      fontdat_W();
      fontdat_X();
      fontdat_Y();
      fontdat_Z();
      penup();
      done();
      break;

    case 'G': // abcdefghijklm
      fontdat_a();
      fontdat_b();
      fontdat_c();
      fontdat_d();
      fontdat_e();
      fontdat_f();
      fontdat_g();
      fontdat_h();
      fontdat_i();
      fontdat_j();
      fontdat_k();
      fontdat_l();
      fontdat_m();
      penup();
      done();
      break;

    case 'H': // opqrstuvwxyz
      fontdat_n();
      fontdat_o();
      fontdat_p();
      fontdat_q();
      fontdat_r();
      fontdat_s();
      fontdat_t();
      fontdat_u();
      fontdat_v();
      fontdat_w();
      fontdat_x();
      fontdat_y();
      fontdat_z();
      penup();
      done();
      break;

    case 'I': // FontSize fs=3
      fs = 3;
      break;

    case 'J': // FontSize fs=2
      fs = 2;
      break;

    case 'K': // Penup
      penup();
      break;

    case 'M': // penup
      pendown();
      break;
  }
}

// ----- HELPER FUNCTIONS -----------
int step(float distance) {
  float steps = distance * steps_rev / (wheel_dia * 3.14159);
  return steps;
}

void forward(float distance) {
  float steps = step(distance);
  float degs = (steps / steps_rev) * 360.0; // #Convert steps to degs in controller.rotate.
  digitalWrite(ENB_Pin, LOW);
  controller.rotate(-degs, degs);
  digitalWrite(ENB_Pin, HIGH);
}

void backward(float distance) {
  float steps = step(distance);
  float degs = (steps / steps_rev) * 360.0; // #Convert steps to degs in controller.rotate.
  digitalWrite(ENB_Pin, LOW);
  controller.rotate(degs, -degs);
  digitalWrite(ENB_Pin, HIGH);
}

void left(float degrees) {
  float rotation = degrees / 360.0;
  float distance = wheel_base * 3.14159 * rotation;//3.14159265359 * rotation;//3.1412
  float steps = step(distance);
  float degs = (steps / steps_rev) * 360.0; // #Convert steps to degs in controller.rotate.
  digitalWrite(ENB_Pin, LOW);
  controller.rotate(degs, degs);
  digitalWrite(ENB_Pin, HIGH);
}

void right(float degrees) {
  float rotation = degrees / 360.0;
  float distance = wheel_base * 3.14159 * rotation;//3.14159265359 * rotation;//3.1412
  float steps = step(distance);
  float degs = (steps / steps_rev) * 360.0
               ; // #Convert steps to degs in controller.rotate.
  digitalWrite(ENB_Pin, LOW);
  controller.rotate(-degs, -degs);
  digitalWrite(ENB_Pin, HIGH);
}

void done() { // unlock stepper to save battery
  digitalWrite(ENB_Pin, HIGH);
}

void penup() {
  delay(250);
  penServo.write(PEN_UP);
  delay(250);
}

void pendown() {
  delay(250);
  penServo.write(PEN_DOWN);
  delay(250);
}

// Add the font data below.