namespace RobotNet10.RobotApp.Services.Simulation;

public class SimulationVelocity(SimulationVisualization Visualization, SimulationConfig Model)
{
    private readonly double Acceleration = Model.Acceleration;
    private readonly double Deceleration = Model.Deceleration;
    private double AngularVelLeft;
    private double AngularVelRight;

    private (double angularVelLeft, double angularVelRight) AccelerationCalculator(double wL, double wR, double wL_Current, double wR_Current)
    {
        var angularVelLeft = wL_Current;
        var angularVelRight = wR_Current;
        if (wL_Current == 0 || wL / wL_Current < 0)
        {
            if (wL != 0) angularVelLeft += wL / Math.Abs(wL) * Acceleration;
            else angularVelLeft = wL;
        }
        else
        {
            if (Math.Abs(wL) - Math.Abs(wL_Current) > Acceleration) angularVelLeft += Acceleration * wL_Current / Math.Abs(wL_Current);
            else if (Math.Abs(wL_Current) - Math.Abs(wL) > Deceleration) angularVelLeft -= Deceleration * wL_Current / Math.Abs(wL_Current);
            else angularVelLeft = wL;
        }

        if (wR_Current == 0 || wR / wR_Current < 0)
        {
            if (wR != 0) angularVelRight += wR / Math.Abs(wR) * Acceleration;
            else angularVelRight = wR;
        }
        else
        {
            if (Math.Abs(wR) - Math.Abs(wR_Current) > Acceleration) angularVelRight += Acceleration * wR_Current / Math.Abs(wR_Current);
            else if (Math.Abs(wR_Current) - Math.Abs(wR) > Deceleration) angularVelRight -= Deceleration * wR_Current / Math.Abs(wR_Current);
            else angularVelRight = wR;
        }

        if (Math.Abs(angularVelLeft) > Math.Abs(wL)) angularVelLeft = wL;
        if (Math.Abs(angularVelRight) > Math.Abs(wR)) angularVelRight = wR;
        return (angularVelLeft, angularVelRight);
    }

    public bool SetSpeed(double angularVelLeft, double angularVelRight, double sampleTime)
    {
        (AngularVelLeft, AngularVelRight) = AccelerationCalculator(angularVelLeft, angularVelRight, AngularVelLeft, AngularVelRight);
        //Console.WriteLine($"AngularVelLeft = {AngularVelLeft:0.####}, AngularVelRight = {AngularVelRight:0.####}");
        _ = Visualization.UpdatePosition(AngularVelLeft, AngularVelRight, sampleTime);
        return true;
    }

    public void Stop()
    {
        (AngularVelLeft, AngularVelRight) = (0, 0);
        _ = Visualization.UpdatePosition(AngularVelLeft, AngularVelRight, 0.05);
    }
}