using System;
using OpenTK;

namespace org.niclasundharald.engine
{
	public static class Linear
	{
		static public bool Solve4x3(Vector3 X,Vector3 Y,Vector3 Z,Vector3 W,out Vector3 c)
		{
			Vector4[] rows = new Vector4[3];
			rows[0] = new Vector4(X.X,Y.X,Z.X,W.X);
			rows[1] = new Vector4(X.Y,Y.Y,Z.Y,W.Y);
			rows[2] = new Vector4(X.Z,Y.Z,Z.Z,W.Z);
			c = new Vector3();

			/*
			Console.WriteLine("Solver Unsolved:");
			Console.WriteLine("X: {0}",rows[0]);
			Console.WriteLine("Y: {0}",rows[1]);
			Console.WriteLine("Z: {0}",rows[2]);
*/

			if (rows[0].Z == 0){
				Vector4 t = rows[0];
				rows[0] = rows[2];
				rows[2] = t;
			}
			if (rows[0].Z == 0){
				Vector4 t = rows[0];
				rows[0] = rows[1];
				rows[1] = t;
			}

			if (rows[0].Z == 0){
				return false;
			} else {
				rows[1] -= rows[0] * (rows[1].Z / rows[0].Z);
				rows[2] -= rows[0] * (rows[2].Z / rows[0].Z);
			}

			if (rows[1].Y == 0){
				Vector4 t = rows[1];
				rows[1] = rows[2];
				rows[2] = t;
			}

			if (rows[1].Y == 0){
				return false;
			} else {
				rows[2] -= rows[1] * (rows[2].Y / rows[1].Y);
			}

			rows[2] /= rows[2].X;
			       
			c.X = rows[2].W;
			c.Y = (rows[1].W - (rows[1].X * c.X)) / rows[1].Y;
			c.Z = (rows[0].W - (rows[0].X * c.X) - (rows[0].Y * c.Y)) / rows[0].Z;
			/*
			Console.WriteLine("Solver Solved:");
			Console.WriteLine("X: {0}",rows[0]);
			Console.WriteLine("Y: {0}",rows[1]);
			Console.WriteLine("Z: {0}",rows[2]);
			Console.WriteLine("P: {0}",c);
			*/
			return true;
		}
	}
}