using System;

namespace Science.Physics.GeneralPhysics
{
	/// <summary>
	/// NewtonEquationInvolvingTorque is written with Force, Mass, 
	/// Acceleration, Torque, MomentOfInertia, AngularAcceleration, 
	/// and Position.
	/// </summary>
	public class NewtonEquationInvolvingTorque
	{
		public NewtonEquationInvolvingTorque()
		{
		}
		private TotalForce[] f;
		private Mass[] m;
		private Acceleration[] a;
		private TotalTorque[] t;
		private MomentOfInertia[] mi;
		private AngularAcceleration[] al;
		private Position[] r;
		public NewtonEquationInvolvingTorque(TotalForce[] ff, Mass[] mm,
			Acceleration[] aa, TotalTorque[] tau, MomentOfInertia[] I, 
			AngularAcceleration[] alpha, Position[] actOn)
		{
			f = ff;
			m = mm;
			a = aa;
			t = tau;
			mi = I;
			al = alpha;
			r = actOn;
	    }		
	
		public delegate double ConstraintFunctionToBeZero(TotalForce[] f, Mass[] m, Acceleration[] a,
			TotalTorque[] tau, MomentOfInertia[] I, AngularAcceleration[] alpha, Position[] position);
		
		private ConstraintFunctionToBeZero[] cf;
		private int numcons = 0;
		public void Constraint(ConstraintFunctionToBeZero[] func)
		{
			cf = func;
			numcons = func.Length;
    	}
    	public void Solve()    
		{
			int ndim = 0;
			for(int k = 0; k < m.Length; k++) if(m[k].VariableQ) ndim += 1;
			for(int k = 0; k < a.Length; k++)
			{
				if(a[k].XVariableQ) ndim += 1;
				if(a[k].YVariableQ) ndim += 1;
				if(a[k].ZVariableQ) ndim += 1;
			}
			for(int k = 0; k < f.Length; k++)
			{
				for(int kk = 0; kk < f[k].NumberOfDecomposedForces; kk++)
				{
					if(f[k].DecomposedForce[kk].XVariableQ) ndim += 1;
					if(f[k].DecomposedForce[kk].YVariableQ) ndim += 1;
					if(f[k].DecomposedForce[kk].ZVariableQ) ndim += 1;
				}
			}
			for(int k = 0; k < t.Length; k++)
			{
				for(int kk = 0; kk < t[k].NumberOfDecomposedTorques; kk++)
				{
					if(t[k].DecomposedTorque[kk].XVariableQ) ndim += 1;
					if(t[k].DecomposedTorque[kk].YVariableQ) ndim += 1;
					if(t[k].DecomposedTorque[kk].ZVariableQ) ndim += 1;
				}
			}
			for(int k = 0; k < mi.Length; k++)
			{
				if(mi[k].XXVariableQ) ndim += 1;
				if(mi[k].XYVariableQ) ndim += 1;
				if(mi[k].XZVariableQ) ndim += 1;
				if(mi[k].YXVariableQ) ndim += 1;
				if(mi[k].YYVariableQ) ndim += 1;
				if(mi[k].YZVariableQ) ndim += 1;
				if(mi[k].ZXVariableQ) ndim += 1;
				if(mi[k].ZYVariableQ) ndim += 1;
				if(mi[k].ZZVariableQ) ndim += 1;
			}
			for(int k = 0; k < al.Length; k++)
			{
				if(al[k].XVariableQ) ndim += 1;
				if(al[k].YVariableQ) ndim += 1;
				if(al[k].ZVariableQ) ndim += 1;
			}
			for(int k = 0; k < r.Length; k++) 
			{
				if(r[k].XVariableQ) ndim += 1;
				if(r[k].YVariableQ) ndim += 1;
				if(r[k].ZVariableQ) ndim += 1;
			}

            Calculus.Function<double[], double> fun
                = new Calculus.Function<double[], double>(Func);
            double[] c = new double[ndim];
            double[] ans = Calculus.MinimumOfFunction(fun, c, 1000.0);
		}
		private double Func(double[] x)
		{
			int i = 0;
			for(int k = 0; k < m.Length; k++) if(m[k].VariableQ) this.m[k].kg = x[i++];
			for(int k = 0; k < a.Length; k++)
			{
				if(a[k].XVariableQ) this.a[k].X = x[i++];
				if(a[k].YVariableQ) this.a[k].Y = x[i++];
				if(a[k].ZVariableQ) this.a[k].Z = x[i++];
			}
			for(int k = 0; k < f.Length; k++)
			{
				for(int kk = 0; kk < f[k].NumberOfDecomposedForces; kk++)
				{
					if(f[k].DecomposedForce[kk].XVariableQ) 
						this.f[k].DecomposedForce[kk].X = x[i++];
					if(f[k].DecomposedForce[kk].YVariableQ)
						this.f[k].DecomposedForce[kk].Y = x[i++];
					if(f[k].DecomposedForce[kk].ZVariableQ) 
						this.f[k].DecomposedForce[kk].Z = x[i++];
				}
			}
			for(int k = 0; k < t.Length; k++)
			{
				for(int kk = 0; kk < t[k].NumberOfDecomposedTorques; kk++)
				{
					if(t[k].DecomposedTorque[kk].XVariableQ) 
						this.t[k].DecomposedTorque[kk].X = x[i++];
					if(t[k].DecomposedTorque[kk].YVariableQ)
						this.t[k].DecomposedTorque[kk].Y = x[i++];
					if(t[k].DecomposedTorque[kk].ZVariableQ) 
						this.t[k].DecomposedTorque[kk].Z = x[i++];
				}
			}
			for(int k = 0; k < mi.Length; k++)
			{
				if(mi[k].XXVariableQ) this.mi[k].XX = x[i++];
				if(mi[k].XYVariableQ) this.mi[k].XY = x[i++];
				if(mi[k].XZVariableQ) this.mi[k].XZ = x[i++];
				if(mi[k].YXVariableQ) this.mi[k].YX = x[i++];
				if(mi[k].YYVariableQ) this.mi[k].YY = x[i++];
				if(mi[k].YZVariableQ) this.mi[k].YZ = x[i++];
				if(mi[k].ZXVariableQ) this.mi[k].ZX = x[i++];
				if(mi[k].ZYVariableQ) this.mi[k].ZY = x[i++];
				if(mi[k].ZZVariableQ) this.mi[k].ZZ = x[i++];
			}
			for(int k = 0; k < al.Length; k++)
			{
				if(al[k].XVariableQ) this.al[k].X = x[i++];
				if(al[k].YVariableQ) this.al[k].Y = x[i++];
				if(al[k].ZVariableQ) this.al[k].Z = x[i++];
			}
			for(int k = 0; k < r.Length; k++) 				
			{
				if(r[k].XVariableQ) this.r[k].X = x[i++];
				if(r[k].YVariableQ) this.r[k].Y = x[i++];
				if(r[k].ZVariableQ) this.r[k].Z = x[i++];
			}
		
			double sum = 0.0;
			for(int k = 0; k < numcons; k++)
			{
				sum += cf[k](this.f,this.m,this.a,this.t,this.mi,this.al,this.r)
					*cf[k](this.f,this.m,this.a,this.t,this.mi,this.al,this.r);
			}
			return BasicConstraint() + sum;
		}	
		private double BasicConstraint()
		{
			double zero = 0.0;
			for(int k = 0; k < this.f.Length; k++)
				zero += (this.f[k].X - this.m[k].kg*this.a[k].X)*
					(this.f[k].X - this.m[k].kg*this.a[k].X)
					+ (this.f[k].Y - this.m[k].kg*this.a[k].Y)*
					(this.f[k].Y - this.m[k].kg*this.a[k].Y)
					+(this.f[k].Z - this.m[k].kg*this.a[k].Z)*
					(this.f[k].Z - this.m[k].kg*this.a[k].Z);
	
			for(int k = 0; k < this.t.Length; k++)
			    zero += (this.t[k].X - this.mi[k].XX*this.al[k].X
						-this.mi[k].XY*this.al[k].Y-this.mi[k].XZ*this.al[k].Z)*
			    		(this.t[k].X - this.mi[k].XX*this.al[k].X
						-this.mi[k].XY*this.al[k].Y-this.mi[k].XZ*this.al[k].Z)
			           +(this.t[k].Y - this.mi[k].YX*this.al[k].X
						-this.mi[k].YY*this.al[k].Y-this.mi[k].YZ*this.al[k].Z)*
			    		(this.t[k].Y - this.mi[k].YX*this.al[k].X
						-this.mi[k].YY*this.al[k].Y-this.mi[k].YZ*this.al[k].Z)
					   +(this.t[k].Z - this.mi[k].ZX*this.al[k].X
						-this.mi[k].ZY*this.al[k].Y-this.mi[k].ZZ*this.al[k].Z)*
					    (this.t[k].Z - this.mi[k].ZX*this.al[k].X
						-this.mi[k].ZY*this.al[k].Y-this.mi[k].ZZ*this.al[k].Z);   
			return zero;
		}	
	}
}