#include "OpticalCurrent.h"
#include "Config.h"

namespace {
struct AdcAccumulator {
  uint64_t senseRaw = 0;
  uint64_t vccRaw = 0;
  uint64_t senseMillivolts = 0;
  uint64_t vccMillivolts = 0;
  uint32_t samples = 0;
  uint32_t startedMs = 0;
};

AdcAccumulator accumulator;

void resetAccumulator(uint32_t now) {
  accumulator = {};
  accumulator.startedMs = now;
}

OpticalCurrentMeasurement finishMeasurement() {
  const float senseAdcVoltage =
    accumulator.senseMillivolts / (1000.0f * accumulator.samples);
  const float vccAdcVoltage =
    accumulator.vccMillivolts / (1000.0f * accumulator.samples);
  const float senseVoltage = senseAdcVoltage * OPTICAL_DIVIDER_RATIO;
  const float vccVoltage = vccAdcVoltage * OPTICAL_DIVIDER_RATIO;
  const float resistorVoltage = vccVoltage - senseVoltage;
  const float currentMa = resistorVoltage * 1000.0f / OPTICAL_SENSE_R;
  return {
    static_cast<uint16_t>(accumulator.senseRaw / accumulator.samples),
    static_cast<uint16_t>(accumulator.vccRaw / accumulator.samples),
    senseAdcVoltage, vccAdcVoltage, senseVoltage, vccVoltage,
    currentMa > 0.0f ? currentMa : 0.0f
  };
}
}

void optical_current_begin(void) {
  analogReadResolution(12);
  pinMode(GPIO_OPTICAL_CURRENT, INPUT);
  pinMode(GPIO_OPTICAL_VCC, INPUT);
  // Both 10k/10k dividers can present about 2.5 V to their ADC inputs.
  analogSetPinAttenuation(GPIO_OPTICAL_CURRENT, ADC_11db);
  analogSetPinAttenuation(GPIO_OPTICAL_VCC, ADC_11db);
  resetAccumulator(0);
}

bool optical_current_poll(OpticalCurrentMeasurement &measurement) {
  const uint32_t now = millis();
  if (!accumulator.startedMs) resetAccumulator(now ? now : 1U);

  accumulator.senseRaw += analogRead(GPIO_OPTICAL_CURRENT);
  accumulator.senseMillivolts += analogReadMilliVolts(GPIO_OPTICAL_CURRENT);
  accumulator.vccRaw += analogRead(GPIO_OPTICAL_VCC);
  accumulator.vccMillivolts += analogReadMilliVolts(GPIO_OPTICAL_VCC);
  ++accumulator.samples;

  if (now - accumulator.startedMs < OPTICAL_CURRENT_AVERAGING_MS) return false;
  measurement = finishMeasurement();
  resetAccumulator(now);
  return true;
}

OpticalCurrentMeasurement optical_get_led_measurement(void) {
  OpticalCurrentMeasurement measurement = {};
  resetAccumulator(millis());
  while (!optical_current_poll(measurement)) delay(1);
  return measurement;
}

float optical_get_led_current_ma(void) {
  return optical_get_led_measurement().currentMa;
}