using UnityEngine;
using System.Collections;

public class SimpleSteerController : SteerController {
	private float _steer;
	public float maxSteerAngle = 30.0f;

	public WheelPhysics frontLeftWheel;
	public WheelPhysics frontRightWheel;
	public WheelPhysics rearLeftWheel;
	public WheelPhysics rearRightWheel;

	private Vector3 frontLeftStartAngle;
	private Vector3 frontRightStartAngle;

    public SteeringWheel steeringWheel;

	public override float Steer {
		get {
			return _steer;
		}
		set {
			_steer = Mathf.Clamp(value, -1.0f, 1.0f);
			UpdateWheelAngles();
		}
	}

	void UpdateWheelAngles(){
		float d = (frontRightWheel.transform.position - frontLeftWheel.transform.position).magnitude;
		float l = (frontRightWheel.transform.position - rearRightWheel.transform.position).magnitude;
		float innerAngle = maxSteerAngle * _steer;
		float outerAngle = OuterWheelAngle (innerAngle, d, l);

		if (_steer < 0.0f) {
			UpdateWheelSteering (frontLeftWheel, frontLeftStartAngle, innerAngle);
			UpdateWheelSteering (frontRightWheel, frontRightStartAngle, outerAngle);
		} else {
			UpdateWheelSteering (frontRightWheel, frontRightStartAngle, innerAngle);
			UpdateWheelSteering (frontLeftWheel, frontLeftStartAngle, outerAngle);
		}

	    steeringWheel.SetAngle(steeringWheel.degrees*0.5f*_steer);
	}

	void UpdateWheelSteering(WheelPhysics wheel, Vector3 initialAngles, float steerAngle){
		initialAngles.y += steerAngle;
		wheel.transform.localEulerAngles = initialAngles;
	}

	public static float OuterWheelAngle(float innerDegree, float d, float l){
		float sign = Mathf.Sign (innerDegree);
		float rad = Mathf.Deg2Rad * innerDegree * sign;
		float ctgB = Mathf.Cos (rad) / Mathf.Sin (rad);
		return Arcctg (ctgB + d / l) * Mathf.Rad2Deg * sign;
	}
	
	static float Arcctg(float x){
		return Mathf.PI / 2.0f - Mathf.Atan (x);
	}


	// Use this for initialization
	void Start () {
		frontLeftStartAngle = frontLeftWheel.transform.localEulerAngles;
		frontRightStartAngle = frontRightWheel.transform.localEulerAngles;
	}

	void OnGUI(){
		Steer = GUI.HorizontalSlider (new Rect (10, 10, 300, 20), Steer, -1, 1);
	}

    public float testSteerSpeed = 20.0f;

    public float torque = 1000;

    void FixedUpdate()
    {
        float finalTorque = Input.GetKey(KeyCode.W) ? torque : (Input.GetKey(KeyCode.S) ? -torque : 0);
        frontLeftWheel.driveTorque = finalTorque;
        frontRightWheel.driveTorque = finalTorque;


        double breakTorque = 0;
        if (Input.GetKey(KeyCode.Space))
        {
            breakTorque = 1600;
        }

        frontLeftWheel.brakeTorque = breakTorque;
        frontRightWheel.brakeTorque = breakTorque;
        rearLeftWheel.brakeTorque = breakTorque;
        rearRightWheel.brakeTorque = breakTorque;
    }

    // Update is called once per frame
    void Update () {
		
		float steerDelta = testSteerSpeed * Time.fixedDeltaTime;
		if (Input.GetKey (KeyCode.D)) {
			Steer += steerDelta;
		}
		if (Input.GetKey (KeyCode.A)) {
			Steer -= steerDelta;
		}


       
    }
}