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


[System.Serializable]
public class AxleInfo
{
    public WheelCollider leftWheel;
    public WheelCollider rightWheel;
    public bool motor;
}


[System.Serializable]
public class PIDController
{
    float error_old = 0f;
    //The controller will be more robust if you are using a further back sample
    float error_old_2 = 0f;
    float error_sum = 0f;
    //If we want to average an error as input
    //float error_sum2 = 0f;

    //PID parameters
    public float gain_P;
    public float gain_I;
    public float gain_D;
    //Sometimes you have to limit the total sum of all errors used in the I-action
    private float error_sumMax = 20f;

    public float GetFactorFromPIDController(float error)
    {
        float output = CalculatePIDOutput(error);

        return output;
    }

    //Use this when experimenting with PID parameters
    public float GetFactorFromPIDController(float gain_P, float gain_I, float gain_D, float error)
    {
        this.gain_P = gain_P;
        this.gain_I = gain_I;
        this.gain_D = gain_D;

        float output = CalculatePIDOutput(error);

        return output;
    }

    //Use this when experimenting with PID parameters and the gains are stored in a Vector3
    public float GetFactorFromPIDController(Vector3 gains, float error)
    {
        this.gain_P = gains.x;
        this.gain_I = gains.y;
        this.gain_D = gains.z;

        float output = CalculatePIDOutput(error);

        return output;
    }

    private float CalculatePIDOutput(float error)
    {
        //The output from PID
        float output = 0f;


        //P
        output += gain_P * error;


        //I
        error_sum += Time.fixedDeltaTime * error;

        //Clamp the sum 
        this.error_sum = Mathf.Clamp(error_sum, -error_sumMax, error_sumMax);

        //Sometimes better to just sum the last errors
        //float averageAmount = 20f;

        //CTE_sum = CTE_sum + ((CTE - CTE_sum) / averageAmount);

        output += gain_I * error_sum;


        //D
        float d_dt_error = (error - error_old) / Time.fixedDeltaTime;

        //Save the last errors
        this.error_old_2 = error_old;

        this.error_old = error;

        output += gain_D * d_dt_error;


        return output;
    }
}