// SPDX-License-Identifier: Apache-2.0
// Copyright (c) 2012-2024 Scott Penrose <scottp@dd.com.au> and WII5 Buoy contributors
//
// This file is part of WII5 Buoy firmware.
// See LICENSE for full terms.

/**
 * @file WII5Battery.cpp
 * @brief Battery monitor: voltage sampling, running average, and threshold logic.
 */

/*

WII5Battery

*/

#include <Arduino.h>
#include <WII5Battery.h>
#include <WII5Sh3dConsole.h>
#include <TimeLib.h>

void WII5Battery::begin() {
  // ??? powerSetPin(POWER_BATTERY_PIN, POWER_IMU_SERIAL_ON, POWER_BATTERY_PIN_OFF);
  #ifdef BATTERY_1_VOLTS_PIN
  pinMode(BATTERY_1_VOLTS_PIN, OUTPUT);
  digitalWrite(BATTERY_1_VOLTS_PIN, LOW);
  #endif
  #ifdef BATTERY_1_VOLTS_ANALOG
  pinMode(BATTERY_1_VOLTS_ANALOG, INPUT);
  #endif
  #ifdef BATTERY_2_VOLTS_ANALOG
  pinMode(BATTERY_2_VOLTS_ANALOG, INPUT);
  #endif

  // Set the age high... never got it.
  age = WII5TIME_1WEEK * 1000;

  // Start by getting the voltage as early as possible
  step = WII5BATTERY_ON; stepWait = 0; stepCount = 0;
};

bool WII5Battery::isRunning() {
  return (step != WII5BATTERY_OFF);
}

void WII5Battery::start(bool force) {
  step = WII5BATTERY_ON; stepWait = 300000; stepCount = 0;
}
void WII5Battery::stop(bool force) {
  step = WII5BATTERY_OFF; stepWait = 0; stepCount = 0;
}

uint16_t WII5Battery::scale(uint16_t data, uint16_t d) {
  return int(0 + (float)data * (float)((float)d / 1000));
}

uint32_t WII5Battery::scale(uint32_t data, uint32_t d) {
  return (uint32_t)(0 + (float)data * (float)((float)d / 1000));
}

void WII5Battery::loop() {
  // Check timeouts (e.g. how long blocked low voltage?)

  // Testing Voltage Mode
  switch (step) {
    case WII5BATTERY_OFF:
      // TODO step counter
      // TIMEOUT - Not applicable
      // off();

      // Hard coded every 5 minutes for now
      if (stepWait > 300000) {
        step = WII5BATTERY_ON; stepWait = 0; stepCount = 0;
      }
      break;

    case WII5BATTERY_ON:
      digitalWrite(BATTERY_1_VOLTS_PIN, HIGH);
      // on();
      avgBattery1.reset();
      avgBattery2.reset();
      step = WII5BATTERY_MEASURE; stepWait = 0; stepCount = 0;
      break;

    case WII5BATTERY_MEASURE:
      // How many should we measure?
      if (stepCount > 100) {  // TODO Hard coded?
        step = WII5BATTERY_STORE; stepWait = 0; stepCount = 0;
      }
      else {
        #ifdef BATTERY_1_VOLTS_ANALOG
        // Should we scale n ow or later?
        uint16_t in = analogRead(BATTERY_1_VOLTS_ANALOG);
        avgBattery1.checkAndAddReading(scale((uint16_t) in, (uint16_t)BATTERY_1_VOLTS_MULT));
        #endif
        #ifdef BATTERY_2_VOLTS_ANALOG
        // avgBattery2.checkAndAddReading(scale((uint16_t) analogRead(BATTERY_2_VOLTS_ANALOG), (uint16_t)BATTERY_2_VOLTS_MULT);
        #endif
      }
      break;

    case WII5BATTERY_STORE:
      // TODO Where do we store this?
      //  - Current Metadata - sure - useful
      //  - SD card or SPIMemory - be nice?
      // console.log(LOG_DEBUG, F("BATTERY: Storage not yet implemented"));
      // TODO Check this is decivolts !
      // TODO can we use openLogs etc to write log to disk
      // #ifdef BATTERY_2_VOLTS_ANALOG
      //int(avgBattery2.getMean() * 100);
      // #endif
      value = int(avgBattery1.getMean());
      age = 0;
      step = WII5BATTERY_ANALYSE; stepWait = 0; stepCount = 0;
      break;

    case WII5BATTERY_ANALYSE:
      if (
        (value < wii5Config.getBatteryLow())
        && (wii5Controller.getMode() != WII5MODE_LOWBATTERY)
      ) {
        console.log(LOG_INFO, F("Low Battery detected V=%d, Low=%d, Mid=%d"),
          value, wii5Config.getBatteryLow(), wii5Config.getBatteryMid()
        );
        wii5Controller.setMode(WII5MODE_LOWBATTERY);
      }

      else if (
        (value > wii5Config.getBatteryMid())
        && (wii5Controller.getMode() == WII5MODE_LOWBATTERY)
      ) {
        console.log(LOG_INFO, F("Mid Battery detected V=%d, Low=%d, Mid=%d"),
          value, wii5Config.getBatteryLow(), wii5Config.getBatteryMid()
        );
        wii5Controller.setDefaultMode();
      }

      #ifdef BATTERY_1_VOLTS_ANALOG
      wii5Display.atDataSend(
        F("battery1"),
        F("volts=%d,swing=%d,min=%d,max=%d"),
        int(avgBattery1.getMean()),
        int(avgBattery1.getStd()),
        int(avgBattery1.getMin()),
        int(avgBattery1.getMax())
      );
      #endif
      #ifdef BATTERY_2_VOLTS_ANALOG
      // TODO
      console.printf(F("# battery: #=2 mean=%d +-%d min=%d max=%d\r\n"),
        // avgAccelZ.getN(),
        int(avgBattery2.getMean()),
        int(avgBattery2.getStd()),
        int(avgBattery2.getMin()),
        int(avgBattery2.getMax())
      );
      #endif

      wii5Display.atCommandSend(F("status"), F("voltage=%ld"),
        wii5Battery.value
      );
      wii5Display.atCommandSend(F("status"), F("update"));

      /*

      RULES:
        * Valid date tinme ?
        * Check date time not too far future, and not in past
        * Batter 2 - work out which to use (not this release)
        * Confidition of battery - how long it has been on this average etc
        if (
          // Low Battery
          (TOO LOW)
          // Disabled still
          && (wii5Config.getDisableLowBattery() > now()
        ) {
        }

      */
      step = WII5BATTERY_FINISH; stepWait = 0; stepCount = 0;
      break;

    case WII5BATTERY_FINISH:
      digitalWrite(BATTERY_1_VOLTS_PIN, LOW);
      step = WII5BATTERY_OFF; stepWait = 0; stepCount = 0;
      break;

    default:
      console.log(LOG_FATAL, F("BATTERY: Default step... step=%d"), step);
      step = WII5BATTERY_OFF; stepWait = 0; stepCount = 0;
      break;
  }

  stepCount++;
  return;
}

WII5Battery wii5Battery;