#include "fuel_learning.h"
#include <Preferences.h>

FuelLearningTable::FuelLearningTable() :
    learningRate(0.01),
    maxCorrection(1.2),
    minCorrection(0.8)
{
    // Инициализация точек нагрузки (кПа)
    for (int i = 0; i < LOAD_POINTS; i++) {
        loadPoints[i] = 20.0 + (i * 6.0); // 20-110 кПа
    }
    
    // Инициализация точек оборотов
    for (int i = 0; i < RPM_POINTS; i++) {
        rpmPoints[i] = 800.0 + (i * 400.0); // 800-6800 RPM
    }
    
    // Инициализация ячеек коррекции
    reset();
}

void FuelLearningTable::begin() {
    load(); // Загрузка сохраненных данных
}

void FuelLearningTable::reset() {
    for (int i = 0; i < LOAD_POINTS; i++) {
        for (int j = 0; j < RPM_POINTS; j++) {
            cells[i][j] = 1.0; // Нейтральная коррекция
        }
    }
}

int FuelLearningTable::findNearestIndex(float value, const float* points, int size) {
    if (value <= points[0]) return 0;
    if (value >= points[size-1]) return size - 2;
    
    for (int i = 0; i < size - 1; i++) {
        if (value >= points[i] && value < points[i+1]) {
            return i;
        }
    }
    return 0;
}

float FuelLearningTable::interpolate2D(float load, float rpm) {
    // Находим ближайшие индексы
    int loadIdx = findNearestIndex(load, loadPoints, LOAD_POINTS);
    int rpmIdx = findNearestIndex(rpm, rpmPoints, RPM_POINTS);
    
    // Находим веса для интерполяции
    float loadWeight = (load - loadPoints[loadIdx]) / 
                      (loadPoints[loadIdx+1] - loadPoints[loadIdx]);
    float rpmWeight = (rpm - rpmPoints[rpmIdx]) / 
                     (rpmPoints[rpmIdx+1] - rpmPoints[rpmIdx]);
    
    // Билинейная интерполяция
    float c00 = cells[loadIdx][rpmIdx];
    float c10 = cells[loadIdx+1][rpmIdx];
    float c01 = cells[loadIdx][rpmIdx+1];
    float c11 = cells[loadIdx+1][rpmIdx+1];
    
    float c0 = c00 * (1 - loadWeight) + c10 * loadWeight;
    float c1 = c01 * (1 - loadWeight) + c11 * loadWeight;
    
    return c0 * (1 - rpmWeight) + c1 * rpmWeight;
}

void FuelLearningTable::updateCell(int loadIdx, int rpmIdx, float correction) {
    // Применяем коррекцию с учетом скорости обучения
    cells[loadIdx][rpmIdx] += correction * learningRate;
    
    // Ограничиваем значение коррекции
    cells[loadIdx][rpmIdx] = constrain(cells[loadIdx][rpmIdx], 
                                     minCorrection, maxCorrection);
}

float FuelLearningTable::getCorrection(float load, float rpm) {
    return interpolate2D(load, rpm);
}

void FuelLearningTable::learn(float load, float rpm, float lambdaError) {
    // Находим ближайшие ячейки
    int loadIdx = findNearestIndex(load, loadPoints, LOAD_POINTS);
    int rpmIdx = findNearestIndex(rpm, rpmPoints, RPM_POINTS);
    
    // Обновляем основную ячейку
    updateCell(loadIdx, rpmIdx, lambdaError);
    
    // Обновляем соседние ячейки с меньшим весом
    if (loadIdx > 0) {
        updateCell(loadIdx-1, rpmIdx, lambdaError * 0.5);
    }
    if (loadIdx < LOAD_POINTS-1) {
        updateCell(loadIdx+1, rpmIdx, lambdaError * 0.5);
    }
    if (rpmIdx > 0) {
        updateCell(loadIdx, rpmIdx-1, lambdaError * 0.5);
    }
    if (rpmIdx < RPM_POINTS-1) {
        updateCell(loadIdx, rpmIdx+1, lambdaError * 0.5);
    }
}

bool FuelLearningTable::save() {
    Preferences prefs;
    prefs.begin("fuel_learn", false);
    
    // Сохраняем таблицу коррекции
    size_t written = prefs.putBytes("cells", cells, 
                                  sizeof(float) * LOAD_POINTS * RPM_POINTS);
    
    prefs.end();
    return written == sizeof(float) * LOAD_POINTS * RPM_POINTS;
}

bool FuelLearningTable::load() {
    Preferences prefs;
    prefs.begin("fuel_learn", true);
    
    // Загружаем таблицу коррекции
    size_t readSize = prefs.getBytes("cells", cells, 
                                   sizeof(float) * LOAD_POINTS * RPM_POINTS);
    
    prefs.end();
    return readSize == sizeof(float) * LOAD_POINTS * RPM_POINTS;
}