#include <Servo.h>

// ********************************************************************************
#define DEBUGGING // Comment this line to stop printing while debugging

#ifdef DEBUGGING
  #define DEBUG_PRINTLN(x) Serial.println(x)  // Debug print macro
  #define DEBUG_PRINT(x) Serial.print(x) 
#else
  #define DEBUG_PRINTLN(x)  // Empty definition when DEBUGGING is not defined
  #define DEBUG_PRINT(x)  
#endif
// ********************************************************************************
Servo hourServo;
Servo minuteServo;

#define SKETCH_VERSION "3.1.0"

// Variabelen voor vloeiende beweging
const int stepSize = 1;  // Fijn afstellen van servo stappen
const int minDelay = 15; // Minimale vertraging tussen stappen (hoger = langzamer)
const int maxDelay = 50; // Maximale vertraging voor vloeiende acceleratie en deacceleratie

const int MaxServoMinuuthoek = 180;
const int MaxServoUurhoek = 180;

// Voorbeeld invoer: uren en minuten
// Gebruikt in v10 - Laatste versie!
int hours = 6;    // Invoer aantal uren
int minutes = 30;  // Invoer aantal minuten

// Oud(er)
int uren = 0;
int minuten = 0;

void setup() {
  Serial.begin(115200);
  Serial.println();
  Serial.println("(c)2024 Eric M. Kok");
  Serial.println("Servos Klok");

  SketchInfo(); // Print sketch info

  Serial.print("Uur Servo ");
  hourServo.attach(9);     // Bevestig de uurservo aan pin 9
  if (hourServo.attached()){
    Serial.println("SUCCESVOL gekoppeld");
  } else {
    Serial.println("NIET gekoppeld!");
  }
  Serial.print("Minuten Servo ");
  minuteServo.attach(10);  // Bevestig de minutenservo aan pin 10
  if (minuteServo.attached()){
    Serial.println("SUCCESVOL gekoppeld");
  } else {
    Serial.println("NIET gekoppeld");
  }

  // Start positie servos
  DEBUG_PRINTLN("---");
  DEBUG_PRINT("Starthoek uur wijzer: ");
  DEBUG_PRINTLN(hourServo.read());
  DEBUG_PRINT("Starthoek minuut wijzer: ");
  DEBUG_PRINTLN(minuteServo.read());

  DEBUG_PRINT("Start test tijd");
  
  uren = 9;
  minuten = 15;

  int urenDegrees = calculateUrenHoekServo(uren, minuten);
  float minutenDegrees = calculateMinutenHoekServo(minuten);
  smoothMoveDual(urenDegrees, minutenDegrees);

  // Resultaat tonen op de seriële monitor
  // Serial.print("Verstreken tijd: ");
  // Serial.print(" - ");
  Serial.print(uren);
  Serial.print(":");
  Serial.print(minuten);
  Serial.println(" minuten");
  Serial.print("= Servo hoeken: ");
  Serial.print(urenDegrees);
  Serial.print(" graden uurwijzer, ");
  Serial.print(minutenDegrees);
  Serial.println(" graden minuutwijzer");

  Serial.print("Hoek naar tijdstip (in uu:mm): ");
  Serial.print(int(HoekNaarUren(urenDegrees,minutenDegrees)));
  Serial.print(":");

  // Serial.print("Hoek naar minuten: ");
  
  if (HoekNaarMinuten(minutenDegrees) < 10) {
    Serial.print("0");
    Serial.print(HoekNaarMinuten(minutenDegrees));
  } else {
    Serial.print(HoekNaarMinuten(minutenDegrees));
  }
  Serial.println();
  Serial.println("---");

} // Einde void setup()
// ********************************************************************************
void loop() {
  uren = random(0,12);  
  DEBUG_PRINT("Uren: ");
  DEBUG_PRINT(uren);

  minuten = (random(0,60));
  DEBUG_PRINT(" Minuten: ");
  DEBUG_PRINTLN(minuten);

  // Bereken de servograden op basis van verstreken tijd (= helft van wijzerhoek van 360 graden)
  int urenDegrees = calculateUrenHoekServo(uren, minuten);
  float minutenDegrees = calculateMinutenHoekServo(minuten);

  smoothMoveDual(urenDegrees, minutenDegrees);

  Serial.print(uren);
  Serial.print(":");
  Serial.print(minuten);
  Serial.println(" minuten");
  Serial.print("= Servo hoeken: ");
  Serial.print(urenDegrees);
  Serial.print(" graden uurwijzer, ");
  Serial.print(minutenDegrees);
  Serial.println(" graden minuutwijzer");

  Serial.print("Hoek naar tijdstip (in uu:mm): ");
  Serial.print(int(HoekNaarUren(urenDegrees,minutenDegrees)));
  Serial.print(":");
  
  // Voorloop nul als minuten kleiner dan 10 zijn
  if (HoekNaarMinuten(minutenDegrees) < 10) {
    Serial.print("0");
    Serial.print(HoekNaarMinuten(minutenDegrees));
  } else {
    Serial.print(HoekNaarMinuten(minutenDegrees));
  }
  Serial.println();
  Serial.println("---");

  delay(2000);  // Wacht voordat de volgende beweging plaatsvindt

} // end loop()
// ********************************************************************************
// ********************************************************************************

int HoekNaarMinuten(int servoHoek) { // servo hoek minuten
    // Vermenigvuldig de servo-hoek weer met 2 om terug naar de wijzer-hoek te gaan
    int wijzerHoek = (servoHoek * 2) % 360;

    // Deel de wijzer-hoek door 6 om het aantal minuten te krijgen (want 360 graden komt overeen met 60 minuten)
    int minuten = wijzerHoek / 6;
    
    return minuten;
}

float HoekNaarUren(int servoHoekUren, int servoHoekMinuten) {
    // Herstel de graden voor de urenwijzer van de servo-hoek naar de wijzer-hoek (maal 2 om terug naar de schaal van 360 te gaan)
    float gradenUrenWijzer = (servoHoekUren * 2) % 360;

    // Herstel de graden voor de minutenwijzer
    float gradenMinutenWijzer = (servoHoekMinuten * 2) % 360;

    // Het aantal hele uren is gelijk aan de urenwijzerhoek gedeeld door 30 (want 360 graden komt overeen met 12 uren, dus 30 graden per uur)
    int uren = int(gradenUrenWijzer / 30);

    // Aantal resterende graden bovenop volledige uren. Elke graad vertegenwoordigt 2 minuten.
    float resterendeGraden = gradenUrenWijzer - (uren * 30);
    
    // Bereken minuten uit de resterende graden (30 graden per uur, dus 0.5 graad per minuut)
    int minuten = int(gradenMinutenWijzer / 6); 

    // Als het urenwijzergedeelte resterende minuten heeft (meer dan volledige uren), moet dit worden verrekend in de uren.
    float fractieUren = resterendeGraden / 30.0;

    return uren + fractieUren;
}

int calculateMinutenHoekServo(int minutes) {
  return ((6 * minutes) / 2) % 180 ; // Van wijzer graden 360 naar servo graden 180 is delen door 2
}

// Functie om verstreken tijd in minuten om te zetten naar graden
int calculateUrenHoekServo(int hours, int minutes) {
  int elapsedMinutes =  (hours * 60) + minutes;

  // Aantal volledige uren afgerond naar beneden
  int fullHours = elapsedMinutes / 60;

  // Extra minuten die overblijven
  int remainingMinutes = elapsedMinutes % 60;

  // Berekening van graden: uren * 30 graden per uur + minuten * 0.5 graden per minuut
  float degrees = (fullHours * 30 * 1.0) + (remainingMinutes / 2.0) ;

  return int(degrees / 2) % 180;  // Van wijzer graden 360 naar servo graden 180 is delen door 2
}


// Functie om zowel de uur- als minutenwijzer tegelijk soepel te bewegen
// Met een tijdsduur voor acceleratie van de wijzers, deacceleratie van de wijzers en daar tussenin is een constante snelheid
void smoothMoveDual(int toHourAngle, int toMinuteAngle) {
  int fromHourAngle = hourServo.read();    // Get the current position of the hour servo
  int fromMinuteAngle = minuteServo.read(); // Get the current position of the minute servo
  
  int hourDirection = (toHourAngle > fromHourAngle) ? 1 : -1;   // Direction for the hour hand
  int minuteDirection = (toMinuteAngle > fromMinuteAngle) ? 1 : -1;   // Direction for the minute hand
  
  int hourSteps = abs(toHourAngle - fromHourAngle);   // Total steps for the hour hand
  int minuteSteps = abs(toMinuteAngle - fromMinuteAngle); // Total steps for the minute hand

  int totalSteps = max(hourSteps, minuteSteps);  // Total iterations needed (based on the largest movement)
  
  float hourStepFactor = (float)hourSteps / totalSteps;    // Scale factor for the hour hand movement
  float minuteStepFactor = (float)minuteSteps / totalSteps; // Scale factor for the minute hand movement
  
  // Acceleration and deceleration time slots in percentages
  float accelPercent = 0.10;  // 10% of the time for acceleration
  float decelPercent = 0.10;   // 10% of the time for deceleration
  
  int accelSteps = totalSteps * accelPercent;       // Steps for acceleration
  int decelSteps = totalSteps * decelPercent;       // Steps for deceleration
  int constantSpeedSteps = totalSteps - accelSteps - decelSteps;  // Steps at constant speed

  for (int i = 0; i < totalSteps; i++) {
    // Calculate current positions for the hour and minute hands
    int currentHourAngle = fromHourAngle + (i * hourStepFactor * hourDirection * stepSize);
    int currentMinuteAngle = fromMinuteAngle + (i * minuteStepFactor * minuteDirection * stepSize);
    
    hourServo.write(currentHourAngle);
    minuteServo.write(currentMinuteAngle);

    int delayTime;

    if (i < accelSteps) {
      // Acceleration phase
      delayTime = map(i, 0, accelSteps, maxDelay, minDelay);  // Gradually reduce delay
    } else if (i < accelSteps + constantSpeedSteps) {
      // Constant speed phase
      delayTime = minDelay;  // Constant minimum delay
    } else {
      // Deceleration phase
      int decelIndex = i - accelSteps - constantSpeedSteps;  // Step index in the deceleration phase
      delayTime = map(decelIndex, 0, decelSteps, minDelay, maxDelay);  // Gradually increase delay
    }

    delay(delayTime);  // Apply the calculated delay for smooth movement
  }
}

// Functie om de servohoek te berekenen
int berekenServoHoek(float wijzerHoek) {
  return wijzerHoek / 2; // Omgerekend van 0-360 graden wijzer naar 0-180 graden servo
}

// Functie om uren om te zetten naar servo graden hoek
int mapHoursToDegrees(int hours) {
  float hoekUurwijzer = (hours % 12) * 30; // 30 graden per uur
  int servoHoekUurwijzer = berekenServoHoek(hoekUurwijzer);
  return servoHoekUurwijzer;
}

// Functie om servo graden hoek om te zetten naar uren
int mapDegreesToHours(int degrees) {
  float wijzerHoek = degrees * 2; // Omgerekend van 0-180 graden naar 0-360 graden
  int hours = wijzerHoek / 30; // 30 graden per uur
  return hours;
}

int mapMinutesToDegrees(int minutes) {
  float hoekMinuutwijzer = (minutes % 60) * 6;
  // Servohoeken berekenen
  int servoHoekMinuutwijzer = berekenServoHoek(hoekMinuutwijzer);
  return servoHoekMinuutwijzer;   
}

int mapDegreesToMinutes(int AantalGradenHoek) {
    return map(AantalGradenHoek, 0, MaxServoMinuuthoek, 0, 60);
}

int LeesServoTijd() {
    int fromHourAngle = hourServo.read();    // Haal de huidige positie van de uurservo op
    int fromMinuteAngle = minuteServo.read(); // Haal de huidige positie van de minutenservo op

    int hours = mapDegreesToHours(fromHourAngle);
    int minutes = mapDegreesToMinutes(fromMinuteAngle);

    DEBUG_PRINT(hours);
    DEBUG_PRINT(":");
    DEBUG_PRINTLN(minutes);
    return hours * 100 + minutes;
}

void SplitTijd(int InKomendeTijd, int &uren, int &minuten) {
    if (InKomendeTijd < 100) {
        // Als de inkomende tijd minder dan 100 is, zijn het alleen minuten, uren wordt dan 0 uur (12 uur)
        uren = 0;
        minuten = InKomendeTijd;
    } else {
        uren = InKomendeTijd / 100;
        minuten = InKomendeTijd % 100;
    } 

    // Zorg ervoor dat minuten binnen het bereik van 0 tot en met 60 liggen
    if (minuten < 0) {
        minuten = 0;
    } else if (minuten > 60) {
        minuten = 60;
    }

    // Zorg ervoor dat uren binnen het bereik van 0 tot en met 12 liggen
    if (uren < 0) {
        uren = 0;
    } else if (uren > 12) {
        uren = 12;
    }
} // end of SplitTijd()

void SketchInfo() {
  String str = "*";  // Replace with your actual string
  String repeatedStr = "";

  for (int i = 0; i < 50; i++) {
    repeatedStr += str;  // Concatenate the string 50 times
  }

  Serial.println(repeatedStr);  // Print the final repeated string

  Serial.print(F("Sketch: "));
  Serial.println(__FILE__);

  Serial.println("Versie: " SKETCH_VERSION);

  Serial.print(F("Compiled: "));
  // Serial.println(__DATE__);

  // Extract the file date
  String fileName = __DATE__;

  // Example: fileName might be something like "Sep 10 2024"

  // Split the filename by spaces
  int firstSpace = fileName.indexOf(' ');
  int secondSpace = fileName.indexOf(' ', firstSpace + 1);

  String firstBlock = fileName.substring(0, firstSpace);
  String secondBlock = fileName.substring(firstSpace + 1, secondSpace);
  String thirdBlock = fileName.substring(secondSpace + 1);

  // Print in the desired order: second, first, third
  Serial.print(secondBlock);
  Serial.print(F(" "));
  Serial.print(firstBlock);
  Serial.print(F(" "));
  Serial.print(thirdBlock);

  Serial.print(F(" "));
  Serial.print(__TIME__);
  Serial.println(F("GMT"));

  Serial.println(repeatedStr);  // Print the final repeated string
} // end of SketchInfo()