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


#include <Arduino.h>
#include <TMC2209.h>
#include <time.h>  
#include "driver/rtc_io.h"
#include <AccelStepper.h>


#define MICRO_STEPS 16
// 0.42855 from the motor, determined experimentally, 4.8 from the gear ratio
// 0.42855 / 48 = 0.08928125
#define DEGREE_PER_STEP (0.08928125)
#define STEPS_PER_DEGREE (11.20056)
#define LED_PIN LED_BUILTIN
// these two are the ones I use with the reed relais.
// GPIO_NUM_23 = D5
#define REED_PIN1 GPIO_NUM_23
// GPIO_NUM_16 = D6
#define REED_PIN2 GPIO_NUM_16
// 
// GPIO_NUM_19 = D8
#define STEP_PIN GPIO_NUM_19
// GPIO_NUM_18 = D10
#define DIR_PIN GPIO_NUM_18
// GPIO_NUM_20 = D9, connected via 10k resistor to GND
#define STDBY_PIN GPIO_NUM_20

// new version mode1_pin and mode2-pin are always high
#define MODE1_PIN GPIO_NUM_16
#define MODE2_PIN GPIO_NUM_23
#define MODE3_PIN STEP_PIN
#define MODE4_PIN DIR_PIN
#define WAKEUP_GPIO GPIO_NUM_7
#define SERIAL_BAUD_RATE 115200
#define RUN_CURRENT_PERCENT 100
#define PULSE_TIME 25

#define MAX_SPEED 1500 
#define PRECISE_SPEED 400
#define ACCELERATION 1500
// slack in the motor, add this amount of degree to compensate)
#define MOTORSLACK 9

#define WAKEUP_GPIO GPIO_NUM_7
#define TOUCH_PIN WAKEUP_GPIO
#define DOUBLE_TOUCH_TIME 500

// enable sleep mode, otherwise keep power on and check for input in the main loop
#define SLEEP
unsigned long touchtime = 0;
AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);

// stores the absolute orientation of the mercury, which drives the rest
float orient = 0;

// holds the orientation of each planet;
//int planet[] = {90,180,270,90,180,270,90,180};
int planet[] = {240,123,67,354,271,176,247,180};
String planetName[]={"Mercury","Venus","Earth","Mars","Jupiter","Saturn","Uranus","Neptune"};
float touchValue[] = {-341, 2491, -2132, 1777, -1420, 1064, -706, 346};
float offsetValue[] = { -3.1, 2.2, -1.78, 1.3, -0.8, 0.7, -0.65,1.5};
int year;
uint8_t month;
uint8_t day;







/*
void rotate(float deg) {
  // positive is counterclockwise
  //Serial.print("rotating ");
  //Serial.println(deg);
  if (deg <0) {
    digitalWrite(DIR_PIN, HIGH);
  } else {
    digitalWrite(DIR_PIN, LOW);
  }
  int i;
  for (i = (abs(deg)/DEGREE_PER_STEP )* MICRO_STEPS; i>0; i--) {
    digitalWrite(STEP_PIN, HIGH);
    delayMicroseconds(PULSE_TIME);
    digitalWrite(STEP_PIN, LOW);
    delayMicroseconds(PULSE_TIME);
  }
  //Serial.printf("rotated %f, orientation %f, (%f) \n", deg, orient, (float) fmod(orient, 360));
}
*/
void rotate(float deg) {
  static int prevdir = 0;
  if (prevdir * deg <0) {
    if (deg <0) deg -= MOTORSLACK;
    else deg += MOTORSLACK;
  }
  prevdir = deg;
  // positive is counterclockwise, but the stepper library has that as negative
  Serial.printf("rotating %4.2f\xC2\xB0\n",deg); 
  stepper.move(-deg * STEPS_PER_DEGREE * MICRO_STEPS );
  stepper.runToPosition();
}
void adjustOrient(float delta) {
  orient = fmod(orient+360+delta,360);
}
/*
// ugly hack. I thought I made a simple connection between the motor and the rod attached to mercury
// It is simple, but even the slightest off-center mounting causes big differences
// this converts the absolute orientation to the motor orientation.
// If your mount is perfect this wouldn't be needed at all.
// The numbers used here are for my specific mount. Don't just use them.
float orientToMotorOrient (float ori) {
  float ret = ori;
  return fmod(ori+360,360);
  ori = fmod(ori,360);
}

void rotateTo(int dir, float newOrient) {
  // rotates from the current orientation (stored in variable orient)
  newOrient = fmod(newOrient+360,360);
  float motorOrient = orientToMotorOrient(orient);
  float newMotorOrient = orientToMotorOrient(newOrient);
  if (dir >0 ) { 
    if (newMotorOrient <= motorOrient) {
      rotate(motorOrient-newMotorOrient);
    } else {
      rotate(360+motorOrient-newMotorOrient);
    }
  
  } else {
    if (newMotorOrient >= motorOrient) {
      rotate(motorOrient- newMotorOrient);
    } else {
      rotate(motorOrient- newMotorOrient - 360);
    }
  }
  orient = newOrient;
}
*/
void rotateTo(int dir, float newOrient) {
  // rotates from the current orientation (stored in variable orient)
  newOrient = fmod(newOrient+360,360);
  if (dir >0 ) { 
    if (newOrient <= orient) {
      rotate(orient-newOrient);
    } else {
      rotate(360+orient-newOrient);
    }
  
  } else {
    if (newOrient >= orient) {
      rotate(orient- newOrient);
    } else {
      rotate(orient- newOrient - 360);
    }
  }
  orient = newOrient;
}

void doPlanets() {
  // assumes a reset has been done with all the planets grabbed
  stepper.setMaxSpeed(MAX_SPEED * MICRO_STEPS);
  int dir = -1;
  enableMotor();
  for (int i = 0; i <8; i++) {
    Serial.printf("spinning %s into place at %0.1f\xC2\xB0\n",planetName[i],planet[i]);
    if (i !=0) {
      // mercury is already touching after the reset
      rotate(touchValue[i]);
    }
    delay(500);
    rotateTo(dir,planet[i]);
    adjustOrient(offsetValue[i]);
    dir = -dir;
  }
  disableMotor();
}

/*
void movePlanets() {
  Serial.print(" Jupiter...");
  // move jupiter
  rotate(7*360);
  Serial.printf("%d.",(int)planet[7]);
  rotateTo(1,fmod(planet[7]-8+360,360));

  rotateTo(-1,fmod(planet[7]-8+360,360));

  Serial.print(" Jupiter...");
  // move jupiter
  rotate(6*360);
  Serial.printf("%d.",(int)planet[6]);
  rotateTo(1,fmod(planet[6]-8+360,360));
 
  rotateTo(-1,fmod(planet[6]-8+360,360));


  Serial.print("Spinning planets... Saturn...");
   // first move saturn into place
  int s = planet[5];
  rotate(-4*360);
  rotateTo(-1,348.5);
  Serial.printf("%d.",(int)planet[5]);
  rotateTo(-1,fmod(s-16+360,360));
  
  rotateTo(1,fmod(s-16+360,360));
  Serial.print(" Jupiter...");
  // move jupiter
  rotate(4*360);
  Serial.printf("%d.",(int)planet[4]);
  rotateTo(1,fmod(planet[4]-8+360,360));
 
  rotateTo(-1,fmod(planet[4]-8+360,360));
  // mars
  Serial.print(" Mars...");
  rotate(-3*360);
 Serial.printf("%d.",(int)planet[3]);
  rotateTo(-1,fmod(planet[3]-16+360,360));
  
  rotateTo(1,fmod(planet[3]-16+360,360));
  // earth
  Serial.print(" Earth...");
  rotate(2*360);
  Serial.printf("%d.",(int)planet[2]);
  rotateTo(1,fmod(planet[2]-8+360,360));
  
  rotateTo(-1,fmod(planet[2]-8+360,360));
  // venus
  Serial.print(" Venus...");
  rotate(-360);
  Serial.printf("%d.",(int)planet[1]);
  rotateTo(-1,fmod(planet[1]-7+360,360));
  
  //delay(2000);
  // mercurius
  Serial.print(" Mercurius...");
  Serial.printf("%d.\n",(int)planet[0]);
  rotateTo(1, planet[0]-6);
}
*/
RTC_DATA_ATTR int ledstat = 0;
RTC_DATA_ATTR int bootCount = 0;

volatile int touch_cnt = 0;
volatile bool double_touch = false;
void touch() {    
    touch_cnt++;
    if (millis() - touchtime < DOUBLE_TOUCH_TIME) {
      double_touch = true;
      Serial.printf("double\n");
    } else {
      double_touch = false;
      Serial.printf("single %d",(int)touchtime);
    }
    touchtime = millis();
}
void print_wakeup_reason() {
  esp_sleep_wakeup_cause_t wakeup_reason;

  wakeup_reason = esp_sleep_get_wakeup_cause();

  switch (wakeup_reason) {
    case ESP_SLEEP_WAKEUP_EXT0:     Serial.println("Wakeup caused by external signal using RTC_IO"); break;
    case ESP_SLEEP_WAKEUP_EXT1:     Serial.println("Wakeup caused by external signal using RTC_CNTL"); break;
    case ESP_SLEEP_WAKEUP_TIMER:    Serial.println("Wakeup caused by timer"); break;
    case ESP_SLEEP_WAKEUP_TOUCHPAD: Serial.println("Wakeup caused by touchpad"); break;
    case ESP_SLEEP_WAKEUP_ULP:      Serial.println("Wakeup caused by ULP program"); break;
    default:                        Serial.printf("Wakeup was not caused by deep sleep: %d\n", wakeup_reason); break;
  }
  touchtime = millis();
}


volatile int zeroCnt = 0;
void zero() {
  static long timezero = 0;
  if (millis() - timezero > 2000) {
    timezero = millis();
    zeroCnt++;
    if (zeroCnt >= 9) {
      stepper.setCurrentPosition(0);
    }
  }
}

void enableMotor() {
  digitalWrite (MODE3_PIN, HIGH);
  digitalWrite (MODE4_PIN, HIGH);
  delay(30);
  digitalWrite(STDBY_PIN, HIGH);
}
void disableMotor() {
  digitalWrite(STDBY_PIN, LOW);
}
void go_home() {
  Serial.println("going home");

  zeroCnt = 0;
  pinMode(REED_PIN1, OUTPUT);
  pinMode(REED_PIN2, INPUT_PULLDOWN);
  digitalWrite(REED_PIN1, 1);

  

  // go around 9 times each time  when the reed relais is activated
  int extra = 0;
  if (digitalRead(REED_PIN2)) {
    extra = 30;
  }
  enableMotor();
  enableInterrupt(REED_PIN2);
  attachInterrupt(digitalPinToInterrupt(REED_PIN2), zero, RISING);
  rotate (-10*360- extra);
  Serial.printf("magnet %d\n", digitalRead(REED_PIN2));
  
  
  
  //stepper.move(360 * STEPS_PER_DEGREE * MICRO_STEPS );
  //stepper.runToPosition();
  disableMotor();
  yield();
  detachInterrupt(digitalPinToInterrupt(REED_PIN2));
  disableInterrupt(REED_PIN2);
  Serial.printf("magnet %d\n", digitalRead(REED_PIN2));
    delay(2000);
  Serial.printf("home done zero count %d\n",zeroCnt);
  orient = 0;
}

void setup() {
  Serial.begin(SERIAL_BAUD_RATE);
  delay(100);
  ++bootCount;
  Serial.println("Boot number: " + String(bootCount));

  //Print the wakeup reason for ESP32
  print_wakeup_reason();
  stepper.setMaxSpeed(MAX_SPEED * MICRO_STEPS);
  stepper.setAcceleration(ACCELERATION * MICRO_STEPS);
 
  pinMode(LED_PIN, OUTPUT);
  pinMode(TOUCH_PIN, INPUT);
  digitalWrite(STDBY_PIN,LOW);
  pinMode(STDBY_PIN, OUTPUT);
  digitalWrite(STDBY_PIN,LOW);
  pinMode(STEP_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  pinMode(STDBY_PIN, OUTPUT);
  // MODE1 and MODE2 are permanently wired high
  // 1/16th step
  digitalWrite (MODE3_PIN, HIGH);
  digitalWrite (MODE4_PIN, HIGH);
  delay(30);
  

  
 
  enableInterrupt(TOUCH_PIN);
  attachInterrupt(digitalPinToInterrupt(TOUCH_PIN), touch, RISING);


  delay(400);
  int time_cnt = 0;
  while (double_touch == false && time_cnt < 10000) {
    delay(50);
    time_cnt +=50;
  } 
  if (double_touch) { 
      
      go_home();
      delay(1000);
      /*for (int i=0; i <3; i++) {
        Serial.printf("\ntouched %d\n",touch_cnt);
        enableMotor();
        rotate(360);
        disableMotor();
        if (ledstat) digitalWrite(LED_PIN, HIGH);
          else digitalWrite(LED_PIN, LOW);
        ledstat = ledstat?0:1;
        delay(3000);
        enableMotor();
        rotate(-720);
        disableMotor();
        delay(3000);
        }
      */
      doPlanets();
        delay (3060);
  }
#ifdef SLEEP
  esp_sleep_enable_ext1_wakeup_io(1ULL << WAKEUP_GPIO, ESP_EXT1_WAKEUP_ANY_HIGH);
  rtc_gpio_init(WAKEUP_GPIO);
  rtc_gpio_set_direction(WAKEUP_GPIO, RTC_GPIO_MODE_INPUT_ONLY);
  
  //Go to sleep now
  Serial.println("Going to sleep now");
  delay(100);
  esp_deep_sleep_start();
  Serial.println("This will never be printed");
#endif
}


int initdone = 0;
void loop() {
  

  /*
  this code is kept here because it might be handy during calibrations
  */
  char inChar;
  int dir;
  inChar = 0;
  if (Serial.available()) {
    inChar = (char) Serial.read();
  }
  switch (inChar) {

    case 'r':
      enableMotor();
      go_home();
      disableMotor();
      break;
    case 'v':
      dir = -1;
      enableMotor();
      rotate(-30);
      disableMotor();
      break;
    case 'z':
      dir = 1;
      enableMotor();
      rotate(30);
      disableMotor();
      break;
    case 'a':
      enableMotor();
      rotate(-270);
      disableMotor();
      break;
    case 'o':
      enableMotor();
      rotate(270);
      disableMotor();
      break;
    case 'f':
      enableMotor();
      rotate(-360);
      disableMotor();
      break;
    case 'b':
      enableMotor();
      rotate(360);
      disableMotor();
      break;
    case '1':
    case '2':
    case '3':
    case '4':
    case '5':
      enableMotor();
      rotate(-(inChar - '0'));
      disableMotor();
      break;
     case '!': enableMotor();rotate(1); disableMotor(); break;
    case '@': enableMotor();rotate(2); disableMotor(); break;
    case '#': enableMotor();rotate(3); disableMotor(); break;
    case '$': enableMotor();rotate(4); disableMotor(); break;
    case '%': enableMotor();rotate(5); disableMotor(); break;
     
  }  
  delay(500);
}