private double[] GetPhi(
            LinearProblemProperties input,
            SparseElement[] A,
            double[] x,
            double[] g)
        {
            double[] result = new double[input.Count];

            for (int i = 0; i < input.Count; i++)
            {
                if (!ClampSolution.GetIfClamped(input, x, i))
                    result[i] = g[i];
                else
                {
                    double min = 0.0;
                    double max = 0.0;
                    ClampSolution.GetConstraintValues(input, x, i, ref min, ref max);

                    if (x[i] >= min && x[i] <= max)
                    {
                        result[i] = g[i];
                    }
                    else
                    { 
                        result[i] = 0.0;
                    }
                }
            }

            return result;
        }
        public static double ComputeSolverError(
            LinearProblemProperties input,
            SolutionValues[] X)
        {
            double error = 0.0;

            for (int i = 0; i < input.Count; i++)
            {
                if (X[i].ConstraintStatus)
                    continue;

                SparseElement m = input.M[i];

                double[] bufValue = m.Value;
                int[] bufIndex = m.Index;

                double bValue = (1.0 / input.D[i]) * X[i].X;

                for (int j = 0; j < m.Count; j++)
                    bValue += bufValue[j] * X[bufIndex[j]].X;

                error += (bValue - input.B[i]) * (bValue - input.B[i]);
            }

            return error;
        }
        public SolutionValues[] Solve(
            LinearProblemProperties input,
            SolutionValues[] X = null)
        {
            if(X == null)
                X = new SolutionValues[input.Count];

			double internalSOR = SolverParameters.SOR;
            double solverError = double.MaxValue;

			for (int k = 0; k < SolverParameters.MaxIteration; k++) 
			{
				double[] sum = ElaborateLowerTriangularMatrix(input, X);

                	for (int i = 0; i < input.Count; i++)
				{
					double sumBuffer = sum [i];

					SparseElement m = input.M [i];

					double[] bufValue = m.Value;
					int[] bufIndex = m.Index;

					//Avoid first row elaboration
					if (i != 0) 
					{
						for (int j = 0; j < m.Count; j++) 
						{
							if (bufIndex[j] < i) 
								sumBuffer += bufValue [j] * X [bufIndex[j]].X;
						}
					}

                    sumBuffer = (input.B[i] - sumBuffer) * input.D[i];

                    	X[i].X += (sumBuffer - X[i].X) * internalSOR;

					X[i] = ClampSolution.Clamp (input, X, i);

                    sum[i] = sumBuffer;
				}

                if (SolverParameters.DynamicSORUpdate)
                {
                    double actualSolverError = SolverHelper.ComputeSolverError(input, X);
                    
                    if (actualSolverError > solverError)
                        internalSOR = Math.Max(internalSOR - SolverParameters.SORStep, SolverParameters.SORStep);
                    else
                        solverError = actualSolverError;

                    if (actualSolverError < SolverParameters.ErrorTolerance)
                        return X;
                }
            }
             
            return X;
        }
        public SolutionValues[] Solve(
            LinearProblemProperties linearProblemProperties, 
            SolutionValues[] X = null)
        {
            if (X == null)
                X = new SolutionValues[linearProblemProperties.Count];

            double[] x = new double[X.Length];
            for (int i = 0; i < x.Length; i++)
                x[i] = X[i].X;
            
            SparseElement[] A = linearProblemProperties.GetOriginalMatrixSparse();
            double[] g = Minus(Multiply(A, x), linearProblemProperties.B);
                        
            if (Dot(g, g) < 1E-50)
                return X;

            double[] pPhi = GetPhi(linearProblemProperties, A, x, g);
            
            for (int i = 0; i < SolverParameters.MaxIteration; i++)
            {
                double alphaKDen = Dot(Multiply(A, pPhi), pPhi);
                double alphaK = 0.0;
                if (alphaKDen != 0)
                    alphaK = Dot(g, pPhi) / alphaKDen;
                else
                {
                    string ciao = "ciao";
                }

                double[] halfX = Minus(x, Multiply(alphaK, pPhi));
                x = Project(linearProblemProperties, halfX);
                
                g = Minus(Multiply(A, x), linearProblemProperties.B);
                pPhi = GetPhi(linearProblemProperties, A, x, g);
            }

            for (int i = 0; i < x.Length; i++)
            {
                X[i].X = x[i];
                X[i] = ClampSolution.Clamp(linearProblemProperties, X, i);
            }
            
            return X;
        }
        public SolutionValues[] Solve(
            LinearProblemProperties input,
            SolutionValues[] X = null)
        {
            if (X == null)
                X = new SolutionValues[input.Count];

            SolutionValues[] Xk = gaussSeidelSolver.Solve(input);
            double[] delta = calculateDelta(Xk, X);
            double[] searchDirection = negateArray(delta);

            SolutionValues[] Xk1 = new SolutionValues[input.Count];

            for (int i = 0; i < solverParam.MaxIteration; i++)
            {
                Xk1 = gaussSeidelSolver.Solve(input, Xk);
                                
                double[] deltaK = calculateDelta(Xk1, Xk);

                deltaErrorCheck = arraySquareModule(delta);
                
                double betaK = 1.1;
				if (Math.Abs(deltaErrorCheck) > 1E-40)
                    betaK = arraySquareModule(deltaK) / deltaErrorCheck;

				if (betaK > 1.0)
					searchDirection = new double[searchDirection.Length];
                else
                {
					Xk1 = calculateDirection (
						input,
						Xk1, 
						deltaK, 
						ref searchDirection, 
						betaK);
                }

                Array.Copy(Xk1, Xk, Xk1.Length);
                Array.Copy(deltaK, delta, deltaK.Length);
            }
            return Xk1;
        }
		public static SolutionValues Clamp(
			LinearProblemProperties input,
			SolutionValues[] X,
            	int i)
		{

            switch (input.ConstraintType[i])
            {
                case ConstraintType.Collision:
                case ConstraintType.JointLimit:
                    if (X[i].X < 0)
                        return new SolutionValues(0.0, true);
                    else
                        return new SolutionValues(X[i].X, false);
                                        
				case ConstraintType.Friction:
                    
                    double frictionLimit = X[input.Constraints[i][0].Value].X * input.ConstraintLimit[i];

                    if (X[i].X < -frictionLimit)
                        return new SolutionValues(-frictionLimit, true);
                    if (X[i].X > frictionLimit)
                        return new SolutionValues(frictionLimit, true);

                    return new SolutionValues(X[i].X, false);
                    

                case ConstraintType.JointMotor:
					double limit = input.ConstraintLimit[i];

                    if (X[i].X < -limit)
                        return new SolutionValues(-limit, true);
                    if (X[i].X > limit)
                        return new SolutionValues(limit, true);

                    return new SolutionValues(X[i].X, false);
                    				
				default:
                    return new SolutionValues(X[i].X, false);
            }
		}
        public static double Clamp(
            LinearProblemProperties input,
            double[] X,
            int i)
        {
            switch (input.ConstraintType[i])
            {
                case ConstraintType.Collision:
                case ConstraintType.JointLimit:
                    if (X[i] < 0)
                        return 0.0;
                    else
                        return X[i];

                case ConstraintType.Friction:
                   
                    double frictionLimit = X[input.Constraints[i][0].Value] * input.ConstraintLimit[i];

                    if (X[i] < -frictionLimit)
                        return -frictionLimit;
                    if (X[i] > frictionLimit)
                        return frictionLimit;

                    return X[i];
                    

                case ConstraintType.JointMotor:
                    double limit = input.ConstraintLimit[i];

                    if (X[i] < -limit)
                        return -limit;
                    if (X[i] > limit)
                        return limit;

                    return X[i];

                default:
                    return X[i];
            }
        }
        public static bool GetIfClamped(
            LinearProblemProperties input,
            double[] X,
            int i)
        {

            switch (input.ConstraintType[i])
            {
                case ConstraintType.Collision:
                case ConstraintType.JointLimit:
                    if (Math.Abs(X[i]) < 1E-50)
                        return true;
                    else
                        return false;

                case ConstraintType.Friction:
                    
                    double frictionLimit = X[input.Constraints[i][0].Value] * input.ConstraintLimit[i];

                    if (Math.Abs(X[i] + frictionLimit) < 1E-50 ||
                        Math.Abs(X[i] - frictionLimit) < 1E-50)
                        return true;
                    else
                        return false;
                    
                case ConstraintType.JointMotor:
                    if (Math.Abs(X[i] - input.ConstraintLimit[i]) < 1E-50 ||
                        Math.Abs(X[i] + input.ConstraintLimit[i]) < 1E-50)
                        return true;
                    else
                        return false;
                                        
                default:
                    return false;
            }
        }
        public static void GetConstraintValues(
            LinearProblemProperties input,
            double[] x,
            int i,
            ref double Min,
            ref double Max)
        {
            switch (input.ConstraintType[i])
            {
                case ConstraintType.Collision:
                case ConstraintType.JointLimit:
                    Min = 0.0;
                    Max = double.MaxValue;
                    break;   

                case ConstraintType.Friction:

                    double frictionLimit = x[input.Constraints[i][0].Value] * input.ConstraintLimit[i];

                    Min = -frictionLimit;
                    Max = frictionLimit;
                    break;

                case ConstraintType.JointMotor:
                    double limit = input.ConstraintLimit[i];

                    Min = -input.ConstraintLimit[i];
                    Max = input.ConstraintLimit[i];
                    break;

                default:
                    Min = double.MinValue;
                    Max = double.MaxValue;
                    break;
            }
        }
		public void Solve(LinearProblemProperties linearProblemProperties)
		{
			if (linearProblemProperties.Count > 0) {

				double internalSOR = this.solverParameters.SOR;
				double moduleOnlineGradient = 0.0;
				double modOGa = 0.0;
				double bk = 0.0;
				double test = 0.0;
				double[] oldX = new double[linearProblemProperties.Count];
				double[] deltaf = new double[linearProblemProperties.Count];
				double[] pk = new double[linearProblemProperties.Count];
				double[] buffer = new double[linearProblemProperties.Count];

				Array.Copy (linearProblemProperties.StartX, oldX, linearProblemProperties.Count);

				this.gaussSeidel (
					linearProblemProperties, 
					this.solverParameters.SOR,
					ref buffer);

				deltaf = this.calculateGradient (oldX, linearProblemProperties.StartX);
				this.copyNegArray (ref pk, deltaf);

				for (int i = 0; i < this.solverParameters.MaxIteration; i++) {
					Array.Copy (linearProblemProperties.StartX, oldX, linearProblemProperties.Count);
					moduleOnlineGradient = this.arrayModule (deltaf);

					if (moduleOnlineGradient < this.solverParameters.ErrorTolerance)
						break;
					if (i == 0)
						modOGa = moduleOnlineGradient;
					else {
						test = modOGa - moduleOnlineGradient;
						if (test < 0.0 && internalSOR > 0.0)
							internalSOR -= this.solverParameters.SORStep;
						modOGa = moduleOnlineGradient;
					}

					this.gaussSeidel (
						linearProblemProperties,  
						internalSOR,
						ref buffer);

					deltaf = this.calculateGradient (oldX, linearProblemProperties.StartX);

					bk = 0.0;

					if (moduleOnlineGradient != 0.0)
						bk = this.arrayModule (deltaf) / moduleOnlineGradient;
					else {
						bk = 0.0;
						continue;
					}


					if (bk <= 1.0) {
						for (int j = 0; j < linearProblemProperties.Count; j++) {
							linearProblemProperties.StartX [j] += bk * pk [j];
							buffer [j] = oldX [j];
							pk [j] = bk * pk [j] - deltaf [j];
							this.clampX (linearProblemProperties, j);
						}	
					}
				}
			}
		}
        private double kernel(
            LinearProblemProperties input,
            SolutionValues[] X,
            int i)
        {
			double sumBuffer = 0.0;

			//Avoid last row elaboration
			if (i + 1 != input.Count)
            {
				double[] bufValue = input.M [i].Value;
				int[] bufIndex = input.M [i].Index;

				for (int j = 0; j < input.M [i].Count; j++) {
					if(bufIndex [j] > i) 
						sumBuffer += bufValue [j] * X [bufIndex [j]].X;
				}
			}
            return sumBuffer;
        }
        private SolutionValues[] calculateDirection(
			LinearProblemProperties input,
			SolutionValues[] Xk1,
            double[] deltaK,
            ref double[] searchDirection,
            double betak)
        {
            SolutionValues[] result = new SolutionValues[Xk1.Length];

            for (int i = 0; i < Xk1.Length; i++)
            {
                double bDirection = betak * searchDirection[i];

                result[i].X = Xk1[i].X + bDirection;

                searchDirection[i] = bDirection - deltaK[i];

                result[i] = ClampSolution.Clamp(input, result, i);
            }

            return result;
        }
		private void internalIteration(
			LinearProblemProperties input,
			ref double[] sum,
			ref double[] buffer,
			double internalSOR,
			int i)
		{
			int index = i * input.Count;
			double sumBuffer = 0.0;

			for (int j = 0; j < i; j++)
				sumBuffer += input.M [index + j] * input.X [j];
			
			sum [i] += sumBuffer;
			sum [i] = (input.B [i] - sum [i]) * input.Diag [i];
			input.X [i] = buffer [i] + (sum [i] - buffer [i]) * internalSOR;
		}
		private void clampX(
			LinearProblemProperties input,
			int i)
		{
			switch (input.ConstraintType[i]) 
			{
				case ConstraintType.Collision:
					input.X [i] = Math.Max (0.0, input.X [i]);
					break;
				case ConstraintType.StaticFriction:
					input.X [i] = GeometryUtilities.Clamp (
						input.X [i],
						input.X [i + input.Constraints[i]] * input.ConstraintLimit [i],
						-input.X [i + input.Constraints[i]] * input.ConstraintLimit [i]);
					break;
				case ConstraintType.DynamicFriction:
					input.X [i] = GeometryUtilities.Clamp (
						input.X [i],
						input.X [i + input.Constraints[i]] * input.ConstraintLimit [i],
						-input.X [i + input.Constraints[i]] * input.ConstraintLimit [i]);
					break;
				case ConstraintType.Joint:
					break;
				default:
					break;
			}
		}
		private void gaussSeidel(
			LinearProblemProperties input,
			double internalSOR,
			ref double[] buffer)
		{
			double[] sum = new double[input.Count];

			this.lowerTriangularMatrix (input, ref sum);
			for (int i = 0; i < input.Count; i++) 
			{
				this.internalIteration (input, ref sum, ref buffer, internalSOR, i);
				this.clampX (input,i);
				buffer [i] = input.X [i];
			}
		}
        private double[] Project(
            LinearProblemProperties input,
            double[] x)
        {
            double[] result = new double[input.Count];

            for (int i = 0; i < input.Count; i++)
            {
                result[i] = ClampSolution.Clamp(input, x, i);
            }

            return result;
        }
		private SolutionValues[] BuildMatrixAndExecuteSolver(
			JacobianContact[] contactConstraints,
			LinearProblemProperties linearProblemProperties,
			int nIterations)
		{
			if (linearProblemProperties != null)
			{
				solver.GetSolverParameters().SetSolverMaxIteration(nIterations);

				SolutionValues[]  solutionValues = solver.Solve(linearProblemProperties);

				for (int j = 0; j < contactConstraints.Length; j++)
				{
					contactConstraints[j].StartImpulse.SetStartValue(solutionValues[j].X);
				}

				return solutionValues;
			}

			return null;
		}
		private void lowerTriangularMatrix(
			LinearProblemProperties input,
			ref double[] sum)
		{
			for (int i = 0; i < input.Count; i++)
				sum[i] = this.kernel (input, i);
		}
		private double kernel(
			LinearProblemProperties input,
			int i)
		{
			double sumBuffer = 0.0;
			int index = i * input.X.Length;
			for (int j = 0; j < input.Count; j++)
				sumBuffer += input.M [index + j] * input.X [j];
			return sumBuffer;
		}
        private double[] ElaborateLowerTriangularMatrix(
            LinearProblemProperties input,
            SolutionValues[] X)
        {
            double[] sum = new double[input.Count];

			Parallel.For (0, 
				input.Count, 
				new ParallelOptions { MaxDegreeOfParallelism = SolverParameters.MaxThreadNumber }, 
				i => {
					sum [i] = kernel (input, X, i);
				});
				
            return sum;
        }