#include <Arduino.h>
#include <Servo.h>
#include <NmraDcc.h>
#include <EEPROM.h>

// Affectation des PINs Arduino
#define DCC_SIGNAL_PIN 2
#define LED_PIN 3
#define PWM_PIN 5
#define SERVO_PIN 6

// Valeurs des CV (Configuration Variables)
#define CV_ADDRESS_LSB 513
#define CV_ADDRESS_MSB 521
#define CV_PALES_SPEED 100
#define CV_MAX_UP 101
#define CV_MAX_DOWN 102

// Valeurs par défaut des CV
#define DEFAULT_PALES_SPEED 255
#define DEFAULT_MAX_UP 180
#define DEFAULT_MAX_DOWN 0
#define DEFAULT_ADDRESS 1
#define VITESSE 40  // Temps en millisecondes entre chaque pas de servo

#define EEPROM_SIG_ADDR 0
#define EEPROM_SIG_VALUE 0xA5
#define DIY_MANUFACTURER_ID 13

#define DEBUG 1

#ifdef DEBUG
  #define DEBUG_INITIALIZE_SERIAL Serial.begin(115200)
  #define DEBUG_PRINT(x) Serial.print(x)
  #define DEBUG_PRINTLN(x) Serial.println(x)
#else
  #define DEBUG_INITIALIZE_SERIAL
  #define DEBUG_PRINT(x)
  #define DEBUG_PRINTLN(x)
#endif

struct DCCAccessoryConfig {
    byte Sig;            // Signature pour vérifier la validité de la structure (0x45)
    byte ManufacturerID; // Manufacturer ID (0-255)
    byte Version;        // Version of the accessory decoder (0-255)

    int address;         // From 1 to 2044
    byte MotorSpeed;     // Speed of the motor (0-255)
    byte ServoMaxUp;     // Maximum position for servo up (0-180)
    byte ServoMaxDown;   // Maximum position for servo down (0-180)
} MyConfig;


NmraDcc Dcc;
Servo servoMotor;
uint8_t PalesSpeed = DEFAULT_PALES_SPEED;
uint8_t ServoMaxUp = DEFAULT_MAX_UP;
uint8_t ServoMaxDown = DEFAULT_MAX_DOWN;
uint16_t currentAddress = DEFAULT_ADDRESS;
int STROBE_ON = 0;
int HelicoUP = 0;
int currentServoPos = 90;
int targetServoPos = 90;
unsigned long servoNextMillis = 0;

/**
 * Génére un clignotement de la LED pour signaler l'accusé de réception d'une commande DCC. 
 */
void notifyCVAck(void)
{
  digitalWrite(LED_PIN, HIGH);
  delay(6);
  digitalWrite(LED_PIN, LOW);
}

/**
 * Notifie le changement d'une CV. Appelé par le décodeur DCC lorsque la valeur d'une CV est modifiée.
 * par la centrale DCC. Cette fonction met à jour les variables correspondantes et peut effectuer des actions supplémentaires si nécessaire.
 * @param CV Le numéro de la CV à modifier.
 * @param Value La nouvelle valeur de la CV.  
 */
void notifyCVChange(uint16_t CV, uint8_t Value)
{
  if (CV == CV_PALES_SPEED) {
    PalesSpeed = Value;
  } else if (CV == CV_MAX_UP) {
    ServoMaxUp = Value;
  } else if (CV == CV_MAX_DOWN) {
    ServoMaxDown = Value;
  } else if (CV == CV_ADDRESS_LSB || CV == CV_ACCESSORY_DECODER_ADDRESS_LSB) {
    currentAddress = (currentAddress & 0xFF00) | Value;
    if (CV == CV_ADDRESS_LSB) {
      Dcc.setCV(CV_ACCESSORY_DECODER_ADDRESS_LSB, Value);
    } else {
      Dcc.setCV(CV_ADDRESS_LSB, Value);
    }
  } else if (CV == CV_ADDRESS_MSB || CV == CV_ACCESSORY_DECODER_ADDRESS_MSB) {
    currentAddress = (currentAddress & 0x00FF) | ((Value & 0x07) << 8);
    if (CV == CV_ADDRESS_MSB) {
      Dcc.setCV(CV_ACCESSORY_DECODER_ADDRESS_MSB, Value & 0x07);
    } else {
      Dcc.setCV(CV_ADDRESS_MSB, Value);
    }
  }
}

/**
 * Appellé par le décodeur DCC pour vérifier si une CV est valide et peut être modifiée.
 * @param CV Le numéro de la CV.
 * @param Writable Indique si la CV est modifiable (1) ou non (0).
 * @return 1 si la CV est valide et modifiable, 0 sinon.
 */
uint8_t notifyCVValid (uint16_t CV, uint8_t Writable)
{
  if (CV == CV_PALES_SPEED || CV == CV_MAX_UP || CV == CV_MAX_DOWN ||
      CV == CV_ADDRESS_LSB || CV == CV_ADDRESS_MSB || CV == CV_ADDRESS_LSB || CV == CV_ADDRESS_MSB) {
    return 1;
  }
 
  return 0;
}

void initDefaultConfig()
{
  MyConfig.Sig = EEPROM_SIG_VALUE;
  MyConfig.ManufacturerID = DIY_MANUFACTURER_ID;
  MyConfig.Version = 1;
  MyConfig.address = DEFAULT_ADDRESS;
  MyConfig.MotorSpeed = DEFAULT_PALES_SPEED;
  MyConfig.ServoMaxUp = DEFAULT_MAX_UP;
  MyConfig.ServoMaxDown = DEFAULT_MAX_DOWN;
}

/**
 * Notifie la demande de réinitialisation des valeurs par défaut de la CV.
 */
void notifyCVResetFactoryDefault()
{
  Dcc.setCV(CV_PALES_SPEED, DEFAULT_PALES_SPEED);
  Dcc.setCV(CV_MAX_UP, DEFAULT_MAX_UP);
  Dcc.setCV(CV_MAX_DOWN, DEFAULT_MAX_DOWN);
  Dcc.setCV(CV_ACCESSORY_DECODER_ADDRESS_LSB, DEFAULT_ADDRESS & 0xFF);
  Dcc.setCV(CV_ACCESSORY_DECODER_ADDRESS_MSB, (DEFAULT_ADDRESS >> 8) & 0x07);
  initDefaultConfig();
  EEPROM.write(EEPROM_SIG_ADDR, EEPROM_SIG_VALUE);
  EEPROM.put(1, MyConfig);
}

/**
 * Charge les valeurs par des CV
 * récupère les veleurs par défaut si elles n'ont pas encore été initialisées.
 */
void loadCv()
{
  bool configValid = EEPROM.read(EEPROM_SIG_ADDR) == EEPROM_SIG_VALUE;

  if (!configValid) {
    DCCAccessoryConfig storedConfig;
    EEPROM.get(1, storedConfig);
    if (storedConfig.Sig == EEPROM_SIG_VALUE) {
      MyConfig = storedConfig;
      EEPROM.write(EEPROM_SIG_ADDR, EEPROM_SIG_VALUE);
      configValid = true;
    }
  }

  if (!configValid) {
    DEBUG_PRINTLN("EEPROM not initialized, setting default values...");
    initDefaultConfig();
    DEBUG_PRINTLN("Writing default configuration to EEPROM...");
    EEPROM.write(EEPROM_SIG_ADDR, EEPROM_SIG_VALUE);
    EEPROM.put(1, MyConfig);
  }
  else
  {
    DEBUG_PRINTLN("EEPROM already initialized, loading configuration...");
    EEPROM.get(1, MyConfig);
  }

  // Charger les valeurs des CV depuis la structure de configuration
  DEBUG_PRINTLN("  MotorSpeed: " + String(MyConfig.MotorSpeed));
  Dcc.setCV(CV_PALES_SPEED, MyConfig.MotorSpeed);
  DEBUG_PRINTLN("  ServoMaxUp: " + String(MyConfig.ServoMaxUp));
  DEBUG_PRINTLN("  ServoMaxDown: " + String(MyConfig.ServoMaxDown));
  DEBUG_PRINTLN("  Address: " + String(MyConfig.address));
  Dcc.setCV(CV_MAX_UP, MyConfig.ServoMaxUp);
  Dcc.setCV(CV_MAX_DOWN, MyConfig.ServoMaxDown);
  Dcc.setCV(CV_ACCESSORY_DECODER_ADDRESS_LSB, MyConfig.address & 0xFF);
  Dcc.setCV(CV_ACCESSORY_DECODER_ADDRESS_MSB, (MyConfig.address >> 8) & 0x07);
}

/**
 * Charge les valeurs des CV depuis le bus DCC.
 */
void loadCvFromDcc()
{
  PalesSpeed = Dcc.getCV(CV_PALES_SPEED);
  if (PalesSpeed == 0) {
    PalesSpeed = DEFAULT_PALES_SPEED;
  }

  ServoMaxUp = Dcc.getCV(CV_MAX_UP);
  if (ServoMaxUp == 0) {
    ServoMaxUp = DEFAULT_MAX_UP;
  }

  ServoMaxDown = Dcc.getCV(CV_MAX_DOWN);
  if (ServoMaxDown == 0) {
    ServoMaxDown = DEFAULT_MAX_DOWN;
  }

  uint8_t addrLo = Dcc.getCV(CV_ADDRESS_LSB);
  uint8_t addrHi = Dcc.getCV(CV_ADDRESS_MSB) & 0x07;
  uint16_t address = ((uint16_t)addrHi << 8) | addrLo;
  if (address == 0) {
    addrLo = Dcc.getCV(CV_ACCESSORY_DECODER_ADDRESS_LSB);
    addrHi = Dcc.getCV(CV_ACCESSORY_DECODER_ADDRESS_MSB) & 0x07;
    address = ((uint16_t)addrHi << 8) | addrLo;
  }
  if (address == 0) {
    address = DEFAULT_ADDRESS;
  }
  currentAddress = address;

  Dcc.setCV(CV_ACCESSORY_DECODER_ADDRESS_LSB, addrLo);
  Dcc.setCV(CV_ACCESSORY_DECODER_ADDRESS_MSB, addrHi);
  Dcc.setCV(CV_ADDRESS_LSB, addrLo);
  Dcc.setCV(CV_ADDRESS_MSB, addrHi);
}

/**
 * Notifie la sortie d'une voie d'accès DCC.
 * @param Addr L'adresse de la voie d'accès.
 * @param Direction La direction de la voie d'accès.
 * @param OutputPower La puissance de sortie.
 */
void notifyDccAccTurnoutOutput(uint16_t Addr, uint8_t Direction, uint8_t OutputPower)
{
  if (Addr == 1) {
    STROBE_ON = Direction;
  } else if (Addr == 2) {
    analogWrite(PWM_PIN, Direction ? PalesSpeed : 0);
  } else if (Addr == 3) {
    HelicoUP = Direction;
    targetServoPos = Direction ? ServoMaxUp : ServoMaxDown;
  }
}

/**
 * Notifie le changement d'état d'une sortie DCC.
 * @param Addr L'adresse de la sortie.
 * @param State L'état de la sortie.
 */
void notifyDccSigOutputState(uint16_t Addr, uint8_t State)
{
  digitalWrite(LED_PIN, State ? HIGH : LOW);
}

/**
 * Initialise le système.
 */
void setup()
{
  DEBUG_INITIALIZE_SERIAL;
  DEBUG_PRINTLN("DCC Helico system initialized...");
  // Sens des PINs
  pinMode(LED_PIN, OUTPUT);
  pinMode(PWM_PIN, OUTPUT);
  // Configure la pin pour le servo moteur
  servoMotor.attach(SERVO_PIN);
  servoMotor.write(90);

  // Initialise les valeurs de la platine
  STROBE_ON = 1;
  HelicoUP = 0;
  currentServoPos = 0;
  targetServoPos = 180;
  // Attache l'interruption pour le signal DCC
#ifdef digitalPinToInterrupt
  Dcc.pin(DCC_SIGNAL_PIN, 0);
#else
  Dcc.pin(0, DCC_SIGNAL_PIN, 1);
#endif
  // Lire les valeurs des CV depuis l'EEPROM et les charger dans le décodeur DCC
  DEBUG_PRINTLN("Loading CVs from EEPROM...");
  loadCv();
  // Initialise le décodeur DCC en mode accessoire avec filtrage d'adresse sur l'adresse DCC courante.
  Dcc.init(MyConfig.ManufacturerID, MyConfig.Version, FLAGS_MY_ADDRESS_ONLY | FLAGS_DCC_ACCESSORY_DECODER | FLAGS_OUTPUT_ADDRESS_MODE, 0);

  
  // loadCvFromDcc();
}


static unsigned long strobeNextMillis = 0;
static uint8_t strobePhase = 0;

/**
 * Boucle principale.
 */
void loop()
{
  unsigned long now = millis();

  // Traite les signaux DCC entrants
  Dcc.process();

  // Mécanisme de déplacement progressif du servo
  if (now >= servoNextMillis) {
    servoNextMillis = now + VITESSE;
    if (currentServoPos < targetServoPos) {
      currentServoPos++;
      servoMotor.write(currentServoPos);
    } else if (currentServoPos > targetServoPos) {
      currentServoPos--;
      servoMotor.write(currentServoPos);
    }
  }
  // servoMotor.write(90);

  if (STROBE_ON) {
    switch (strobePhase) {
      case 0:
        digitalWrite(LED_PIN, HIGH);
        digitalWrite(LED_BUILTIN, HIGH);
        strobePhase = 1;
        strobeNextMillis = now + 100;
        break;
      case 1:
        if (now >= strobeNextMillis) {
          digitalWrite(LED_PIN, LOW);
          digitalWrite(LED_BUILTIN, LOW);
          strobePhase = 2;
          strobeNextMillis = now + 100;
        }
        break;
      case 2:
        if (now >= strobeNextMillis) {
          digitalWrite(LED_PIN, HIGH);
          digitalWrite(LED_BUILTIN, HIGH);
          strobePhase = 3;
          strobeNextMillis = now + 100;
        }
        break;
      case 3:
        if (now >= strobeNextMillis) {
          digitalWrite(LED_PIN, LOW);
          digitalWrite(LED_BUILTIN, LOW);
          strobePhase = 4;
          strobeNextMillis = now + 1000;
        }
        break;
      case 4:
        if (now >= strobeNextMillis) {
          strobePhase = 0;
        }
        break;
    }
  } else {
    strobePhase = 0;
    digitalWrite(LED_PIN, LOW);
    digitalWrite(LED_BUILTIN, LOW);
  }
}