using System;
using Controller.Bicycle;
using Controller.Lean;
using Sensors;
using Sensors.ANT;
using Sensors.USB;
using Tracking;
using UnityEngine;

namespace Controller
{
    [Serializable]
    public struct FrontWheelTrackerConfig
    {
        public FrontWheelTracker frontWheelTracker;
        public float multiplicator;
        public float AdjustedRotation => frontWheelTracker.SteerRotation * multiplicator;
    }

    [RequireComponent(typeof(IBicycleController))]
    public class SensorBikeController : MonoBehaviour
    {
        public PolarRotationMapping polarRotationMapping;
        public FrontWheelTrackerConfig frontWheelTrackerConfig;

        public bool steer = true;
        public bool accelerate = true;
        public bool lean = true;
        public bool breaking = true;

        private IBicycleController bicycleController;
        private bool isFrontWheelTrackerNotNull;
        private BikeSensorData sensorData;
        private float brakeIncreasePerSecond = 2.5f;

        private void Start()
        {
            isFrontWheelTrackerNotNull = frontWheelTrackerConfig.frontWheelTracker != null;
            bicycleController = GetComponent<IBicycleController>();
            sensorData = BikeSensorData.Instance;
        }

        private void Update()
        {
            var speedData = sensorData.SpeedData;

            if (speedData != null && accelerate) SetSpeed(speedData.Value);
            //SetSpeed(new SpeedSensorData());

            var breakData = sensorData.UsbData;
            if(breakData != null && breaking){
                SetBreaking(breakData.Value);
            }
            if (isFrontWheelTrackerNotNull && steer) SetSteer();

            if (lean) SetLeaningAngle();
        }

        private void SetSteer()
        {
            bicycleController.CurrentSteerAngle =
                frontWheelTrackerConfig.AdjustedRotation;
        }

        private void SetLeaningAngle()
        {
            //bicycleController.CurrentLeaningAngle = 
        }

        private void SetSpeed(SpeedSensorData speedData)
        {
            bicycleController.CurrentSpeed = speedData.Speed;
            //bicycleController.CurrentSpeed = 16 / 3.6f;
        }
        private void SetBreaking(USBSensorData breakData)
        {
            if(breakData.isBreaking){
                Debug.Log("Currently breaking");
                bicycleController.CurrentBreakForce = brakeIncreasePerSecond * Time.deltaTime;
                //bicycleController.CurrentSpeed = Mathf.Max(0, bicycleController.CurrentSpeed - brakeIncreasePerSecond * Time.deltaTime);
            }
        }
    }
}