#include <Arduino.h>

#include "baro_channel.h"
#include "config.h"

namespace {

uint32_t nextReportAt = 0;

//  Serial.print(float, digits) resolves to Print::print(double, int), which
//  would pull the double-precision soft-float routines into a 32 KB image.
//  Formatting from a scaled integer keeps the whole build single-precision.
void printFixed2(float value)
{
  if (!isfinite(value))
  {
    Serial.print(F("nan"));
    return;
  }

  int32_t hundredths = (int32_t)(value * 100.0f + (value < 0.0f ? -0.5f : 0.5f));

  if (hundredths < 0)
  {
    Serial.print('-');
    hundredths = -hundredths;
  }

  const int32_t fraction = hundredths % 100;

  Serial.print(hundredths / 100);
  Serial.print('.');
  if (fraction < 10) Serial.print('0');
  Serial.print(fraction);
}

void printChannel(BaroChannel &channel)
{
  Serial.print(',');
  Serial.print(baroStatusName(channel.status()));
  Serial.print(',');
  printFixed2(channel.pressure());
  Serial.print(',');
  printFixed2(channel.temperature());
}

}  // namespace


void setup()
{
  Serial.begin(UART_BAUD);

  const uint32_t now = millis();
  baroI2cChannel().begin(now);
  baroSpiChannel().begin(now);

  Serial.println();
  Serial.println(F("ms,i2c_status,i2c_p_mbar,i2c_t_c,spi_status,spi_p_mbar,spi_t_c"));

  nextReportAt = millis() + REPORT_PERIOD_MS;
}


void loop()
{
  uint32_t now = millis();

  baroI2cChannel().poll(now);
  baroSpiChannel().poll(now);

  //  poll() blocks for the ADC conversions, so re-read the clock before the
  //  LED and report deadlines are evaluated.
  now = millis();

  baroI2cChannel().updateLed(now);
  baroSpiChannel().updateLed(now);

  if ((int32_t)(now - nextReportAt) < 0) return;

  //  Advancing by a fixed period keeps the long-run average at exactly 10 Hz
  //  instead of drifting by one loop iteration per report.
  nextReportAt += REPORT_PERIOD_MS;

  //  Unless a stall put us more than a full period behind, in which case the
  //  grid is re-based rather than emitting a burst of catch-up lines.
  if ((int32_t)(now - nextReportAt) >= 0) nextReportAt = now + REPORT_PERIOD_MS;

  Serial.print(now);
  printChannel(baroI2cChannel());
  printChannel(baroSpiChannel());
  Serial.println();
}
