namespace RobotNet10.RobotApp.Services.Simulation;

public class SimulationVisualization
{
    public double X { get; private set; }
    public double Y { get; private set; }
    public double Theta { get; private set; }
    public double Vx { get; private set; }
    public double Vy { get; private set; }
    public double Omega { get; private set; }

    private double RadiusWheel = 0;
    private double RadiusRobot = 0;

    public void SetPhysical(double radiusWheel, double robotWidth)
    {
        RadiusWheel = radiusWheel;
        RadiusRobot = robotWidth / 2;
    }

    public (double x, double y, double angle) UpdatePosition(double wL, double wR, double time)
    {
        Theta = (Theta + time * (-wR - wL) * RadiusWheel / RadiusRobot * 180 / Math.PI) % 360;
        X += time * (-wR + wL) * RadiusWheel * Math.Cos(Theta * Math.PI / 180) / 2;
        Y += time * (-wR + wL) * RadiusWheel * Math.Sin(Theta * Math.PI / 180) / 2;
        _ = UpdateVelocity(wL, wR);
        if (Theta > 180) Theta -= 360;
        else if (Theta < -180) Theta += 360;
        return (X, Y, Theta);
    }

    public (double vx, double vy, double omega) UpdateVelocity(double wL, double wR)
    {
        Vx = (-wR + wL) * RadiusWheel / 2;
        Omega = (-wR - wL) * RadiusWheel / RadiusRobot;
        return (Vx, 0, Omega);
    }

    public void LocalizationInitialize(double x, double y, double theta)
    {
        X = x;
        Y = y;
        Theta = theta;
    }
}