/*
 * STM32 Nucleo-F303RE Cargo Lift Control System v6.0 - LIGHTWEIGHT PRODUCTION
 * CONVERTED FROM: ESP32-WROOM version
 * OPTIMIZED: Reduced complexity, hardened for years of continuous operation
 * SAFETY: Sensor conflict detection (3+ sensors = stop), relay interlock
 * DEEPHY Tec - Mazenkovic
 */

#include <Arduino.h>
#include <IWatchdog.h>  // STM32 watchdog library

// ============================================
// CONFIGURATION - Tuned for reliability
// ============================================
namespace Config {
  // Timings (ms) - Conservative for industrial reliability
  constexpr uint32_t BTN_HOLD_TIME = 3000;
  constexpr uint32_t DEBOUNCE_MS = 50;
  constexpr uint32_t INIT_DELAY = 2000;
  constexpr uint32_t RELAY_DELAY = 100;
  constexpr uint32_t MAX_MOVE_TIME = 70000;
  constexpr uint32_t WDT_TIMEOUT = 80000;  // 70s - reset if hung
  constexpr uint32_t LOOP_PERIOD = 10;    // 10ms cycle - robust timing
  
  // Fault tolerance
  constexpr uint8_t MAX_FAULTS = 5;       // Compact fault history
}

// ============================================
// PIN DEFINITIONS - STM32 Nucleo-F303RE Mapping
// ============================================
// Nucleo-F303RE pinout: PA0-PA15, PB0-PB15, PC0-PC15
// Using pins that don't conflict with onboard LEDs/ST-Link
namespace Pins {
  constexpr int BTN_UP = PA0;        // A0 - User button available
  constexpr int BTN_DOWN = PA1;      // A1
  constexpr int SENSOR_HIGH_1 = PA4; // A2
  constexpr int SENSOR_HIGH_2 = PA5; // A3
  constexpr int SENSOR_LOW_1 = PA6;  // A4
  constexpr int SENSOR_LOW_2 = PA7;  // A5
  constexpr int RELAY_UP = PB0;      // D0 - PWM capable
  constexpr int RELAY_DOWN = PB1;    // D1 - PWM capable
}

// ============================================
// SIMPLIFIED STATE MANAGEMENT
// ============================================
enum class State : uint8_t {
  INIT,
  IDLE,
  MOVE_UP,
  MOVE_DOWN,
  FAULT
};

enum class Fault : uint8_t {
  NONE,
  SENSOR_ERR,
  TIMEOUT,
  BTN_CONFLICT,
  MULTI_SENSOR
};

// ============================================
// COMPACT SYSTEM STATE
// ============================================
struct System {
  // Current state
  volatile State state = State::INIT;
  volatile Fault fault = Fault::NONE;
  
  // Timing (uint32_t handles 49.7 days overflow safely)
  uint32_t initTime = 0;
  uint32_t moveStart = 0;
  uint32_t relayTimer = 0;
  uint32_t btnTimer = 0;
  uint32_t lastLoop = 0;
  
  // Debounced inputs
  bool btnUp = false;
  bool btnDown = false;
  bool lastBtnUp = false;
  bool lastBtnDown = false;
  uint32_t debounceUp = 0;
  uint32_t debounceDown = 0;
  
  // Sensor state
  bool high1, high2, low1, low2;
  bool atHigh, atLow;
  uint8_t sensorCount;
  
  // Operational flags
  bool btnHeld = false;
  bool relayWait = false;
  bool initDone = false;
  
  // Fault history (circular buffer)
  uint8_t faultLog[Config::MAX_FAULTS] = {0};
  uint8_t faultIdx = 0;
};

static System sys;

// ============================================
// UTILITY - Overflow-safe time comparison
// ============================================
inline bool timePassed(uint32_t start, uint32_t duration) {
  return (millis() - start) >= duration;  // Handles overflow automatically
}

// ============================================
// HARDWARE CONTROL - Direct, minimal overhead
// ============================================
inline void relayUp(bool on) {
  digitalWrite(Pins::RELAY_UP, on ? LOW : HIGH);
}

inline void relayDown(bool on) {
  digitalWrite(Pins::RELAY_DOWN, on ? LOW : HIGH);
}

inline void relaysOff() {
  digitalWrite(Pins::RELAY_UP, HIGH);
  digitalWrite(Pins::RELAY_DOWN, HIGH);
}

inline bool isRelayUp() {
  return digitalRead(Pins::RELAY_UP) == LOW;
}

inline bool isRelayDown() {
  return digitalRead(Pins::RELAY_DOWN) == LOW;
}

// ============================================
// SETUP - Minimal, deterministic
// ============================================
void setup() {
  // Configure pins - explicit and immediate
  pinMode(Pins::RELAY_UP, OUTPUT);
  pinMode(Pins::RELAY_DOWN, OUTPUT);
  relaysOff();  // Safe state immediately
  
  // STM32: Use INPUT_PULLUP for buttons (active LOW) or INPUT_PULLDOWN
  // Assuming buttons pull to VCC when pressed (use INPUT_PULLDOWN)
  // If buttons pull to GND when pressed, use INPUT_PULLUP and invert logic
  pinMode(Pins::BTN_UP, INPUT_PULLDOWN);
  pinMode(Pins::BTN_DOWN, INPUT_PULLDOWN);
  pinMode(Pins::SENSOR_HIGH_1, INPUT_PULLDOWN);
  pinMode(Pins::SENSOR_HIGH_2, INPUT_PULLDOWN);
  pinMode(Pins::SENSOR_LOW_1, INPUT_PULLDOWN);
  pinMode(Pins::SENSOR_LOW_2, INPUT_PULLDOWN);
  
  // Verify safe start
  if (isRelayUp() || isRelayDown()) {
    sys.fault = Fault::SENSOR_ERR;  // Welded relay detected
    sys.state = State::FAULT;
  }
  
  // STM32 Independent Watchdog (IWDG) configuration
  // Timeout = WDT_TIMEOUT milliseconds (80000ms = 80s)
  // IWDG runs on independent 32kHz RC oscillator
  // Max timeout ~26200ms with prescaler, so we use window watchdog or system reset
  if (!IWatchdog.isEnabled()) {
    // Initialize with approximately 80 second timeout
    // IWDG prescaler /64 with reload value ~40000 gives ~80s at 32kHz
    IWatchdog.begin(80000000);  // 80 seconds in microseconds
  }
  
  sys.initTime = millis();
}

// ============================================
// INPUT HANDLING - Debounced, efficient
// ============================================
void readInputs() {
  uint32_t now = millis();
  
  // Button UP with debounce
  bool raw = digitalRead(Pins::BTN_UP);
  if (raw != sys.lastBtnUp) sys.debounceUp = now;
  if (timePassed(sys.debounceUp, Config::DEBOUNCE_MS)) sys.btnUp = raw;
  sys.lastBtnUp = raw;
  
  // Button DOWN with debounce
  raw = digitalRead(Pins::BTN_DOWN);
  if (raw != sys.lastBtnDown) sys.debounceDown = now;
  if (timePassed(sys.debounceDown, Config::DEBOUNCE_MS)) sys.btnDown = raw;
  sys.lastBtnDown = raw;
  
  // Sensors - read once per cycle
  sys.high1 = digitalRead(Pins::SENSOR_HIGH_1);
  sys.high2 = digitalRead(Pins::SENSOR_HIGH_2);
  sys.low1 = digitalRead(Pins::SENSOR_LOW_1);
  sys.low2 = digitalRead(Pins::SENSOR_LOW_2);
  
  // Derived states
  sys.atHigh = sys.high1 && sys.high2;
  sys.atLow = sys.low1 && sys.low2;
  sys.sensorCount = (sys.high1 + sys.high2 + sys.low1 + sys.low2);
}

// ============================================
// SAFETY CHECKS - Fast, decisive
// ============================================
bool checkSensors() {
  // Conflict: both high and low simultaneously
  if (sys.atHigh && sys.atLow) return false;
  
  // Individual sensor conflicts (high1 vs low1, etc.)
  if ((sys.high1 && sys.low1) || (sys.high2 && sys.low2)) return false;
  
  // At position with opposite sensor active
  if (sys.atHigh && (sys.low1 || sys.low2)) return false;
  if (sys.atLow && (sys.high1 || sys.high2)) return false;
  
  return true;
}

void logFault(Fault f) {
  sys.faultLog[sys.faultIdx] = static_cast<uint8_t>(f);
  sys.faultIdx = (sys.faultIdx + 1) % Config::MAX_FAULTS;
}

void enterFault(Fault f) {
  relaysOff();
  sys.fault = f;
  sys.state = State::FAULT;
  sys.moveStart = 0;
  sys.relayWait = false;
  logFault(f);
}

// ============================================
// STATE HANDLERS - Lean, no abstraction overhead
// ============================================
void handleFault() {
  // Require 6s button hold to reset
  static uint32_t resetStart = 0;
  
  if (!sys.btnUp && !sys.btnDown) {
    resetStart = 0;
    return;
  }
  
  if (sys.btnUp && sys.btnDown) return;  // Both pressed = invalid
  
  if (resetStart == 0) {
    resetStart = millis();
    return;
  }
  
  if (timePassed(resetStart, 6000)) {
    // Validate we can move in pressed direction
    if (sys.btnUp && !sys.atHigh) {
      sys.state = State::IDLE;
      sys.fault = Fault::NONE;
      sys.initDone = true;
    } else if (sys.btnDown && !sys.atLow) {
      sys.state = State::IDLE;
      sys.fault = Fault::NONE;
      sys.initDone = true;
    }
    resetStart = 0;
  }
}

void handleInit() {
  relaysOff();
  
  if (!sys.initDone) {
    if (!timePassed(sys.initTime, Config::INIT_DELAY)) return;
    sys.initDone = true;
  }
  
  // Auto-transition to IDLE when ready
  sys.state = State::IDLE;
  sys.btnHeld = false;
}

void handleIdle() {
  // Check for conflicting inputs
  if (sys.btnUp && sys.btnDown) {
    enterFault(Fault::BTN_CONFLICT);
    return;
  }
  
  // Handle UP request
  if (sys.btnUp && !sys.atHigh) {
    if (!sys.btnHeld) {
      sys.btnTimer = millis();
      sys.btnHeld = true;
    } else if (timePassed(sys.btnTimer, Config::BTN_HOLD_TIME)) {
      // Start move up sequence
      sys.moveStart = millis();
      sys.relayWait = false;
      sys.state = State::MOVE_UP;
      sys.btnHeld = false;
    }
  }
  // Handle DOWN request
  else if (sys.btnDown && !sys.atLow) {
    if (!sys.btnHeld) {
      sys.btnTimer = millis();
      sys.btnHeld = true;
    } else if (timePassed(sys.btnTimer, Config::BTN_HOLD_TIME)) {
      sys.moveStart = millis();
      sys.relayWait = false;
      sys.state = State::MOVE_DOWN;
      sys.btnHeld = false;
    }
  } else {
    sys.btnHeld = false;  // Button released
  }
}

void handleMoveUp() {
  // Safety: too many sensors active
  if (sys.sensorCount >= 3) {
    enterFault(Fault::MULTI_SENSOR);
    return;
  }
  
  // Timeout check
  if (timePassed(sys.moveStart, Config::MAX_MOVE_TIME)) {
    enterFault(Fault::TIMEOUT);
    return;
  }
  
  // Relay interlock sequence
  if (!sys.relayWait) {
    relayDown(false);
    sys.relayTimer = millis();
    sys.relayWait = true;
    return;
  }
  
  if (!timePassed(sys.relayTimer, Config::RELAY_DELAY)) return;
  
  relayUp(true);
  
  // Check destination reached
  if (sys.atHigh) {
    relaysOff();
    sys.state = State::IDLE;
    sys.moveStart = 0;
  }
}

void handleMoveDown() {
  if (sys.sensorCount >= 3) {
    enterFault(Fault::MULTI_SENSOR);
    return;
  }
  
  if (timePassed(sys.moveStart, Config::MAX_MOVE_TIME)) {
    enterFault(Fault::TIMEOUT);
    return;
  }
  
  if (!sys.relayWait) {
    relayUp(false);
    sys.relayTimer = millis();
    sys.relayWait = true;
    return;
  }
  
  if (!timePassed(sys.relayTimer, Config::RELAY_DELAY)) return;
  
  relayDown(true);
  
  if (sys.atLow) {
    relaysOff();
    sys.state = State::IDLE;
    sys.moveStart = 0;
  }
}

// ============================================
// MAIN LOOP - Deterministic, bounded execution
// ============================================
void loop() {
  // Feed watchdog - if we hang, system resets
  IWatchdog.reload();
  
  // Fixed timing: 10ms cycle (100Hz) - robust and sufficient
  uint32_t now = millis();
  if ((now - sys.lastLoop) < Config::LOOP_PERIOD) return;
  sys.lastLoop = now;
  
  // Read all inputs
  readInputs();
  
  // Global safety check
  if (!checkSensors()) {
    if (sys.state != State::FAULT) enterFault(Fault::SENSOR_ERR);
  }
  
  // State machine - simple switch, no function pointers
  switch (sys.state) {
    case State::FAULT:
      handleFault();
      break;
      
    case State::INIT:
      handleInit();
      break;
      
    case State::IDLE:
      handleIdle();
      break;
      
    case State::MOVE_UP:
      handleMoveUp();
      break;
      
    case State::MOVE_DOWN:
      handleMoveDown();
      break;
      
    default:
      // Should never happen - corruption detected
      enterFault(Fault::SENSOR_ERR);
      break;
  }
}