#include <Arduino.h>
#include <SPI.h>
#include <SD.h>
#include <DCC_Decoder.h>

extern DCC_Decoder DCC;

#ifndef kDCC_INTERRUPT
  #define kDCC_INTERRUPT 0
#endif

// ===================== Pins =====================
const uint8_t PIN_595_DATA  = 3;  // SER
const uint8_t PIN_595_CLK   = 4;  // SRCLK
const uint8_t PIN_595_LATCH = 5;  // RCLK (STCP)

const uint8_t PIN_SD_CS = 10;

const uint8_t DIP_BASE_PINS_0_9[10] = {6, 7, 8, 9, A0, A1, A2, A3, A4, A5};
const uint8_t DIP_BASE_PIN_10 = A6;

// ===================== Konfiguration =====================
const uint8_t  NUM_SIGNALS = 10;
const uint8_t  MAX_IMAGES  = 7;
const uint16_t BLINK_PERIOD_MS = 500;

// A6 robust læsning
const int A6_ON_TH  = 200;
const int A6_OFF_TH = 800;
const uint8_t A6_SAMPLES = 8;

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
// VIGTIGE TOGLES (prøv disse hvis du ser "løbende tænding")
//
// 1) Hvis LED er koblet til +5V og 595 "synker" strøm (typisk), er de active-low:
//    INVERT_595_OUTPUTS = true
// 2) Hvis latch er inverteret / forbundet forkert polaritet, så sæt INVERT_LATCH = true
//
// Start med:
//   INVERT_595_OUTPUTS = false
//   INVERT_LATCH       = false
// Hvis alt stadig tænder "løbende", så prøv INVERT_LATCH=true.
// Hvis "alt er tændt" forkert, så skift INVERT_595_OUTPUTS.
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
const bool INVERT_595_OUTPUTS = false;   // false=1 tænder fysisk, true=0 tænder fysisk
const bool INVERT_LATCH       = true;   // false=LOW under shift, HIGH latch. true=omvendt
// =========================================================

// ===================== Data =====================
uint16_t baseAddr = 0;

uint8_t  currentImage[NUM_SIGNALS];
uint8_t  signalProfile[NUM_SIGNALS];
uint8_t  imagesPerSignal[NUM_SIGNALS];

uint16_t startAddr[NUM_SIGNALS];
uint16_t totalAddresses = 0;

bool     blinkOn = false;
uint32_t lastBlinkMs = 0;

struct ImageDef { uint8_t onMask; uint8_t blinkMask; };
ImageDef profiles[8][MAX_IMAGES];

const uint8_t PROFILE_ACTIVE_MASK[8] = {
  0x00, 0x0C, 0x0E, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F
};

uint8_t lastOut595[5] = {0,0,0,0,0};

// ===================== 74HC595 low-level =====================
static inline void pulseClock() {
  digitalWrite(PIN_595_CLK, HIGH);
  digitalWrite(PIN_595_CLK, LOW);
}

static inline void latchBeginShift() {
  // Normal: LOW under shift. Inverteret: HIGH under shift.
  digitalWrite(PIN_595_LATCH, INVERT_LATCH ? HIGH : LOW);
}

static inline void latchCommit() {
  // Normal: pulse HIGH (latch). Inverteret: pulse LOW (latch).
  if (!INVERT_LATCH) {
    digitalWrite(PIN_595_LATCH, HIGH);
    delayMicroseconds(5);
    digitalWrite(PIN_595_LATCH, LOW);
  } else {
    digitalWrite(PIN_595_LATCH, LOW);
    delayMicroseconds(5);
    digitalWrite(PIN_595_LATCH, HIGH);
  }
}

void write595_5bytes(const uint8_t b[5]) {
  memcpy(lastOut595, b, 5);

  latchBeginShift();

  // b[4] længst væk, b[0] tættest på Arduino
  for (int i = 4; i >= 0; i--) {
    uint8_t v = INVERT_595_OUTPUTS ? (uint8_t)~b[i] : b[i];
    for (uint8_t m = 0x80; m; m >>= 1) {
      digitalWrite(PIN_595_DATA, (v & m) ? HIGH : LOW);
      pulseClock();
    }
  }

  latchCommit();
}

// ===================== Robust A6 DIP bit =====================
int readA6Avg() {
  long sum = 0;
  for (uint8_t i = 0; i < A6_SAMPLES; i++) {
    sum += analogRead(DIP_BASE_PIN_10);
    delay(2);
  }
  return (int)(sum / A6_SAMPLES);
}

uint8_t readA6DipBitStable(int *outAvg = nullptr) {
  int a6 = readA6Avg();
  if (outAvg) *outAvg = a6;

  static uint8_t last = 0;
  if (a6 < A6_ON_TH)       last = 1;
  else if (a6 > A6_OFF_TH) last = 0;
  return last;
}

uint16_t readBaseAddr_Dip11(int *outA6Avg = nullptr) {
  uint16_t v = 0;

  for (uint8_t i = 0; i < 10; i++) {
    uint8_t bit = (digitalRead(DIP_BASE_PINS_0_9[i]) == LOW) ? 1 : 0;
    v |= (uint16_t(bit) << i);
  }

  uint8_t b10 = readA6DipBitStable(outA6Avg);
  v |= (uint16_t(b10) << 10);

  if (v > 2047) v = 2047;
  return v;
}

// ===================== SD parsing helpers =====================
String trimLine(const String &s) {
  int a = 0, b = s.length() - 1;
  while (a <= b && isspace((unsigned char)s[a])) a++;
  while (b >= a && isspace((unsigned char)s[b])) b--;
  if (b < a) return "";
  return s.substring(a, b + 1);
}
bool isCommentOrEmpty(const String &line) {
  if (line.length() == 0) return true;
  char c = line[0];
  return (c == '#' || c == ';');
}
bool parseSlotToken(const String &tok, uint8_t slotBit, uint8_t &onMask, uint8_t &blinkMask) {
  if (tok.length() == 0) return false;
  char c = tok[0];
  if (c == '0') return true;
  if (c == '1') { onMask |= (1 << slotBit); return true; }
  if (c == 'y' || c == 'Y') { blinkMask |= (1 << slotBit); return true; }
  return false;
}

// ===================== Defaults (fallback) =====================
void loadDefaultProfilesAndSignals() {
  for (uint8_t s = 0; s < NUM_SIGNALS; s++) {
    signalProfile[s]   = 3;
    imagesPerSignal[s] = 7;
  }

  for (uint8_t p = 0; p < 8; p++) {
    for (uint8_t i = 0; i < MAX_IMAGES; i++) profiles[p][i] = {0,0};
  }

  for (uint8_t i = 0; i < MAX_IMAGES; i++) profiles[0][i] = {0x00, 0x00};

  ImageDef def[MAX_IMAGES] = {
    {0x04,0x00},
    {0x08,0x00},
    {0x02,0x00},
    {0x06,0x00},
    {0x04,0x02},
    {0x08,0x04},
    {0x01,0x00},
  };

  for (uint8_t p = 1; p < 8; p++) {
    for (uint8_t i = 0; i < MAX_IMAGES; i++) profiles[p][i] = def[i];
  }
}

bool loadProfilesFromSD() {
  File f = SD.open("profiles.txt", FILE_READ);
  if (!f) return false;

  int currentP = -1;
  int imgIndex = 0;

  while (f.available()) {
    String line = trimLine(f.readStringUntil('\n'));
    if (isCommentOrEmpty(line)) continue;

    if (line.length() >= 2 && (line[0] == 'P' || line[0] == 'p')) {
      int p = line.substring(1).toInt();
      if (p < 0 || p > 7) { currentP = -1; continue; }
      currentP = p;
      imgIndex = 0;
      continue;
    }

    if (currentP < 0) continue;
    if (imgIndex >= MAX_IMAGES) continue;

    uint8_t onMask = 0, blinkMask = 0;
    int pos = 0, slot = 0;
    bool ok = true;

    while (slot < 4) {
      while (pos < (int)line.length() && isspace((unsigned char)line[pos])) pos++;
      if (pos >= (int)line.length()) break;

      int start = pos;
      while (pos < (int)line.length() && !isspace((unsigned char)line[pos])) pos++;
      String tok = line.substring(start, pos);

      if (!parseSlotToken(tok, slot, onMask, blinkMask)) { ok = false; break; }
      slot++;
    }

    if (ok) {
      onMask &= ~blinkMask;
      profiles[currentP][imgIndex].onMask = onMask;
      profiles[currentP][imgIndex].blinkMask = blinkMask;
    }
    imgIndex++;
  }

  f.close();
  return true;
}

bool loadSignalsFromSD() {
  File f = SD.open("signals.txt", FILE_READ);
  if (!f) return false;

  uint8_t s = 0;
  while (f.available() && s < NUM_SIGNALS) {
    String line = trimLine(f.readStringUntil('\n'));
    if (isCommentOrEmpty(line)) continue;

    int sp = line.indexOf(' ');
    int tab = line.indexOf('\t');
    int sep = (sp >= 0) ? sp : tab;

    int p = 3, n = 7;
    if (sep < 0) { p = line.toInt(); n = 7; }
    else { p = line.substring(0, sep).toInt(); n = line.substring(sep + 1).toInt(); }

    if (p < 0) p = 0; if (p > 7) p = 7;
    if (n < 1) n = 1; if (n > MAX_IMAGES) n = MAX_IMAGES;

    signalProfile[s] = (uint8_t)p;
    imagesPerSignal[s] = (uint8_t)n;
    if (currentImage[s] >= imagesPerSignal[s]) currentImage[s] = 0;

    s++;
  }

  f.close();

  while (s < NUM_SIGNALS) {
    signalProfile[s] = 3;
    imagesPerSignal[s] = 7;
    if (currentImage[s] >= imagesPerSignal[s]) currentImage[s] = 0;
    s++;
  }
  return true;
}

// ===================== Adresse-map =====================
void buildAddressMap() {
  uint16_t a = baseAddr;
  totalAddresses = 0;
  for (uint8_t s = 0; s < NUM_SIGNALS; s++) {
    startAddr[s] = a;
    totalAddresses += imagesPerSignal[s];
    a += imagesPerSignal[s];
  }
}

// ===================== Outputs =====================
void updateAllOutputs() {
  uint8_t out[5] = {0,0,0,0,0}; // logisk: 1=tænd

  for (uint8_t s = 0; s < NUM_SIGNALS; s++) {
    uint8_t p = signalProfile[s] & 0x07;
    uint8_t img = currentImage[s];
    if (img >= imagesPerSignal[s]) img = 0;
    if (img >= MAX_IMAGES) img = 0;

    uint8_t activeMask = PROFILE_ACTIVE_MASK[p];
    uint8_t onMask    = profiles[p][img].onMask & activeMask;
    uint8_t blinkMask = profiles[p][img].blinkMask & activeMask;

    uint8_t finalSlots = (onMask & ~blinkMask);
    if (blinkOn) finalSlots |= blinkMask;

    uint8_t base = s * 4;
    for (uint8_t slot = 0; slot < 4; slot++) {
      if ((finalSlots >> slot) & 0x01) {
        uint8_t outIndex = base + slot; // 0..39
        out[outIndex / 8] |= (1 << (outIndex % 8));
      }
    }
  }

  write595_5bytes(out);
}

// ===================== TEST helpers =====================
void forceAllOn()  { uint8_t o[5]={0xFF,0xFF,0xFF,0xFF,0xFF}; write595_5bytes(o); Serial.println(F("TEST: Alle 40 outputs = ON (logisk)")); }
void forceAllOff() { uint8_t o[5]={0,0,0,0,0};                write595_5bytes(o); Serial.println(F("TEST: Alle 40 outputs = OFF (logisk)")); }

void dumpOut595() {
  Serial.print(F("OUT595(logisk) = "));
  for (int i=4;i>=0;i--) {
    if (lastOut595[i] < 16) Serial.print('0');
    Serial.print(lastOut595[i], HEX);
    if (i) Serial.print(' ');
  }
  Serial.println();
  Serial.print(F("INVERT_595_OUTPUTS=")); Serial.println(INVERT_595_OUTPUTS ? F("true") : F("false"));
  Serial.print(F("INVERT_LATCH="));       Serial.println(INVERT_LATCH ? F("true") : F("false"));
}

// Latch-test: hvis latch virker korrekt, skifter det “momentant” mellem alt ON/OFF (ikke løbende)
void latchTest() {
  Serial.println(F("Latch-test: OFF -> ON -> OFF"));
  forceAllOff();
  delay(500);
  forceAllOn();
  delay(500);
  forceAllOff();
}

// ===================== Address mapping =====================
bool mapAddressToSignalAndImage(int address, uint8_t &sig, uint8_t &img) {
  if (address < (int)baseAddr) return false;
  uint16_t offset = (uint16_t)(address - (int)baseAddr);
  if (offset >= totalAddresses) return false;

  for (uint8_t s = 0; s < NUM_SIGNALS; s++) {
    uint16_t start = startAddr[s] - baseAddr;
    uint16_t end   = start + imagesPerSignal[s];
    if (offset >= start && offset < end) {
      sig = s;
      img = (uint8_t)(offset - start);
      return true;
    }
  }
  return false;
}

void triggerAddress(int address) {
  uint8_t s, img;
  if (!mapAddressToSignalAndImage(address, s, img)) {
    Serial.print(F("SIM addr=")); Serial.print(address);
    Serial.print(F(" -> UDENFOR blok (base=")); Serial.print(baseAddr);
    Serial.print(F(", total=")); Serial.print(totalAddresses);
    Serial.println(F(")"));
    return;
  }

  currentImage[s] = img;
  updateAllOutputs();

  Serial.print(F("SIM/DCC addr=")); Serial.print(address);
  Serial.print(F(" -> Signal ")); Serial.print(s+1);
  Serial.print(F(" = Billede ")); Serial.println(img+1);
}

void BasicAccDecoderPacket_Handler(int address, boolean activate, byte data) {
  (void)data;
  if (!activate) return;
  triggerAddress(address);
}

// ===================== Serial commands =====================
void printConfig() {
  Serial.println();
  Serial.print(F("BaseAddr = ")); Serial.println(baseAddr);
  Serial.println(F("Signal -> profil, N og adresser:"));
  for (uint8_t s = 0; s < NUM_SIGNALS; s++) {
    Serial.print(F("  Signal ")); Serial.print(s+1);
    Serial.print(F(": profil ")); Serial.print(signalProfile[s]);
    Serial.print(F(", N=")); Serial.print(imagesPerSignal[s]);
    Serial.print(F(" -> "));
    for (uint8_t i=0;i<imagesPerSignal[s];i++){
      Serial.print(startAddr[s]+i);
      if (i<imagesPerSignal[s]-1) Serial.print(',');
    }
    Serial.println();
  }
  Serial.print(F("Total adresser i blok = ")); Serial.println(totalAddresses);
}

void serialHelp() {
  Serial.println(F("=== Serial ==="));
  Serial.println(F("H help, P print, D read DIP"));
  Serial.println(F("A<addr> trigger (fx A1) / <addr> (fx 15)"));
  Serial.println(F("T<n> next image signal n (1..10), R reset image1"));
  Serial.println(F("G builtin testprofile, U dump out, L latch-test"));
  Serial.println(F("X all ON, Z all OFF"));
}

void resetAllSignalsToImage1() {
  for (uint8_t s=0;s<NUM_SIGNALS;s++) currentImage[s]=0;
  updateAllOutputs();
  Serial.println(F("Reset: alle signaler -> billede 1"));
}

void nextImageOnSignal(uint8_t sig1based) {
  if (sig1based < 1 || sig1based > NUM_SIGNALS) { Serial.println(F("Brug: T<n> hvor n=1..10")); return; }
  uint8_t s = sig1based - 1;
  uint8_t n = imagesPerSignal[s];
  if (n < 1) n = 1;
  currentImage[s] = (currentImage[s] + 1) % n;
  updateAllOutputs();
  Serial.print(F("Signal ")); Serial.print(sig1based);
  Serial.print(F(" -> billede ")); Serial.println(currentImage[s]+1);
}

// Indbygget testprofil (Signal1: slot0/slot1)
void applyBuiltInTestProfile() {
  for (uint8_t s=0;s<NUM_SIGNALS;s++){
    signalProfile[s]=0;
    imagesPerSignal[s]=1;
    currentImage[s]=0;
  }

  for (uint8_t i=0;i<MAX_IMAGES;i++) profiles[7][i] = {0,0};
  profiles[7][0] = {0x01, 0x00};
  profiles[7][1] = {0x02, 0x00};

  signalProfile[0]=7;
  imagesPerSignal[0]=2;
  currentImage[0]=0;

  buildAddressMap();
  updateAllOutputs();

  Serial.println(F("G: TESTPROFIL aktiv. Brug T1 for at skifte slot0/slot1."));
}

void pollSerial() {
  static char line[48];
  static uint8_t idx=0;

  while (Serial.available()) {
    char c=(char)Serial.read();
    if (c=='\r') continue;

    if (c=='\n') {
      line[idx]='\0'; idx=0;
      char *p=line; while(*p==' '||*p=='\t') p++;
      if (*p=='\0') return;

      if (p[1]=='\0') {
        char cmd=(char)toupper(*p);
        if (cmd=='H'){ serialHelp(); return; }
        if (cmd=='P'){ printConfig(); return; }
        if (cmd=='D'){
          int a6avg=0;
          baseAddr = readBaseAddr_Dip11(&a6avg);
          buildAddressMap();
          Serial.print(F("BaseAddr=")); Serial.println(baseAddr);
          return;
        }
        if (cmd=='X'){ forceAllOn(); return; }
        if (cmd=='Z'){ forceAllOff(); return; }
        if (cmd=='U'){ dumpOut595(); return; }
        if (cmd=='L'){ latchTest(); return; }
        if (cmd=='R'){ resetAllSignalsToImage1(); return; }
        if (cmd=='G'){ applyBuiltInTestProfile(); return; }
      }

      if (*p=='T'||*p=='t'){ int n=atoi(p+1); nextImageOnSignal((uint8_t)n); return; }

      if (*p=='A'||*p=='a') p++;
      int addr=atoi(p);
      triggerAddress(addr);
      return;
    }

    if (idx < sizeof(line)-1) line[idx++]=c;
    else idx=0;
  }
}

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

  pinMode(PIN_595_DATA, OUTPUT);
  pinMode(PIN_595_CLK, OUTPUT);
  pinMode(PIN_595_LATCH, OUTPUT);
  digitalWrite(PIN_595_DATA, LOW);
  digitalWrite(PIN_595_CLK, LOW);
  digitalWrite(PIN_595_LATCH, INVERT_LATCH ? HIGH : LOW);

  for (uint8_t i=0;i<10;i++) pinMode(DIP_BASE_PINS_0_9[i], INPUT_PULLUP);

  for (uint8_t s=0;s<NUM_SIGNALS;s++) currentImage[s]=0;
  loadDefaultProfilesAndSignals();

  delay(10);
  baseAddr = readBaseAddr_Dip11(nullptr);

  bool sdOk = SD.begin(PIN_SD_CS);
  if (!sdOk) {
    Serial.println(F("SD: IKKE fundet – bruger DEFAULT profiler/signals"));
  } else {
    bool pOk = loadProfilesFromSD();
    bool sOk = loadSignalsFromSD();
    Serial.print(F("SD: profiles.txt=")); Serial.print(pOk?F("OK"):F("fejl"));
    Serial.print(F(", signals.txt=")); Serial.println(sOk?F("OK"):F("fejl"));
  }

  buildAddressMap();
  updateAllOutputs();

  DCC.SetBasicAccessoryDecoderPacketHandler(BasicAccDecoderPacket_Handler, true);
  DCC.SetupDecoder(0x00, 0x00, kDCC_INTERRUPT);

  serialHelp();
  printConfig();
}

void loop() {
  DCC.loop();
  pollSerial();

  uint32_t now = millis();
  if (now - lastBlinkMs >= BLINK_PERIOD_MS) {
    lastBlinkMs = now;
    blinkOn = !blinkOn;
    updateAllOutputs();
  }
}