// this code is written by illusionmanager in the year 2023

#include <Arduino.h>
#include <TMC2209.h>
#include <time.h> 
#include <math.h>


#define DEBUG_ESP_PORT Serial

// set by the way the controller is hardwired
#define MICRO_STEPS 64
#define GEAR_RATIO 64.0
#define EARTH_GEAR_RATIO 3
//#define STEPS_PER_DEGREE (567424.0 / 8910.0 * 64.0 / 360.0 /2.0)
#define STEPS_PER_DEGREE (64 * 64.0 / 360.0 /2.0)
#define DIR_PIN 4
#define STEP_PIN 5
#define LED_PIN 8
#define ANALOG_PIN 0
#define ENABLE_PIN 1
#define SWITCH1_PIN 2
#define SWITCH2_PIN 3
#define SECONDS2025 2025
// minimum delay between microsteps so it still works
#define STEP_DELAY 18
#define SERIAL_BAUD_RATE 115200

// UART connection to controller Should be Serial0, but that seems to be gone.
HardwareSerial & serial_stream = Serial0;
// Instantiate TMC2209
TMC2209 stepper_driver;



long long steps_done = 0;
time_t start_time;

void setup() {
  digitalWrite(ENABLE_PIN, HIGH);
  pinMode(ENABLE_PIN, OUTPUT);
  digitalWrite(ENABLE_PIN, HIGH);
  Serial.begin(SERIAL_BAUD_RATE);
  delay(1000);
  pinMode(LED_PIN, OUTPUT);
  pinMode(STEP_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  pinMode(SWITCH1_PIN,INPUT);
  pinMode(SWITCH2_PIN,INPUT);
  pinMode(ENABLE_PIN, OUTPUT);
  digitalWrite(ENABLE_PIN, HIGH);
  stepper_driver.enable();
  stepper_driver.moveUsingStepDirInterface();
  start_time = time(NULL);
  digitalWrite(ENABLE_PIN, HIGH);
}

time_t now;
void loop() {
  now = time(NULL);
  int switch1 = digitalRead(SWITCH1_PIN);
  int switch2 = digitalRead(SWITCH2_PIN);
   // wait a while in case user switches from clockwise
  // to counter clockwise rotation and the switch is just 
  // briefly in the center
  int cnt = 0;
  while (switch2 == 0 && cnt <5) {   
    delay(100);
    switch2 = digitalRead(SWITCH2_PIN);
    cnt++;
  }
  cnt = 0;
  // do it again to ensure solid contact in the switch
  while (switch2 == 0 && cnt <5) {
    delay(100);
    switch2 = digitalRead(SWITCH2_PIN);
    cnt++;
  }
  
  while (switch2 == 0 ) {
    // normal mode
    // steps needed, every 2025 seconds we need to do exactly 32 steps
    int steps_needed = ( now - start_time ) * 32.0 / SECONDS2025;
    if (steps_needed != steps_done) {
      digitalWrite(ENABLE_PIN, LOW);
      Serial.printf("steps needed %d, steps done %d\n",(int)steps_needed, (int)steps_done);
    }
    
    while (steps_needed != steps_done && switch2 == 0 ) {    
      if (steps_needed < steps_done) {
        digitalWrite(DIR_PIN,LOW);
        steps_done--;
      } else {
        digitalWrite(DIR_PIN,HIGH);
        steps_done++;
      }
      digitalWrite(LED_PIN,LOW);
      for (int j = 0; j < MICRO_STEPS; j++) {
        digitalWrite(STEP_PIN, HIGH);
        delayMicroseconds(STEP_DELAY + 20);
        digitalWrite(STEP_PIN, LOW);
        delayMicroseconds(STEP_DELAY + 20);
      }
      
      now = time(NULL);
      steps_needed = ( now - start_time ) * 32.0 / SECONDS2025;
      switch2 = digitalRead(SWITCH2_PIN);     
    }
    digitalWrite(ENABLE_PIN, HIGH);
    delay(200);
    digitalWrite(LED_PIN,HIGH);
    now = time(NULL);
    steps_needed = ( now - start_time ) * 32.0 / SECONDS2025;
    switch2 = digitalRead(SWITCH2_PIN);
  }
  // play mode direction is set by switch1
    digitalWrite(ENABLE_PIN, LOW);
    int speed;
    while (switch2 == 1) {
      switch1 = digitalRead(SWITCH1_PIN);
      switch2 = digitalRead(SWITCH2_PIN);
      digitalWrite(DIR_PIN,switch1);
      speed = ( analogRead(ANALOG_PIN) -50 );
      if (speed <1) speed = 1;
      speed = exp(speed / 600.0) + STEP_DELAY;
      digitalWrite(LED_PIN,LOW);
      for (int j = 0; j < MICRO_STEPS; j++) {
        digitalWrite(STEP_PIN, HIGH);
        delayMicroseconds(speed);
        digitalWrite(STEP_PIN, LOW);
        delayMicroseconds(speed);
      }
      digitalWrite(LED_PIN,HIGH);
      if (switch1 == 1) {
        steps_done++;
      } else {
        steps_done--;
      }
    }
    digitalWrite(ENABLE_PIN, HIGH);  
}