// Copyright (C) 2024 The Qt Company Ltd.
// SPDX-License-Identifier: LicenseRef-Qt-Commercial
#include "simulation/drivetrain.h"
#include <assert.h>
#include <algorithm>
#include <cmath>
#include "mathutils.h"

namespace {
const uint32_t ShiftUpDelay = 800;
const uint16_t AccChangeRandomizePeriod = 600;
const float AccMax = 6.5f;
const float RandomAccMax = 4.f;
const int ProportionalAccDivider = 2;
const float SpeedFinishStepPrecision = 2.f;

const float FuelMinDiff = 0.2f;
const float SpeedToDistanceCoef = 1000000.f;

const float cmPerMinuteToKmhRatio = 0.0006;

const int NeutralGearNumber = 0;

float getRandomizeAccChange()
{
    return randomize(-RandomAccMax, RandomAccMax);
}

uint32_t slowDownRpmBeforeGearShift(const uint32_t rpm, const uint32_t gearShiftRpmLvl)
{
    return (rpm * 3 + gearShiftRpmLvl * 2) / 5;
}
} // namespace

namespace Simulation {
Drivetrain::Drivetrain(const Config &config)
    : _config(config)
{
    assert(_config.gearNum <= Config::MAX_GEARS);
    reset();
}

void Drivetrain::forceNeutraulGear(const bool setNeutral)
{
    _data.gear = setNeutral ? NeutralGearNumber : 1;
}

void Drivetrain::setSpeedTarget(const int targetSpeed, const bool withRandomAccChanges)
{
    _data.targetSpeed = targetSpeed;
    _enableRandomAccChanges = withRandomAccChanges;
}

void Drivetrain::setFuelTarget(const float targetFuel, const float fuelSpeed)
{
    _data.targetFuel = targetFuel;
    _data.fuelSpeed = fuelSpeed;
}

void Drivetrain::setDistanceToTarget(const float distance)
{
    _data.distanceToTarget = distance;
}

void Drivetrain::update(const uint32_t tick)
{
    _timeFromLastRandomAccChange += tick;

    const float delta = _data.targetSpeed - _data.speed;
    float acceleration = clamp(delta, -AccMax, AccMax) / ProportionalAccDivider;

    if (_enableRandomAccChanges && _timeFromLastRandomAccChange >= AccChangeRandomizePeriod) {
        _timeFromLastRandomAccChange = 0;
        acceleration += getRandomizeAccChange();
    }

    calculateSpeedData(tick, acceleration);
}

void Drivetrain::calculateSpeedData(const uint32_t tick, const float acceleration)
{
    if (_data.gear > NeutralGearNumber) {
        updateRpm(tick, acceleration);
        updateGearShift(tick);
        updateSpeed();
        updateOdometers(tick);
    }
    updateFuelLevel();
}

void Drivetrain::updateRpm(const uint32_t tick, const float acceleration)
{
    const float currentRatio = (_config.gearRatios[_data.gear]);
    _data.acceleration = (acceleration * 3 + _data.acceleration) / 4;
    _data.rpm += tick * _data.acceleration * currentRatio;
    if (_data.gear != _config.gearNum - 1 && _data.rpm > _config.shiftUpAtRpm) {
        _data.rpm = slowDownRpmBeforeGearShift(_data.rpm, _config.shiftUpAtRpm);
    }

    _data.rpm = clamp(_data.rpm, _config.minRpm, _config.maxRpm);
}

void Drivetrain::updateGearShift(const uint32_t tick)
{
    if (_data.rpm >= _config.shiftUpAtRpm && _data.gear < (_config.gearNum - 1)) {
        if (_timeCounterForShiftUp >= ShiftUpDelay) {
            _timeCounterForShiftUp = 0;
            shiftGear(1);
        } else {
            _timeCounterForShiftUp += tick;
        }
    } else if (_data.rpm <= _config.shiftDownAtRpm && _data.gear > NeutralGearNumber + 1) {
        shiftGear(-1);
    } else {
        _timeCounterForShiftUp = 0;
    }
}

void Drivetrain::shiftGear(const int delta)
{
    _data.gear += delta;
    assert(_data.gear > NeutralGearNumber && _data.gear < _config.gearNum);
    _data.rpm = calculateRpm(_data.speed, _data.gear);
}

int Drivetrain::calculateRpm(const int speed, const int gear) const
{
    if (gear < 1 || gear >= _config.gearNum) {
        assert(!"Wrong gear!");
        return 0;
    }
    return int((speed * _config.gearRatios[gear] * _config.diffRatio)
               / (_config.tireCircumference * cmPerMinuteToKmhRatio));
}

void Drivetrain::updateSpeed()
{
    _data.speed = calculateSpeed(_data.rpm, _data.gear);
}

int Drivetrain::calculateSpeed(const int rpm, const int gear) const
{
    if (gear < 1 || gear >= _config.gearNum) {
        assert(!"Wrong gear!");
        return 0;
    }
    return int((_config.tireCircumference * rpm) / (_config.gearRatios[gear] * _config.diffRatio)
               * cmPerMinuteToKmhRatio);
}

void Drivetrain::updateFuelLevel()
{
    float &currentFuel = _data.fuelLevel;
    float &targetFuel = _data.targetFuel;

    if (fabs(targetFuel - currentFuel) <= FuelMinDiff) {
        currentFuel = targetFuel;
    } else {
        currentFuel += ((targetFuel - currentFuel) > 0 ? 1 : -1 * _data.speed / 100.f) * _data.fuelSpeed;
    }
}

void Drivetrain::updateOdometers(const uint32_t tick)
{
    const float newDistance = _data.speed * tick / SpeedToDistanceCoef;
    _data.odometer += newDistance;
    _data.tripDistance += newDistance;
    _data.distanceToTarget -= newDistance;
    _data.distanceToTarget = _data.distanceToTarget > 0 ? _data.distanceToTarget : 0;
}

void Drivetrain::reset()
{
    _data.speed = 0;
    _data.rpm = 0;
    _data.gear = NeutralGearNumber + 1;

    _data.fuelLevel = 30;
    _data.odometer = 5891;
    _data.tripDistance = 247.4;
    _data.distanceToTarget = 9.1;

    _data.acceleration = 0.f;
    _data.targetSpeed = 0.f;
}

const Drivetrain::DriveData &Drivetrain::getDriveData() const
{
    return _data;
}

bool Drivetrain::isTargetSpeedReached() const
{
    return abs(_data.targetSpeed - _data.speed) < SpeedFinishStepPrecision;
}

} // namespace Simulation