#include <cstdint>
#include <assert.h>
#include "libIgnTiming/timing.h"

#if !defined WRITABLE_TABLE
#define CONST_ATTR constexpr
#endif

#if defined __cplusplus
extern "C"
{
#endif

    namespace
    {
        int8_t timingAdjust = 0 * TIMING_SCALE; // in TIMING_SCALE

        unsigned constexpr INTERP_SCALE = 256;

        int constexpr TimingScale = TIMING_SCALE;
        int16_t constexpr NO_DATA = -1;

        int16_t constexpr MAX_ADVANCE = 50 * TIMING_SCALE;
        int16_t constexpr MIN_ADVANCE = 7 * TIMING_SCALE;

        // array of column headings
        int16_t CONST_ATTR rpmMap[MAX_RPM_POINTS] = {400, 750, 1000, 1500, 2500, 3500, 4500, 6000};
        // column of row values - in 1000-pressure
        int16_t CONST_ATTR vacuumMap[MAX_VACUUM_POINTS] = {0, 166, 225, 300, 700, 1000, NO_DATA, NO_DATA};
        uint8_t CONST_ATTR mapping[MAX_VACUUM_POINTS][MAX_RPM_POINTS] = {
            /* Table in degrees. */
            /* row for 0mb = centrifugal only */
            {12, 7, 7, 19, 25, 29, 29, 22},
            /* row for 166 mB*/
            {12, 7, 7, 21, 27, 31, 31, 24},
            /*   row for 225 mB */
            {12, 7, 7, 25, 31, 35, 35, 28},
            /* row for 300 mB*/
            {12, 7, 7, 29, 35, 39, 39, 33},
            /* row for 700 mB*/
            {12, 7, 7, 19, 25, 29, 29, 22},
            /* row for 1000 mB - used when pressure drops off the scale */
            {7, 7, 7, 7, 7, 7, 7, 7},
            /* unused */
            {0, 0, 0, 0, 0, 0, 0, 0},
            /* unused */
            {0, 0, 0, 0, 0, 0, 0, 0},
            /* unused */

        };

    }

    uint8_t getTimingAdjust() { return timingAdjust; };

    void setTimingAdjust(int8_t adjust) { timingAdjust = adjust; }

    int16_t getRpmMap(unsigned int i)
    {
        if (i >= 0 && i < MAX_RPM_POINTS)
            return rpmMap[i];
        else
            return 0;
    }

    void setRpmMap(unsigned int i, uint16_t val)
    {
#if WRITABLE_TABLE
        if (i >= 0 && i < MAX_RPM_POINTS)
            rpmMap[i] = val;
#endif
    }

    uint16_t getVacuumMap(unsigned int i)
    {
        if (i >= 0 && i < MAX_VACUUM_POINTS)
            return vacuumMap[i];
        else
            return 0;
    }

    void setVacuumMap(unsigned int i, uint16_t val)
    {
#if WRITABLE_TABLE
        if (i >= 0 && i < MAX_VACUUM_POINTS)
            vacuumMap[i] = val;
#endif
    }

    void setTiming(unsigned int vacuumIndex, unsigned int rpmIndex, uint8_t value)
    {

#if WRITABLE_TABLE
        if (vacuumIndex < 0 && vacuumIndex >= MAX_VACUUM_POINTS)
            return;
        if (rpmIndex < 0 && rpmIndex >= MAX_RPM_POINTS)
            return;
        mapping[vacuumIndex][rpmIndex] = value;
#endif
    }

    uint8_t getTiming(unsigned int vacuumIndex, unsigned int rpmIndex)
    {
        if (vacuumIndex < 0 && vacuumIndex >= MAX_VACUUM_POINTS)
            return 0;
        if (rpmIndex < 0 && rpmIndex >= MAX_RPM_POINTS)
            return 0;
        return mapping[vacuumIndex][rpmIndex];
    }

    /// @brief Lookup a point in a 1 dimensional array -
    /// @param point Value to lookup
    /// @param curve Lookup table
    /// @param size Size of lookup table
    /// @param [out] frac fraction of distance from first point in array
    /// @return index in array or NO_DATA if operations fail 
    int lookup(int point, int16_t const curve[], int size, int16_t *frac)
    {
        // check lower bounds
        if (point < curve[0])
        {
            *frac = 0;
            return 0;
        }
        // check upper bounds
        // find the upper boundary by looking for non -1 points
        int upper = size - 1;
        while (upper != 0 && curve[upper] == NO_DATA)
            upper--;

        if (point >= curve[upper])
        {
            *frac = 0;
            return upper;
        }
        for (int pt = 1; pt <= upper; pt++)
        {
            if ((point >= curve[pt - 1]) && (point < curve[pt]))
            {

                int range1 = curve[pt] - curve[pt - 1];

                if (range1 == 0)
                {
                    *frac = 0;
                    return pt - 1;
                }

                // how far along axis ?
                int offset = point - curve[pt - 1];

                int range2 = INTERP_SCALE;

                *frac = ((offset * range2) / range1);
                return pt - 1;
            }
        }
        *frac = 0;
        return NO_DATA; // give up.
    };

    extern "C"
    {

        int mapTiming(int rpm, int vacuumMb)
        {
            int angle = 0;
            /* lookup the interpolated RPM point */
            int16_t rpm_frac = 0;
            int rpm_index = lookup(rpm, rpmMap, MAX_RPM_POINTS, &rpm_frac);
            if (rpm_index == NO_DATA)
                return timingAdjust + MIN_ADVANCE;

            /* lookup the interpolated vacuum point */
            int16_t vacuum_frac = 0;
            int vacuum_index = lookup(vacuumMb, vacuumMap, MAX_VACUUM_POINTS, &vacuum_frac);
            /* if there is a problem, bail out */
            if (vacuum_index == NO_DATA)
                return timingAdjust + MIN_ADVANCE;

            /* perform a bilinear mapping */
            int top_advance;
            // we now have a position between two points in X and Y
            if (rpm_frac == 0)
                top_advance = mapping[vacuum_index][rpm_index] * INTERP_SCALE;
            // if fractional part then interpolate points off the map
            else
                top_advance = mapping[vacuum_index][rpm_index] * (INTERP_SCALE - rpm_frac) + mapping[vacuum_index][rpm_index + 1] * rpm_frac;

            int bottom_advance;
            // if no fractional part, then the top and bottom advance point is the same
            if (vacuum_frac == 0)
            {
                angle = top_advance * TimingScale / INTERP_SCALE;
            }
            else
            {
                bottom_advance = mapping[vacuum_index + 1][rpm_index] * (INTERP_SCALE - rpm_frac) + mapping[vacuum_index + 1][rpm_index + 1] * rpm_frac;
                /* interpolate down Y axis this time */
                int advance = top_advance * (INTERP_SCALE - vacuum_frac) + bottom_advance * vacuum_frac;
                /* point is scaled by two multiplications */
                angle = advance * TimingScale / (INTERP_SCALE * INTERP_SCALE);
            }

            if (angle < MIN_ADVANCE)
                angle = MIN_ADVANCE;
            if (angle > MAX_ADVANCE)
                angle = MAX_ADVANCE;

            return angle + timingAdjust;
        }
    }
#if defined __cplusplus
}
#endif