using System;
using OpenTK;

namespace org.budnhead.core
{
	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("New Linear Equation System to solve:");
			//dumpLinearSystem(rows);

			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);
			}

			//dumpLinearSystem(rows);

			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);
			}

			//dumpLinearSystem(rows);

			rows[2] /= rows[2].X;
			c.X = rows[2].W;

			rows[1].W -= rows[1].X * c.X;
			rows[1].X = 0;
			rows[1] /= rows[1].Y;
			c.Y = rows[1].W;

			rows[0].W -= rows[0].X * c.X;
			rows[0].X = 0;
			rows[0].W -= rows[0].Y * c.Y;
			rows[0].Y = 0;
			rows[0] /= rows[0].Z;
			c.Z = rows[0].W;

			//dumpLinearSystem(rows);
			//Console.WriteLine("Solved to {0}",c);
			return true;
		}

		static void dumpLinearSystem(Vector4[] rows){
			Console.WriteLine("Linear Equation System Dump:");
			foreach (Vector4 v4 in rows){
				Console.WriteLine(" {0}",v4);
			}
		}
	}
}