#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 1000000 / FREQUENCY
#define PERIOD_US 10

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 = 500,
      .spark_pulse_us = 100,
      .spark_delay_us = 100,
      .main_task = NULL};

void setup()
{

  Serial.begin(115200);
  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(12, ANALOG);
  stsA.main_task = xTaskGetCurrentTaskHandleForCore(1);

  // Timer: 80MHz / 80 = 1MHz → 1 tick = 1 µs
  timerA = timerBegin(FREQUENCY);
  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(12), 0, 4096, 100, 800) / PERIOD_US);
  stsA.spark_delay_us = spark_delay * PERIOD_US;
  if (stsA.spark_delay_us > 500) {
    stsA.soft_start = true;
    stsA.spark_delay_us -= 500;
  } else {
    stsA.soft_start = false;
  }
  LOG_INFO("Spark Delay uS: ", stsA.spark_delay_us, "\tSoft Start: ", stsA.soft_start ? "TRUE" : "FALSE");
  delay(500);
}