#include "timer.h"

volatile static bool wait_sent = false;

void onTimer(void *arg)
{
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    timerStatus *params = (timerStatus *)(arg);
    TaskHandle_t task = params->main_task;

    // increment state time
    params->state_time += params->clock_period_us;

    digitalWrite(PIN_TRIG_B12P, HIGH);

    switch (params->state)
    {

    case S_12P:
        if (params->state_time == params->clock_period_us && !params->coil12p_high)
        {
            // xTaskNotifyFromISR(task, PIN_TRIG_A12P, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
            digitalWrite(PIN_TRIG_A12P, HIGH);
            params->coil12p_high = true;
            wait_sent = false;
        }

        if (!params->soft_start)
        {
            if (params->state_time == params->spark_delay_us)
            {
                // xTaskNotifyFromISR(task, SPARK_A12, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
                digitalWrite(SPARK_A12, HIGH);
            }

            if (params->state_time == (params->spark_delay_us + params->spark_pulse_us))
            {
                // xTaskNotifyFromISR(task, ~SPARK_A12, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
                digitalWrite(SPARK_A12, LOW);
            }
        }

        if (params->state_time >= params->coil_pulse_us && params->coil12p_high)
        {
            // xTaskNotifyFromISR(task, ~PIN_TRIG_A12P, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
            digitalWrite(PIN_TRIG_A12P, LOW);
            params->coil12p_high = false;
        }

        if (params->state_time >= params->pause_short_us)
        {
            params->state = S_12N;
            params->state_time = 0;
        }
        break;

    case S_12N:
        if (params->state_time == params->clock_period_us && !params->coil12n_high)
        {
            // xTaskNotifyFromISR(task, PIN_TRIG_A12N, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
            digitalWrite(PIN_TRIG_A12N, HIGH);
            params->coil12n_high = true;
        }

        if (params->soft_start)
        {
            if (params->state_time == params->spark_delay_us)
            {
                // xTaskNotifyFromISR(task, SPARK_A12, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
                digitalWrite(SPARK_A12, HIGH);
            }

            if (params->state_time == (params->spark_delay_us + params->spark_pulse_us))
            {
                // xTaskNotifyFromISR(task, ~SPARK_A12, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
                digitalWrite(SPARK_A12, LOW);
            }
        }

        if (params->state_time >= params->coil_pulse_us && params->coil12n_high)
        {
            // xTaskNotifyFromISR(task, ~PIN_TRIG_A12N, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
            digitalWrite(PIN_TRIG_A12N, LOW);
            params->coil12n_high = false;
            params->state = S_WAIT_10MS;
            params->state_time = 0;
        }
        break;

    case S_WAIT_10MS:
        if (!wait_sent)
        {
            // xTaskNotifyFromISR(task, S_WAIT_10MS, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
            wait_sent = true;
        }
        if (params->state_time >= params->pause_long_us)
        {
            params->state = S_34P;
            params->state_time = 0;
        }
        break;

    case S_34P:
        if (params->state_time == params->clock_period_us && !params->coil34p_high)
        {
            // xTaskNotifyFromISR(task, PIN_TRIG_A34P, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
            digitalWrite(PIN_TRIG_A34P, HIGH);
            params->coil34p_high = true;;
            wait_sent = false;
        }

        if (!params->soft_start)
        {
            if (params->state_time == params->spark_delay_us)
            {
                // xTaskNotifyFromISR(task, SPARK_A34, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
                digitalWrite(SPARK_A34, HIGH);
            }

            if (params->state_time == params->spark_delay_us + params->spark_pulse_us)
            {
                // xTaskNotifyFromISR(task, ~SPARK_A34, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
                digitalWrite(SPARK_A34, LOW);
            }
        }

        if (params->state_time >= params->coil_pulse_us && params->coil34p_high)
        {
            // xTaskNotifyFromISR(task, ~PIN_TRIG_A34P, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
            digitalWrite(PIN_TRIG_A34P, LOW);
            params->coil34p_high = false;
        }

        if (params->state_time >= params->pause_short_us)
        {
            params->state = S_34N;
            params->state_time = 0;
        }
        break;

    case S_34N:
        if (params->state_time == params->clock_period_us && !params->coil34n_high)
        {
            // xTaskNotifyFromISR(task, PIN_TRIG_A34N, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
            digitalWrite(PIN_TRIG_A34N, HIGH);
            params->coil34n_high = true;
        }

        if (params->soft_start)
        {
            if (params->state_time == params->spark_delay_us)
            {
                // xTaskNotifyFromISR(task, SPARK_A34, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
                digitalWrite(SPARK_A34, HIGH);
            }

            if (params->state_time == params->spark_delay_us + params->spark_pulse_us)
            {
                // xTaskNotifyFromISR(task, ~SPARK_A34, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
                digitalWrite(SPARK_A34, LOW);
            }
        }

        if (params->state_time >= params->coil_pulse_us && params->coil34n_high)
        {
            // xTaskNotifyFromISR(task, ~PIN_TRIG_A34N, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
            digitalWrite(PIN_TRIG_A34N, LOW);
            params->coil34n_high = false;
            params->state = S_WAIT_10MS_END;
            params->state_time = 0;
        }
        break;

    case S_WAIT_10MS_END:
        if (!wait_sent)
        {
            // xTaskNotifyFromISR(task, S_WAIT_10MS_END, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
            wait_sent = true;
        }
        if (params->state_time >= params->pause_long_us)
        {
            params->state = S_12P;
            params->state_time = 0;
        }
        break;
    }

    digitalWrite(PIN_TRIG_B12P, LOW);

    if (xHigherPriorityTaskWoken)
        portYIELD_FROM_ISR();
}
