// ==================================================
// Ver V5.0  (diagnostic LED enabled)
// ==================================================



// --------------------------------------------------
// S1 / S2 – VOĽBA NAPÄŤOVÉHO PRAHU (12 V systém)
//
//  S1     S2     Napäťový prah
// --------------------------------
//  HIGH   HIGH   12.2 V
//  LOW    HIGH   12.4 V
//  HIGH   LOW    12.6 V
//  LOW    LOW    12.7 V
//
// Poznámka:
// - Pri detekcii 24 V systému sa prah násobí ×2
//
// --------------------------------------------------
// S3 / S4 – VOĽBA PARKOVACIEHO ČASU
//
//  S3     S4     Parkovací čas
// --------------------------------
//  LOW    LOW     5 minút
//  HIGH   LOW     1 hodina
//  LOW    HIGH    3 hodiny
//  HIGH   HIGH    24 hodín 
// ==================================================

// ==================================================
// NAPÄŤOVÉ PRAHY (12 V systém)
// ==================================================
const float VOLTAGE_1 = 12.2f;
const float VOLTAGE_2 = 12.4f;
const float VOLTAGE_3 = 12.6f;
const float VOLTAGE_4 = 12.7f;

float voltageThreshold = VOLTAGE_1;

// ==================================================
// DETEKCIA 24 V SYSTÉMU
// ==================================================
const float V_24V_DETECT = 20.0f;
const int   V_SYS_SAMPLES = 5;

// ==================================================
// MERANIE NAPÄTIA BATÉRIE
// ==================================================
const float VREF = 2.495f;
const float RTOP = 24000.0f;
const float RBOT = 4700.0f;

const float K = VREF * (RTOP + RBOT) / RBOT;

const float CAL_A = 1.0669134f;
const float CAL_B = -1.8181848f;


// ==================================================
// ČASOVÉ KONŠTANTY
// ==================================================
const unsigned long T_LOW_VOLTAGE = 120000UL;

const int WTIME_5MIN = 5;
const int WTIME_1H   = 60;
const int WTIME_3H   = 180;
const int WTIME_24H  = 1440;

const unsigned long T_PM_BLOCK_AFTER_START = 60000UL;
const unsigned long T_PM_MIN_DWELL         = 30000UL;
const unsigned long T_NO_CONV_OFF_AFTER_PM = 30000UL;


// ==================================================
// PINY
// ==================================================
const int PIN_VYPPIN = 4;
const int PIN_ACC    = 2;
const int PIN_PM     = 10;
const int PIN_LED    = 7;

const int PIN_S1 = 9;
const int PIN_S2 = 8;
const int PIN_S3 = 6;
const int PIN_S4 = 5;

const int PIN_BAT  = A2;
const int PIN_VREF = A4;


// ==================================================
// STAV ACC
// ==================================================
enum AccState {
  ACC_HIGH,
  ACC_LOW
};


// ==================================================
// STAV PM
// ==================================================
enum PmState {
  PM_HIGH_NORMAL,
  PM_LOW_PARK
};


// ==================================================
// PREMENNÉ STAVU
// ==================================================
float boardVoltage = 0.0f;

bool uvActive = false;
unsigned long uvStartMs = 0;

unsigned long parkingStartMs = 0;
unsigned long wtimeMs = 0;

AccState lastAccState = ACC_HIGH;

PmState pmState = PM_HIGH_NORMAL;
unsigned long pmLastChangeMs = 0;
unsigned long camStartMs = 0;
unsigned long convOffInhibitUntil = 0;


// ==================================================
// POMOCNÁ FUNKCIA: medián z 5 hodnôt
// ==================================================
static uint16_t median5(uint16_t v0, uint16_t v1, uint16_t v2,
                        uint16_t v3, uint16_t v4)
{
  uint16_t a[5] = { v0, v1, v2, v3, v4 };

  for (uint8_t i = 0; i < 5; i++) {
    for (uint8_t j = i + 1; j < 5; j++) {
      if (a[j] < a[i]) {
        uint16_t t = a[i];
        a[i] = a[j];
        a[j] = t;
      }
    }
  }
  return a[2];
}


// ==================================================
// FUNKCIA: Meranie napätia batérie (kalibrované)
// ==================================================
static float readBatteryVoltageCalibrated()
{
  const int N = 1000;
  const int G = 5;

  uint16_t bufBat[G];
  uint16_t bufRef[G];
  uint8_t idx = 0;

  unsigned long sumMedBat = 0;
  unsigned long sumMedRef = 0;
  int medCount = 0;

  for (int i = 0; i < N; i++) {
    analogRead(PIN_VREF);
    uint16_t ref = analogRead(PIN_VREF);

    analogRead(PIN_BAT);
    uint16_t bat = analogRead(PIN_BAT);

    bufRef[idx] = ref;
    bufBat[idx] = bat;
    idx++;

    if (idx >= G) {
      idx = 0;

      uint16_t medRef = median5(bufRef[0], bufRef[1], bufRef[2], bufRef[3], bufRef[4]);
      uint16_t medBat = median5(bufBat[0], bufBat[1], bufBat[2], bufBat[3], bufBat[4]);

      sumMedRef += medRef;
      sumMedBat += medBat;
      medCount++;
    }
  }

  float a4 = (float)sumMedRef / (float)medCount;
  float a2 = (float)sumMedBat / (float)medCount;

  if (a4 < 1.0f) a4 = 1.0f;

  float vRaw = a2 * K / a4;
  return CAL_A * vRaw + CAL_B;
}


// ==================================================
// FUNKCIA: Bezpečná zmena PM pinu
// ==================================================
static void setPmState(PmState newState)
{
  if (newState == pmState) return;

  pmState = newState;
  pmLastChangeMs = millis();
  convOffInhibitUntil = pmLastChangeMs + T_NO_CONV_OFF_AFTER_PM;

  digitalWrite(PIN_PM, (pmState == PM_HIGH_NORMAL) ? HIGH : LOW);
}


// ==================================================
// SETUP
// ==================================================
void setup()
{
  pinMode(PIN_VYPPIN, OUTPUT);
  pinMode(PIN_PM, OUTPUT);
  pinMode(PIN_LED, OUTPUT);

  pinMode(PIN_ACC, INPUT);
  pinMode(PIN_BAT, INPUT);
  pinMode(PIN_VREF, INPUT);

  digitalWrite(PIN_VYPPIN, HIGH);
  digitalWrite(PIN_PM, HIGH);
  digitalWrite(PIN_LED, LOW);

  pmState = PM_HIGH_NORMAL;
  pmLastChangeMs = millis();
  camStartMs = millis();

  pinMode(PIN_S1, INPUT_PULLUP);
  pinMode(PIN_S2, INPUT_PULLUP);
  pinMode(PIN_S3, INPUT_PULLUP);
  pinMode(PIN_S4, INPUT_PULLUP);

  bool s1 = digitalRead(PIN_S1);
  bool s2 = digitalRead(PIN_S2);

  if ( s1 &&  s2) voltageThreshold = VOLTAGE_1;
  if (!s1 &&  s2) voltageThreshold = VOLTAGE_2;
  if ( s1 && !s2) voltageThreshold = VOLTAGE_3;
  if (!s1 && !s2) voltageThreshold = VOLTAGE_4;

// ==================================================
// DETEKCIA 12 V / 24 V SYSTÉMU 
// ==================================================

// zahodenie prvého ADC čítania (ADC settle)
readBatteryVoltageCalibrated();

float vSum = 0.0f;
for (int i = 0; i < V_SYS_SAMPLES; i++) {
  vSum += readBatteryVoltageCalibrated();
}

boardVoltage = vSum / (float)V_SYS_SAMPLES;

if (boardVoltage >= V_24V_DETECT) {
  voltageThreshold *= 2.0f;
}


  bool s3 = digitalRead(PIN_S3);
  bool s4 = digitalRead(PIN_S4);

  int wMin = WTIME_24H;
  if (!s3 && !s4) wMin = WTIME_5MIN;
  if ( s3 && !s4) wMin = WTIME_1H;
  if (!s3 &&  s4) wMin = WTIME_3H;

  wtimeMs = (unsigned long)wMin * 60000UL;
}


// ==================================================
// LOOP – HLAVNÁ STAVOVÁ LOGIKA
// ==================================================
void loop()
{
  const unsigned long now = millis();

  boardVoltage = readBatteryVoltageCalibrated();
  AccState acc = digitalRead(PIN_ACC) ? ACC_HIGH : ACC_LOW;

  if (lastAccState == ACC_LOW && acc == ACC_HIGH) {
    camStartMs = now;
  }

  if (lastAccState == ACC_HIGH && acc == ACC_LOW) {
    parkingStartMs = now;
  }

  lastAccState = acc;

  // --- PODNAPÄTIE (LED vždy reaguje) ---
  if (boardVoltage <= voltageThreshold) {
    if (!uvActive) {
      uvActive = true;
      uvStartMs = now;
      digitalWrite(PIN_LED, HIGH);
    }
  } else {
    uvActive = false;
    digitalWrite(PIN_LED, LOW);
  }

  // --- UV OFF len pri ACC = LOW ---
  if (acc == ACC_LOW) {
    if (uvActive && (now - uvStartMs >= T_LOW_VOLTAGE)) {
      digitalWrite(PIN_VYPPIN, LOW);
      return;
    }
  }

  // ==================================================
  // ACC = HIGH (jazda)
  // ==================================================
  if (acc == ACC_HIGH) {
    digitalWrite(PIN_VYPPIN, HIGH);

    if (pmState != PM_HIGH_NORMAL) {
      if (now - pmLastChangeMs >= T_PM_MIN_DWELL) {
        setPmState(PM_HIGH_NORMAL);
      }
    } else {
      digitalWrite(PIN_PM, HIGH);
    }

    parkingStartMs = 0;
    return;
  }

  // ==================================================
  // ACC = LOW (parking)
  // ==================================================
  bool pmAllowed = (now - camStartMs >= T_PM_BLOCK_AFTER_START);
  PmState desiredPm = pmAllowed ? PM_LOW_PARK : PM_HIGH_NORMAL;

  if (desiredPm != pmState) {
    if (now - pmLastChangeMs >= T_PM_MIN_DWELL) {
      setPmState(desiredPm);
    }
  } else {
    digitalWrite(PIN_PM, (pmState == PM_HIGH_NORMAL) ? HIGH : LOW);
  }

  if (parkingStartMs > 0 &&
      now - parkingStartMs >= wtimeMs &&
      now >= convOffInhibitUntil) {
    digitalWrite(PIN_VYPPIN, LOW);
    return;
  }

  digitalWrite(PIN_VYPPIN, HIGH);
}