using System;

namespace Vector3D
{
	/// <summary>
	/// Summary description for CSVector.
	/// </summary>
	public class V3DOperations
	{
		public V3DOperations()
		{
			//
			// TODO: Add constructor logic here
			//
		}

		/// <summary>
		/// Vector multiplication of two vectors
		/// </summary>
		/// <param name="x">First vector</param>
		/// <param name="y">Second vector</param>
		/// <returns></returns>
		public static double[] VectorPoduct(double[] x, double[] y)
		{
			double[] z = new double[3];
			z[0] = x[1] * y[2] - x[2] * y[1];
			z[1] = x[2] * y[0] - x[0] * y[2];
			z[2] = x[0] * y[1] - x[1] * y[0];
			return z;
		}

		/// <summary>
		/// Vector multiplication of two vectors
		/// </summary>
		/// <param name="x">First vector</param>
		/// <param name="y">Second vector</param>
		/// <param name="z">Result vector</param>
		public static void VectorPoduct(double[] x, double[] y, double[] z)
		{
			z[0] = x[1] * y[2] - x[2] * y[1];
			z[1] = x[2] * y[0] - x[0] * y[2];
			z[2] = x[0] * y[1] - x[1] * y[0];
		}

		/// <summary>
		/// Scalar multiplication of two vectors
		/// </summary>
		/// <param name="x">First vector</param>
		/// <param name="y">Second vector</param>
		/// <returns></returns>
		public static double ScalarProduct(double[] x, double[] y)
		{
			return x[0] * y[0] + x[1] * y[1] + x[2] * y[2];
		}
		
		/// <summary>
		/// Scalar norn of vector
		/// </summary>
		/// <param name="x">The vector</param>
		/// <returns></returns>
		public static double ScalarNorm(double[] x)
		{
			return Math.Sqrt(ScalarProduct(x, x));
		}


		public static double[] VectorNorm(double[] x)
		{
			double a = ScalarNorm(x);
			return Multiply(1 / a, x);
		}

		/// <summary>
		/// Normalization of vector
		/// </summary>
		/// <param name="x">The vector</param>
		public static void Normalize(double[] x)
		{
			double a = 1 / ScalarNorm(x);
			for (int i = 0; i < x.Length; i++)
			{
				x[i] *= a;
			}
		}
		
		/// <summary>
		/// Multiplication of scalar and vector
		/// </summary>
		/// <param name="a">The scalar</param>
		/// <param name="x">The vector</param>
		/// <returns></returns>
		public static double[] Multiply(double a, double[] x)
		{
			double[] y = new double[3];
			for (int i = 0; i < 3; i++)
			{
				y[i] = x[i] * a;
			}
			return y;
		}
		/// <summary>
		/// Construction of reper by two vectors
		/// </summary>
		/// <param name="x">First vector</param>
		/// <param name="y">Second vector</param>
		/// <returns>Reper matrix</returns>
		public static double[][] Reper(double[] x, double[] y)
		{
			double[][] a = new double[3][];
			a[0] = VectorNorm(x);
			a[2] = VectorPoduct(a[0], y);
			a[2] = VectorNorm(a[2]);
			a[1] = VectorPoduct(a[2], a[0]);
			return a;
		}

        /// <summary>
        /// Construction of reper by 6D vector
        /// </summary>
        /// <param name="x">The vector</param>
        /// <returns>The reper</returns>
        public static double[][] Reper(double[] x)
        {
            double[] v = new double[3];
            for (int i = 0; i < 3; i++)
            {
                v[i] = x[i + 3];
            }
            return Reper(x, v);
        }

        /// <summary>
        /// The miss between line and zero point
        /// </summary>
        /// <param name="x">Radius-vector</param>
        /// <param name="v">Velocity</param>
        /// <param name="miss">Value of miss</param>
        /// <returns></returns>
        public static double[] Miss(double[] x, double[] v, ref double miss)
		{
			double[][] a = Reper(v, x);
			miss = ScalarProduct(a[1], x);
			return Multiply(miss, a[1]);
		}
		
		/// <summary>
		/// Normalization of transition matrix
		/// </summary>
		/// <param name="matrix">The matrix</param>
		public static void NormMatrix(double[,] matrix)
		{
			double[][] a = new double[2][];
			for (int i = 0; i < 2; i++)
			{
				a[i] = new double[3];
				for (int j = 0; j < 3; j++)
				{
					a[i][j] = matrix[i, j];
				}
			}
			a = Reper(a[0], a[1]);
			for (int i = 0; i < 3; i++)
			{
				for (int j = 0; j < 3; j++)
				{
					matrix[i, j] = a[i][j];
				}
			}
		}

		/// <summary>
		/// Calculates the coefficients of transition matrix by quaternion
		/// </summary>
		/// <param name="q">Coefficients of quaternion</param>
		/// <param name="m">Transition matrix</param>
		/// <param name="qq">Auxiliary matrix</param>
		public static void CalculateMatrix(double[] q, double[,] m, double[,] qq)
		{
			double norm = 1 / Math.Sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
			for (int i = 0; i < 4; i++)
			{
				q[i] *= norm; 
			}
			for (int i = 0; i < 4; i++)
			{
				for (int j = 0; j <= i; j++)
				{
					qq[i, j] = q[i] * q[j];
				}
			}
			m[0, 0] = qq[0, 0] + qq[1, 1] - qq[2, 2] - qq[3, 3];
			m[0, 1] = 2 * (qq[2, 1] - qq[3, 0]);
			m[0, 2] = 2 * (qq[2, 0] + qq[3, 1]);
			m[1, 0] = 2 * (qq[3, 0] + qq[2, 1]);
			m[1, 1] = qq[0, 0] - qq[1, 1] + qq[2, 2] - qq[3, 3];
			m[1, 2] = 2 * (qq[3, 2] - qq[1, 0]);
			m[2, 0] = 2 * (qq[3, 1]-qq[2, 0]);
			m[2, 1] = 2 * (qq[1, 0] + qq[3, 2]);
			m[2, 2] = qq[0, 0] - qq[1, 1] - qq[2, 2] + qq[3, 3];

		}

		
		public static void CalculateDynamics(double[] q, double[] der, double[,] m, double[] omega, double[,] qq, double[,] qd)
		{
            CalculateDynamics(q, der, omega, qd);
            for (int i = 0; i < 4; i++)
            {
                for (int j = 0; j <= i; j++)
                {
                    qq[i, j] = q[i] * q[j];
                }
            }

            m[0, 0] = qq[0, 0] + qq[1, 1] - qq[2, 2] - qq[3, 3];
            m[0, 1] = 2 * (qq[2, 1] - qq[3, 0]);
            m[0, 2] = 2 * (qq[2, 0] + qq[3, 1]);
            m[1, 0] = 2 * (qq[3, 0] + qq[2, 1]);
            m[1, 1] = qq[0, 0] - qq[1, 1] + qq[2, 2] - qq[3, 3];
            m[1, 2] = 2 * (qq[3, 2] - qq[1, 0]);
            m[2, 0] = 2 * (qq[3, 1] - qq[2, 0]);
            m[2, 1] = 2 * (qq[1, 0] + qq[3, 2]);
            m[2, 2] = qq[0, 0] - qq[1, 1] - qq[2, 2] + qq[3, 3];
        }

        public static void CalculateDynamics(double[] q, double[] der, double[] omega, double[,] qd)
        {
            double norm = 1 / Math.Sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
            for (int i = 0; i < 4; i++)
            {
                q[i] *= norm;
                der[i] *= norm;
            }
            for (int i = 0; i < 4; i++)
            {
                 for (int j = 0; j < 4; j++)
                {
                    qd[i, j] = q[i] * der[j];
                }

            }
            omega[0] = 2 * (-qd[2, 3] + qd[3, 2] + qd[0, 1] - qd[1, 0]);
            omega[1] = 2 * (-qd[3, 1] + qd[1, 3] + qd[0, 2] - qd[2, 0]);
            omega[2] = 2 * (-qd[1, 2] + qd[2, 1] + qd[0, 3] - qd[3, 0]);
        }

	

		public static void ConvertMatrixToQuaternion(double[] q, double[,] m)
		{
			q[0] = 0.5 * Math.Sqrt(1.0 + m[0, 0] + m[1, 1] + m[2, 2]);
			double	m1 = 0;
			if (Math.Abs(q[0]) > 0.001)
			{
				m1 = 1 / (4 * q[0]);
				q[1] = (m[2, 1] - m[1, 2]) * m1;
				q[2] = (m[0, 2] - m[2, 0]) * m1;
				q[3] = (m[1, 0] - m[0, 1]) * m1;
				return;
			}
			q[1] = 0.5 * Math.Sqrt(1.0 + m[0, 0] - m[1, 1] - m[2, 2]);
			if (Math.Abs(q[1]) > 0.001)
			{
				m1 = 1.0 / (4 * q[1]);
				q[0] = (m[2, 1] - m[1, 2]) * m1;
				q[2] = (m[0, 1] + m[1, 0]) * m1;
				q[3] = (m[0, 2] + m[2, 0]) * m1;
				return;
			}
			q[2] = 0.5 * Math.Sqrt(1.0 - m[0, 0] + m[1, 1] - m[2, 2]);
			if (Math.Abs(q[2]) > 0.001)
			{
				m1 = 1.0 / (4 * q[2]);
				q[0] = (m[0, 2] - m[2, 0]) * m1;
				q[1] = (m[0, 1] + m[1, 0]) * m1;
				q[3] = (m[1, 2] + m[2, 1]) * m1;
				return;
			}
			q[3] = 0.5 * Math.Sqrt(1 - m[0, 0] - m[1, 1] + m[2, 2]);
			m1 = 1.0 / (4 * q[3]);
			q[0] = (m[0, 1] - m[1, 0]) * m1;
			q[1] = (m[0, 2] + m[2, 0]) * m1;
			q[2] = (m[1, 2] + m[2, 1]) * m1;

		}

        public static void QuaternionMultiply(double[] x, double[] y, double[] z)
        {
            z[0] = x[0] * y[0] - x[1] * y[1] - x[2] * y[2] - x[3] * y[3];
            z[1] = x[0] * y[1] + x[1] * y[0] + x[2] * y[3] - x[3] * y[2];
            z[2] = x[0] * y[2] + x[2] * y[0] + x[3] * y[1] - x[1] * y[3];
            z[3] = x[0] * y[3] + x[3] * y[0] + x[1] * y[2] - x[2] * y[1];
        }
        
        public static void QuaternionInvertMultiply(double[] x, double[] y, double[] z)
        {
            z[0] = x[0] * y[0] + x[1] * y[1] + x[2] * y[2] + x[3] * y[3];
            z[1] = x[0] * y[1] - x[1] * y[0] - x[2] * y[3] + x[3] * y[2];
            z[2] = x[0] * y[2] - x[2] * y[0] + x[3] * y[1] + x[1] * y[3];
            z[3] = x[0] * y[3] - x[3] * y[0] + x[1] * y[2] + x[2] * y[1];
        }

        public static void Rotate(double angle, int axis, double[] quaternion)
        {
            for (int i = 1; i < 4; i++)
            {
                quaternion[i] = 0;
            }
            double a = 0.5 * angle;
            quaternion[0] = Math.Cos(a);
            quaternion[axis] = Math.Sin(a);
           
        }

        public static double PartialSquare(double[] x, int startIndex, int length)
        {
            double a = 0;
            for (int i = 0; i < length; i++)
            {
                double c = x[i + startIndex];
                a += c * c;
            }
            return a;
        }


        public static double PartialNorm(double[] x, int startIndex, int length)
        {
            return Math.Sqrt(PartialSquare(x, startIndex, length));
        }

        static public double[,] ToSquare(double[][] x)
        {
            double[,] a = new double[x.Length, x[0].Length];
            for (int i = 0; i < a.GetLength(0); i++)
            {
                for (int j = 0; j < a.GetLength(1); j++)
                {
                    a[i, j] = x[i][j];
                }
            }
            return a;
        }

        static public double[,] SquareReper(double[] x)
        {
            return ToSquare(Reper(x));
        }
    }
}
