#include <Arduino.h>
#include <DebugLog.h>

#include "timer.h"
#include <map>

static hw_timer_t *timerA = NULL;
static hw_timer_t *timerB = NULL;
TaskHandle_t main_t = NULL;
static uint32_t count = 0;

#define FREQUENCY 100000 // 100 KHz
#define PERIOD_US 10

#define SPARK_DLY_MIN 10
#define SPARK_DLY_MAX 490

#define PAUSE_LONG_MIN 5000
#define PAUSE_LONG_MAX PAUSE_LONG_MIN*100

#define RPM_MIN 800
#define RPM_MAX 5500

void clearScreen(){
    Serial.print("\033[2J"); // clear screen
    Serial.print("\033[H");  // cursor home
    Serial.flush();
}

static double filtered_rpm = 0;

static const std::map<const uint32_t, const char *> pin2Name = {
    {PIN_TRIG_A12P, "HIGH_PIN_TRIG_A12P"},
    {~PIN_TRIG_A12P, "LOW_PIN_TRIG_A12P"},
    {PIN_TRIG_A12N, "HIGH_PIN_TRIG_A12N"},
    {~PIN_TRIG_A12N, "LOW_PIN_TRIG_A12N"},
    {PIN_TRIG_A34P, "HIGH_PIN_TRIG_A34P"},
    {~PIN_TRIG_A34P, "LOW_PIN_TRIG_A34P"},
    {PIN_TRIG_A34N, "HIGH_PIN_TRIG_A34N"},
    {~PIN_TRIG_A34N, "LOW_PIN_TRIG_A34N"},
    {SPARK_A12, "HIGH_SPARK_A12"},
    {~SPARK_A12, "LOW_SPARK_A12"},
    {SPARK_A34, "HIGH_SPARK_A34"},
    {~SPARK_A34, "LOW_SPARK_A34"},
    {State::S_WAIT_10MS_END, "S_WAIT_10MS_END"},
    {State::S_WAIT_10MS, "S_WAIT_10MS"}};

static timerStatus stsA = {
      .clock_period_us = (uint32_t)PERIOD_US,
      .pause_long_us = 10000,
      .pause_short_us = 1000,
      .coil_pulse_us = 1000,
      .spark_pulse_us = 100,
      .spark_delay_us = 50,
      .main_task = NULL};

static bool isEnabled = false;

void setup()
{

  Serial.begin(921600);
  delay(1000);
  LOG_ATTACH_SERIAL(Serial);

  pinMode(PIN_TRIG_A12P, OUTPUT);
  pinMode(PIN_TRIG_A12N, OUTPUT);
  pinMode(PIN_TRIG_A34P, OUTPUT);
  pinMode(PIN_TRIG_A34N, OUTPUT);
  pinMode(SPARK_A12, OUTPUT);
  pinMode(SPARK_A34, OUTPUT);

  pinMode(PIN_TRIG_B12P, OUTPUT);
  pinMode(PIN_TRIG_B12N, OUTPUT);
  pinMode(PIN_TRIG_B34P, OUTPUT);
  pinMode(PIN_TRIG_B34N, OUTPUT);
  pinMode(SPARK_B12, OUTPUT);
  pinMode(SPARK_B34, OUTPUT);

  pinMode(SPARK_DELAY_POT, ANALOG);
  pinMode(FREQ_POT, ANALOG);

  pinMode(ENABLE_PIN, INPUT_PULLUP);

  stsA.main_task = xTaskGetCurrentTaskHandleForCore(1);

  timerA = timerBegin(FREQUENCY);
  timerStop(timerA);
  timerAttachInterruptArg(timerA, &onTimer, (void *)&stsA);
  timerAlarm(timerA, 1, true, 0);

  LOG_INFO("Setup Complete");
}

void loop()
{
  LOG_INFO("Loop: ", count++);
  uint32_t spark_delay = (uint32_t)(map(analogRead(SPARK_DELAY_POT), 0, 4096, SPARK_DLY_MIN, SPARK_DLY_MAX) / PERIOD_US);
  stsA.spark_delay_us = spark_delay * PERIOD_US;
  if (stsA.spark_delay_us > (SPARK_DLY_MIN + SPARK_DLY_MAX) / 2) {
    stsA.soft_start = true;
    stsA.spark_delay_us -= (SPARK_DLY_MIN + SPARK_DLY_MAX) / 2;
  } else {
    stsA.soft_start = false;
  }

  double new_rpm = (double)(map(analogRead(FREQ_POT), 0, 4096, RPM_MIN, RPM_MAX));
  filtered_rpm = filtered_rpm + 0.1 * (new_rpm - filtered_rpm);
  stsA.pause_long_us = (uint32_t)(60000000.0f / filtered_rpm / 2.0f);

  if (isEnabled) {
    LOG_INFO("==== System is ENABLED ====");
  } else {
    LOG_INFO("==== System is DISABLED ====");
  }

  LOG_INFO("Spark Delay uS: ", stsA.spark_delay_us, "\tSoft Start: ", stsA.soft_start ? "TRUE" : "FALSE");
  LOG_INFO("Engine Rpm: ", (uint32_t)(filtered_rpm));
  LOG_INFO("Coil Pulse: ", stsA.coil_pulse_us, "us");
  LOG_INFO("Spark Pulse: ", stsA.spark_pulse_us, "us");
  
  if (digitalRead(ENABLE_PIN) == LOW && !isEnabled) {
    timerStart(timerA);
    isEnabled = true;
  } else if (digitalRead(ENABLE_PIN) == HIGH && isEnabled) {
    timerStop(timerA);
    isEnabled = false;
  }

  delay(100);
  clearScreen();
}
