// ==== LIBRARIES ====
#include <ESP32Servo.h>

// ==== CONSTANTS, PINNING, DEFINES ====

// Servo absolute fysieke grenzen
#define SERVOMIN  1      // Voor demo: stel veilig 10 graden
#define SERVOMAX  180     // Voor demo: stel veilig 4000 graden

#define NUM_SERVOS 1
#define NUM_EMOTIES 2

#define SERVO_STEP        1
#define CALIB_PAUSE_MS  300
#define MOVE_PAUSE_MS    25

const uint8_t servoPins[NUM_SERVOS]      = {19};
const uint8_t switchPins[NUM_SERVOS][2]  = { {12, 13} };
const uint8_t buttonPins[NUM_EMOTIES]    = {22, 23};

enum ServoDirection { NORMAL };
const ServoDirection servoDirections[NUM_SERVOS] = { NORMAL }; //normal of reverse voor servos in ander richting

float emotiePerc[NUM_SERVOS][NUM_EMOTIES] = {
  {0.1, 0.9}, //meer, 0.0's voor meer emoties meer  ,{0.1, 0.3} voor meer servos
};

const char* EmotieNamen[NUM_EMOTIES] = { "Blij", "Boos"};

// ==== VARIABELEN ====
unsigned long lastPrint = 0;
unsigned long lastUserAction = 0;
const unsigned long printInterval = 1000;
int lastButtonState[NUM_EMOTIES] = {HIGH, HIGH}; // meer HIGH's met meet emoties
unsigned long lastDebounceTime[NUM_EMOTIES] = {0, 0};
const unsigned long debounceDelay = 50;
int calibIndex = 0;
int currentEmotie = 1;
int lastSelectedEmotie = -1; // slaat de laatst GEKOZEN emotie op (knopinput)

// Voor state-tracking zodat we alleen bij verandering printen:
struct ServoState {
  int hoek;
  bool calibrated;
};
ServoState prevState[NUM_SERVOS];
int prevEmotie = -1;
bool prevCalibrating = true;

// ==== COMPACTE SERVOCLASS ====
class SmartServo {
public:
  Servo servo;
  uint8_t pinServo, pinMin, pinMax;
  ServoDirection direction;
  bool isCalibrated;
  int minAngle, maxAngle, currentAngle;
  enum CalibState { SEEK_MIN, SEEK_MAX, DONE } calibState;
  float actualAngle; // Simuleert de echte servo-aspositie (geen directe sprong)
  static constexpr float MOVE_SPEED = 0.002; // graden per loopstap (per 25 ms)(maak dit lager om hem "langzamer te laaten gaan")

  float getActualPercentage() {
    if (!isCalibrated || (maxAngle==minAngle)) return 0.0;
    if (direction == NORMAL)
      return (float)(actualAngle - minAngle) / (float)(maxAngle - minAngle);
    else
      return (float)(maxAngle - actualAngle) / (float)(maxAngle - minAngle);
  }

  SmartServo(uint8_t pServo, uint8_t pMin, uint8_t pMax, ServoDirection dir)
    : pinServo(pServo), pinMin(pMin), pinMax(pMax), direction(dir) {}

  void begin() {
    servo.attach(pinServo);
    pinMode(pinMin, INPUT_PULLUP);
    pinMode(pinMax, INPUT_PULLUP);
    isCalibrated = false;
    calibState = SEEK_MIN;
    currentAngle = (SERVOMIN + SERVOMAX) / 2;
    servo.write(currentAngle);
    actualAngle = currentAngle;    
  }

  void calibrateStep() {
    switch (calibState) {
      case SEEK_MIN:
        if (digitalRead(pinMin) == LOW) {
          minAngle = currentAngle;
          calibState = SEEK_MAX;
          delay(CALIB_PAUSE_MS);
          currentAngle = minAngle + 10;
          servo.write(currentAngle);
          delay(CALIB_PAUSE_MS);
        } else {
          if (currentAngle > SERVOMIN) currentAngle -= SERVO_STEP;
          servo.write(currentAngle);
          delay(MOVE_PAUSE_MS);
        }
        break;
      case SEEK_MAX:
        if (digitalRead(pinMax) == LOW) {
          maxAngle = currentAngle;
          calibState = DONE;
          isCalibrated = true;
          delay(CALIB_PAUSE_MS);
        } else {
          if (currentAngle < SERVOMAX) currentAngle += SERVO_STEP;
          servo.write(currentAngle);
          delay(MOVE_PAUSE_MS);
        }
        break;
      case DONE:
        break;
    }
  }

  void goToEmotie(float perc) {
    if (!isCalibrated) return;
    int aMin = minAngle, aMax = maxAngle;
    aMin = constrain(aMin, SERVOMIN, SERVOMAX);
    aMax = constrain(aMax, SERVOMIN, SERVOMAX);
    if (aMax == aMin) aMax = aMin + 1;
    int doelhoek = direction == NORMAL
      ? aMin + (int)((aMax - aMin) * perc + 0.5)
      : aMax - (int)((aMax - aMin) * perc + 0.5);
    doelhoek = constrain(doelhoek, SERVOMIN, SERVOMAX);
    servo.write(doelhoek);
    currentAngle = doelhoek;
  }

  float getCurrentPercentage() {
    if (!isCalibrated || (maxAngle==minAngle)) return 0.0;
    if (direction == NORMAL)
      return (float)(currentAngle - minAngle) / (float)(maxAngle - minAngle);
    else
      return (float)(maxAngle - currentAngle) / (float)(maxAngle - minAngle);
  }

  void updateActualPosition() {
    if (!isCalibrated) {
      actualAngle = currentAngle;
      return;
    }
    if (fabs(currentAngle - actualAngle) < MOVE_SPEED) {
      actualAngle = currentAngle;
    } else if (currentAngle > actualAngle) {
      actualAngle += MOVE_SPEED;
    } else if (currentAngle < actualAngle) {
      actualAngle -= MOVE_SPEED;
    }
  }
};

SmartServo* servos[NUM_SERVOS];

// ==== SETUP ====
// Hier worden alle pinnen, servo's en knoppen klaargezet
void setup() {
  Serial.begin(115200);                       // Seriële monitor starten
  delay(200);                                 // Even wachten voor stabiliteit

  // Maak voor elke servo een object aan en initialiseer
  for (int i = 0; i < NUM_SERVOS; i++) {
    servos[i] = new SmartServo(servoPins[i], switchPins[i][0], switchPins[i][1], servoDirections[i]);
    servos[i]->begin();
  }
  // Stel alle emotieknoppen in als input met pullup
  for (int i = 0; i < NUM_EMOTIES; i++) {
    pinMode(buttonPins[i], INPUT_PULLUP);
  }
  lastPrint = millis();
  lastUserAction = millis();

  Serial.println("==== Compact SmartServo Kalibratie Demo ====");
  printStatus(); // Toon meteen de beginstatus
}

// ==== LOOP ====
// Hier wordt de hoofdlogica uitgevoerd: kalibreren, knoppen, status
void loop() {
  bool calibrating = (calibIndex < NUM_SERVOS);

  // Kalibreren: alleen printen als status wijzigt
  if (calibrating) {
    if (!servos[calibIndex]->isCalibrated) {
      servos[calibIndex]->calibrateStep();
      checkAndPrintState(true); // force: bij elke stap in calibratie
      return;
    } else {
      calibIndex++;
      lastUserAction = millis();
    }
    return;
  }

  // Niet meer aan het kalibreren, dus emotie-modus!
  checkButtons();
  checkAndPrintState(false); // alleen printen als er wat wijzigt

  // Elke seconde altijd een statusregel:
  if (millis() - lastPrint > printInterval) {
    printStatus();
    lastPrint = millis();
  }

  for (int i = 0; i < NUM_SERVOS; i++) {
    servos[i]->updateActualPosition();
  }
}

// Print altijd de actuele posities van alle servo’s
void printServoPositions() {
  static int lastAngle[NUM_SERVOS] = {-1}; // Print alleen als iets verandert (optioneel) //  !!!!!! voeg heer een extr ,-1 toe bij meer servos
  for (int i = 0; i < NUM_SERVOS; i++) {
    if (servos[i]->currentAngle != lastAngle[i]) { // Print alleen bij wijziging (kan ook altijd printen!)
      Serial.print("Update: Servo ");
      Serial.print(i);
      Serial.print(" | Huidige hoek: ");
      Serial.print(servos[i]->currentAngle);
      Serial.print("° | Gekalibreerd: ");
      Serial.print(servos[i]->isCalibrated ? "JA" : "NEE");
      Serial.print(" | Emotie: ");
      Serial.print(EmotieNamen[currentEmotie]);
      Serial.print(" | Positie %: ");
      Serial.print(100 * servos[i]->getCurrentPercentage(), 0);
      Serial.println("%");
      lastAngle[i] = servos[i]->currentAngle;
    }
  }
}


// ==== ANDERE FUNCTIES ====

// Knoppen uitlezen en bij verandering juiste emotie kiezen
void checkButtons() {
  for (int i = 0; i < NUM_EMOTIES; i++) {
    int reading = digitalRead(buttonPins[i]);
    if (reading != lastButtonState[i]) {
      lastDebounceTime[i] = millis();
      lastButtonState[i] = reading;
    }
    // Alleen als knop echt lang genoeg ingedrukt blijft
    if ((millis() - lastDebounceTime[i]) > debounceDelay) {
      if (reading == LOW && currentEmotie != i) { // Alleen reageren op nieuwe emotie
        currentEmotie = i;
        setEmotie(i);         // Zet alle servo's naar emotiepositie
        lastUserAction = millis(); // Timer resetten: gebruikersactie
      }
    }
  }
}

// Stuur alle servo's naar de gewenste emotie (percentage per servo)
void setEmotie(int emotieIndex) {
  lastSelectedEmotie = emotieIndex;  // <--- voeg toe!
  for (int i = 0; i < NUM_SERVOS; i++) {
    servos[i]->goToEmotie(emotiePerc[i][emotieIndex]);
  }
  Serial.print("["); Serial.print(millis()); Serial.print("ms] Emotie gekozen: ");
  Serial.println(EmotieNamen[emotieIndex]);
  printStatus();
}

// Print status van alle servo's, de tijd en gebruikers-timer
void printStatus() {
  Serial.print("["); Serial.print(millis()/1000); Serial.print("s] ");
  for (int i = 0; i < NUM_SERVOS; i++) {
    Serial.print("Servo "); Serial.print(i);
    Serial.print(" |CAL: "); Serial.print(servos[i]->isCalibrated ? "JA" : "NEE");
    Serial.print(" |MIN: "); Serial.print(servos[i]->minAngle);
    Serial.print(" |MAX: "); Serial.print(servos[i]->maxAngle);
    Serial.print(" |D:"); Serial.print(servoDirections[i] == NORMAL ? "+" : "-");
    Serial.print(" |Emotie: "); Serial.print(EmotieNamen[currentEmotie]);
    Serial.print(" |T%: "); Serial.print(100*emotiePerc[i][currentEmotie],0);
    Serial.print("% |Positie nu: ");
    Serial.print(100*servos[i]->getActualPercentage(),1);
    Serial.print("% (SIG: ");
    Serial.print(servos[i]->actualAngle,1);
    Serial.print("PWM)");
    if (i < NUM_SERVOS-1) Serial.print(" || ");
  }
  Serial.print(" | T- user-act: ");
  Serial.print((millis()-lastUserAction)/1000); Serial.println("s");
}

// Print alleen als er iets wezenlijk verandert of force = true
void checkAndPrintState(bool force) {
  bool changed = force;
  for (int i = 0; i < NUM_SERVOS; i++) {
    if (servos[i]->currentAngle != prevState[i].hoek ||
        servos[i]->isCalibrated != prevState[i].calibrated)
      changed = true;
  }
  if (currentEmotie != prevEmotie)
    changed = true;
  bool calibrating = (calibIndex < NUM_SERVOS);
  if (calibrating != prevCalibrating)
    changed = true;

  if (changed) {
    Serial.print("[STATE UPDATE] ");
    for (int i = 0; i < NUM_SERVOS; i++) {
      Serial.print("Servo "); Serial.print(i);
      Serial.print(" | Hoek: "); Serial.print(servos[i]->currentAngle);
      Serial.print(" | CAL: ");
      Serial.print(servos[i]->isCalibrated ? "JA" : "NEE");
      if (i < NUM_SERVOS-1) Serial.print(" || ");
      prevState[i].hoek = servos[i]->currentAngle;
      prevState[i].calibrated = servos[i]->isCalibrated;
    }
    Serial.print(" | Gekozen emotie: ");
    if (lastSelectedEmotie >= 0) Serial.print(EmotieNamen[lastSelectedEmotie]);
    else Serial.print("-");
    Serial.print(" | Actief: ");
    Serial.print(EmotieNamen[currentEmotie]);
    Serial.print(" | Mode: ");
    if (calibrating) Serial.print("KALIBRATIE");
    else Serial.print("EMOTIE");
    Serial.println();

    prevEmotie = currentEmotie;
    prevCalibrating = calibrating;
  }
}