using RobotNet.RobotManager.Services.Simulation.Algorithm;
using RobotNet.RobotManager.Services.Simulation.Models;

namespace RobotNet.RobotManager.Services.Simulation;

public class VisualizationService
{
    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;
    private double RadiusRobot;

    public VisualizationService WithRadiusWheel(double radiusWheel)
    {
        RadiusWheel = radiusWheel;
        return this;
    }

    public VisualizationService WithRadiusRobot(double radiusRobot)
    {
        RadiusRobot = radiusRobot;
        return this;
    }

    public (double x, double y, double angle) UpdatePosition(double wL, double wR, double time)
    {
        Theta = (Theta + time * (-wR - wL) * RadiusWheel / RadiusRobot * MathExtension.ToDegree) % 360;
        X += time * (-wR + wL) * RadiusWheel * Math.Cos(Theta * MathExtension.ToRad) / 2;
        Y += time * (-wR + wL) * RadiusWheel * Math.Sin(Theta * MathExtension.ToRad) / 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;
    }
}