#include "maps.h"
#include <Arduino.h>

// Базовая карта УОЗ (16x16)
const float baseIgnitionMap[16][16] = {
    {10, 12, 14, 16, 18, 20, 22, 24, 26, 28, 30, 32, 34, 35, 35, 35}, // 0% нагрузки
    {10, 12, 14, 16, 18, 20, 22, 24, 26, 28, 30, 32, 34, 35, 35, 35},
    {8,  10, 12, 14, 16, 18, 20, 22, 24, 26, 28, 30, 32, 34, 34, 34},
    {8,  10, 12, 14, 16, 18, 20, 22, 24, 26, 28, 30, 32, 33, 33, 33},
    {6,  8,  10, 12, 14, 16, 18, 20, 22, 24, 26, 28, 30, 31, 31, 31},
    {6,  8,  10, 12, 14, 16, 18, 20, 22, 24, 26, 28, 29, 30, 30, 30},
    {4,  6,  8,  10, 12, 14, 16, 18, 20, 22, 24, 26, 27, 28, 28, 28},
    {4,  6,  8,  10, 12, 14, 16, 18, 20, 22, 24, 25, 26, 27, 27, 27},
    {2,  4,  6,  8,  10, 12, 14, 16, 18, 20, 22, 23, 24, 25, 25, 25},
    {2,  4,  6,  8,  10, 12, 14, 16, 18, 20, 21, 22, 23, 24, 24, 24},
    {0,  2,  4,  6,  8,  10, 12, 14, 16, 18, 19, 20, 21, 22, 22, 22},
    {0,  2,  4,  6,  8,  10, 12, 14, 15, 16, 17, 18, 19, 20, 20, 20},
    {0,  2,  4,  6,  8,  10, 11, 12, 13, 14, 15, 16, 17, 18, 18, 18},
    {0,  2,  4,  6,  7,  8,  9,  10, 11, 12, 13, 14, 15, 16, 16, 16},
    {0,  2,  3,  4,  5,  6,  7,  8,  9,  10, 11, 12, 13, 14, 14, 14},
    {0,  1,  2,  3,  4,  5,  6,  7,  8,  9,  10, 11, 12, 13, 13, 13}  // 100% нагрузки
};

// Базовая карта длительности впрыска (16x16)
const float baseInjectionMap[16][16] = {
    {1.0, 1.2, 1.4, 1.6, 1.8, 2.0, 2.2, 2.4, 2.6, 2.8, 3.0, 3.2, 3.4, 3.6, 3.8, 4.0}, // 0% нагрузки
    {1.2, 1.4, 1.6, 1.8, 2.0, 2.2, 2.4, 2.6, 2.8, 3.0, 3.2, 3.4, 3.6, 3.8, 4.0, 4.2},
    {1.4, 1.6, 1.8, 2.0, 2.2, 2.4, 2.6, 2.8, 3.0, 3.2, 3.4, 3.6, 3.8, 4.0, 4.2, 4.4},
    {1.6, 1.8, 2.0, 2.2, 2.4, 2.6, 2.8, 3.0, 3.2, 3.4, 3.6, 3.8, 4.0, 4.2, 4.4, 4.6},
    {1.8, 2.0, 2.2, 2.4, 2.6, 2.8, 3.0, 3.2, 3.4, 3.6, 3.8, 4.0, 4.2, 4.4, 4.6, 4.8},
    {2.0, 2.2, 2.4, 2.6, 2.8, 3.0, 3.2, 3.4, 3.6, 3.8, 4.0, 4.2, 4.4, 4.6, 4.8, 5.0},
    {2.2, 2.4, 2.6, 2.8, 3.0, 3.2, 3.4, 3.6, 3.8, 4.0, 4.2, 4.4, 4.6, 4.8, 5.0, 5.2},
    {2.4, 2.6, 2.8, 3.0, 3.2, 3.4, 3.6, 3.8, 4.0, 4.2, 4.4, 4.6, 4.8, 5.0, 5.2, 5.4},
    {2.6, 2.8, 3.0, 3.2, 3.4, 3.6, 3.8, 4.0, 4.2, 4.4, 4.6, 4.8, 5.0, 5.2, 5.4, 5.6},
    {2.8, 3.0, 3.2, 3.4, 3.6, 3.8, 4.0, 4.2, 4.4, 4.6, 4.8, 5.0, 5.2, 5.4, 5.6, 5.8},
    {3.0, 3.2, 3.4, 3.6, 3.8, 4.0, 4.2, 4.4, 4.6, 4.8, 5.0, 5.2, 5.4, 5.6, 5.8, 6.0},
    {3.2, 3.4, 3.6, 3.8, 4.0, 4.2, 4.4, 4.6, 4.8, 5.0, 5.2, 5.4, 5.6, 5.8, 6.0, 6.2},
    {3.4, 3.6, 3.8, 4.0, 4.2, 4.4, 4.6, 4.8, 5.0, 5.2, 5.4, 5.6, 5.8, 6.0, 6.2, 6.4},
    {3.6, 3.8, 4.0, 4.2, 4.4, 4.6, 4.8, 5.0, 5.2, 5.4, 5.6, 5.8, 6.0, 6.2, 6.4, 6.6},
    {3.8, 4.0, 4.2, 4.4, 4.6, 4.8, 5.0, 5.2, 5.4, 5.6, 5.8, 6.0, 6.2, 6.4, 6.6, 6.8},
    {4.0, 4.2, 4.4, 4.6, 4.8, 5.0, 5.2, 5.4, 5.6, 5.8, 6.0, 6.2, 6.4, 6.6, 6.8, 7.0}  // 100% нагрузки
};

// Вспомогательные функции для преобразования значений
float mapf(float x, float in_min, float in_max, float out_min, float out_max) {
    if (in_max == in_min) return out_min; // Защита от деления на ноль
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

float interpolate(float x, float in_min, float in_max, float out_min, float out_max) {
    if (in_max == in_min) return out_min; // Защита от деления на ноль
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

int findNearestIndex(float value, float* array, int size) {
    int idx = 0;
    float minDiff = abs(value - array[0]);
    for (int i = 1; i < size; i++) {
        float diff = abs(value - array[i]);
        if (diff < minDiff) {
            minDiff = diff;
            idx = i;
        }
    }
    return idx;
}

float getBaseIgnition(float load, float rpm) {
    // Находим ближайшие индексы для оборотов и нагрузки
    float rpmMap[] = {0, 500, 1000, 1500, 2000, 2500, 3000, 3500, 4000, 4500, 5000, 5500, 6000, 6500, 7000};
    float mapMap[] = {0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100};
    int rpmIdx = findNearestIndex(rpm, rpmMap, 16);
    int loadIdx = findNearestIndex(load, mapMap, 16);
    
    // Получаем значение из карты
    float baseValue = baseIgnitionMap[rpmIdx][loadIdx];
    
    // Если нужна интерполяция и мы не на краях карты
    if (rpmIdx < 15 && loadIdx < 15) {
        float rpmFrac = (rpm - rpmMap[rpmIdx]) / (rpmMap[rpmIdx + 1] - rpmMap[rpmIdx]);
        float loadFrac = (load - mapMap[loadIdx]) / (mapMap[loadIdx + 1] - mapMap[loadIdx]);
        
        // Билинейная интерполяция
        float v1 = interpolate(rpmFrac, 0, 1, baseIgnitionMap[rpmIdx][loadIdx], baseIgnitionMap[rpmIdx + 1][loadIdx]);
        float v2 = interpolate(rpmFrac, 0, 1, baseIgnitionMap[rpmIdx][loadIdx + 1], baseIgnitionMap[rpmIdx + 1][loadIdx + 1]);
        baseValue = interpolate(loadFrac, 0, 1, v1, v2);
    }
    
    return baseValue;
}

float getBaseInjection(float load, float rpm) {
    // Находим ближайшие индексы для оборотов и нагрузки
    float rpmMap[] = {0, 500, 1000, 1500, 2000, 2500, 3000, 3500, 4000, 4500, 5000, 5500, 6000, 6500, 7000};
    float mapMap[] = {0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100};
    int rpmIdx = findNearestIndex(rpm, rpmMap, 16);
    int loadIdx = findNearestIndex(load, mapMap, 16);
    
    // Получаем значение из карты
    float baseValue = baseInjectionMap[rpmIdx][loadIdx];
    
    // Если нужна интерполяция и мы не на краях карты
    if (rpmIdx < 15 && loadIdx < 15) {
        float rpmFrac = (rpm - rpmMap[rpmIdx]) / (rpmMap[rpmIdx + 1] - rpmMap[rpmIdx]);
        float loadFrac = (load - mapMap[loadIdx]) / (mapMap[loadIdx + 1] - mapMap[loadIdx]);
        
        // Билинейная интерполяция
        float v1 = interpolate(rpmFrac, 0, 1, baseInjectionMap[rpmIdx][loadIdx], baseInjectionMap[rpmIdx + 1][loadIdx]);
        float v2 = interpolate(rpmFrac, 0, 1, baseInjectionMap[rpmIdx][loadIdx + 1], baseInjectionMap[rpmIdx + 1][loadIdx + 1]);
        baseValue = interpolate(loadFrac, 0, 1, v1, v2);
    }
    
    return baseValue;
}