// code to compute RPM
#include "stdint.h"

#include "libIgnTiming/rpm.h"
#if defined RPMTIMER

extern "C"
{
    typedef enum
    {
        PULSE_LOW = 1,
        PULSE_HIGH = 2,
        PULSE_BOTH = 3
    } pulseState_t;
    // with a dwell angle of 45 degrees , 4 cylinders and a maximum RPM of 5000
    // freq = 5000/60 * 2 = 166Hz.
    // the TIM2 counter counts in 10uS increments,
    // Need to accumulate low level for a 400th of a second before accepting it as a pulse
    const uint16_t ACCUM_MAX = (RPM_COUNT_RATE / 400);

    // shared variables used in calculation - a pipeline of samples
    static volatile unsigned long RPM_Time[RPM_SAMPLES]; // sampled on both edges
    static volatile unsigned long RPM_Count;             // incremented every reading

    void TIMER_IRQ_HANDLER(void)
    {
        static char level = 0;
        char valid = 0;
        uint16_t high_count = 0;
        uint16_t low_count = 0;
        // rising edge CB pulse
        if (__HAL_TIM_GET_FLAG(&TIMER_HANDLE, TIM_FLAG_CC1))
        {
            __HAL_TIM_CLEAR_FLAG(&TIMER_HANDLE, TIM_FLAG_CC1);
            low_count = __HAL_TIM_GET_COMPARE(&TIMER_HANDLE, TIM_CHANNEL_1);
            valid = PULSE_LOW; // record we have a low_count val

            // trigger any other event at rising edge
            AUXILIARY_HIGH;
        }
        // falling edge trigger CB pulse
        if (__HAL_TIM_GET_FLAG(&TIMER_HANDLE, TIM_FLAG_CC2))
        {
            __HAL_TIM_CLEAR_FLAG(&TIMER_HANDLE, TIM_FLAG_CC2);

            high_count = __HAL_TIM_GET_COMPARE(&TIMER_HANDLE, TIM_CHANNEL_2);
            valid |= PULSE_HIGH;
            // trigger any other event at falling edge
            AUXILIARY_LOW;
        }

        switch (valid)
        {
        case pulseState_t::PULSE_LOW:
            // count width of a low period
            RPM_Time[RPM_Count] = low_count;
            RPM_Count = (RPM_Count + 1) % RPM_SAMPLES;
            level = 0; // remember level
            break;
        case pulseState_t::PULSE_HIGH:
            // count width of a high period
            RPM_Time[RPM_Count] = high_count | RPM_FLAG;
            RPM_Count = (RPM_Count + 1) % RPM_SAMPLES;
            level = 1; // remember level
            break;
            // there has been both a high level and a low level
        case pulseState_t::PULSE_BOTH:
            if (level == 1) // next level = 0 ,then 1 again
            {
                RPM_Time[RPM_Count] = low_count;
                RPM_Count = (RPM_Count + 1) % RPM_SAMPLES;
                RPM_Time[RPM_Count] = high_count | RPM_FLAG;
                RPM_Count = (RPM_Count + 1) % RPM_SAMPLES;
            }
            else
            {
                RPM_Time[RPM_Count] = high_count | RPM_FLAG;
                RPM_Count = (RPM_Count + 1) % RPM_SAMPLES;
                RPM_Time[RPM_Count] = low_count;
                RPM_Count = (RPM_Count + 1) % RPM_SAMPLES;
            }
            break;
        default:
            break;
        }
    }

    int CalculateRPM(void)
    {
        // compute the timer values
        // snapshot timers

        // Next state of pulse high/low
        static unsigned char RPM_State = 1;
        // Current state of pulse high/low
        static unsigned char RPM_State_Curr = 1;

        // variables used in calculation of RPM value
        static uint16_t last_dwell_end = 0;
        static uint16_t RPM_Period[RPM_AVERAGE];
        static unsigned int RPM_Period_Ptr = 0;

        static uint16_t RPM_Count_Latch = 0;

        // accumulators
        static int16_t RPM_Pulsecount = 0;

        __disable_irq(); // copy the counter value
        // current RPM pulse next slot index
        uint16_t RPM_Count_Val = RPM_Count;
        __enable_irq();
        // do calculations

        // if there is only one entry, cannot get difference
        if (RPM_Count_Latch != RPM_Count_Val)
        {
            while (1)
            {
                unsigned int base_time;
                unsigned int new_time;
                // if we are at N-1, stop.
                unsigned int next_count = (RPM_Count_Latch + 1) % RPM_SAMPLES;
                if (next_count == RPM_Count_Val)
                {
                    break; // completed loop
                }
                char pulse_level = (RPM_Time[RPM_Count_Latch] & RPM_FLAG) ? 1 : 0;
                base_time = RPM_Time[RPM_Count_Latch] & ~RPM_FLAG;
                new_time = RPM_Time[next_count] & ~RPM_FLAG;
                RPM_Count_Latch = next_count;

                uint16_t RPM_Pulsewidth = new_time - base_time;

                if (pulse_level == 0 && (RPM_Pulsewidth > ACCUM_MAX))
                    RPM_State = 1;
                if (pulse_level == 1)
                    RPM_State = 0;

                // low pulse has reached at least minimum width, count it.
                if ((RPM_State == 1) && (RPM_State_Curr == 0))
                {
                    // Rev counter processing from original RevCounter Project
                    uint16_t RPM_Diff = new_time - last_dwell_end;

                    RPM_Period[RPM_Period_Ptr] = RPM_Diff;
                    RPM_Period_Ptr = (RPM_Period_Ptr + 1) % RPM_AVERAGE;
                    if (RPM_Pulsecount < RPM_AVERAGE)
                        RPM_Pulsecount++; // count one pulse
                    last_dwell_end = new_time;
                }
                RPM_State_Curr = RPM_State;
            }
        }

        if (RPM_Pulsecount == RPM_AVERAGE)
        {
            // now have time for N pulses in clocks
            // 1Hz is 30 RPM
            int i;
            unsigned int RPM_FilteredWidth = 0;
            for (i = 0; i < RPM_AVERAGE; i++)
                RPM_FilteredWidth += RPM_Period[i];

#if !defined MY_DEBUG
            // reset here unless we want to debug
            RPM_Pulsecount = 0;
#endif
            return (Scale * 30.0 * RPM_AVERAGE * RPM_COUNT_RATE) / (RPM_FilteredWidth);
        }
        else
        {
            return -1; // flag no reading
        }
    }
}
#else

int CalculateRPM(void) { return 0; };

#endif // RPMTIMER