// Copyright (C) 2024 The Qt Company Ltd.
// SPDX-License-Identifier: LicenseRef-Qt-Commercial
#include "simulation/drivestates.h"
#include "simulation/drivetrain.h"
#include "simulation/speedlimits.h"
#include "simulation/lights.h"
#include "mathutils.h"

#include "Automotive/TellTalesModel.h"
#include "Automotive/NaviModel.h"

using namespace Automotive;

namespace {
MainModel &mainModel = MainModel::instance();
TellTalesModel &tellTalesModel = TellTalesModel::instance();
NaviModel &naviModel = NaviModel::instance();

static Simulation::Drivetrain::Config drivetrainConfig = {
    200,                                        // float maxSpeed;
    7000,                                       // float maxRpm;
    800,                                        // float minRpm;
    6000,                                       // float shiftUpAtRpm;
    2500,                                       // float shiftDownAtRpm;
    207.3f,                                     // float tireCircumference; // cm for 225/45/R18
    7.5f,                                       // float diffRatio;
    6,                                          // int gearNum;
    {2.97f, 2.07f, 1.43f, 1.00f, 0.84f, 0.56f}, // float gearRatios[MAX_GEARS];
    0.000025f,                                  // float batteryFillRatio;
    0.00002f,                                   // float batteryDrainRatio;
    0.000055f,                                  // float coolantHeatRatio;
    0.00002f,                                   // float coolantCooldownRatio;
    90.f,                                       // float optimalTemp;
    180.f,                                      // float maxTemp;
    (float) mainModel.fullRange.value()         // float maxRange;
};

Simulation::Drivetrain drivetrain(drivetrainConfig);
Simulation::SpeedLimits speedLimits(20000, 10000);
Simulation::Lights lights;
} // namespace

namespace CommonDriveConstants {
const uint16_t DelayDriveAfterModeChange = 1500;
const float AcceChangeFilteringFactor = 0.15f;
const uint8_t StateUpdatePeriod = 100;
} // namespace CommonDriveConstants

namespace LightConstants {
const uint8_t LowBeamHeadlightsMinimalSpeed = 100;
const uint16_t LowBeamHeadlightsRetryPeriod = 5000;
const uint16_t LowBeamHeadlightsTurnOnTimeMin = 5000;
const uint16_t LowBeamHeadlightsTurnOnTimeMax = 15000;
const uint16_t TurnSignalShort = 1500;
const uint16_t TurnSignalLong = 5000;
const uint16_t TurnSingalTurnOnTimeScaleMin = 1;
const uint16_t TurnSingalTurnOnTimeScaleMax = 2;
const uint8_t TurnSignalShortMinSpeed = 70;
const uint16_t TurnSignalRetryPeriodMin = 5000;
const uint16_t TurnSignalRetryPeriodMax = 25000;
const uint16_t TurnSignalInitialDelay = 10000;
} // namespace LightConstants

namespace LaneAssistConstants {
const uint16_t InitialDelay = 10000;
const uint16_t RetryPeriodMin = 5000;
const uint16_t RetryPeriodMax = 15000;
} // namespace LaneAssistConstants

namespace ArrowGuidesConstants {
const uint8_t HighSpeedForwardOnly = 80;
const uint8_t MediumSpeedLightTurns = 50;
const float HighSpeedDistanceMin = 0.5f; // 500m
const float HighSpeedDistanceMax = 1.f;  // 1km
const float DistanceMin = 0.1f;          // 100m
const float DistanceMax = 0.3f;          // 300m
const float InitialDistance = 0.05f;
} // namespace ArrowGuidesConstants

namespace NormalDriveConstants {
const uint16_t AccChangeRandomizePeriod = 3000;
const float AccChangeMin = -0.1f;
const float AccChangeMax = 1.0f;
const float AccMin = -0.3f;
const float AccMax = 0.4f;
const float AccInitial = AccMax;
const float AccChangeSpeedFactor = 10000.f;
const uint8_t DefaultSpeedLimit = 160;
const uint8_t ExcessiveSpeedingThreshold = 50;
const float ExcessiveSpeedingBraking = -0.5f;
const uint8_t LightSpeedingThreshold = 15;
const float LightSpeedingBraking = -0.2f;
const float LightSpeedingBrakingRelaxValue = -0.4f;
} // namespace NormalDriveConstants

namespace SportDriveConstants {
const float MinSpeed = 10.f;
const float SpeedBoostFactor = 20.f;
const float AccMin = 0.2f;
const float AccMax = 2.0f;
const float CooldownSpeedBoostFactor = 2.f;
const float CooldownSpeedReductionFactor = 5.f;
const float CooldownAccMin = 0.05f;
const float CooldownAccMax = 0.15f;
const float CooldownAccReductionMin = 0.1f;
const float CooldownAccReductionMax = 1.f;
const float CooldownStartMaxTempRatio = 0.9f;
const float CooldownStopOptimalTempRatio = 1.1f;
const float SpeedFactorMin = 0.f;
const float SpeedFactorMax = 1.f;
} // namespace SportDriveConstants

namespace Simulation {
bool isDriveState(const StateId &state)
{
    return state == State_NormalDrive || state == State_SportDrive;
}

DriveState::DriveState(MainModel::ClusterMode mode_, char gearLabel)
    : ClusterModeState(mode_)
    , _gearLabel(gearLabel)
    , _cumulatedTime(0)
    , _lastGear(INT32_MIN)
    , _lowBeamHeadlightsTimestamp(0)
    , _turnSignalTimestamp(0)
    , _laneAssistTimestamp(0)
    , _guideArrowNextChange(0)
    , _acceleration(0)
    , _comesFromDriveState(false)
{}

void DriveState::onEnter(const StateId &prevState, const Layer &layer, Machine &sm)
{
    ClusterModeState::onEnter(prevState, layer, sm);
    _lastGear = INT32_MIN;
    _cumulatedTime = 0;
    _comesFromDriveState = isDriveState(prevState);

    if (!_comesFromDriveState) {
        mainModel.laneAssistCarMoving.setValue(true);
        drivetrain.reset();
        drivetrain.resetOdo(mainModel.odo.value());
        drivetrain.resetBattery(mainModel.batteryLevel.value());
        speedLimits.reset((SpeedLimitValues::Limit) mainModel.speedLimitWarning.value());
        lights.setLightState(Lights::ParkingLights, true);
        naviModel.triggerInstruction(NaviModel::ArrowForward);

        sm.requestUpdate(CommonDriveConstants::StateUpdatePeriod, true, layer);
    }

    _turnSignalTimestamp
        = LightConstants::TurnSignalInitialDelay; // Don't turn on turn signals for at least 10s from state begin
    _laneAssistTimestamp = LaneAssistConstants::InitialDelay; // Same for lane assist
    // Use previous value from MainModel as odo can be changed via SimulatorController
    _guideArrowNextChange = mainModel.odo.value() + ArrowGuidesConstants::InitialDistance;
    _lowBeamHeadlightsTimestamp = 0;
}

void DriveState::onUpdate(uint32_t tick, const Layer &, Machine &)
{
    _cumulatedTime += tick;

    // When switching drive states, don't start to drive immediately,
    // keep prev acc value for a while
    if (getStateTime() > CommonDriveConstants::DelayDriveAfterModeChange || !_comesFromDriveState) {
        drive(tick);
    } else {
        updateDrivetrain(tick, _acceleration);
    }

    updateLights(tick);
    updateLaneAssist();
    updateGuides();
    updateModels();
}

void DriveState::updateModels()
{
    if (mainModel.showSimulatedDriveData.value()) {
        const Drivetrain::DriveData &data = drivetrain.getDriveData();

        mainModel.speed.setValue(data.speed);
        mainModel.rpm.setValue(data.rpm);
        mainModel.odo.setValue(data.odo);
        mainModel.batteryLevel.setValue(data.battery);
        mainModel.temp.setValue(data.coolantTemp);
        mainModel.range.setValue(int(data.range));
        mainModel.fuelLevel.setValue(data.fuel);
        mainModel.gear.setValue(data.gear + 1);
    }

    if (tellTalesModel.showSimulatedLightIndicators.value()) {
        tellTalesModel.turnLeftBlinking.setValue(lights.getLightState(Lights::TurnLeft));
        tellTalesModel.turnRightBlinking.setValue(lights.getLightState(Lights::TurnRight));
        tellTalesModel.parkingLightsActive.setValue(lights.getLightState(Lights::ParkingLights));
        tellTalesModel.lowBeamHeadlightsActive.setValue(lights.getLightState(Lights::LowBeamHeadlights));
    }

    const int currentGear = mainModel.gear.value();
    if (currentGear != _lastGear) {
        // This is workaround for QUL string using static buffer underneath
        // (no value change could be detected then)
        static char gearText[3] = {'\0', '\0', '\0'};
        if (currentGear > 0) {
            gearText[0] = _gearLabel;
            gearText[1] = '0' + currentGear;
        } else {
            gearText[0] = 'P';
            gearText[1] = '\0';
        }

        mainModel.gearShiftText.setValue(gearText);
        mainModel.gearShiftText.markDependencyDirty();
        _lastGear = currentGear;
    }
}

void DriveState::onLeave(const StateId &nextState, const Layer &layer, Machine &sm)
{
    if (!isDriveState(nextState)) {
        sm.dismissUpdate(layer);
    }
}

void DriveState::updateLights(uint32_t tick)
{
    using namespace LightConstants;

    lights.update(tick);

    if (drivetrain.getDriveData().speed > LowBeamHeadlightsMinimalSpeed
        && lights.getLightState(Lights::LowBeamHeadlights) == false
        && int64_t(getStateTime() - _lowBeamHeadlightsTimestamp) > LowBeamHeadlightsRetryPeriod) {
        const bool turnOn = randomChoice(1u) == 0;
        if (turnOn) {
            const uint32_t turnOnTime = randomize(LowBeamHeadlightsTurnOnTimeMin, LowBeamHeadlightsTurnOnTimeMax);
            lights.setLightState(Lights::LowBeamHeadlights, turnOn, turnOnTime);
            _lowBeamHeadlightsTimestamp = getStateTime() + turnOnTime;
        } else {
            _lowBeamHeadlightsTimestamp = getStateTime();
        }
    }

    if (int64_t(getStateTime() - _turnSignalTimestamp) > 0) {
        // Choices:
        // 0 - turn left
        // 1 - turn right
        // 2 - off
        uint8_t choice = randomChoice(2u);
        if (choice == 0 || choice == 1) {
            // When going fast simulate lane changes by shorter period of signal enabled
            const uint16_t turnOnBaseTime = (drivetrain.getDriveData().speed > TurnSignalShortMinSpeed
                                                 ? TurnSignalShort
                                                 : TurnSignalLong);
            const uint16_t turnOnTime = randomScale(turnOnBaseTime,
                                                    TurnSingalTurnOnTimeScaleMin,
                                                    TurnSingalTurnOnTimeScaleMax);
            lights.setLightState(choice == 0 ? Lights::TurnLeft : Lights::TurnRight, true, turnOnTime);
        } else {
            lights.setLightState(Lights::TurnLeft, false);
            lights.setLightState(Lights::TurnRight, false);
        }

        _turnSignalTimestamp = getStateTime() + randomize(TurnSignalRetryPeriodMin, TurnSignalRetryPeriodMax);
    }
}

void DriveState::updateLaneAssist()
{
    using namespace LaneAssistConstants;

    if (int64_t(getStateTime() - _laneAssistTimestamp) > 0) {
        // Choices:
        // 0 - left
        // 1 - right
        // 2 - off
        uint8_t choice = mainModel.laneAssistEnabled.value() ? randomChoice(2u) : 2;

        // Don't signal same side as current turn indicator
        if ((choice == 0 && lights.getLightState(Lights::TurnLeft))
            || (choice == 1 && lights.getLightState(Lights::TurnRight))) {
            choice = (choice + 1) % 2;
        }
        mainModel.triggerLaneAssist(choice);

        _laneAssistTimestamp = getStateTime() + randomize(RetryPeriodMin, RetryPeriodMax);
    }
}

void DriveState::updateGuides()
{
    using namespace ArrowGuidesConstants;

    // Use previous values from MainModel as odo and speed can be changed via SimulatorController
    const float odo = mainModel.odo.value();
    const float speed = mainModel.speed.value();

    if (odo >= _guideArrowNextChange) {
        // Tight turns and roundabuts possible only at low speeds
        NaviModel::ArrowType maxAllowedManoeuver = NaviModel::ArrowNone;
        const bool highSpeed = speed > HighSpeedForwardOnly;
        if (highSpeed) {
            maxAllowedManoeuver = NaviModel::ArrowForward;
        } else if (speed > MediumSpeedLightTurns) {
            maxAllowedManoeuver = NaviModel::ArrowRight;
        } else {
            maxAllowedManoeuver = NaviModel::ArrowRound;
        }
        NaviModel::ArrowType choice = randomChoice(maxAllowedManoeuver, NaviModel::ArrowForward);
        naviModel.triggerInstruction(choice);

        // All manoeuvers should change quickly, just forward direction should stay longer
        float distanceToChange = highSpeed ? randomize(HighSpeedDistanceMin, HighSpeedDistanceMax)
                                           : randomize(DistanceMin, DistanceMax);
        _guideArrowNextChange = odo + distanceToChange;
    }

    const uint32_t distanceInMeters = uint32_t((_guideArrowNextChange - odo) * 1000);
    naviModel.distanceToNextManoeuver.setValue(distanceInMeters);
}

void DriveState::updateDrivetrain(uint32_t tick, float acceleration)
{
    using namespace CommonDriveConstants;
    _acceleration = AcceChangeFilteringFactor * acceleration + (1 - AcceChangeFilteringFactor) * _acceleration;
    drivetrain.update(tick, _acceleration);
}

NormalDriveState::NormalDriveState()
    : DriveState(MainModel::ModeNormal, 'D')
    , _accChangeTimestamp(0)
    , _targetAcc(0)
    , _accChange(0)
{}

void NormalDriveState::onEnter(const StateId &prevState, const Layer &layer, Machine &sm)
{
    DriveState::onEnter(prevState, layer, sm);
    _targetAcc = NormalDriveConstants::AccInitial;
    randomizeAccChange();
}

void NormalDriveState::onUpdate(uint32_t tick, const Layer &layer, Machine &sm)
{
    speedLimits.update(tick);
    DriveState::onUpdate(tick, layer, sm);
}

void NormalDriveState::randomizeAccChange()
{
    using namespace NormalDriveConstants;
    _accChange = randomize(AccChangeMin, AccChangeMax);
    _accChangeTimestamp = 0;
}

void NormalDriveState::drive(uint32_t tick)
{
    using namespace NormalDriveConstants;

    _accChangeTimestamp += tick;
    float limit = speedLimits.getCurrentSpeedLimitInKmh();

    if (limit <= 10e-5f) {
        // There is not speed limit, but in normal mode don't go to fast
        limit = DefaultSpeedLimit;
    }

    if (limit > 0 && drivetrain.getDriveData().speed > limit) {
        if (drivetrain.getDriveData().speed - limit > ExcessiveSpeedingThreshold) {
            // Excessive speeding slow down fast
            _targetAcc = ExcessiveSpeedingBraking;
        } else if (_targetAcc < LightSpeedingBrakingRelaxValue
                   && drivetrain.getDriveData().speed - limit < LightSpeedingThreshold) {
            // Lot of braking, but not so much speeding any more, relax
            _targetAcc = LightSpeedingBraking;
        } else if (_targetAcc > LightSpeedingBraking) {
            // Slight speeding, slow down gently
            _targetAcc -= tick / AccChangeSpeedFactor;
        }
    } else {
        // Just randomize acc change
        if (_accChangeTimestamp >= AccChangeRandomizePeriod) {
            randomizeAccChange();
        }

        _targetAcc += tick / AccChangeSpeedFactor * _accChange;
        _targetAcc = clamp(_targetAcc, AccMin, AccMax);
    }

    DriveState::updateDrivetrain(tick, _targetAcc);
}

void NormalDriveState::updateModels()
{
    DriveState::updateModels();
    mainModel.speedLimitWarning.setValue(speedLimits.getCurrentSpeedLimit());
}

SportDriveState::SportDriveState()
    : DriveState(MainModel::ModeSport, 'S')
    , _targetSpeed(0)
    , _cooldown(false)
{}

void SportDriveState::onEnter(const StateId &prevState, const Layer &layer, Machine &sm)
{
    DriveState::onEnter(prevState, layer, sm);
    mainModel.speedLimitWarning.setValue(SpeedLimitValues::None);
    randomizeTargetSpeed();
}

void SportDriveState::onUpdate(uint32_t tick, const Layer &layer, Machine &sm)
{
    updateCooldown();
    DriveState::onUpdate(tick, layer, sm);
}

void SportDriveState::drive(uint32_t tick)
{
    using namespace SportDriveConstants;

    const Drivetrain::DriveData &data = drivetrain.getDriveData();
    const int dir = _targetSpeed - data.speed > 0 ? 1 : -1;
    const float speedFactor = std::abs(_targetSpeed - data.speed) / drivetrainConfig.maxSpeed;
    float targetAcc = 0;

    if (_cooldown) {
        if (dir > 0) {
            targetAcc = linearPartitionSelect(clamp(speedFactor * CooldownSpeedBoostFactor,
                                                    SpeedFactorMin,
                                                    SpeedFactorMax),
                                              CooldownAccMin,
                                              CooldownAccMax);
        } else {
            targetAcc = linearPartitionSelect(clamp(speedFactor * CooldownSpeedReductionFactor,
                                                    SpeedFactorMin,
                                                    SpeedFactorMax),
                                              CooldownAccReductionMin,
                                              CooldownAccReductionMax);
        }
    } else {
        targetAcc = sqrtPartitionSelect(clamp(speedFactor * SpeedBoostFactor, SpeedFactorMin, SpeedFactorMax),
                                        AccMin,
                                        AccMax);
    }
    targetAcc *= dir;

    DriveState::updateDrivetrain(tick, targetAcc);

    if ((dir > 0 && data.speed >= _targetSpeed) || (dir < 0 && data.speed <= _targetSpeed)) {
        // Target speed boundary just crossed
        randomizeTargetSpeed();
    }
}

void SportDriveState::updateCooldown()
{
    using namespace SportDriveConstants;
    if (_cooldown
        && drivetrain.getDriveData().coolantTemp <= drivetrainConfig.optimalTemp * CooldownStopOptimalTempRatio) {
        _cooldown = false;
    } else if (!_cooldown
               && drivetrain.getDriveData().coolantTemp > drivetrainConfig.maxTemp * CooldownStartMaxTempRatio) {
        _cooldown = true;
        randomizeTargetSpeed();
    }
}

void SportDriveState::randomizeTargetSpeed()
{
    const float halfSpeed = drivetrainConfig.maxSpeed * 0.5f;
    if (drivetrain.getDriveData().speed > halfSpeed || _cooldown) {
        _targetSpeed = randomize(SportDriveConstants::MinSpeed, halfSpeed);
    } else {
        _targetSpeed = randomize(halfSpeed, drivetrainConfig.maxSpeed);
    }
}

} // namespace Simulation