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

namespace CommonUsage.Chassis
{
    public class DiffSteerWheel:SteerWheel
    {
        public DiffSteerWheel(float wheelDistance,Vector2 position, float angleLowerLimit, float angleUpperLimit, Action<float> speedWriter,
            Func<float> speedReader, Action<float> angleWriter, Func<float> angleReader, Action<float> leftSpeedWriter, Action<float> rightSpeedWriter,
            float angleLimitMarginDeg = 15f) : base(position,
            angleLowerLimit, angleUpperLimit, speedWriter, speedReader, angleWriter, angleReader, angleLimitMarginDeg)
        {
            _leftSpeedWriter = leftSpeedWriter;
            _rightSpeedWriter = rightSpeedWriter;
            WheelDistance = wheelDistance;
        }

        public float GetLeftSendSpeed()
        {
            return _leftSendSpeed;
        }

        public float GetRightSendSpeed()
        {
            return _rightSendSpeed;
        }

        public void WriteLeftSpeed(float speed)
        {
            _leftSpeedWriter(_leftSendSpeed = speed);
        }

        public void WriteRightSpeed(float speed)
        {
            _rightSpeedWriter(_rightSendSpeed = speed);
        }

        public float WheelDistance;
        private readonly Action<float> _leftSpeedWriter;
        private readonly Action<float> _rightSpeedWriter;

        private float _leftSendSpeed;
        private float _rightSendSpeed;
    }
}