using System;
using System.Numerics;
using ClumsyCore;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using MDCSToolBox.Clumsy.MotionControllers;
using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Pilot;

namespace MultiWheelC;

public class ChassisController : MovementDefinition<MultiWheelGeometricController>
{
    public float BaseSpeed = Configuration.conf.basicSpeed;

    public override MultiWheelGeometricController Get()
    {
        return new MultiWheelGeometricController
        {
            Chassis = BasicPilotBase.Chassis,
            BaseSpeed = BaseSpeed,
            SlowDistance = PilotDefinition.Conf.SlowDistance,
            SlowingPow = PilotDefinition.Conf.SlowingPow,
            FinishDistance = PilotDefinition.Conf.FinishDistance,
            FinishSpeed = PilotDefinition.Conf.FinishSpeed,
            FirstThAccuracy = PilotDefinition.Conf.FirstThAccuracy,
            FirstRotateSpeedFac = PilotDefinition.Conf.FirstRotateSpeedFac,
            FirstRotateMaxSpeed = PilotDefinition.Conf.FirstRotateMaxSpeed,
            NotContinuousAngle = PilotDefinition.Conf.NotContinuousAngle,
            DebugMode = PilotDefinition.Conf.MotionDebugPrint,
            DebugCurvature = PilotDefinition.Conf.DebugCurvature,
            PowerSteeringLookAhead = PilotDefinition.Conf.PowerSteeringLookAhead,
            SpeedLookAhead = PilotDefinition.Conf.SpeedLookAhead,
            SpeedLookAheadCurveDiff = PilotDefinition.Conf.SpeedLookAheadCurveDiff,
            SpeedLookBackCurveDiff = PilotDefinition.Conf.SpeedLookBackCurveDiff,
            SpeedLimitCurveDiffMin = PilotDefinition.Conf.SpeedLimitCurveDiffMin,
            SpeedLimitCurveMin = PilotDefinition.Conf.SpeedLimitCurveMin,
            MaxRotateSpeed = PilotDefinition.Conf.MaxRotateSpeedCurveLimit,
            MaxRotateAcc = PilotDefinition.Conf.MaxRotateAccCurveLimit,
            GcpThetaThreshold = PilotDefinition.Conf.GcpThetaThreshold,
            DthLinearFac = PilotDefinition.Conf.DthLinearFac,
            DthLinearThreshold = PilotDefinition.Conf.DthLinearThreshold,
            BiasFac = PilotDefinition.Conf.BiasFac,
            BiasThreshold = PilotDefinition.Conf.BiasThreshold,

            MultiVehicleSendMotion = (speed, frontTh, rearTh, idealPos, idealAngle) =>
            {
                PilotDefinition.Self.MultiVehicleAutoEnabled = true;
                lock (PilotDefinition.Self.MultiVehicleFleet)
                {
                    if (PilotDefinition.Self.MultiVehicleFleet.Count != PilotDefinition.Conf.MultiVehicleFleetNum)
                        return;
                }

                PilotDefinition.Self.MultiVehicleAutoVx = speed;
                PilotDefinition.Self.MultiVehicleAutoFrontTh = frontTh;
                PilotDefinition.Self.MultiVehicleAutoRearTh = rearTh;
            },

            MultiVehicleGetFleetPos = () => new Location
            {
                x = PilotDefinition.Self.CenterX,
                y = PilotDefinition.Self.CenterY,
                th = PilotDefinition.Self.CenterTh,
                l_step = 1,
                tick = DateTime.Now.Ticks
            }
        };
    }
}