// =====  Solar Tracker: BLE + 30 s interval (20 s burst), AZIMUTH then ELEVATION =====

#include 
#include 
#include 
#include 
#include 

// ---------------- Pins ----------------
#define RELAY_EXTEND   D5      // linear actuator relays (ACTIVE LOW)
#define RELAY_RETRACT  D4

#define STEP_PIN       D6      // stepper STEP
#define DIR_PIN        D7      // stepper DIR

// Light sensors (XIAO back pads use GPIO numbers for 4/5/6)
#define SENSOR_N       5
#define SENSOR_S       D0
#define SENSOR_E       4
#define SENSOR_W       6

// ---------------- Direction toggles (no rewiring) ----------------
#define EAST_IS_CW              1   // 1: CW aims EAST, 0: CW aims WEST
#define MORE_VERTICAL_IS_EXTEND 0   // 1: EXTEND raises elevation

// ---------------- Tuning ----------------
static const int      DEADBAND          = 200;             // both axes
static const uint32_t CONTROL_PERIOD_MS = 300;             // auto step cadence
static const uint32_t TRACK_INTERVAL_MS = 30UL * 1000UL;   // 30 s between bursts
static const uint32_t TRACK_BURST_MS    = 20000UL;         // 20 seconds per burst
static const uint32_t DEFAULT_RUN_MS    = 10000UL;         // actuator safety cap
static const uint32_t NIGHT_RETRACT_MS  = 8000UL;          // retract 8 s in night mode
static const uint32_t HOME_RETRACT_MS   = 8000UL;          // retract 8 s on HOME   (NEW)
static const int      NIGHT_THR         = 400;             // brightest < NIGHT_THR => night

// ---------------- Stepper ----------------
AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);
const float MAX_SPEED = 300.0f;     // steps/s
const float JOG_SPEED = 100.0f;     // steps/s
float currentSpeed = 0.0f;
static inline void setSpeedClamped(float s){
  if (s >  MAX_SPEED) s =  MAX_SPEED;
  if (s < -MAX_SPEED) s = -MAX_SPEED;
  currentSpeed = s;
  stepper.setSpeed(currentSpeed);
}

// ---------------- State ----------------
enum Motion { STOPPED, EXTENDING, RETRACTING };
volatile Motion motion = STOPPED;

bool trackingEnabled = false;
bool inBurst         = false;

BLECharacteristic* cmdChar = nullptr;
uint32_t stopAtMs    = 0;     // actuator safety timer
uint32_t nextBurstAt = 0;
uint32_t burstEndsAt = 0;

// ---------------- Helpers ----------------
static inline void stopActuator(){ digitalWrite(RELAY_EXTEND, HIGH); digitalWrite(RELAY_RETRACT, HIGH); motion = STOPPED; }
static inline void extendActuator(){ digitalWrite(RELAY_EXTEND, LOW);  digitalWrite(RELAY_RETRACT, HIGH); motion = EXTENDING; }
static inline void retractActuator(){ digitalWrite(RELAY_EXTEND, HIGH); digitalWrite(RELAY_RETRACT, LOW);  motion = RETRACTING; }
static inline int bright(uint8_t pin){ return 4095 - analogRead(pin); } // higher = brighter

// ---- Auto step (runs at most every CONTROL_PERIOD_MS). AZIMUTH first, then ELEVATION.
static inline void autoStep(){
  static uint32_t last = 0;
  uint32_t now = millis();
  if (now - last < CONTROL_PERIOD_MS) return;
  last = now;

  int n = bright(SENSOR_N);
  int s = bright(SENSOR_S);
  int e = bright(SENSOR_E);
  int w = bright(SENSOR_W);

  // ---- Serial plot: N,S,W,E (CSV) ----
  Serial.print(n); Serial.print(',');
  Serial.print(s); Serial.print(',');
  Serial.print(w); Serial.print(',');
  Serial.println(e);

  // night mode: retract 8 s, no rotation
  int brightest = max(max(n, s), max(e, w));
  if (brightest < NIGHT_THR){
    if (motion != RETRACTING){ retractActuator(); stopAtMs = now + NIGHT_RETRACT_MS; } // 8 s
    setSpeedClamped(0);   // no rotation
    return;               // stay in night behaviour during burst; scheduler will end burst on time
  }

  // 1) AZIMUTH (E/W)
  int diffEW = e - w;                      // >0 => EAST brighter
  if (abs(diffEW) > DEADBAND){
    float spd = (diffEW > 0) ? JOG_SPEED : -JOG_SPEED;  // toward brighter
  #if EAST_IS_CW
    setSpeedClamped(spd);
  #else
    setSpeedClamped(-spd);
  #endif
    if (motion != STOPPED){ stopActuator(); stopAtMs = 0; }
    return;                                 // finish azimuth first
  } else {
    if (currentSpeed != 0.0f) setSpeedClamped(0);
  }

  // 2) ELEVATION (N/S)
  int diffNS = s - n;                      // >0 => SOUTH brighter
  if (abs(diffNS) > DEADBAND){
    bool moreVertical = (diffNS > 0);
  #if MORE_VERTICAL_IS_EXTEND
    Motion desired = moreVertical ? EXTENDING : RETRACTING;
  #else
    Motion desired = moreVertical ? RETRACTING : EXTENDING;
  #endif
    if (desired == EXTENDING && motion != EXTENDING){
      extendActuator(); stopAtMs = now + DEFAULT_RUN_MS;
    } else if (desired == RETRACTING && motion != RETRACTING){
      retractActuator(); stopAtMs = now + DEFAULT_RUN_MS;
    }
  } else {
    if (motion != STOPPED){ stopActuator(); stopAtMs = 0; }
  }
}

// ---------------- BLE  ----------------
class WriteCB : public BLECharacteristicCallbacks {
  void onWrite(BLECharacteristic* ch) override {
    String s = String(ch->getValue().c_str());
    s.trim(); s.toUpperCase();            // "home" -> "HOME"
    if (!s.length()) return;

    // Manual tilt
    if (s=="EXTEND" || s=="E"  || s=="1"){  extendActuator();  stopAtMs = millis() + DEFAULT_RUN_MS; return; }
    if (s=="RETRACT"|| s=="R"  || s=="-1"){ retractActuator(); stopAtMs = millis() + DEFAULT_RUN_MS; return; }
    if (s=="STOP"   || s=="S"  || s=="0"){  stopActuator();     stopAtMs = 0;                         return; }

    // Manual azimuth
    if (s=="CW"){     setSpeedClamped((currentSpeed > 0) ? currentSpeed : JOG_SPEED);  return; }
    if (s=="CCW"){    setSpeedClamped((currentSpeed < 0) ? currentSpeed : -JOG_SPEED); return; }
    if (s=="STOP_R"){ setSpeedClamped(0);                                              return; }

    // --- HOME (retract 8 s, stop rotation, end burst) ---
    if (s=="HOME"){ 
      setSpeedClamped(0);                 // no rotation while homing
      retractActuator();
      stopAtMs  = millis() + HOME_RETRACT_MS;   // 8 s retract
      inBurst   = false;                        // end any active burst
      nextBurstAt = millis() + TRACK_INTERVAL_MS; // schedule next burst
      return;
    }

    // Scheduler
    if (s=="START_TRACK"){ trackingEnabled = true;  inBurst = false; nextBurstAt = millis(); return; }
    if (s=="STOP_TRACK"){  trackingEnabled = false; inBurst = false; setSpeedClamped(0); stopActuator(); return; }
  }
};

// ---------------- Setup / Loop ----------------
void setup(){
  Serial.begin(115200);

  pinMode(RELAY_EXTEND,  OUTPUT);
  pinMode(RELAY_RETRACT, OUTPUT);
  stopActuator();

  pinMode(STEP_PIN, OUTPUT);
  pinMode(DIR_PIN,  OUTPUT);
  stepper.setMaxSpeed(MAX_SPEED);
  setSpeedClamped(0);

  BLEDevice::init("XIAO_C6_Solar_Tracker");
  BLEServer*  server  = BLEDevice::createServer();
  BLEService* service = server->createService("4fafc201-1fb5-459e-8fcc-c5c9c331914b");
  BLECharacteristic* ch = service->createCharacteristic(
      "beb5483e-36e1-4688-b7f5-ea07361b26a8",
      BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);
  ch->setCallbacks(new WriteCB());
  ch->setValue("READY");
  service->start();
  BLEAdvertising* adv = BLEDevice::getAdvertising();
  adv->addServiceUUID("4fafc201-1fb5-459e-8fcc-c5c9c331914b");
  adv->start();
}

void loop(){
  // Keep stepper running at last speed
  stepper.runSpeed();

  // Actuator safety stop
  uint32_t now = millis();
  if (stopAtMs && now >= stopAtMs){ stopActuator(); stopAtMs = 0; }

  // 30-second scheduler with 20-second burst
  if (trackingEnabled){
    if (!inBurst){
      if (now >= nextBurstAt){
        inBurst = true;
        burstEndsAt = now + TRACK_BURST_MS;
      }
    } else {
      autoStep();                               // azimuth first, then elevation
      if (now >= burstEndsAt){
        setSpeedClamped(0);
        stopActuator();
        inBurst = false;
        nextBurstAt = millis() + TRACK_INTERVAL_MS;  // 30 s
      }
    }
  }

  delay(1);
}