using RobotNet10.NavigationTune.Shared.Models;
using RobotNet10.NavigationTune.Navigation.Core;
using NavScenarios = RobotNet10.NavigationTune.Scenarios;

namespace RobotNet10.NavigationTune.Test.Helpers;

/// <summary>
/// Helper methods for creating test data
/// </summary>
public static class TestHelpers
{
    /// <summary>
    /// Create a default NavigationParameterSet for testing
    /// </summary>
    public static NavigationParameterSet CreateDefaultParameterSet()
    {
        return new NavigationParameterSet
        {
            Name = "Test Parameters",
            Description = "Test parameter set",
            MovePidConfig = new PIDConfig { Kp = 1.0, Ki = 0.0001, Kd = 0.6 },
            RotatePidConfig = new PIDConfig { Kp = 10.0, Ki = 0.01, Kd = 0.1 },
            PurePursuitConfig = new PurePursuitConfig(),
            EstimatorConfig = new VelocityEstimatorConfig(),
            SignalConfig = new VelocitySignalProcessingConfig(),
            MotorDynamicsConfig = new MotorDynamicsConfig { Tau = 0.3, Delta = 0.05f },
            NavigationConfig = new NavigationConfig()
        };
    }

    /// <summary>
    /// Create a custom NavigationParameterSet with specified values
    /// </summary>
    public static NavigationParameterSet CreateCustomParameterSet(
        double moveKp = 1.0, double moveKi = 0.0001, double moveKd = 0.6,
        double rotateKp = 10.0, double rotateKi = 0.01, double rotateKd = 0.1,
        double lookaheadMin = 0.3, double lookaheadMax = 2.0)
    {
        return new NavigationParameterSet
        {
            Name = "Custom Parameters",
            MovePidConfig = new PIDConfig { Kp = moveKp, Ki = moveKi, Kd = moveKd },
            RotatePidConfig = new PIDConfig { Kp = rotateKp, Ki = rotateKi, Kd = rotateKd },
            PurePursuitConfig = new PurePursuitConfig
            {
                LookaheadMin = lookaheadMin,
                LookaheadMax = lookaheadMax
            },
            EstimatorConfig = new VelocityEstimatorConfig(),
            SignalConfig = new VelocitySignalProcessingConfig(),
            MotorDynamicsConfig = new MotorDynamicsConfig { Tau = 0.3, Delta = 0.05f },
            NavigationConfig = new NavigationConfig()
        };
    }

    /// <summary>
    /// Create a straight line scenario for testing
    /// </summary>
    public static NavScenarios.StraightLineScenario CreateStraightLineScenario(double length = 10.0)
    {
        return new NavScenarios.StraightLineScenario
        {
            Id = Guid.NewGuid(),
            Name = "Test Straight Line",
            Description = "Test scenario",
            Length = length
        };
    }

    /// <summary>
    /// Create a circle scenario for testing
    /// </summary>
    public static NavScenarios.CircleScenario CreateCircleScenario(double radius = 2.0)
    {
        return new NavScenarios.CircleScenario
        {
            Id = Guid.NewGuid(),
            Name = "Test Circle",
            Description = "Test scenario",
            Radius = radius
        };
    }

    /// <summary>
    /// Create a simple reference path for testing
    /// </summary>
    public static List<PathPoint> CreateSimplePath(int pointCount = 10, double length = 10.0)
    {
        var path = new List<PathPoint>();
        for (int i = 0; i < pointCount; i++)
        {
            var distance = (length / (pointCount - 1)) * i;
            path.Add(new PathPoint
            {
                X = distance,
                Y = 0.0,
                DistanceFromStart = distance
            });
        }
        return path;
    }

    /// <summary>
    /// Create telemetry data for testing
    /// </summary>
    public static TelemetryData CreateTelemetryData(
        double x = 0.0, double y = 0.0, double theta = 0.0,
        double linearVel = 0.0, double angularVel = 0.0,
        double cte = 0.0, double headingError = 0.0)
    {
        return new TelemetryData
        {
            TimestampMs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds(),
            RobotPose = new Pose2D(x, y, theta),
            RobotTwist = new Twist2D(linearVel, angularVel),
            ReferencePose = new Pose2D(x, y, theta),
            CrossTrackError = cte,
            HeadingError = headingError,
            LookaheadDistance = 1.0,
            ModelConfidence = 1.0,
            DistanceToGoal = 0.0
        };
    }

    /// <summary>
    /// Create a list of telemetry data points for testing
    /// </summary>
    public static List<TelemetryData> CreateTelemetryHistory(int count = 100)
    {
        var history = new List<TelemetryData>();
        for (int i = 0; i < count; i++)
        {
            history.Add(CreateTelemetryData(
                x: i * 0.1,
                y: Math.Sin(i * 0.1) * 0.1, // Small sinusoidal deviation
                theta: 0.0,
                linearVel: 1.0,
                angularVel: 0.0,
                cte: (Math.Abs(Math.Sin(i * 0.1)) * 0.1),
                headingError: 0.0
            ));
        }
        return history;
    }
}