/**
 * @file    advance_map.cpp
 *
 * @date Mar 27, 2013
 * @author Andrey Belomutskiy, (c) 2012-2015
 *
 * This file is part of rusEfi - see http://rusefi.com
 *
 * rusEfi is free software; you can redistribute it and/or modify it under the terms of
 * the GNU General Public License as published by the Free Software Foundation; either
 * version 3 of the License, or (at your option) any later version.
 *
 * rusEfi is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without
 * even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along with this program.
 * If not, see <http://www.gnu.org/licenses/>.
 */

#include "main.h"
#include "advance_map.h"
#include "interpolation.h"
#include "efilib2.h"
#include "engine_configuration.h"
#include "engine_math.h"

EXTERN_ENGINE;

static ign_Map3D_t advanceMap;
static ign_Map3D_t iatAdvanceCorrectionMap;
static int knockCount;

static const float iatTimingRpmBins[IGN_LOAD_COUNT] = {880, 1260,   1640,   2020,   2400,   2780,   3000,   3380,   3760,   4140,   4520,   5000,   5700,   6500,   7200,   8000};

//880   1260    1640    2020    2400    2780    3000    3380    3760    4140    4520    5000    5700    6500    7200    8000
static const ignition_table_t defaultIatTiming = {
        { 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2},
        { 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2},
        { 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2},
        { 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2},
        {3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 2, 2, 2, 2, 2},
        { 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 2, 2, 2, 2, 2},
        { 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 0, 0, 0, 0, 0},
        { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
        { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
        { 0, 0, -0.9, -0.9, -0.9, -0.9, -0.9, -0.9, -0.9, -0.9, -0.9, -0.9, -0.9, -0.9, -0.9, -0.9},
        { -3.3, -3.4, -4.9, -4.9, -4.9, -4.9, -4.4, -4.4, -4.4, -4.4, -4.4, -0.9, -0.9, -0.9, -0.9, -0.9},
        { -4.4, -4.9, -5.9, -5.9, -5.9, -5.9, -4.9, -4.9, -4.9, -4.9, -4.9, -2.4, -2.4, -2.4, -2.4, -2.4},
        { -4.4, -4.9, -5.9, -5.9, -5.9, -5.9, -4.9, -4.9, -4.9, -4.9, -4.9, -2.9, -2.9, -2.9, -2.9, -2.9},
        {-4.4, -4.9, -5.9, -5.9, -5.9, -5.9, -4.9, -4.9, -4.9, -4.9, -4.9, -3.9, -3.9, -3.9, -3.9, -3.9},
        {-4.4, -4.9, -5.9, -5.9, -5.9, -5.9, -4.9, -4.9, -4.9, -4.9, -4.9, -3.9, -3.9, -3.9, -3.9, -3.9},
        {-4.4, -4.9, -5.9, -5.9, -5.9, -5.9, -4.9, -4.9, -4.9, -4.9, -4.9, -3.9, -3.9, -3.9, -3.9, -3.9},
};

bool_t isStep1Condition(int rpm DECLARE_ENGINE_PARAMETER_S) {
    return  boardConfiguration->enabledStep1Limiter && rpm >= engineConfiguration->step1rpm;
}

/**
 * @return ignition timing angle advance before TDC
 */
static angle_t getRunningAdvance(int rpm, float engineLoad DECLARE_ENGINE_PARAMETER_S) {
    engine->m.beforeAdvance = GET_TIMESTAMP();
    if (cisnan(engineLoad)) {
        warning(OBD_PCM_Processor_Fault, "NaN engine load");
        return NAN;
    }
    efiAssert(!cisnan(engineLoad), "invalid el", NAN);
    efiAssert(!cisnan(engineLoad), "invalid rpm", NAN);
    engine->m.beforeZeroTest = GET_TIMESTAMP();
    engine->m.zeroTestTime = GET_TIMESTAMP() - engine->m.beforeZeroTest;

    if (isStep1Condition(rpm PASS_ENGINE_PARAMETER)) {
        return engineConfiguration->step1timing;
    }

    float iatCorrection = iatAdvanceCorrectionMap.getValue(engine->engineState.clt, (float) rpm);

    float result = advanceMap.getValue(engineLoad, (float) rpm) + iatCorrection - engine->knockCount;
    engine->m.advanceLookupTime = GET_TIMESTAMP() - engine->m.beforeAdvance;
    return result;
}

angle_t getAdvance(int rpm, float engineLoad DECLARE_ENGINE_PARAMETER_S) {
    angle_t angle;
    if (isCrankingR(rpm)) {
        angle = engineConfiguration->crankingTimingAngle;
    } else {
        angle = getRunningAdvance(rpm, engineLoad PASS_ENGINE_PARAMETER);
    }
    angle -= engineConfiguration->ignitionBaseAngle;
    fixAngle(angle);
    return angle;
}

void setDefaultIatTimingCorrection(DECLARE_ENGINE_PARAMETER_F) {
    setTableBin2(config->ignitionIatCorrLoadBins, IGN_LOAD_COUNT, -40, 110, 1);
    memcpy(config->ignitionIatCorrRpmBins, iatTimingRpmBins, sizeof(iatTimingRpmBins));
    copyTimingTable(defaultIatTiming, config->ignitionIatCorrTable);
}

void prepareTimingMap(DECLARE_ENGINE_PARAMETER_F) {
    advanceMap.init(config->ignitionTable, config->ignitionLoadBins,
            config->ignitionRpmBins);
    iatAdvanceCorrectionMap.init(config->ignitionIatCorrTable, config->ignitionIatCorrLoadBins,
            config->ignitionIatCorrRpmBins);
}