/*
 * Belt Clock – ESP32-S (38 pin) + ULN2003 + TCRT5000
By Misfit Maker
Please subscribe, if you like my project.
 */

#include <WiFi.h>
#include <time.h>
#include "credentials.h"   // contains my_ssid1 and my_password1

// -------- WiFi -----------
const char* ssid     = my_ssid1;
const char* password = my_password1;

// -------- Time ----------
#define MY_TZ "IST-5:30"
unsigned long prevMillis = 0;
const long interval = 1000;
int old_hour = -1, old_minute = -1;

// -------- Step Sequence (Half-step 8 step) ----------
const int step_seq[8][4] = {
  {1, 0, 0, 0},
  {1, 1, 0, 0},
  {0, 1, 0, 0},
  {0, 1, 1, 0},
  {0, 0, 1, 0},
  {0, 0, 1, 1},
  {0, 0, 0, 1},
  {1, 0, 0, 1}
};

// -------- Motor Pins (NO MUX) ----------
int motor_pins[3][4] = {
  {16, 17, 18, 19},   // Hour stepper
  {21, 22, 23, 25},   // Tens stepper
  {26, 27, 32, 33}    // Ones stepper
};

// -------- TCRT5000 sensor pins ----------
#define TCRT_HOUR 13
#define TCRT_TENS 4
#define TCRT_ONES 14

// -------- Mechanical Setup ----------
const float STEPS_PER_REV = 2048.0;
const float MOTOR_TO_PULLEY_RATIO = 8;
const float PULLEY_DIAMETER_MM = 16.8;
const float BELT_LENGTH_MM = 350.0;

const float STEPS_PER_MM_F =
  (STEPS_PER_REV / MOTOR_TO_PULLEY_RATIO) / (PI * PULLEY_DIAMETER_MM);

const int STEPS_PER_MM = (int)(STEPS_PER_MM_F + 0.5);

const float HOUR_SPACING = ( BELT_LENGTH_MM / 12.0);
const float TENS_SPACING = ( BELT_LENGTH_MM / 6.0);
const float ONES_SPACING = ( BELT_LENGTH_MM / 10.0);

const int STEPS_PER_HOUR_POS = (int)(STEPS_PER_MM_F * HOUR_SPACING + 0.5);
const int STEPS_PER_TENS_POS = (int)(STEPS_PER_MM_F * TENS_SPACING + 0.5);
const int STEPS_PER_ONES_POS = (int)(STEPS_PER_MM_F * ONES_SPACING + 0.5);

// -------- Alignment offsets ----------Change these values if needed to get the belt aligned correctly with the front plate
#define HOME_OFFSET_HOUR 0              
#define HOME_OFFSET_TENS 0
#define HOME_OFFSET_ONES 0

int pos_now[3] = {0, 0, 0};
int pos_new[3] = {0, 0, 0};

wl_status_t lastWiFiStatus = WL_NO_SHIELD;

// allow shortest-path only once after homing (disabled to avoid large immediate moves)
bool use_shortest_after_homing = false;

// -------- INDIVIDUAL AUTO-HOMING TIMERS ----------
// Minute Ones → 10 minutes
unsigned long lastHomeOnes = 0;
const unsigned long INTERVAL_ONES = 10UL * 60UL * 1000UL;
// Minute Tens → 60 minutes
unsigned long lastHomeTens = 0;
const unsigned long INTERVAL_TENS = 60UL * 60UL * 1000UL;
// Hour Belt → 720 minutes (12 hours)
unsigned long lastHomeHour = 0;
const unsigned long INTERVAL_HOUR = 720UL * 60UL * 1000UL;

// -------- Movement helpers ----------
volatile bool movement_in_progress = false; // prevents overlapping moves

void write_step(int motor, int seq) {
  for (int i = 0; i < 4; i++)
    digitalWrite(motor_pins[motor][i], step_seq[seq][i]);
}

void deenergize(int motor) {
  for (int i = 0; i < 4; i++)
    digitalWrite(motor_pins[motor][i], LOW);
}

// step_motor with optional us_delay parameter (microseconds). Default 1000 us.
void step_motor(int motor, int steps, int us_delay = 1000) {
  if (steps == 0) return;

  movement_in_progress = true;
  int dir = (steps > 0) ? 1 : -1;
  int total = abs(steps);

  for (int s = 0; s < total; s++) {
    if (dir > 0) {
      for (int seq = 0; seq < 8; seq++) {
        write_step(motor, seq);
        delayMicroseconds(us_delay); // tuning: larger -> slower
      }
    } else {
      for (int seq = 7; seq >= 0; seq--) {
        write_step(motor, seq);
        delayMicroseconds(us_delay);
      }
    }
  }

  deenergize(motor);
  movement_in_progress = false;
}

// chunked stepping to avoid a single long blocking call
void step_motor_chunked(int motor, int steps, int chunkSize = 200, int us_delay = 1000) {
  if (steps == 0) return;
  int remaining = steps;
  while (remaining != 0) {
    int take;
    if (abs(remaining) > chunkSize) take = (remaining > 0) ? chunkSize : -chunkSize;
    else take = remaining;

    step_motor(motor, take, us_delay);

    remaining -= take;
    // small breathing room so other tasks (WiFi, time) can proceed
    delay(2);
  }
}

// -------- Homing ----------
// dir: +1 to step forward, -1 to step backward while searching
// maxSteps: safe limit to avoid endless loops
void homing_one(int motor, int sensorPin, int offset, int dir = 1, int maxSteps = 6000) {
  // Read initial stable state (with small settling)
  int initial = digitalRead(sensorPin);

  int counter = 0;

  // Step until sensor state toggles or timeout
  while (counter < maxSteps) {
    // step one half-step in the chosen direction; step_motor handles sign
    step_motor(motor, dir * 1, 1000); // faster homing single half-step
    counter++;

    int nowState = digitalRead(sensorPin);
    if (nowState != initial) {
      // debounce / settle: wait a few ms and re-read
      delay(10);
      int confirm = digitalRead(sensorPin);
      if (confirm != initial) {
        // optionally back off a few steps so the marker sits centered on sensor
        if (offset != 0) {
          step_motor(motor, offset, 1000);
        }
        return;
      } else {
        // transient; continue
      }
    }
    // tiny delay to avoid hogging
    delay(1);
  }

  // still attempt to apply offset so position is consistent
  if (offset != 0) {
    step_motor(motor, offset, 1000);
  }
}

void Homing() {
  // NOTE: you may need to invert homing direction per motor depending on
  // your sensor orientation. Try +1 or -1 if the motor never finds the sensor.
  homing_one(0, TCRT_HOUR, HOME_OFFSET_HOUR, +1);
  pos_now[0] = 0;

  homing_one(1, TCRT_TENS, HOME_OFFSET_TENS, +1);
  pos_now[1] = 0;

  homing_one(2, TCRT_ONES, HOME_OFFSET_ONES, +1);
  pos_now[2] = 0;

  // keep shortest-path disabled to avoid large immediate reverse moves
  use_shortest_after_homing = false;

  unsigned long now = millis();
  lastHomeHour = now;
  lastHomeTens = now;
  lastHomeOnes = now;
}

// -------- Path helpers ----------
int shortest_position_delta_signed(int current, int target, int wrap) {
  int fwd = (target - current + wrap) % wrap;
  int back = fwd - wrap;
  return (abs(fwd) <= abs(back)) ? fwd : back;
}

int forward_position_delta(int current, int target, int wrap) {
  return (target - current + wrap) % wrap;
}

// -------- Belt update ----------
// CHUNK_THRESHOLD controls when to use chunked moves
const int CHUNK_THRESHOLD = 300; // steps (half-steps) threshold to start chunking
const int CHUNK_SIZE = 200;      // half-steps per chunk
const int DEFAULT_US_DELAY = 1000;

void update_belts(int hour24, int minute) {
  int hour12 = hour24 % 12;
  if (hour12 == 0) hour12 = 12;

  pos_new[0] = hour12 - 1;
  pos_new[1] = minute / 10;
  pos_new[2] = minute % 10;

  bool useShortest = use_shortest_after_homing; // currently false by default

  // Hour
  if (pos_new[0] != pos_now[0]) {
    int d = useShortest ?
      shortest_position_delta_signed(pos_now[0], pos_new[0], 12) :
      forward_position_delta(pos_now[0], pos_new[0], 12);

    int steps = d * STEPS_PER_HOUR_POS;
    if (abs(steps) > CHUNK_THRESHOLD) {
      step_motor_chunked(0, steps, CHUNK_SIZE, DEFAULT_US_DELAY);
    } else {
      step_motor(0, steps, DEFAULT_US_DELAY);
    }
    pos_now[0] = (pos_now[0] + d + 12) % 12;
  }

  // Tens
  if (pos_new[1] != pos_now[1]) {
    int d = useShortest ?
      shortest_position_delta_signed(pos_now[1], pos_new[1], 6) :
      forward_position_delta(pos_now[1], pos_new[1], 6);

    int steps = d * STEPS_PER_TENS_POS;
    if (abs(steps) > CHUNK_THRESHOLD) {
      step_motor_chunked(1, steps, CHUNK_SIZE, DEFAULT_US_DELAY);
    } else {
      step_motor(1, steps, DEFAULT_US_DELAY);
    }
    pos_now[1] = (pos_now[1] + d + 6) % 6;
  }

  // Ones
  if (pos_new[2] != pos_now[2]) {
    int d = useShortest ?
      shortest_position_delta_signed(pos_now[2], pos_new[2], 10) :
      forward_position_delta(pos_now[2], pos_new[2], 10);

    int steps = d * STEPS_PER_ONES_POS;
    if (abs(steps) > CHUNK_THRESHOLD) {
      step_motor_chunked(2, steps, CHUNK_SIZE, DEFAULT_US_DELAY);
    } else {
      step_motor(2, steps, DEFAULT_US_DELAY);
    }
    pos_now[2] = (pos_now[2] + d + 10) % 10;
  }

  if (use_shortest_after_homing) {
    use_shortest_after_homing = false;
  }
}

// -------- WiFi ----------
void setup_wifi() {
  WiFi.mode(WIFI_STA);
  WiFi.setAutoReconnect(true);
  WiFi.persistent(true);

  WiFi.begin(ssid, password);
  unsigned long start = millis();

  while (WiFi.status() != WL_CONNECTED && millis() - start < 20000) {
    delay(300);
    // silent retry
  }

  // silent; status tracked in loop
}

// -------- NTP ----------
void init_time_server(const char* tz) {
  configTzTime(tz, "pool.ntp.org", "time.nist.gov");
  delay(1000);
}

bool get_time_now(int &h, int &m, int &s) {
  struct tm t;
  if (!getLocalTime(&t, 2000)) {
    return false;
  }
  h = t.tm_hour;
  m = t.tm_min;
  s = t.tm_sec;
  return true;
}

// -------- Setup ----------
void setup() {

  for (int m = 0; m < 3; m++)
    for (int i = 0; i < 4; i++) {
      pinMode(motor_pins[m][i], OUTPUT);
      digitalWrite(motor_pins[m][i], LOW);
    }

  pinMode(TCRT_HOUR, INPUT_PULLUP);
  pinMode(TCRT_TENS, INPUT_PULLUP);
  pinMode(TCRT_ONES, INPUT_PULLUP);

  setup_wifi();
  init_time_server(MY_TZ);

  lastWiFiStatus = WiFi.status();

  Homing();
  unsigned long now = millis();
  lastHomeHour = now;
  lastHomeTens = now;
  lastHomeOnes = now;

  int hh, mm, ss;
  if (get_time_now(hh, mm, ss)) {
    // time available
  } else {
    // time not available yet
  }
}

// -------- Loop ----------
void loop() {
  unsigned long now = millis();

  // --- WiFi Monitoring every 3s ---
  static unsigned long lastWiFiCheck = 0;
  if (now - lastWiFiCheck > 3000) {
    lastWiFiCheck = now;

    wl_status_t st = WiFi.status();

    if (st == WL_CONNECTED && lastWiFiStatus != WL_CONNECTED) {
      // WiFi reconnected — re-init time server and homing
      init_time_server(MY_TZ);
      Homing();
      // After Homing(), we can immediately update belts if time is available
      int hh, mm, ss;
      if (get_time_now(hh, mm, ss)) {
        update_belts(hh, mm);
      }
    }

    if (st != WL_CONNECTED) {
      WiFi.reconnect();
    }

    lastWiFiStatus = st;
  }

  // --- Time update every 1 sec ---
  if (now - prevMillis >= interval) {
    prevMillis = now;

    int h, m, s;
    if (get_time_now(h, m, s)) {
      if (h != old_hour || m != old_minute) {
        update_belts(h, m);
        old_hour = h;
        old_minute = m;
      }
    }
  }

  // === Individual Auto-Homing ===
  // Hour belt (motor 0) — every 720 min
  if (now - lastHomeHour >= INTERVAL_HOUR && !movement_in_progress) {
    homing_one(0, TCRT_HOUR, HOME_OFFSET_HOUR, +1);
    pos_now[0] = 0;
    // do not enable shortest-path; reposition using forward-only but chunked
    lastHomeHour = now;
    // immediate reposition to current time (chunked inside update_belts)
    int hh, mm, ss;
    if (get_time_now(hh, mm, ss)) update_belts(hh, mm);
  }

  // Tens belt (motor 1) — every 60 min
  if (now - lastHomeTens >= INTERVAL_TENS && !movement_in_progress) {
    homing_one(1, TCRT_TENS, HOME_OFFSET_TENS, +1);
    pos_now[1] = 0;
    lastHomeTens = now;
    int hh, mm, ss;
    if (get_time_now(hh, mm, ss)) update_belts(hh, mm);
  }

  // Ones belt (motor 2) — every 10 min
  if (now - lastHomeOnes >= INTERVAL_ONES && !movement_in_progress) {
    homing_one(2, TCRT_ONES, HOME_OFFSET_ONES, +1);
    pos_now[2] = 0;
    lastHomeOnes = now;
    int hh, mm, ss;
    if (get_time_now(hh, mm, ss)) update_belts(hh, mm);
  }

  delay(1);
}