﻿using System.Collections;
using System.Collections.Generic;
using System.Data;
using UnityEditor.ShaderGraph.Internal;
using UnityEngine;

[RequireComponent(typeof(Rigidbody))]
public class VehicleController : MonoBehaviour
{
    // Start is called before the first frame update
    WheelColliderProperty[] wheels;
    Transform[] wheelMeshes;
    public Transmission transmission;
    public VehicleProperty vehicleProperty;
    public Engine engine;
    public int gear = 1;
    public float accelerator;
    public float engineRPM;//950-3800
    readonly float RPMdamper = 0.05f;
    WheelHelper[] wheelHelper;
    float targtRPM;
    IVehicleConstructer constructer;

    int driverCount;

    Rigidbody r;
#if UNITY_EDITOR
    private void OnDrawGizmos()
    {
        if (r == null)
            r = GetComponent<Rigidbody>();
        Gizmos.color = Color.green;
        Gizmos.DrawSphere( transform.position+ r.centerOfMass,0.1f);
    }
#endif
    void Start()
    {
        constructer = new GTAVVehicleConstructer(this);
        constructer.Construct(out wheelMeshes,out wheels);
        foreach (var i in wheels)
        {
            if (i.TP.Dirver)
                driverCount++;
        }
        engineRPM = 950;
        r = GetComponent<Rigidbody>();
        r.centerOfMass = new Vector3(0, r.centerOfMass.y-0.5f,0);
        r.mass = engine.Mass + transmission.Mass + vehicleProperty.Mass;
        r.interpolation = RigidbodyInterpolation.Interpolate;
        wheelHelper = new WheelHelper[wheels.Length];
        /*
        for (int i = 0; i < wheelHelper.Length; i++)
        {
            wheelHelper[i] = wheelMeshes[i].gameObject.AddComponent<WheelHelper>();
            wheelHelper[i].radius = wheels[i].radius;
        }*/

    }

    // Update is called once per frame
    void Update()
    {
        if (Input.GetKeyDown(KeyCode.LeftShift)) gear++;
        if (Input.GetKeyDown(KeyCode.LeftControl)) gear--;

    }
    void FixedUpdate()
    {
        //r.drag = (r.velocity.magnitude/(maxSpeed/3.6f));
        float vel = r.velocity.magnitude;
        //r.drag = vel/66;
        //if (transform.position.x > 200) transform.Translate(new Vector3(-400, 0, 0), Space.World);
        //if (transform.position.x < -200) transform.Translate(new Vector3(400, 0, 0), Space.World);

        if (gear < 0) gear = 0;
        if (gear >= transmission.gearSets.Length) gear = transmission.gearSets.Length - 1;
        
        float tireRPM =  GetDriverRPM();
        float engineTorque;
        float transmissionTorque;
        float outputTorque=0;
        targtRPM = tireRPM * transmission.gearSets[gear] * vehicleProperty.WheelRatio ;
        if (transmission.gearSets[gear] == 0) targtRPM = engine.IdleRPM;
        if (tireRPM >= 0)
        {
            engineRPM = engineRPM + (targtRPM - engineRPM) * RPMdamper;
            //if (engineRPM > 7000) engineRPM = 7000;
            //if (engineRPM < 950) engineRPM = 950;
            //if (transmission.gearSets[gear] == 0) engineRPM = engine.IdleRPM;
            if (engineRPM > 100000) engineRPM = engine.MaxRPM;
            if (engineRPM < 0) engineRPM = 0;
            accelerator = Input.GetAxis("Accelerator");
            //engineRPM = 3800 * power;
            engineTorque = engine.Evaluate(engineRPM, accelerator);  //power * maxpower / RPM2Angular(engineRPM);
            transmissionTorque = engineTorque * transmission.gearSets[gear];
            outputTorque = transmissionTorque * vehicleProperty.WheelRatio;// * (1 - tireRPM / (7000 * gearSets[gear]));
        }
        else 
        {
            outputTorque = 0; 
            ApplyUserControl(outputTorque,8000);

        }
        //Debug.Log(totalTorque);
        if (transmission.gearSets[gear] == 0) outputTorque = 0;  
        ApplyUserControl(outputTorque);

        r.AddForce(-((0.5f* vehicleProperty.AirFrictionFactor * 1.293f* vehicleProperty.FrontalArea * r.velocity.sqrMagnitude))*r.velocity.normalized,ForceMode.Force);
        if (r.velocity.sqrMagnitude > 0.00001f) r.AddForce(-r.velocity.normalized* vehicleProperty.FixedFriction);
    }
    float GetDriverRPM()
    {
        float rpm = 0;
        foreach (var i in wheels)
        {
            if (i.TP.Dirver)
                rpm += i.WC.rpm;
        }
        return rpm/driverCount;
    }
    float RPM2Angular(float rpm)
    {
        return rpm / 60 * 2 * Mathf.PI;
    }
    void ApplyUserControl(float torque,float brake = 0)
    {   
        float inputSteer = Input.GetAxis("LeftXAxis");
        float inputBrake = Input.GetAxis("Brake");
        float steerFactor = 1 - r.velocity.magnitude / 40;
        if (steerFactor < 0.1f)
            steerFactor = 0.1f;
        for (int i = 0; i < wheels.Length; i++)
        {
            wheels[i].WC.brakeTorque = 0;
            wheels[i].WC.steerAngle = 0;
            if (wheels[i].TP.Dirver)
            {
                wheels[i].WC.motorTorque = torque / driverCount;
            }
            if (wheels[i].TP.Steer)
            {
                wheels[i].WC.steerAngle = inputSteer * steerFactor * vehicleProperty.MaxSteerAngel;
            }
            if (wheels[i].TP.Brake)
            {
                wheels[i].WC.brakeTorque = inputBrake * vehicleProperty.BrakeTorque;
            }
        }
        Vector3 p;Quaternion q;
        for (int i = 0; i < wheelMeshes.Length; i++)
        {
            wheels[i].WC.GetWorldPose(out p, out q);
            wheelMeshes[i].position = p;
            wheelMeshes[i].rotation = q;
        }
    }
    float Smooth(float x)
    {
        return -x * x + 1;
    }
}

public struct WheelColliderProperty
{
    public WheelCollider WC;
    public TireProperty TP;
}

public interface IVehicleConstructer
{
    void Construct(out Transform[] wheelMeshes, out WheelColliderProperty[] wheelColliders);
}

public class GTAVVehicleConstructer : IVehicleConstructer
{
    VehicleController vController;
    public GTAVVehicleConstructer(VehicleController vc)
    {
        vController = vc;
    }
    public void Construct(out Transform[] wheelMeshes, out WheelColliderProperty[] wheelColliders)
    {
        List<Transform> wheels = new List<Transform>();
        Transform chassis = vController.transform.Find("chassis");
        GameObject colliderEmpty = new GameObject("WheelColliders");
        colliderEmpty.transform.parent = vController.transform;
        colliderEmpty.transform.localPosition = Vector3.zero;
        colliderEmpty.transform.rotation = Quaternion.Euler(0, 0, 0);
        List<WheelCollider> lWC = new List<WheelCollider>();
        List<WheelColliderProperty> lWCP = new List<WheelColliderProperty>(); // lWC.ToArray();

        foreach (var i in vController.vehicleProperty.TireSets)
        {
            Transform wheel = chassis.Find(i.Template.Name);
            if (wheel == null)
                throw new System.Exception("Template whell "+i+" is not available.");
            MeshFilter mf = wheel.GetComponent<MeshFilter>();
            MeshRenderer mr = wheel.GetComponent<MeshRenderer>();
            WheelColliderProperty wcp;
            Vector3 size = mf.sharedMesh.bounds.size;
            float radius = Mathf.Max(size.x, size.y, size.z) / 2;
            lWC.Add(CreateWheelCollider(colliderEmpty.transform,wheel.position,radius,i));
            wheels.Add(wheel);
            wcp.WC = lWC[lWC.Count - 1];
            wcp.TP = i.Template;
            lWCP.Add(wcp);
            foreach (var n in i.UsingName)
            {
                wheel = chassis.Find(n.Name);
                if (wheel == null)
                    throw new System.Exception("Whell " + i + " is not available.");
                wheel.transform.GetChild(0).gameObject.AddComponent<MeshFilter>().sharedMesh = mf.sharedMesh;
                wheel.transform.GetChild(0).gameObject.AddComponent<MeshRenderer>().sharedMaterials = mr.sharedMaterials;
                if (n.Right)
                {
                    wheel.GetChild(0).localRotation = Quaternion.Euler(0, 0, 180);
                }
                lWC.Add(CreateWheelCollider(colliderEmpty.transform, wheel.position, radius, i));
                wheels.Add(wheel);
                wcp.WC = lWC[lWC.Count-1];
                wcp.TP = n;
                lWCP.Add(wcp);
            }
        }
        wheelMeshes = wheels.ToArray();
        wheelColliders = lWCP.ToArray();
    }
    WheelCollider CreateWheelCollider(Transform parent,Vector3 position,float radius,TireSet tireSet)
    {
        WheelCollider wc;
        GameObject go = new GameObject("WheelCollider", typeof(WheelCollider));
        wc = go.GetComponent<WheelCollider>();
        go.transform.parent = parent;
        go.transform.position = position;
        go.transform.Translate(new Vector3(0, tireSet.Suspension.SuspensionDistance / 2, 0));
        JointSpring js;
        js.spring = tireSet.Suspension.SuspensionSpring;
        js.damper = tireSet.Suspension.SuspensionDamper;
        js.targetPosition = 0.5f;
        wc.suspensionDistance = tireSet.Suspension.SuspensionDistance;
        wc.suspensionSpring = js;
        wc.mass = tireSet.Tire.Mass;
        WheelFrictionCurve wfc = new WheelFrictionCurve();
        wfc.asymptoteSlip = 0.4f;
        wfc.asymptoteValue = tireSet.Tire.ForwardFirction;
        wfc.extremumSlip = 0.8f;
        wfc.extremumValue = 1;
        wfc.stiffness = tireSet.Tire.ForwardStiffness;
        wc.forwardFriction = wfc;
        wfc.asymptoteValue = tireSet.Tire.SideFriction;
        wfc.stiffness = tireSet.Tire.SideStiffness;
        wc.sidewaysFriction = wfc;
        wc.radius = radius;
        return wc;
    }

}