using Newtonsoft.Json;
using System;
using System.Collections.Generic;
using System.Numerics;
using System.Text;
using CommonUsage.Mathematics;

namespace CommonUsage.Chassis
{
	public class SteerWheel : Wheel
	{
		public SteerWheel(Vector2 position, float angleLowerLimit, float angleUpperLimit, Action<float> speedWriter,
			Func<float> speedReader, Action<float> angleWriter, Func<float> angleReader, float angleLimitMarginDeg = 15f) : base(position, speedWriter,
			speedReader)
		{
			_angleLowerLimit = angleLowerLimit;
			_angleUpperLimit = angleUpperLimit;
			_angleWriter = angleWriter;
			_angleReader = angleReader;
			_centerDistance = position.Length();
            AngleLimitMarginDeg = angleLimitMarginDeg;
		}

        public float AngleLimitMarginDeg = 15f;

		public bool TrySetDirection(bool allowReverse, ref float desireDirection, ref int dir)
		{
            if (TryNormalizeAngleInLimit(desireDirection, out var normalized))
			{
				dir = 1;
                desireDirection = normalized;
				return true;
			}

			if (!allowReverse) return false;

			var oppositeTh = (float)CommonMath.RoundTh(desireDirection + 180);
			if (TryNormalizeAngleInLimit(oppositeTh, out normalized))
			{
				dir = -1;
				desireDirection = normalized;
				return true;
			}

			dir = 0;
			return false;
		}

        private bool TryNormalizeAngleInLimit(float angle, out float normalized)
        {
            var lower = CommonMath.RoundTh(_angleLowerLimit);
            var upper = CommonMath.RoundTh(_angleUpperLimit);
            while (upper < lower) upper += 360;

            normalized = (float)CommonMath.RoundTh(angle);
            while (normalized < lower) normalized += 360;
            while (normalized > upper && normalized - 360 >= lower) normalized -= 360;

            var margin = Math.Min(normalized - lower, upper - normalized);
            return normalized >= lower && normalized <= upper && margin >= Math.Max(0, AngleLimitMarginDeg);
        }

		public float ReadAngle()
		{
			return _angleReader();
		}

		public void WriteAngle(float angle)
        {
            _angleWriter.Invoke(_sendAngle = Math.Max(_angleLowerLimit, Math.Min(angle, _angleUpperLimit)));
        }

        public float GetSendAngle()
        {
            return _sendAngle;
        }

        public float GetAngleRelativeToChassis()
        {
            return ZeroDirection + _sendAngle;
        }

		public float CenterDistance()
		{
			return _centerDistance;
		}

        public float AngleLowerLimit => _angleLowerLimit;

		public float AngleUpperLimit => _angleUpperLimit;

		[JsonIgnore] private readonly Action<float> _angleWriter;
		[JsonIgnore] private readonly Func<float> _angleReader;

		private float _angleLowerLimit = -90, _angleUpperLimit = 90;
		private float _centerDistance;

        public float _sendAngle = 0f;
    }
}