#include "larsmmode.h"

// local includes
#include "globals.h"
#include "utils.h"

namespace modes {
LarsmMode larsmMode;
} // namespace modes

void LarsmMode::start()
{
    Base::start();

    adc1_filtered = 0.f;
    adc2_filtered = 0.f;
    speed = 0;
    weak = 0.f;
}

void LarsmMode::update()
{
    if (!gas || !brems)
    {
        start();

        for (bobbycar::protocol::serial::MotorState &motor : motors())
        {
            motor.ctrlTyp = bobbycar::protocol::ControlType::FieldOrientedControl;
            motor.ctrlMod = bobbycar::protocol::ControlMode::OpenMode;
            motor.pwm = 0;
            motor.cruiseCtrlEna = false;
            motor.nCruiseMotTgt = 0;
        }
    }
    else
    {
        for (uint8_t i = 0; i < profileSettings.larsmMode.iterations; i++) // run multiple times to emulate higher refreshrate
        {
            // ####### larsm's bobby car code #######

            // LOW-PASS FILTER (fliessender Mittelwert)
            adc1_filtered = adc1_filtered * 0.9 + *gas * 0.1;  // ADC1, TX, rechts, vorwaerts, blau
            adc2_filtered = adc2_filtered * 0.9 + *brems * 0.1;  // ADC2, RX, links, rueckwearts, gruen

            // magic numbers die ich nicht mehr nachvollziehen kann, faehrt sich aber gut ;-)
            #define LOSLASS_BREMS_ACC 0.996f  // naeher an 1 = gemaechlicher
            #define DRUECK_ACC2 (1.0f - LOSLASS_BREMS_ACC + 0.001f)  // naeher an 0 = gemaechlicher
            #define DRUECK_ACC1 (1.0f - LOSLASS_BREMS_ACC + 0.001f)  // naeher an 0 = gemaechlicher
            //die + 0.001f gleichen float ungenauigkeiten aus.

            #define ADC1_MIN 0
            #define ADC2_MIN 0
            #define ADC1_MAX 1000
            #define ADC2_MAX 1000

            #define ADC2_DELTA (ADC2_MAX - ADC2_MIN)
            #define ADC1_DELTA (ADC1_MAX - ADC1_MIN)

            #define CLAMP(x, low, high) (((x) > (high)) ? (high) : (((x) < (low)) ? (low) : (x)))

            if (profileSettings.larsmMode.mode == LarsmModeMode::Mode1) {  // Mode 1, links: 3 kmh
              speed = (float)speed * LOSLASS_BREMS_ACC  // bremsen wenn kein poti gedrueckt
                      - (CLAMP(*brems - ADC2_MIN, 0, ADC2_DELTA) / (ADC2_DELTA / 280.0f)) * DRUECK_ACC2   // links gedrueckt = zusatzbremsen oder rueckwaertsfahren
                      + (CLAMP(*gas - ADC1_MIN, 0, ADC1_DELTA) / (ADC1_DELTA / 350.0f)) * DRUECK_ACC1;  // vorwaerts gedrueckt = beschleunigen 12s: 350=3kmh
              weak = 0;

            } else if (profileSettings.larsmMode.mode == LarsmModeMode::Mode2) { // Mode 2, default: 6 kmh
              speed = (float)speed * LOSLASS_BREMS_ACC
                      - (CLAMP(*brems - ADC2_MIN, 0, ADC2_DELTA) / (ADC2_DELTA / 310.0f)) * DRUECK_ACC2
                      + (CLAMP(*gas - ADC1_MIN, 0, ADC1_DELTA) / (ADC1_DELTA / 420.0f)) * DRUECK_ACC1;  // 12s: 400=5-6kmh 450=7kmh
              weak = 0;

            } else if (profileSettings.larsmMode.mode == LarsmModeMode::Mode3) { // Mode 3, rechts: 12 kmh
              speed = (float)speed * LOSLASS_BREMS_ACC
                      - (CLAMP(*brems - ADC2_MIN, 0, ADC2_DELTA) / (ADC2_DELTA / 340.0f)) * DRUECK_ACC2
                      + (CLAMP(*gas - ADC1_MIN, 0, ADC1_DELTA) / (ADC1_DELTA / 600.0f)) * DRUECK_ACC1;  // 12s: 600=12kmh
              weak = 0;

            } else if (profileSettings.larsmMode.mode == LarsmModeMode::Mode4) { // Mode 4, l + r: full kmh
              // Feldschwaechung wird nur aktiviert wenn man schon sehr schnell ist. So gehts: Rechts voll druecken und warten bis man schnell ist, dann zusaetzlich links schnell voll druecken.
              if (adc2_filtered > (ADC2_MAX - 450) && speed > 800) { // field weakening at high speeds
                speed = (float)speed * LOSLASS_BREMS_ACC
                      + (CLAMP(*gas - ADC1_MIN, 0, ADC1_DELTA) / (ADC1_DELTA / 1000.0f)) * DRUECK_ACC1;
                weak = weak * 0.95 + 400.0 * 0.05;  // sanftes hinzuschalten des turbos, 12s: 400=29kmh
              } else { //normale fahrt ohne feldschwaechung
                speed = (float)speed * LOSLASS_BREMS_ACC
                      - (CLAMP(*brems - ADC2_MIN, 0, ADC2_DELTA) / (ADC2_DELTA / 340.0f)) * DRUECK_ACC2
                      + (CLAMP(*gas - ADC1_MIN, 0, ADC1_DELTA) / (ADC1_DELTA / 1000.0f)) * DRUECK_ACC1;  // 12s: 1000=22kmh
                weak = weak * 0.95;  // sanftes abschalten des turbos
              }
              // weak should never exceed 400 or 450 MAX!!
            }

            speed = CLAMP(speed, -1000, 1000);  // clamp output
        }

        for (bobbycar::protocol::serial::MotorState &motor : motors())
        {
            const auto pair = split(profileSettings.larsmMode.modelMode);
            motor.ctrlTyp = pair.first;
            motor.ctrlMod = pair.second;
            motor.pwm = speed + weak;
            motor.cruiseCtrlEna = false;
            motor.nCruiseMotTgt = 0;
        }
    }

    fixCommonParams();

    sendCommands();
}