/* Styring af remiseporte
Styres af DCC magnetartikel kommandoer:
 DCCADR[0] rød luk port 1
 DCCADR[0] grøn åbn port 1
 DCCADR[1] rød luk port 2
 DCCADR[1] grøn åbn port 2
 DCCADR[2] rød luk port 3
 DCCADR[2] grøn åbn port 3
 LysDDCADR rød sluk lys i remise
 LysDDCADR grøn tænd lys i remise
*/
#include <NmraDcc.h>
#include <Servo.h>

NmraDcc Dcc;
DCC_MSG Packet;
Servo PortServo[3];

#define DCC_PIN 3  // Define the Arduino input Pin number for the DCC Signal
const int
  PortPin[] = { 8, 9, 10 },
  DCCAdr[] = { 241, 242, 243 },
  LysDCCAdr = 244,
  LysPin = 2,
  PortOp[] = {1300,1300,1300 ],
  PortLuk[] = {1800,1800,1800 },
  DccAckPin = 4;
unsigned long lastmove;

struct CVPair {
  uint16_t CV;
  uint8_t Value;
};
struct PortStatus {
  int target;
  int vinkel;
};
PortStatus Port[3];

CVPair FactoryDefaultCVs[] = {
  { CV_ACCESSORY_DECODER_ADDRESS_LSB, DEFAULT_ACCESSORY_DECODER_ADDRESS & 0xFF },
  { CV_ACCESSORY_DECODER_ADDRESS_MSB, DEFAULT_ACCESSORY_DECODER_ADDRESS >> 8 },
};

uint8_t FactoryDefaultCVIndex = 0;

void notifyCVResetFactoryDefault() {
  // Make FactoryDefaultCVIndex non-zero and equal to num CV's to be reset
  // to flag to the loop() function that a reset to Factory Defaults needs to be done
  FactoryDefaultCVIndex = sizeof(FactoryDefaultCVs) / sizeof(CVPair);
}
void notifyCVAck(void) {
  // This function is called by the NmraDcc library when a DCC ACK needs to be sent
  // Calling this function should cause an increased 60ma current drain on the power supply for 6ms to ACK a CV Read
  Serial.println("notifyCVAck");
  digitalWrite(DccAckPin, HIGH);
  delay(6);
  digitalWrite(DccAckPin, LOW);
}
// #define NOTIFY_DCC_MSG// Uncomment to print all DCC Packets
#ifdef NOTIFY_DCC_MSG
void notifyDccMsg(DCC_MSG* Msg) {
  Serial.print("notifyDccMsg: ");
  for (uint8_t i = 0; i < Msg->Size; i++) {
    Serial.print(Msg->Data[i], HEX);
    Serial.write(' ');
  }
  Serial.println();
}
#endif

void notifyDccAccTurnoutOutput(uint16_t Addr, uint8_t Direction, uint8_t OutputPower) {
  // This function is called whenever a normal DCC Turnout Packet is received and we're in Output Addressing Mode
  // Her behandles DCC kommandoerne til at styre Porte og belysning
  Serial.print("notifyDccAccTurnoutOutput: ");
  Serial.print(Addr, DEC);
  Serial.print(',');
  Serial.print(Direction, DEC);
  Serial.print(',');
  Serial.println(OutputPower, HEX);

  if (OutputPower > 0) {
    for (int i = 0; i < 3; i++) {
      if (Addr == DCCAdr[i]) {
        if (Direction == 0) Port[i].target = PortLuk[i];
        else Port[i].target = PortOp[i];
      }
    }
  }
  if (Addr == LysDCCAdr) {
    if (Direction == 0) digitalWrite(LysPin, LOW);
    else digitalWrite(LysPin, HIGH);
  }
}  // notifyDccAccTurnoutOutput

void setup() {
  Serial.begin(115200);
  uint8_t maxWaitLoops = 255;
  while (!Serial && maxWaitLoops--) delay(20);

  // Configure the DCC CV Programing ACK pin for an output
  pinMode(DccAckPin, OUTPUT);
  Serial.println("NMRA DCC remisestyring");

  Dcc.pin(digitalPinToInterrupt(DCC_PIN), DCC_PIN, 1);
  // Call the main DCC Init function to enable the DCC Receiver
  Dcc.init(MAN_ID_DIY, 10, CV29_ACCESSORY_DECODER | CV29_OUTPUT_ADDRESS_MODE, 0);
  Serial.println("Init Done");

  for (int i = 0; i < 3; i++) {
    PortServo[i].attach(PortPin[i], 1000, 2000);
    Port[i].target = 1500;
    Port[i].vinkel = Port[i].target;
    PortServo[i].writeMicroseconds(Port[i].vinkel);
  }
  pinMode(LysPin, OUTPUT);

}  // setup

void loop() {
  //  NmraDcc.process() skal kaldes jævnligt i Arduino loop() for at det virker stabilt
  Dcc.process();
  if (FactoryDefaultCVIndex && Dcc.isSetCVReady()) {
    FactoryDefaultCVIndex--;  // Decrement first as initially it is the size of the array
    Dcc.setCV(FactoryDefaultCVs[FactoryDefaultCVIndex].CV, FactoryDefaultCVs[FactoryDefaultCVIndex].Value);
  }
  for (int i = 0; i < 3; i++) {
    if (Port[i].vinkel != Port[i].target) {
      if (Port[i].vinkel < Port[i].target) Port[i].vinkel++;
      if (Port[i].vinkel > Port[i].target) Port[i].vinkel--;
      PortServo[i].writeMicroseconds(Port[i].vinkel);
      Serial.print("Port ");
      Serial.print(i + 1);
      Serial.print(" vinkel ");
      Serial.println(Port[i].vinkel);
    }
  }
}  // loop