using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class SteeringLineRobot : MonoBehaviour
{
    [Tooltip("Maximum torque applied to the driving wheels")]
    [Range(0f, 10f)]
    public float maxTorque = 5f;
    [Tooltip("Maximum brake torque applied to the driving wheels")]
    public float brakeTorque = 30f;
    [Tooltip("Visual left wheel")]
    public GameObject wheelRenderLeft;
    [Tooltip("Visual left wheel")]
    public GameObject wheelRenderRight;
    [Tooltip("Visual back wheel")]
    public GameObject wheelRenderBack;
    [Tooltip("Transform of the intended center of mass")]
    public Transform centerOfMass;

    [Tooltip("The vehicle's speed when the physics engine can use different amount of sub-steps (in m/s).")]
    public float criticalSpeed = 5f;
    [Tooltip("Simulation sub-steps when the speed is above critical.")]
    public int stepsBelow = 5;
    [Tooltip("Simulation sub-steps when the speed is below critical.")]
    public int stepsAbove = 1;

    public WheelCollider wheelColliderLeft; 
    public WheelCollider wheelColliderRight;
    public WheelCollider wheelColliderBack;

    private Rigidbody body;

    [Range(0.1f, 10f)]
    public float KSpeedController = 1f;


    public float leftspeedsetpoint = 0f; 
    public float rightspeedsetpoint = 0f;
    public bool handBrake = false;  // true or false

   // [HideInInspector]
    public float speed;

    public float lefttorqueout;
    public float righttorqueout;

    public float leftspeedreal;
    public float rightspeedreal;
    public Vector3 leftEuler;

    public float leftspeederror;
    public float rightspeederror;

    void Awake()
    {
        wheelColliderBack.motorTorque = 0.000001f; //make it an idle wheel = workaround
    }
    private void Start()
    {
        body = GetComponent<Rigidbody>();
        body.centerOfMass = body.transform.InverseTransformPoint(centerOfMass.position) * body.transform.lossyScale.x;
    }

    private float realwheelspeed (WheelCollider wheel)
    {
        float speed = wheel.rpm / 60f * 2 * 3.141592654f * wheel.radius;
        return speed;
    }
    public float LeftSpeedController(float setpoint)
    {
        float torque = KSpeedController * (setpoint- realwheelspeed(wheelColliderLeft));
        leftspeederror = setpoint - realwheelspeed(wheelColliderLeft);
        torque = Clip(torque, maxTorque);
        return torque;
    }
    public float RightSpeedController(float setpoint)
    {
        float torque = KSpeedController * (setpoint - realwheelspeed(wheelColliderRight));
        rightspeederror = setpoint - realwheelspeed(wheelColliderRight);
        torque = Clip(torque, maxTorque);
        return torque;
    }
    private float Clip(float value, float maxvalue)
    {
        float clippedvalue;
        clippedvalue = value;
        if (value > maxvalue)
        {
            clippedvalue = maxvalue;
        }
        else if (value < -maxvalue)
        {
            clippedvalue = -maxvalue;
        }
        return clippedvalue;
    }
    private void FixedUpdate()
    {
        wheelColliderLeft.ConfigureVehicleSubsteps(criticalSpeed, stepsBelow, stepsAbove);
        speed = transform.InverseTransformVector(body.velocity).x;

        if (handBrake == true)
        {
            wheelColliderLeft.motorTorque = 0f;
            wheelColliderLeft.brakeTorque = brakeTorque;
        }
        else
        {
            wheelColliderLeft.motorTorque = LeftSpeedController(leftspeedsetpoint);
            wheelColliderLeft.brakeTorque = 0f;
        }

        if (handBrake == true)
        {
            wheelColliderRight.motorTorque = 0f;
            wheelColliderRight.brakeTorque = brakeTorque;
        }
        else
        {
            wheelColliderRight.motorTorque = RightSpeedController(rightspeedsetpoint);
            wheelColliderRight.brakeTorque = 0f;
        }

        lefttorqueout = LeftSpeedController(leftspeedsetpoint);
        righttorqueout = RightSpeedController(rightspeedsetpoint);
        leftspeedreal = realwheelspeed(wheelColliderLeft);
        rightspeedreal = realwheelspeed(wheelColliderRight);
        
        RenderWheelMotion();

    }

    private void RenderWheelMotion()
    {
        Quaternion q;
        Vector3 p;
        
        wheelColliderLeft.GetWorldPose(out p, out q);
        wheelRenderLeft.transform.rotation = q;
        
        wheelColliderRight.GetWorldPose(out p, out q);
        wheelRenderRight.transform.rotation = q;

        wheelColliderBack.GetWorldPose(out p, out q);
        wheelRenderBack.transform.rotation = q;
        //wheelColliderBack.motorTorque = 0f;
        //wheelColliderBack.brakeTorque = 0f;
    }
}
