using System;
using UnityEngine;
using UnityEngine.Serialization;


public class SensorBikeController : MonoBehaviour
{
    public float maxSpeed = 40f;
    public float maxMotorTorque = 1000;
    public float leaningAngleMultiplier = 0.1f;
    public float centerAngle = 1000f;

    public SpeedSensorConfig speedSensorConfig;
    public PolarSensorConfig polarSensorConfig;

    private BicycleController bicycleController;
    private BikeSensorData sensorData;

    private void Start()
    {
        bicycleController = GetComponent<BicycleController>();
        sensorData = BikeSensorData.Instance;
        sensorData.StartListening(polarSensorConfig: polarSensorConfig, speedSensorConfig: speedSensorConfig);
    }

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

        if (speedData != null)
        {
            SetSpeed(speedData.Value);
        }

        if (polarData != null)
        {
            SetLeaningAngle(polarData.Value);
        }
    }

    private void OnDestroy()
    {
        sensorData.Dispose();
    }

    private void SetLeaningAngle(PolarSensorData polarData)
    {
        //don't lean while standing / walking to bike
        if (bicycleController.rigidBody.velocity.magnitude > .5f)
        {
            bicycleController.CurrentLeaningAngle = (-polarData.Acc.y - centerAngle) * leaningAngleMultiplier;
        }
    }

    private void SetSpeed(SpeedSensorData speedData)
    {
        var currentSpeed = bicycleController.rigidBody.velocity.magnitude;
        var speedDif = speedData.Speed - currentSpeed;
        var ratio = speedDif / maxSpeed;
        var torque = maxMotorTorque * ratio;
        if (speedDif >= .1f) // 0.36 km/h
        {
            Debug.Log($"SpeedDif = {speedDif} -> applying Torque {torque} (Ratio: {ratio})");
            bicycleController.CurrentBrakeTorque = 0;
            bicycleController.CurrentMotorTorque = torque;
        }
        else if (speedDif <= -.1f)
        {
            Debug.Log($"SpeedDif = {speedDif} -> applying brake Torque {torque} (Ratio: {ratio})");
            bicycleController.CurrentMotorTorque = 0;
            bicycleController.CurrentBrakeTorque = -torque;
        }
        else
        {
            bicycleController.CurrentMotorTorque = 0;
            bicycleController.CurrentBrakeTorque = 0;
        }
    }
}