#include "baro_channel.h"

const char *baroStatusName(BaroStatus status)
{
  switch (status)
  {
    case BaroStatus::NotInitialised: return "INIT";
    case BaroStatus::Ok:             return "OK";
    case BaroStatus::InitFailed:     return "ERR_INIT";
    case BaroStatus::PromCrcError:   return "ERR_CRC";
    case BaroStatus::ReadError:      return "ERR_READ";
    case BaroStatus::OutOfRange:     return "ERR_RANGE";
    case BaroStatus::Stale:          return "ERR_STALE";
  }
  return "ERR_UNKNOWN";
}


void BaroChannel::begin(uint32_t now)
{
  pinMode(_ledPin, OUTPUT);
  digitalWrite(_ledPin, LOW);
  _ledOn = false;

  tryInit(now);
}


bool BaroChannel::tryInit(uint32_t now)
{
  _lastInitAttempt = now;
  _initialised     = false;
  _repeatCount     = 0;

  if (!driverBegin())
  {
    fail(BaroStatus::InitFailed, now);
    return false;
  }

  uint16_t prom[8] = { 0 };
  driverReadProm(prom);

  //  An absent part reads back as all-zero (I2C NACK) or all-ones (floating
  //  MISO). Neither driver rejects 0xFFFF, and an all-zero PROM would satisfy
  //  the CRC, so the calibration words are screened before the checksum runs.
  for (uint8_t i = 1; i <= 6; i++)
  {
    if (prom[i] == 0x0000 || prom[i] == 0xFFFF)
    {
      fail(BaroStatus::PromCrcError, now);
      return false;
    }
  }

  if (!ms5611PromCrcOk(prom))
  {
    fail(BaroStatus::PromCrcError, now);
    return false;
  }

  _initialised = true;
  _errorStreak = 0;
  return true;
}


void BaroChannel::poll(uint32_t now)
{
  if (!_initialised)
  {
    if ((uint32_t)(now - _lastInitAttempt) < BARO_INIT_RETRY_MS) return;
    if (!tryInit(now)) return;
  }

  if (!driverRead())
  {
    fail(BaroStatus::ReadError, now);
    return;
  }

  const float pressure    = driverPressure();
  const float temperature = driverTemperature();

  //  Catches the failure the drivers miss: an ADC read taken before the
  //  conversion finished returns 0 (datasheet p.11), which the I2C driver
  //  still reports as MS5611_READ_OK but which lands far outside this envelope.
  if (!isfinite(pressure) || !isfinite(temperature) ||
      pressure < BARO_PRESSURE_MIN_MBAR || pressure > BARO_PRESSURE_MAX_MBAR ||
      temperature < BARO_TEMP_MIN_C || temperature > BARO_TEMP_MAX_C)
  {
    _pressure    = pressure;
    _temperature = temperature;
    fail(BaroStatus::OutOfRange, now);
    return;
  }

  const bool repeated = (pressure == _pressure) && (temperature == _temperature);

  _pressure    = pressure;
  _temperature = temperature;

  if (repeated)
  {
    if (_repeatCount < BARO_STALE_LIMIT) _repeatCount++;
  }
  else
  {
    _repeatCount = 0;
  }

  if (_repeatCount >= BARO_STALE_LIMIT)
  {
    fail(BaroStatus::Stale, now);
    return;
  }

  _status      = BaroStatus::Ok;
  _errorStreak = 0;
}


void BaroChannel::fail(BaroStatus status, uint32_t now)
{
  _status = status;
  _errorCount++;

  _ledOn    = true;
  _ledOffAt = now + LED_ERROR_HOLD_MS;
  digitalWrite(_ledPin, HIGH);

  if (_errorStreak < BARO_REINIT_AFTER_ERRORS) _errorStreak++;

  if (_errorStreak >= BARO_REINIT_AFTER_ERRORS)
  {
    _initialised     = false;
    _errorStreak     = 0;
    _lastInitAttempt = now;
  }
}


void BaroChannel::updateLed(uint32_t now)
{
  //  Signed difference so the comparison survives the millis() rollover.
  if (_ledOn && (int32_t)(now - _ledOffAt) >= 0)
  {
    _ledOn = false;
    digitalWrite(_ledPin, LOW);
  }
}
