#include "BasicStepperDriver.h"
#include "MultiDriver.h"
#include "SyncDriver.h"
#include "ESP32Seavo.h"

#define MOTOR_STEPS 200 //1回転200スッテプモータを使用
#define MICROSTEPS 16 //モータードライバーを1/16マイクロステップに設定
#define L_Motor_RPM 7 //speed:Wheel rotations per minute.
#define R_Motor_RPM 7 //speed:Wheel rotations per minute.

#define ENB_Pin 27 //ESP32 Chip 27pin
#define L_Motor_DIR 26 //ESP32 Chip 26pin
#define L_Motor_STEP 33 //ESP32 Chip 33pin
#define R_Motor_DIR 32 //ESP32 Chip 32pin
#define R_Motor_STEP 25 //ESP32 Chip 25pin

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);
Servo penServo;
int servoPin = 23; //ESP32 Chip 23pin
int PEN_DOWN = 60; // f-sizengle of servo when pen is down
int PEN_UP = 20;   // f-sizengle of servo when pen is up
float wheel_dia = 70.0;  // # mm (increase = spiral out)
float wheel_base = 90.0; // # mm (increase = spiral in, ccw)
int steps_rev = 3200;    // # 200*16 MICROSTEPS

void setup() {
  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() {
  for (int x = 0; x < 12; x++) { //100mm Square ccw Calibration
    forward(100);
    left(90);
  }
  while(1); 
}

// ----- HELPER FUNCTIONS -----------
int step(float distance) {
  int steps = distance * steps_rev / (wheel_dia * 3.14159);
  //Example. #1455 = 100 * 3200 / (70.0 * 3.14159) #Here,100mm movement.
  Serial.print(distance);
  Serial.print(" ");
  Serial.print(steps_rev);
  Serial.print(" ");
  Serial.print(wheel_dia);
  Serial.print(" ");
  Serial.println(steps);
  delay(1000);
  return steps;
}

void forward(float distance) {
  float steps = step(distance);
  float degs = (steps / steps_rev) * 360.0; // #Convert steps to degs in controller.rotate.
  //Example. #163.69(degs) = (1455 / 3200)*360.0 #Here, 100mm movement.
  Serial.print("degs = ");
  Serial.println(degs);
  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;
  Serial.print("rotation = ");
  Serial.println(rotation);
  //Example. #0.25 = 90 /360.0 #Here, 0.25(rotation) = 90 / 360.0 #When rotated 90 degrees.
  float distance = wheel_base * 3.14159 * rotation;
  Serial.print("distance = ");
  Serial.println(distance);
  //Example.#Here, 70.69(distance) = 90.0(wheel_base) * 3.14159 * 0.25(rotation)
  //#1028(steps) = 70.69(distance) * 3200 / (70(wheel_dia) * 3.14159)
  float steps = step(distance);
  Serial.print("steps = ");
  Serial.println(steps);
  float degs = (steps / steps_rev) * 360.0; // #Convert steps to degs in controller.rotate.
  //Example. #115.65(degs) = (1028(steps) / 3200) * 360.0
  Serial.print("degs = ");
  Serial.println(degs);
  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;
  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);
}