using UnityEngine;
using System.Collections;



public class QuadcopterFlightController : FlightController {
	public ElectronicSpeedController[] esc;

	private Vector2 tiltDirection;
	private float tiltAmt;
	private float yawError;

	public PID yawPid;
	public PID altitudePid;
	public PID tiltPid;
	private PID[] tiltPids;

	public BiquadFilter[] motorTiltErrorFilters = new BiquadFilter[4];

	public float targetAltitiude;

	public Oscilloscope oscilloscope;

	public class EngineStabilizationParameters{
		public float tiltError;
	}

	private EngineStabilizationParameters[] stabilizationParameters = new EngineStabilizationParameters[4];

	// Use this for initialization
	void Start () {
		stabilizationParameters = new EngineStabilizationParameters[esc.Length];
		for (int i = 0; i < esc.Length; ++i) {
			stabilizationParameters[i] = new EngineStabilizationParameters();
		}

		if (oscilloscope != null) {
			oscilloscope.channels[0].sampler = SamplerP;
			oscilloscope.channels[1].sampler = SamplerI;
			oscilloscope.channels[2].sampler = SamplerD;
			oscilloscope.channels[3].sampler = SamplerErr;
		}

		tiltPids = new PID[esc.Length];

		for(int i = 0; i < tiltPids.Length; ++i){
			tiltPids[i] = new PID();
			tiltPids[i].Setpoint = 0;
		}
	}

	float SamplerP(){
		return tiltPids[0].P;
	}

	float SamplerI(){
		return tiltPids[0].I;
	}

	float SamplerD(){
		return tiltPids[0].D;
	}

	float SamplerErr(){
		return tiltPids[0].LastError;
	}

	void UpdateTiltDirection(bool updateErrors){
		if (stabilizationParameters == null) {
			stabilizationParameters = new EngineStabilizationParameters[esc.Length];
		}

		for (int i = 0; i < esc.Length; ++i) {
			if(stabilizationParameters[i] == null){
				stabilizationParameters[i] = new EngineStabilizationParameters();
			}
		}

		var qBody = orientationSensor.orientation;
		var qDesired = targetOrientation;
		
		var qBodyInv = Quaternion.Inverse (qBody);
		var qError = qBodyInv * qDesired;
		
		//decompose error
		Vector3 qErrAxis;
		float qErrAngle;
		qError.ToAngleAxis (out qErrAngle, out qErrAxis);
		var re = qErrAxis;
		var rb = qBody * (qBodyInv * re);
		var invQb = Quaternion.AngleAxis (qErrAngle, rb);

		float q0 = invQb.w;
		float q1 = invQb.x;
		float q2 = invQb.z;
		float q3 = invQb.y;
		
		float aH = Mathf.Acos (1 - 2.0f * (q1*q1 + q2*q2));
		float phi = 2.0f * Mathf.Atan2 (q3, q0);

		yawError = phi;
		if (yawError > Mathf.PI) {
			yawError = yawError - Mathf.PI * 2.0f;
		}else if(yawError < -Mathf.PI) {
			yawError = yawError + Mathf.PI * 2.0f;
		}

		tiltAmt = Mathf.Abs(aH);


		//Debug.LogFormat ("X{0}Y{1}Z{2}W{3} {4}", qBody.x, qBody.y, qBody.z, qBody.w, tiltAmt);
		Vector2 tiltLocalVector;
		if (tiltAmt <= 0.00001f) {
			tiltDirection = Vector3.up;
			tiltLocalVector = Vector2.zero;
		} else {

			float rx = (Mathf.Cos (phi / 2) * q1 - Mathf.Sin (phi / 2) * q2) / Mathf.Sin (aH / 2);
			float ry = (Mathf.Sin (phi / 2) * q1 + Mathf.Cos (phi / 2) * q2) / Mathf.Sin (aH / 2);
			
			float bH = Mathf.Atan2 (ry, rx);
			float gH = Mathf.Atan2 (rx, -ry);
			tiltDirection = new Vector2 (rx, ry);
			tiltLocalVector = tiltDirection.RotateRadians(-phi);
			tiltDirection = tiltLocalVector;
		}

		if (updateErrors) {
			for(int i = 0; i < esc.Length; ++i) {
				var c = esc[i];
				var motorPos = c.motor.transform.position;
				motorPos = transform.InverseTransformPoint(motorPos);
				var motorDir = motorPos.normalized;
				motorDir.y = 0;
				var motorK = Vector3.Dot (motorDir, new Vector3(tiltLocalVector.y, 0, -tiltLocalVector.x));
				
				float motorError = motorK * (tiltAmt / (float)esc.Length);
				motorError = (float)motorTiltErrorFilters[i].Process((double)motorError);
				//tiltAmt = orientationLopassFilter.DoFilter (tiltAmt, Time.fixedDeltaTime);
				
				stabilizationParameters[i].tiltError = motorError;
			}
		}

	}

		

	void Stabilize(){

	}

	void FixedUpdate(){
		UpdateTiltDirection (true);

		foreach (var p in tiltPids) {
			p.kp = tiltPid.kp;
			p.kd = tiltPid.kd;
			p.ki = tiltPid.ki;

			p.outMin = tiltPid.outMin;
			p.outMax = tiltPid.outMax;
		}

		yawPid.Setpoint = 0;
		var yawK = yawPid.Process (Time.fixedDeltaTime, yawError);
		altitudePid.Setpoint = targetAltitiude;
		var altK = altitudePid.Process (Time.fixedDeltaTime, transform.position.y);

		for(int i = 0; i < esc.Length; ++i) {
			float tiltK = tiltPids[i].Process (Time.fixedDeltaTime,  stabilizationParameters[i].tiltError);

			var e = esc[i];
			float sign = e.inverseDirection ? -1 : 1;

			float yawPart =  (yawK / esc.Length) * sign;
			//Debug.LogFormat ("yawPart: {0}", yawPart);
			e.inputSpeed = Mathf.Clamp01(targetThrolle)  - tiltK +altK + yawPart;
			/*if(i == 0){
				Debug.Log ("Engine 0 speed: " + e.inputSpeed + "Err = " + stabilizationParameters[i].tiltError);
			}else if(i == 3){
				Debug.Log ("Engine 3 speed: " + e.inputSpeed + "Err = " + stabilizationParameters[i].tiltError);
			}*/
		}



		//Debug.Log("D = " + tiltPids[0].D + "E=" + tiltPids[0].LastError);
	}

	// Update is called once per frame
	void Update () {

	}

	void OnDrawGizmos(){
		if (!Application.isPlaying) {

			orientationSensor.FixedUpdate();
			UpdateTiltDirection (true);
		}


		var up = targetOrientation * Vector3.up;
		var right = targetOrientation * Vector3.right;
		var forward = targetOrientation * Vector3.forward;

		var p = transform.position;

		//draw target plane
		Gizmos.color = Color.red;
		Gizmos.DrawLine (p, p + right);
		Gizmos.color = Color.green;
		Gizmos.DrawLine (p, p + up);
		Gizmos.color = Color.blue;
		Gizmos.DrawLine (p, p + forward);

		//draw current frame

		float cfOpacity = 0.2f;
		Gizmos.color = new Color (1, 0, 0, cfOpacity);

		Gizmos.DrawLine (p, p + orientationSensor.right);
		Gizmos.color = new Color (0, 1, 0, cfOpacity);
		Gizmos.DrawLine (p, p + orientationSensor.up);
		Gizmos.color = new Color (0, 0, 1, cfOpacity);
		Gizmos.DrawLine (p, p + orientationSensor.forward);

		//draw tilt
		Gizmos.color = Color.yellow;
		Gizmos.DrawLine (p, p + orientationSensor.orientation * new Vector3(-tiltDirection.y, 0, tiltDirection.x));

		if (stabilizationParameters != null) {
			for(int i = 0; i < esc.Length; ++i) {
				var c = esc[i];
				var motorPos = c.motor.transform.position;
				Gizmos.DrawLine (motorPos, motorPos + c.motor.transform.up * stabilizationParameters[i].tiltError * 50);
			}
		}

	}

	void OnGUI(){

	}
}