/*
 * ElektronischeLast.cpp
 *
 *  Created on: Jun 23, 2023
 *      Author: Carst
 */

#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <cinttypes>
#include <cmath>
#include <stm32g0xx.h>
#include <stm32g0xx_ll_lpuart.h>
#include "STM32G071KBT6.hpp"
#include "LED.hpp"
#include "DAC.hpp"
#include "ADC.hpp"
#include "serial.hpp"
#include "PID.h"
#include "FanControl.hpp"
#include "CLI.h"
#include "I2C.hpp"

using namespace ElektronischeLast;

#define EEPROM_ADDRESS_VOLTAGE 0U
#define EEPROM_ADDRESS_CURRENT 4U

static bool do_icall(iADC& adc, iI2C& eeprom);
static bool do_ucall(iADC& dac, iI2C& eeprom);

uint16_t set_solltrom (CLI_OutFunction pfvOutFunction, char *acCommands[], uint16_t u16ArgCount);
uint16_t set_dutyCyle (CLI_OutFunction pfvOutFunction, char *acCommands[], uint16_t u16ArgCount);
uint16_t ist_werte(CLI_OutFunction pfvOutFunction, char *acCommands[], uint16_t u16ArgCount);
uint16_t set_ucall(CLI_OutFunction pfvOutFunction, char *acCommands[], uint16_t u16ArgCount);
uint16_t set_icall(CLI_OutFunction pfvOutFunction, char *acCommands[], uint16_t u16ArgCount);
uint16_t reboot(CLI_OutFunction pfvOutFunction, char *acCommands[], uint16_t u16ArgCount);

static CLI_Command_t commands[] =
{
  { "isoll", "Zielstrom für Last", set_solltrom },
  { "ist",   "Zeige Istwerte an [-r für wiederholende Anzeige]", ist_werte },
  { "fan",   "Setze Tastverhältnis für Lüfter", set_dutyCyle },
  { "icall", "Starte Strom Kalibrierung", set_icall },
  { "ucall", "Starte Spannungs Kalibrierung", set_ucall },
  { "reboot", "System neu starten", reboot },
};

static std::uint32_t i_soll = 0U;
static PIDController pid =
{
  .Kp = 0.75f,
  .Ki = 10.0f,
  .Kd = 0.0f,
  .limMin = 0.0f,
  .limMax = 4095.0f,
  .limMinInt = 0.0f,
  .limMaxInt = 2048.0f,
  .T = 0.001f,
};
static uint32_t dac_value;
static uint32_t strom;
static uint32_t spannung;
static uint32_t temperatur;
static uint32_t geschwindigkeit;
static enum { normal, icall, ucall } modus;

int main (void)
{
  __enable_irq();
  serial_init();
  printf("\r\nElektronische Last\r\n");
  CLI_Init(commands, sizeof(commands)/sizeof(commands[0]));
  LED led = LED(500U);
  FanControl fan = FanControl();
  iI2C eeprom = iI2C(0xA0U);

  float voltage_gain = NAN;
  float current_gain = NAN;
  eeprom.read(EEPROM_ADDRESS_VOLTAGE, 4UL, (uint8_t*)&voltage_gain);
  eeprom.read(EEPROM_ADDRESS_CURRENT, 4UL, (uint8_t*)&current_gain);
  if(std::isnan(voltage_gain))
  {
    printf("* Default für voltage_gain\r\n");
    voltage_gain = 22.272125f;
  }
  if(std::isnan(current_gain))
  {
    printf("* Default für current_gain\r\n");
    current_gain = 3.685331f;
  }

  iDAC dac = iDAC();
  iADC adc = iADC(voltage_gain, current_gain);
  std::uint32_t last_tick = systick;

  printf("- Initialisierung erfolgreich\r\n");
  printf("- Verstärkung U: %f, Verstärkung I: %f\r\n", voltage_gain, current_gain);
  printf("Elektronische Last>");

  PIDController_Init(&pid);

  while(1)
  {
    led.blink();

    if(last_tick != systick)
    {
      last_tick = systick;
      switch(modus)
      {
        case icall:
          if(do_icall(adc, eeprom))
          {
            while(!LL_LPUART_IsActiveFlag_TC(LPUART1));
            NVIC_SystemReset();
          }
          break;
        case ucall:
          if(do_ucall(adc, eeprom))
          {
            while(!LL_LPUART_IsActiveFlag_TC(LPUART1));
            NVIC_SystemReset();
          }
          break;
        case normal:
        default:
          dac_value = (uint32_t)PIDController_Update(&pid, i_soll, adc.get_current());
          dac.write(iDAC::CHANNEL_1, dac_value);

          fan.run(adc.get_temperature());

          strom = adc.get_current();
          spannung = adc.get_voltage();
          temperatur = adc.get_temperature();
          geschwindigkeit = fan.get_speed();
          break;
      }
      serial_cyclic();
    }
  }
}

/**
 * @brief  Callback Function for Command Line
 * @param  [in] pfvOutFunction Function to Print Data to Output
 * @param  [in] acCommands array with char pointers to the different arguments
 * @param  [in] u16ArgCount number of Arguments
 * @return 0 = don't recall, 1 = call again
 */
uint16_t set_solltrom(CLI_OutFunction pfvOutFunction, char *acCommands[], uint16_t u16ArgCount)
{
  char buf[40];
  uint32_t cur = i_soll;
  int32_t pos = snprintf(buf, sizeof(buf), "\r\nAktueller Soll-Strom: %" PRIu32, cur);
  if(u16ArgCount == 1U)
  {
    uint32_t in = strtoul(acCommands[0], NULL, 10);
    snprintf(&buf[pos], sizeof(buf) - pos, " Neu: %" PRIu32, in);
    i_soll = in;
  }
  pfvOutFunction(buf);
  return 0;
}

uint16_t ist_werte(CLI_OutFunction pfvOutFunction, char *acCommands[], uint16_t u16ArgCount)
{
  static uint32_t last_call = 0U;
  uint16_t recall = 0U;
  int32_t recall_time = 999;
  bool newline = false;
  if(u16ArgCount >= 1U)
  {
    recall = (acCommands[0][0] == '-' && acCommands[0][1] == 'r' && acCommands[0][2] == '\0');
  }
  if(u16ArgCount >= 2U)
  {
    recall_time = strtol(acCommands[1], NULL, 10);
    newline = true;
  }

  if(((recall == 1U) && ((int32_t)(systick - last_call) > recall_time)) || recall == 0U)
  {
    char buffer[64];
    if(newline)
    {
      buffer[0] = '\n';
      buffer[1] = '\0';
      pfvOutFunction(buffer);
    }
    snprintf(buffer, sizeof(buffer), "\rI: %5" PRIu32 "mA U: %5" PRIu32 "mV T: %3" PRIu32 "°C V: %4" PRIu32 "RPM",
             strom, spannung, temperatur, geschwindigkeit);
    pfvOutFunction(buffer);
    last_call = systick;
  }

  return recall;
}

uint16_t set_dutyCyle (CLI_OutFunction pfvOutFunction, char *acCommands[], uint16_t u16ArgCount)
{
  char buf[40];
  if(u16ArgCount == 1U)
  {
    uint32_t in = strtoul(acCommands[0], NULL, 10);
    snprintf(buf, sizeof(buf), "\r\nNeu: %" PRIu32, in);
    FanControl_SetDuty(in);
  }
  else
  {
    snprintf(buf, sizeof(buf), "\r\nMissing Argument Dutycycle!");
  }
  pfvOutFunction(buf);
  return 0;
}
uint16_t set_ucall(CLI_OutFunction pfvOutFunction, char *acCommands[], uint16_t u16ArgCount)
{
  spannung = 0UL;
  strom = 0UL;
  modus = ucall;
  return 0U;
}

uint16_t set_icall(CLI_OutFunction pfvOutFunction, char *acCommands[], uint16_t u16ArgCount)
{
  spannung = 0UL;
  strom = 0UL;
  modus = icall;
  return 0U;
}

uint16_t reboot(CLI_OutFunction pfvOutFunction, char *acCommands[], uint16_t u16ArgCount)
{
  printf("\r\nReboot System...");
  while(!LL_LPUART_IsActiveFlag_TC(LPUART1));
  NVIC_SystemReset();
}

static bool do_icall(iADC& adc, iI2C& eeprom)
{
  bool done = false;
  strom += adc.get_current_raw();
  spannung++;
  if(spannung == 1000UL)
  {
    float current_gain = 1.0f / ((float)strom / 1000.0f * 3.3f / 4096.0f);
    eeprom.write(EEPROM_ADDRESS_CURRENT, sizeof(float), (uint8_t*)&current_gain);
    printf("\r\nStromverstärkung: %f", current_gain);
    modus = normal;
    spannung = 0UL;
    strom = 0UL;
    done = true;
  }
  return done;
}
static bool do_ucall(iADC& adc, iI2C& eeprom)
{
  bool done = false;
  spannung += adc.get_voltage_raw();
  strom++;
  if(strom == 1000UL)
  {
    float voltage_gain = 15.0f / ((float)spannung / 1000.0f * 3.3f / 4096.0f);
    eeprom.write(EEPROM_ADDRESS_VOLTAGE, sizeof(float), (uint8_t*)&voltage_gain);
    printf("\r\nSpannungsverstärkung: %f", voltage_gain);
    modus = normal;
    spannung = 0UL;
    strom = 0UL;
    done = true;
  }
  return done;
}