using FundamentalLib;
using System;
using System.Collections.Generic;
using System.Numerics;
using System.Text;
using System.Diagnostics;

namespace CommonUsage.Chassis
{
	public class DifferentialChassis : AbstractChassis
	{
		public void SetLeftRightWheels(Wheel wheelL, Wheel wheelR)
		{
			_leftWheel = wheelL;
			_rightWheel = wheelR;
			_halfWheelTrack = Math.Abs(_leftWheel.Position.Y);
		}

        public override void Visualize()
        {
            
        }

        public override CarSpeed GetCarSpeed(bool isActual = false)
        {
            if (!isActual)
            {
                return new CarSpeed()
                {
                    Vx = (_speedL + _speedR) / 2f,
                    Vw = (_speedR - _speedL) / Math.Abs(_leftWheel.Position.Y - _rightWheel.Position.Y) /
                        (float)Math.PI * 180f * 1000f,
                    Vy = 0
                };
            }
            else
            {
                return new CarSpeed()
                {
                    Vx = GetLinearSpeed(),
                    Vw = (_rightWheel.ReadSpeed() - _leftWheel.ReadSpeed()) /
                        Math.Abs(_leftWheel.Position.Y - _rightWheel.Position.Y) /
                        (float)Math.PI * 180f * 1000f,
                    Vy = 0
                };
            }
            
        }

        public (Wheel,Wheel) GetWheels()
        {
            return (_leftWheel, _rightWheel);
        }

		public float GetLinearSpeed()
		{
			return (_leftWheel.ReadSpeed() + _rightWheel.ReadSpeed()) / 2f;
		}

		public override void Initialize()
		{
			GeometricControlPoints.Add(new GeometricControlPoint(new Vector2(0, 0)));
			Valid = true;
		}

		public override void AfterDirectionChanged()
		{

		}

		public override void PredefinedDriveStop()
		{
			if (!Valid) return;
			_sendSpeedL = _sendSpeedR = 0;
            _speedL = _speedR = 0;
			_leftWheel.WriteSpeed(_sendSpeedL);
			_rightWheel.WriteSpeed(_sendSpeedR);
			GoingActive = false;
			RotatingActive = false;
		}

        protected override void DefineGeometricWheelComputation(float speed)
        {
            var now = DateTime.Now;
            if (!GoingActive) LastMoveTime = now;

            SendSpeed(speed, GeometricControlPoints[0].Theta, now - LastMoveTime);

            GoingActive = true;
            RotatingActive = false;
        }

		public override bool ComputeRotateWheels(float rotSpeed)
		{
			if (!RotatingActive) LastMoveTime = DateTime.Now;

			SendSpeed(0, rotSpeed);

			GoingActive = false;
			RotatingActive = true;
			return true;
		}

		public override float CalculateTurningSpeedDecayFac(float turn)
		{
			return 1 - Math.Min(turn, MaxTurnThreshold) / MaxTurnThreshold * MinTurnSpeedFac;
		}

		public void SendSpeed(float linearSpeed, float angularSpeed, TimeSpan? deltaTime = null)
		{
			var edgeLinearSpeed = (float)(angularSpeed / 180f * Math.PI * _halfWheelTrack / 1000);
			var vl = linearSpeed - edgeLinearSpeed;
			var vr = linearSpeed + edgeLinearSpeed;
            _speedL = vl;
			_speedR = vr;
			AccumulateSpeed(vl, vr, deltaTime);
            LastMoveTime = DateTime.Now;
        }

		private void AccumulateSpeed(float vl, float vr, TimeSpan? deltaTime = null)
		{
            // var dTime = (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds;
            //
            // var speedSignL = Math.Sign(vl - _sendSpeedL);
            // var accL = Math.Abs(vl) > Math.Abs(_sendSpeedL) ? AccPerSecond : DeAccPerSecond;
            // _sendSpeedL += speedSignL * Math.Min(Math.Abs(vl - _sendSpeedL), accL * dTime);
            // _leftWheel.WriteSpeed(_sendSpeedL);
            //
            // var speedSignR = Math.Sign(vr - _sendSpeedR);
            // var accR = Math.Abs(vr) > Math.Abs(_sendSpeedR) ? AccPerSecond : DeAccPerSecond;
            // _sendSpeedR += speedSignR * Math.Min(Math.Abs(vr - _sendSpeedR), accR * dTime);
            // _rightWheel.WriteSpeed(_sendSpeedR);

            // if (Debug)
            // 	Console.WriteLine($"DiffChassis, target:{v:0.00},send:{_sendSpeed:0.0}");
            // var dTime = (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds;
            var dTime = (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds;
            float diffL = vl - _sendSpeedL;
            float diffR = vr - _sendSpeedR;

            float accL = Math.Abs(vl) > Math.Abs(_sendSpeedL) ? AccPerSecond : DeAccPerSecond;
            float accR = Math.Abs(vr) > Math.Abs(_sendSpeedR) ? AccPerSecond : DeAccPerSecond;

            float maxDeltaL = accL * dTime;
            float maxDeltaR = accR * dTime;

            float factorL = Math.Abs(diffL) > maxDeltaL ? maxDeltaL / Math.Abs(diffL) : 1.0f;
            float factorR = Math.Abs(diffR) > maxDeltaR ? maxDeltaR / Math.Abs(diffR) : 1.0f;

            float factor = Math.Min(factorL, factorR);

            _sendSpeedL += diffL * factor;
            _sendSpeedR += diffR * factor;

            _leftWheel.WriteSpeed(_sendSpeedL);
            _rightWheel.WriteSpeed(_sendSpeedR);
            // Console.WriteLine($"DiffChassis, target:{vl:0.00},send:{_sendSpeedL:0.00} dTime：{dTime} diffL:{diffL} factor:{factor}" );
        }

        private Wheel _leftWheel;
		private Wheel _rightWheel;
		private float _halfWheelTrack; // millimeter

		private float _sendSpeedL;
		private float _sendSpeedR;
		private int _direction = 1; // 1 forward, -1 backward
        private float _speedL;
        private float _speedR;
    }
}