namespace RobotNet.RobotManager.Services.Simulation;

public class VelocityController(VisualizationService Visualization)
{
    private double Acceleration = 2;
    private double Deceleration = 10;
    private double AngularVelLeft;
    private double AngularVelRight;
    private double SampleTime = 0.05;

    public VelocityController WithAcceleration(double acc)
    {
        Acceleration = acc;
        return this;
    }

    public VelocityController WithDeceleration(double dec)
    {
        Deceleration = dec;
        return this;
    }

    public VelocityController WithSampleTime(double time)
    {
        SampleTime = time;
        return this;
    }

    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)
    {
        (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, SampleTime);
    }
}