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

namespace CommonUsage.Chassis
{
    public class Wheel
    {
	    public Wheel(Vector2 position, Action<float> speedWriter, Func<float> speedReader)
	    {
		    PhysicalPosition = Position = position;
			SpeedWriter = speedWriter;
			SpeedReader = speedReader;
	    }

	    public void WriteSpeed(float speed)
	    {
		    SpeedWriter.Invoke(_sendSpeed = speed);
        }

        public float GetSendSpeed()
        {
            return _sendSpeed;
        }

	    public float ReadSpeed()
	    {
		    return SpeedReader();
	    }

        // PhysicalPosition ===(chassis transform)===> Position
		// useful in dual agv coordination
        public readonly Vector2 PhysicalPosition;
        public Vector2 Position;

        public float ZeroDirection = 0;

        [JsonIgnore] public readonly Action<float> SpeedWriter;
        [JsonIgnore] public readonly Func<float> SpeedReader;

        public float _sendSpeed;
    }

    public class GeometricControlPoint
    {
	    public GeometricControlPoint(Vector2 position)
	    {
			Position = position;
	    }

	    public Vector2 Position;
	    public float Theta;
    }
}