/*
  Wokwi: https://wokwi.com/projects/416080727447694337
  Forum: https://forum.arduino.cc/t/probleme-mit-meinem-mikrofon-roboter-esp32/1327686/3

  Auf die Auswertung der Eingabestrings und die Stepperansteuerung abgespeckte Version

  Eingabemöglichkeit Serial:
  "calibrate"
    Setzt sliderValue1 = "50" und sliderValue2 = "100"
  "getValues"
    Gibt die Inhalte der beiden o.a. Strings über die Serielle Schnittstelle aus
  "1sX" mit X = 0 bis 100
     Setzt  sliderValue1 auf den Wert X
  "2sX" mit X = 0 bis 100
     Setzt  sliderValue2 auf den Wert X
*/
// Import required libraries
#include <Arduino.h>

// Breadboard position from left to right: ESP32, TMC2208 (MOTORPOS) for
// speaker position, TMC2208 (MOTORDIST) for speaker distance.
// POS = Microphone horizontal position to front of speaker
// DIST = Microphone distance from front of speaker
// EN = Motor driver enable/disable
// DIR = Motor spin direction clockwise/anticlockwise
// STEP = Step motor step pulse
#define MOTORPOSEN 26 // HIGH turns motor off, LOW turns motor on
#define MOTORDISTEN 32 // HIGH turns motor off, LOW turns motor on
#define MOTORPOSDIR 14
#define MOTORPOSSTEP 27
#define MOTORDISTDIR 25
#define MOTORDISTSTEP 33

// See README.md
#define STEPSPERUNITMOVEMENT 58
#define MOTORSPEED 700






/*
  Physical parts setup
*/
//**************************************************
// Motor setup. You will need to tweak these values according to the size
// or your microphone stand. The below settings apply to TMC2208 motor drivers.

// CAUTION: The following 2 variables were used for testing when the hardware
// did not exist.
// The new value at stepsForDividedTurn is used with the actual hardware.

// Change this to fit the number of motor steps per revolution
// 2048 for ULN2003 motors but not sure about microsteps for TMC2208 -
// TMC2208 supposedly 200 with default settings.
//const int stepsPerRevolution = 200;

// Set the division according to how much the motor should rotate per one
// slider step
//const int stepsForDividedTurn = stepsPerRevolution / 5;

// TWEAK THIS FOR THE LENGTH OF THE ALUMINIUM!
// This value represents the amount of steps the motor should turn for
// a single increase or decrease in value on the web GUI slider
// In this case 5800 steps would move from position 1 to 100. This must be
// adjusted according to the length of your aluminium extrusion.
// This constant also assumes that both position and distance extrusions are
// the same length.
const int stepsForOneSliderMove = STEPSPERUNITMOVEMENT;

// Time spent waiting between step pulses. Lower values rotate the motors
// faster. This constant controls the speed of both motors simultaneously.
const int motorStepDelay = MOTORSPEED;
//**************************************************

// x_pos = Horizontal position to speaker
// y_pos = Distance from speaker
int stand_x_pos_control = 50; // Value retrieved from web server on pos change
int stand_y_pos_control = 100;
int stand_x_pos = 50; // Real microphone position
int stand_y_pos = 100;

// Difference ints to prevent the creation of a new var each cycle of
// controlSteppers()
int x_diff = 0;
int y_diff = 0;

/*
  WebServer
*/

String sliderValue1 = "50"; // ESP32 tracking of web server slider position
String sliderValue2 = "100";

void checkSerialInput() {
  static String message = "";
  if (Serial.available()) {
    char c = Serial.read();
    if (c < ' ') {
      simulateWebSocketMessage(message);
      message = "";
    } else {
      message += c;
    }
  }
}


void notifyClients(String text) {
  Serial.println(text);
}

String getSliderAndLiveValues() {
  return sliderValue1 + ", " + sliderValue2;
}

void simulateWebSocketMessage(String message) {
  if (message.indexOf("1s") >= 0) {
    sliderValue1 = message.substring(2);
    stand_x_pos_control = sliderValue1.toInt();
    notifyClients(getSliderAndLiveValues());
  }
  if (message.indexOf("2s") >= 0) {
    sliderValue2 = message.substring(2);
    stand_y_pos_control = sliderValue2.toInt();
    notifyClients(getSliderAndLiveValues());
  }
  if (message.indexOf("getValues") >= 0) {
    notifyClients(getSliderAndLiveValues());
  }
  if (message.indexOf("calibrate") >= 0) {
    stand_x_pos_control = 50;
    stand_y_pos_control = 100;
    stand_x_pos = 50;
    stand_y_pos = 100;
    sliderValue1 = "50";
    sliderValue2 = "100";
    notifyClients(getSliderAndLiveValues());
  }
}




/*
  setup()
*/
void setup() {
  // Serial port for debugging purposes
  Serial.begin(115200);
  Serial.println("Start");

  pinMode(2, OUTPUT); // set the LED pin mode
  // Set pin modes for motor control pins
  pinMode(MOTORPOSEN, OUTPUT);
  pinMode(MOTORDISTEN, OUTPUT);
  pinMode(MOTORPOSDIR, OUTPUT);
  pinMode(MOTORPOSSTEP, OUTPUT);
  pinMode(MOTORDISTDIR, OUTPUT);
  pinMode(MOTORDISTSTEP, OUTPUT);
  // Motor Setup
  // Disable motors so that current is not drawn when not in use
  digitalWrite(MOTORPOSEN, HIGH);
  digitalWrite(MOTORDISTEN, HIGH);
  Serial.println("Enter loop");
}

/*
  loop() functions
*/
// Motor direction (false is clockwise, true is anticlockwise) and number of steps
void spinMotor1(const bool &dir, const int &amt) {
  digitalWrite(MOTORPOSEN, LOW); // Enable motor
  delayMicroseconds(motorStepDelay);

  if (dir == false) { // Set direction
    digitalWrite(MOTORPOSDIR, HIGH);
  } else if (dir == true) {
    digitalWrite(MOTORPOSDIR, LOW);
  }
  delayMicroseconds(motorStepDelay);

  for (int i = 0; i < amt; i++) { // Turn motor
    digitalWrite(MOTORPOSSTEP, HIGH);
    delayMicroseconds(motorStepDelay);
    digitalWrite(MOTORPOSSTEP, LOW);
    delayMicroseconds(motorStepDelay);
  }
  digitalWrite(MOTORPOSEN, HIGH); // Disable motor when work is done
}


// Motor direction (true is clockwise, false is anticlockwise) and number of steps
void spinMotor2(const bool &dir, const int &amt) {
  digitalWrite(MOTORDISTEN, LOW); // Enable motor
  delayMicroseconds(motorStepDelay);

  if (dir == false) { // Set direction
    digitalWrite(MOTORDISTDIR, HIGH);
  } else if (dir == true) {
    digitalWrite(MOTORDISTDIR, LOW);
  }
  delayMicroseconds(motorStepDelay);

  for (int i = 0; i < amt; i++) { // Turn motor
    digitalWrite(MOTORDISTSTEP, HIGH);
    delayMicroseconds(motorStepDelay);
    digitalWrite(MOTORDISTSTEP, LOW);
    delayMicroseconds(motorStepDelay);
  }
  digitalWrite(MOTORDISTEN, HIGH); // Disable motor when work is done
}


void controlSteppers() {
  // If the real position is not the same as the GUI control
  if (stand_x_pos != stand_x_pos_control) {

    if (stand_x_pos > stand_x_pos_control) {
      // Calc the difference between the real position and control
      x_diff = stand_x_pos - stand_x_pos_control;

      // Turn the motor to bring the real pos to the GUI control
      spinMotor1(false, (x_diff * stepsForOneSliderMove));

      // Update the real position
      stand_x_pos = stand_x_pos - x_diff;
    }

    if (stand_x_pos < stand_x_pos_control) {
      x_diff = stand_x_pos_control - stand_x_pos;
      spinMotor1(true, (x_diff * stepsForOneSliderMove));
      stand_x_pos = stand_x_pos + x_diff;
    }
  }

  if (stand_y_pos != stand_y_pos_control) {

    if (stand_y_pos > stand_y_pos_control) {
      y_diff = stand_y_pos - stand_y_pos_control;
      spinMotor2(true, (y_diff * stepsForOneSliderMove));
      stand_y_pos = stand_y_pos - y_diff;
    }

    if (stand_y_pos < stand_y_pos_control) {
      y_diff = stand_y_pos_control - stand_y_pos;
      spinMotor2(false, (y_diff * stepsForOneSliderMove));
      stand_y_pos = stand_y_pos + y_diff;
    }
  }
}

/*
  loop()
*/

void loop() {
  checkSerialInput();
  //Physical stepper motor response to value changes from sliders and buttons
  controlSteppers();
}