//
// Console_Node
// Chaegeon-CAN center-console I/O node for Arduino Mega.
//
#include <SPI.h>
#include <mcp_can.h>

// ============================================================
// CAN
// ============================================================
const byte CAN_CS_PIN = 53;
const byte CAN_INT_PIN = 2;
const unsigned long CAN_ID_MAIN_HEARTBEAT = 0x100UL;
const unsigned long CAN_ID_CONSOLE_COMMAND = 0x101UL;
const unsigned long CAN_ID_CONSOLE_STATUS = 0x200UL;
const unsigned long MAIN_HEARTBEAT_TIMEOUT_MS = 2000UL;
const unsigned long STATUS_INTERVAL_MS = 100UL;

// ============================================================
// I/O
// ============================================================
const byte CHANNEL_COUNT = 7;
const byte CH_DRIVER_VENT = 0;
const byte CH_DRIVER_HEAT = 1;
const byte CH_PASSENGER_VENT = 2;
const byte CH_PASSENGER_HEAT = 3;
const byte CH_STEERING_HEAT = 4;
const byte CH_FRONT_SENSOR = 5;
const byte CH_VESS = 6;

// YYNMOS-8 style low-side MOSFET board. Change these two if the board is inverted.
const byte OUTPUT_ACTIVE_LEVEL = HIGH;
const byte OUTPUT_INACTIVE_LEVEL = LOW;
const byte OUTPUT_PINS[CHANNEL_COUNT] = {
  22, 23, 24, 25, 26, 27, 28
};

// Button press detect inputs. PC817 output modules are commonly active-low.
const bool BUTTON_USE_INTERNAL_PULLUP = true;
const byte BUTTON_INPUT_PINS[CHANNEL_COUNT] = {
  30, 31, 32, 33, 34, 35, 36
};
const bool BUTTON_ACTIVE_LOW[CHANNEL_COUNT] = {
  true, true, true, true, true, true, true
};
const unsigned long BUTTON_DEBOUNCE_MS = 40UL;
const unsigned long OWN_PRESS_MASK_MS = 1200UL;

// STATE_LED analog inputs.
// 0..2   driver vent high/mid/low
// 3..5   driver heat high/mid/low
// 6..8   passenger vent high/mid/low
// 9..11  passenger heat high/mid/low
// 12     steering heat
// 13     front sensor
// 14     VESS
const byte LED_COUNT = 15;
const byte LED_PINS[LED_COUNT] = {
  A0, A1, A2, A3, A4, A5, A6, A7,
  A8, A9, A10, A11, A12, A13, A14
};

// 5V ADC reference: 1.8V ~= 368, 3.3V ~= 675.
// Tune each channel after raw measurements. For inverted PC817 outputs, set false.
const int LED_ON_THRESHOLD[LED_COUNT] = {
  300, 300, 300, 300, 300,
  300, 300, 300, 300, 300,
  300, 300, 300, 300, 300
};
const bool LED_ON_WHEN_ABOVE[LED_COUNT] = {
  true, true, true, true, true,
  true, true, true, true, true,
  true, true, true, true, true
};

const byte CLIMATE_COUNT = 4;
const byte TOGGLE_COUNT = 3;
const byte CLIMATE_LED_BASE[CLIMATE_COUNT] = {
  0, 3, 6, 9
};
const byte TOGGLE_LED_INDEX[TOGGLE_COUNT] = {
  12, 13, 14
};

// ============================================================
// Pulse execution
// ============================================================
const byte PULSE_QUEUE_SIZE = 16;
byte pulseQueueChannel[PULSE_QUEUE_SIZE] = {0};
unsigned int pulseQueueOnMs[PULSE_QUEUE_SIZE] = {0};
unsigned int pulseQueueGapMs[PULSE_QUEUE_SIZE] = {0};
byte pulseQueueHead = 0;
byte pulseQueueTail = 0;

bool pulseOnPhase = false;
bool pulseGapPhase = false;
byte activePulseChannel = CHANNEL_COUNT;
unsigned int activePulseOnMs = 0;
unsigned int activePulseGapMs = 0;
unsigned long pulsePhaseStartTime = 0;
unsigned long buttonMaskUntil[CHANNEL_COUNT] = {0};

// ============================================================
// State
// ============================================================
MCP_CAN consoleCan(CAN_CS_PIN);

bool canReady = false;
bool mainAlive = false;
bool mainIgnitionOn = false;
bool mainParkingOn = false;
byte mainHeartbeatSeq = 0;
byte lastAcceptedCommandSeq = 0;
bool commandSeqSeen = false;
bool pulseQueueOverflow = false;
unsigned long lastMainHeartbeatTime = 0;
unsigned long lastStatusTime = 0;

bool lastButtonRaw[CHANNEL_COUNT] = {false};
bool buttonStable[CHANNEL_COUNT] = {false};
unsigned long buttonChangeTime[CHANNEL_COUNT] = {0};

void setup() {
  Serial.begin(115200);
  setupPins();
  setupCan();
  Serial.println(F("Console_Node I/O ready. Send h for help."));
}

void loop() {
  unsigned long currentTime = millis();

  updateCanReceive(currentTime);
  updateMainAlive(currentTime);
  updateButtonInputs(currentTime);
  updatePulseOutput(currentTime);
  updateStatusSend(currentTime);
  handleSerial();
}

void setupPins() {
  for (byte channel = 0; channel < CHANNEL_COUNT; channel++) {
    pinMode(OUTPUT_PINS[channel], OUTPUT);
    digitalWrite(OUTPUT_PINS[channel], OUTPUT_INACTIVE_LEVEL);
  }

  for (byte channel = 0; channel < CHANNEL_COUNT; channel++) {
    pinMode(BUTTON_INPUT_PINS[channel], BUTTON_USE_INTERNAL_PULLUP ? INPUT_PULLUP : INPUT);
    bool pressed = readButtonPressedRaw(channel);
    lastButtonRaw[channel] = pressed;
    buttonStable[channel] = pressed;
    buttonChangeTime[channel] = millis();
  }

  for (byte led = 0; led < LED_COUNT; led++) {
    pinMode(LED_PINS[led], INPUT);
  }
}

void setupCan() {
  pinMode(CAN_INT_PIN, INPUT);
  if (consoleCan.begin(MCP_ANY, CAN_125KBPS, MCP_8MHZ) == CAN_OK) {
    consoleCan.setMode(MCP_NORMAL);
    canReady = true;
  }
}

void updateCanReceive(unsigned long currentTime) {
  if (!canReady) {
    return;
  }

  while (consoleCan.checkReceive() == CAN_MSGAVAIL) {
    unsigned long rxId = 0;
    byte len = 0;
    byte payload[8] = {0};
    if (consoleCan.readMsgBuf(&rxId, &len, payload) != CAN_OK) {
      return;
    }

    if (rxId == CAN_ID_MAIN_HEARTBEAT && len >= 3 && payload[0] == 1) {
      byte flags = payload[1];
      mainIgnitionOn = (flags & 0x01) != 0;
      mainParkingOn = (flags & 0x02) != 0;
      mainHeartbeatSeq = payload[2];
      mainAlive = true;
      lastMainHeartbeatTime = currentTime;
    } else if (rxId == CAN_ID_CONSOLE_COMMAND && len == 8 && payload[0] == 1) {
      processCommand(payload, currentTime);
    }
  }
}

void processCommand(byte payload[8], unsigned long currentTime) {
  byte command = payload[1];
  byte seq = payload[7];

  if (commandSeqSeen && seq == lastAcceptedCommandSeq) {
    return;
  }

  if (command == 1) {
    byte channel = payload[2];
    unsigned int onMs = (unsigned int)payload[3] | ((unsigned int)payload[4] << 8);
    unsigned int gapMs = (unsigned int)payload[5] | ((unsigned int)payload[6] << 8);
    if (enqueuePulse(channel, onMs, gapMs)) {
      lastAcceptedCommandSeq = seq;
      commandSeqSeen = true;
    }
  } else if (command == 2) {
    clearPulseQueue();
    allOutputsOff();
    lastAcceptedCommandSeq = seq;
    commandSeqSeen = true;
  }

  (void)currentTime;
}

void updateMainAlive(unsigned long currentTime) {
  bool aliveNow = lastMainHeartbeatTime != 0 &&
                  currentTime - lastMainHeartbeatTime <= MAIN_HEARTBEAT_TIMEOUT_MS;
  if (!aliveNow && mainAlive) {
    clearPulseQueue();
    allOutputsOff();
  }

  mainAlive = aliveNow;
}

void updateStatusSend(unsigned long currentTime) {
  if (!canReady || currentTime - lastStatusTime < STATUS_INTERVAL_MS) {
    return;
  }
  lastStatusTime = currentTime;

  byte flags = 0;
  if (mainAlive) {
    flags |= 0x01;
  }
  if (isPulseBusy()) {
    flags |= 0x02;
  }
  if (pulseQueueOverflow) {
    flags |= 0x04;
  }
  if (mainIgnitionOn) {
    flags |= 0x08;
  }
  if (mainParkingOn) {
    flags |= 0x10;
  }

  byte payload[8] = {
    1,
    flags,
    packClimateLevels(),
    packToggleStates(),
    packButtonStates(),
    lastAcceptedCommandSeq,
    queueLength(),
    mainHeartbeatSeq
  };

  consoleCan.sendMsgBuf(CAN_ID_CONSOLE_STATUS, 0, 8, payload);
}

byte packClimateLevels() {
  byte packed = 0;
  for (byte climate = 0; climate < CLIMATE_COUNT; climate++) {
    packed |= (readClimateLevel(climate) & 0x03) << (climate * 2);
  }
  return packed;
}

byte packToggleStates() {
  byte packed = 0;
  for (byte toggle = 0; toggle < TOGGLE_COUNT; toggle++) {
    if (readLedOn(TOGGLE_LED_INDEX[toggle])) {
      packed |= (1 << toggle);
    }
  }
  return packed;
}

byte packButtonStates() {
  byte packed = 0;
  unsigned long currentTime = millis();
  for (byte channel = 0; channel < CHANNEL_COUNT; channel++) {
    if (isButtonMasked(channel, currentTime)) {
      continue;
    }
    if (buttonStable[channel]) {
      packed |= (1 << channel);
    }
  }
  return packed;
}

void updateButtonInputs(unsigned long currentTime) {
  for (byte channel = 0; channel < CHANNEL_COUNT; channel++) {
    bool rawPressed = readButtonPressedRaw(channel);
    if (rawPressed != lastButtonRaw[channel]) {
      lastButtonRaw[channel] = rawPressed;
      buttonChangeTime[channel] = currentTime;
    }

    if (currentTime - buttonChangeTime[channel] >= BUTTON_DEBOUNCE_MS) {
      buttonStable[channel] = rawPressed;
    }
  }
}

bool readButtonPressedRaw(byte channel) {
  bool high = digitalRead(BUTTON_INPUT_PINS[channel]) == HIGH;
  if (BUTTON_ACTIVE_LOW[channel]) {
    return !high;
  }
  return high;
}

bool isButtonMasked(byte channel, unsigned long currentTime) {
  return (long)(buttonMaskUntil[channel] - currentTime) > 0;
}

byte readClimateLevel(byte climate) {
  byte base = CLIMATE_LED_BASE[climate];
  if (readLedOn(base)) {
    return 3;
  }
  if (readLedOn(base + 1)) {
    return 2;
  }
  if (readLedOn(base + 2)) {
    return 1;
  }
  return 0;
}

bool readLedOn(byte ledIndex) {
  int raw = analogRead(LED_PINS[ledIndex]);
  bool above = raw >= LED_ON_THRESHOLD[ledIndex];
  return LED_ON_WHEN_ABOVE[ledIndex] ? above : !above;
}

int readLedRaw(byte ledIndex) {
  return analogRead(LED_PINS[ledIndex]);
}

bool enqueuePulse(byte channel, unsigned int onMs, unsigned int gapMs) {
  byte nextTail = nextQueueIndex(pulseQueueTail);
  if (channel >= CHANNEL_COUNT || nextTail == pulseQueueHead) {
    pulseQueueOverflow = true;
    return false;
  }

  pulseQueueChannel[pulseQueueTail] = channel;
  pulseQueueOnMs[pulseQueueTail] = onMs;
  pulseQueueGapMs[pulseQueueTail] = gapMs;
  pulseQueueTail = nextTail;

  unsigned long maskUntil = millis() + onMs + gapMs + OWN_PRESS_MASK_MS;
  if ((long)(maskUntil - buttonMaskUntil[channel]) > 0) {
    buttonMaskUntil[channel] = maskUntil;
  }

  return true;
}

void updatePulseOutput(unsigned long currentTime) {
  if (!pulseOnPhase && !pulseGapPhase) {
    startNextPulse(currentTime);
    return;
  }

  if (pulseOnPhase && currentTime - pulsePhaseStartTime >= activePulseOnMs) {
    setOutput(activePulseChannel, false);
    pulseOnPhase = false;
    pulseGapPhase = true;
    pulsePhaseStartTime = currentTime;
    return;
  }

  if (pulseGapPhase && currentTime - pulsePhaseStartTime >= activePulseGapMs) {
    pulseGapPhase = false;
    activePulseChannel = CHANNEL_COUNT;
    startNextPulse(currentTime);
  }
}

void startNextPulse(unsigned long currentTime) {
  if (pulseQueueHead == pulseQueueTail) {
    return;
  }

  activePulseChannel = pulseQueueChannel[pulseQueueHead];
  activePulseOnMs = pulseQueueOnMs[pulseQueueHead];
  activePulseGapMs = pulseQueueGapMs[pulseQueueHead];
  pulseQueueHead = nextQueueIndex(pulseQueueHead);

  setOutput(activePulseChannel, true);
  pulseOnPhase = true;
  pulsePhaseStartTime = currentTime;
}

void setOutput(byte channel, bool active) {
  if (channel >= CHANNEL_COUNT) {
    return;
  }
  digitalWrite(OUTPUT_PINS[channel], active ? OUTPUT_ACTIVE_LEVEL : OUTPUT_INACTIVE_LEVEL);
}

void allOutputsOff() {
  for (byte channel = 0; channel < CHANNEL_COUNT; channel++) {
    setOutput(channel, false);
  }
  pulseOnPhase = false;
  pulseGapPhase = false;
  activePulseChannel = CHANNEL_COUNT;
}

void clearPulseQueue() {
  pulseQueueHead = pulseQueueTail;
}

bool isPulseBusy() {
  return pulseOnPhase || pulseGapPhase || pulseQueueHead != pulseQueueTail;
}

byte queueLength() {
  if (pulseQueueTail >= pulseQueueHead) {
    return pulseQueueTail - pulseQueueHead;
  }
  return PULSE_QUEUE_SIZE - pulseQueueHead + pulseQueueTail;
}

byte nextQueueIndex(byte index) {
  index++;
  if (index >= PULSE_QUEUE_SIZE) {
    index = 0;
  }
  return index;
}

void handleSerial() {
  while (Serial.available()) {
    char command = Serial.read();
    if (command == '\r' || command == '\n') {
      continue;
    }

    if (command == 'h') {
      printHelp();
    } else if (command == 's') {
      printStatus();
    } else if (command == 'x') {
      clearPulseQueue();
      allOutputsOff();
      Serial.println(F("ACK: outputs off"));
    } else {
      Serial.println(F("ERR: h/s/x only"));
    }
  }
}

void printHelp() {
  Serial.println();
  Serial.println(F("+------------------------------------------------------+"));
  Serial.println(F("| Console_Node I/O                                     |"));
  Serial.println(F("+------------------------------------------------------+"));
  Serial.println(F("| h = help                                             |"));
  Serial.println(F("| s = status and raw LED values                         |"));
  Serial.println(F("| x = stop outputs and clear pulse queue                |"));
  Serial.println(F("| Memory and automation live in Auto_Seat Nano.         |"));
  Serial.println(F("+------------------------------------------------------+"));
}

void printStatus() {
  Serial.println();
  Serial.println(F("[CONSOLE_IO_STATUS]"));
  printNamedBool(F("canReady"), canReady);
  printNamedBool(F("mainAlive"), mainAlive);
  printNamedBool(F("mainIgnitionOn"), mainIgnitionOn);
  printNamedBool(F("mainParkingOn"), mainParkingOn);
  printNamedBool(F("pulseBusy"), isPulseBusy());
  printNamedBool(F("pulseQueueOverflow"), pulseQueueOverflow);
  printNamedInt(F("queueLength"), queueLength());
  printNamedInt(F("lastAcceptedCommandSeq"), lastAcceptedCommandSeq);
  printNamedInt(F("driverVentLevel"), readClimateLevel(CH_DRIVER_VENT));
  printNamedInt(F("driverHeatLevel"), readClimateLevel(CH_DRIVER_HEAT));
  printNamedInt(F("passengerVentLevel"), readClimateLevel(CH_PASSENGER_VENT));
  printNamedInt(F("passengerHeatLevel"), readClimateLevel(CH_PASSENGER_HEAT));
  printNamedBool(F("steeringHeatOn"), readLedOn(12));
  printNamedBool(F("frontSensorOn"), readLedOn(13));
  printNamedBool(F("vessOn"), readLedOn(14));
  for (byte led = 0; led < LED_COUNT; led++) {
    Serial.print(F("ledRaw"));
    Serial.print(led);
    Serial.print(F("="));
    Serial.println(readLedRaw(led));
  }
}

void printNamedBool(const __FlashStringHelper *name, bool value) {
  Serial.print(name);
  Serial.print(F("="));
  Serial.println(value ? F("true") : F("false"));
}

void printNamedInt(const __FlashStringHelper *name, long value) {
  Serial.print(name);
  Serial.print(F("="));
  Serial.println(value);
}