#include <stdio.h>
#include <windows.h>
#include <float.h>
#include <math.h>
#include <commctrl.h>


/*!
\n	ヘッセ行列を解く
\n	渡される関数は ax^2 + bx の形で入れてください。

		@param	pHessianMatrix		[out]	出来上がったヘッセ行列を入れる。
		@param	pFirstOrderCoeff	[out]	偏微分の結果、定数になる項（一次項の部分）
		@param	pFormula			[in]	ヘッセ行列の元になる2次式の関数。3次元の場合は（ax^2 + by^2 + cz^2 + dxy + exz + fyz + gx + hy + iz) で９つ必要。
\n											実際は最後の + DIM分は計算には必要ないが、、、。
\n											順番は、基底のみ(DIM-1)、基底の組み合わせ(DIM-2)と入れること。
 */
template< const int NUMofDIM >
void PartialDifferential( OUT double *pHessianMatrix, OUT double *pFirstOrderCoeff, IN const double *pFormula )
{
	int uBaseIdx = NUMofDIM * (NUMofDIM+1) / 2;
	int uBase;

	// ヘッセ行列の作成
	for( int y = 0 ; y<NUMofDIM ; ++y )
	{
		// 主対角成分
		pHessianMatrix[(y * NUMofDIM) + y] = pFormula[y] * 2;

		uBase  = uBaseIdx;										// 2次項の全体数
		uBase -= (NUMofDIM-(y+1)) * (NUMofDIM-(y+1)+1) / 2;		// 現在参照している次元項（xαのxの部分）
		uBase -= y + 1;											// 次の式の基礎補正分を引く

		// U部分を設定
		for( int x = y + 1 ; x<NUMofDIM ; ++x )
			pHessianMatrix[(y * NUMofDIM) + x] = pFormula[uBase + x];

		// L部分を設定。対称行列なのでU行列のコピー
		for( int x = 0 ; x<y ; ++x )
			pHessianMatrix[(y * NUMofDIM) + x] = pHessianMatrix[(x * NUMofDIM) + y];
	}


	// 偏微分された結果、定数になってしまった部分（一次項の部分）を入れる
	for( int u = 0 ; u<NUMofDIM ; ++u )
		pFirstOrderCoeff[u] = pFormula[uBaseIdx + u];
}




/*!

\n	LCP を Lemke 法で解く。
\n	最小化する問題を解くようになっています。
\n	　　目的条件 : ax^2 + bx
\n	　　制約条件は cx >= d
\n	の形で入れるようにしてください。
\n
\n	最大化を行うときは、符号と不等号を逆にして
\n	　　目的条件 : -ax^2 - bx
\n	　　制約条件 : -cx <= -d
\n	とすれば、最大化問題を解くことができます
\n
			@param	pMin				[out]	最小化時の値
			@param	pCoeff				[out]	最小化時のKKTベクトル
			@param	pTargetFunc			[in]	目的関数。3次元の場合は（ax^2 + by^2 + cz^2 + dxy + exz + fyz + gx + hy + iz) で９つ必要。
\n												実際は最後の + DIM分は計算には必要ないが、、、。
\n												順番は、基底のみ(DIM-1)、基底の組み合わせ(DIM-2)と入れること。
			@param	pConstrainFormula	[in]	制約式の定数項以外の部分。DIM が3次元なら3つ必要。(ax + by + cz = d) なら a, b, c の部分
			@param	pConstant			[in]	制約式の定数項の部分。(ax + by + cz = d) なら d の部分
			@retval	true						最適解が得られた
			@retval	false						最適解は存在しない
 */
template< const int NUMofDIM, const int NUMofCON >
BOOL LemkeAlgorithm( OUT double *pMin, OUT double *pCoeff, IN const double *pTargetFunc, IN const double *pConstrainFormula, IN const double *pConstant )
{
	static const int NUMofCOMPDIM = NUMofDIM + NUMofCON;
	double	pCompMatrix[NUMofCOMPDIM][NUMofCOMPDIM];			// 相補条件行列
	double	pCompConstant[NUMofCOMPDIM];						// 相補条件定数項

	// 初期化
	{
		double	pHessianMatrix[NUMofDIM][NUMofDIM];				// ヘッセ行列
		double	pFirstOrderCoeff[NUMofDIM];						// １次項の係数

		// ヘッセ行列と一次項の部分を格納する
		PartialDifferential<NUMofDIM>( (double *)pHessianMatrix, (double *)pFirstOrderCoeff, pTargetFunc );


		// 各行列の作成
		{
			double pConstrainMatrix[NUMofCON][NUMofDIM];			// 制約条件行列（制約式の数×次元数　のマトリクス）

			// 制約条件を行列に格納
			for( int y = 0 ; y<NUMofCON ; ++y )
			{
				for( int x = 0 ; x<NUMofDIM ; ++x )
					pConstrainMatrix[y][x] = pConstrainFormula[y * NUMofDIM + x];
			}


			// 相補条件行列の作成
			{
				// 0 クリア
				memset( pCompMatrix, 0, sizeof(pCompMatrix) );

				// Q 部分を代入する
				//   | Q, -A^t|
				//   | A,  0  | 
				// ここで
				//   | Q, A^t|
				//   |-A, 0  | 
				// とならないのは、そもそも Q を
				//	cx - x^t * Q * x		→今野さんの教科書などではこっちを使用している
				// と置いてるわけではなく、
				//  cx + x^t * Q * x		→より一般的にはこっちだとおもうので
				// と置いていることに寄る。
				// 
				// これにより、ヘシアンの部分だけ符号が反転するので、全体の符号も反転する必要があり、
				//   |-Q, A^t| * -1 = | Q, -A^t|
				//   |-A, 0  |      = | A,  0  |
				// となる。
				for( int y = 0 ; y<NUMofDIM ; ++y )
					for( int x = 0 ; x<NUMofDIM ; ++x )
						pCompMatrix[y][x] = pHessianMatrix[y][x];

				
				// A 部分を代入する
				//   | Q, -A^t|
				//   | A,  0  | 
				for( int y = 0 ; y<NUMofCON ; ++y )
				{
					for( int x = 0 ; x<NUMofDIM ; ++x )
					{
						pCompMatrix[NUMofDIM + y][x] =  pConstrainFormula[y * NUMofDIM + x];		// - にして配置
						pCompMatrix[x][NUMofDIM + y] = -pConstrainFormula[y * NUMofDIM + x];		// 転置して配置
					}
				}
			}
		}


		// 相補条件行列に対応する定数項を設定する
		{
			// ここの部分も本当は
			// 　|-r|		-> １次項の部分
			// 　| c|		-> 定数項の部分
			// となるが、ヘシアンが反転したことにより全体が -1 をかける必要があるので、
			//   |-r| * -1 = | r|
			//   | c|        |-c|
			// となっている。
			for( int u = 0 ; u<NUMofDIM ; ++u )
				pCompConstant[u] = pFirstOrderCoeff[u];

			// 制約式の定数項の部分
			for( int u = 0 ; u<NUMofCON ; ++u )
				pCompConstant[NUMofDIM + u] = -pConstant[u];
		}
	}


	// 線形相補性問題の相補性条件の式（w = Mx + q）が求まったので、スラック変数、人為変数を導入し、
	// Lemke の相補掃き出し法（レムケ法）で解いていく
	{
		double	pExtendMtx[NUMofCOMPDIM][NUMofCOMPDIM + NUMofCOMPDIM + 1];			// スラック変数(NUMofCOMPDIM 数)・人為変数(1 つ)の分だけ行列を拡張する
		double	pExtendConst[NUMofCOMPDIM];											// 計算用の定数項部分
		BOOL	pBasicVariableFlg[NUMofCOMPDIM][NUMofCOMPDIM + NUMofCOMPDIM + 1];	// 基底変数フラグ

		// 拡張行列の作成と計算用の定数項を作成
		{
			// 0 クリア
			memset( pExtendMtx, 0, sizeof(pExtendMtx) );
			
			// 相補行列を入れる
			for( int y = 0 ; y<NUMofCOMPDIM ; ++y )
				for( int x = 0 ; x<NUMofCOMPDIM ; ++x )
					pExtendMtx[y][x] = pCompMatrix[y][x];


			// スラック変数と人為変数の分だけ拡張
			for( int u = 0 ; u<NUMofCOMPDIM ; ++u )
			{
				pExtendMtx[u][NUMofCOMPDIM+u]				= -1;		// 計算前のスラック変数の係数はすべて -1
				pExtendMtx[u][NUMofCOMPDIM+NUMofCOMPDIM]	= 1;		// 計算前の人為変数の係数はすべて      1
			}


			// 計算用の定数項の設定
			memcpy( pExtendConst, pCompConstant, sizeof(pExtendConst) );
		}


		// 基底変数フラグの初期化
		{
			// 0 クリア
			memset( pBasicVariableFlg, 0, sizeof(pBasicVariableFlg) );

			// 当初の基底変数は各スラック変数
			for( int u = 0 ; u<NUMofCOMPDIM ; ++u )
				pBasicVariableFlg[u][NUMofCOMPDIM+u] = true;
		}


		int		uTradeFormula, uTradeDimension;
		int		uPastTradeDimension;
		BOOL	bContinue;


		// Step 1
		//   現在の基底変数はスラック変数(1〜NUMofCOMPDIM)。
		//   人為変数を基底にするため、相補条件定数項の一番小さなものからはじめる。
		//   （左辺を基底、右辺を非基底とした場合、定数項も右辺に置いた場合。
		//   　もし定数項を左辺に置いている場合、一番大きなものからはじめる）

		// 右辺に置いているので、定数項が最小のスラック変数を人為変数と置き換え、人為変数を基底変数にする
		{
			uTradeDimension = NUMofCOMPDIM + NUMofCOMPDIM;

			uTradeFormula = 0;
			for( int u = 1 ; u<NUMofCOMPDIM ; ++u )
			{
				if( pExtendConst[uTradeFormula] > pExtendConst[u] )
					uTradeFormula = u;
			}

			// 入れ替える
			{
				uPastTradeDimension = NUMofCOMPDIM + uTradeFormula;
				pBasicVariableFlg[uTradeFormula][uPastTradeDimension]	= false;
				pBasicVariableFlg[uTradeFormula][uTradeDimension]		= true;

				double fDiv = double(-1) / pExtendMtx[uTradeFormula][uTradeDimension];
				
				// 行列部分
				for( int x = 0 ; x<(NUMofCOMPDIM + NUMofCOMPDIM + 1) ; ++x )
					pExtendMtx[uTradeFormula][x] *= fDiv;

				// 定数項の部分
				pExtendConst[uTradeFormula] *= fDiv;
			}

			// 各基底式から人為変数を削除する
			for( int y = 0 ; y<NUMofCOMPDIM ; ++y )
			{
				// 入れ替えたものと同じだった場合は処理しない
				if( y == uTradeFormula )
					continue;

				double fMul = pExtendMtx[y][uTradeDimension];

				// 行列部分
				for( int x = 0 ; x<(NUMofCOMPDIM + NUMofCOMPDIM + 1) ; ++x )
					pExtendMtx[y][x] += fMul * pExtendMtx[uTradeFormula][x];

				// 定数項の部分
				pExtendConst[y] += fMul * pExtendConst[uTradeFormula];
			}
		}


		do
		{
			// Step 2
			//   wz = 0 で非基底変数になっている組み合わせを調べ、z を基底変数にする。
			//   この時、調べている z の次元以外の項を 0 とみなし、定数項と等しくなる様に z の数字を設定する
			//   やってることは、シンプレックス法でやったのと同じことです。
			{
				// w(スラック変数)、z(列に対応した変数) が両方とも非基底変数になってるのを探す。
				uTradeDimension = int(~0);
				for( int u = 0 ; u<NUMofCOMPDIM ; ++u )
				{
					BOOL bFind = false;
					for( int y = 0 ; y<NUMofCOMPDIM ; ++y )
					{
						if( !pBasicVariableFlg[y][u] && !pBasicVariableFlg[y][NUMofCOMPDIM + u] )
							continue;

						bFind = true;
						break;
					}

					if( bFind )
						continue;

					uTradeDimension = u;
					if( uPastTradeDimension < NUMofCOMPDIM )
						uTradeDimension = NUMofCOMPDIM + u;
					break;
				}


				// 次元が決まったので、ベースとなる制約式を基底式から探す
				{
					double fMin = DBL_MAX;
					double fTemp;

					uTradeFormula = int(~0);
					for( int y = 0 ; y<NUMofCOMPDIM ; ++y )
					{
						// 0 の場合は処理しない
						if( fabs(pExtendMtx[y][uTradeDimension]) < DBL_EPSILON )
							continue;

						// z ≧ 0 の制約より、人為変数の場合だけ同じ符号の場合は処理できない。
						if( ((pExtendMtx[y][uTradeDimension] > 0) && (pExtendConst[y] >= 0)) || ((pExtendMtx[y][uTradeDimension] < 0) && (pExtendConst[y] < 0)) )
							continue;

						fTemp = pExtendConst[y] / -pExtendMtx[y][uTradeDimension];

						// 0 より上の制約を加えないと基底が見つからない場合があるので、解は 0 より上とする。
						if( (fTemp > DBL_EPSILON) && fMin > fTemp )
						{
							// 非基底変数の対の場合は交換しない
							fMin			= fTemp;
							uTradeFormula	= y;
						}
					}


					// 基底変数が負にならないので、最適解は存在しない
					if( uTradeFormula == int(~0) )
						return false;
				}


				// 入れ替える
				{
					// 変更前の基底変数の位置を取得する
					for( int x = 0 ; x<(NUMofCOMPDIM + NUMofCOMPDIM + 1) ; ++x )
					{
						if( !pBasicVariableFlg[uTradeFormula][x] )
							continue;

						uPastTradeDimension	= x;
						break;
					}

					// 行列対応変数を基底にする
					pBasicVariableFlg[uTradeFormula][uPastTradeDimension]	= false;
					pBasicVariableFlg[uTradeFormula][uTradeDimension]		= true;

					// 後の後退代入で単純に置き換えるので、選択された次元の割った後の変数の符号を必ず - に保つ。
					double fDiv = double(-1) / pExtendMtx[uTradeFormula][uTradeDimension];
					
					// 行列部分
					for( int x = 0 ; x<(NUMofCOMPDIM + NUMofCOMPDIM + 1) ; ++x )
						pExtendMtx[uTradeFormula][x] *= fDiv;

					// 定数項の部分
					pExtendConst[uTradeFormula] *= fDiv;
				}

				// 選択された次元の変数を各基底式から削除する
				for( int y = 0 ; y<NUMofCOMPDIM ; ++y )
				{
					// 入れ替えたものと同じだった場合は処理しない
					if( y == uTradeFormula )
						continue;

					double fMul = pExtendMtx[y][uTradeDimension];

					if( fabs(fMul) < DBL_EPSILON )
					{
						pExtendMtx[y][uTradeDimension] = double(0);
						continue;
					}

					// 行列部分
					for( int x = 0 ; x<(NUMofCOMPDIM + NUMofCOMPDIM + 1) ; ++x )
						pExtendMtx[y][x] += fMul * pExtendMtx[uTradeFormula][x];

					// 定数項の部分
					pExtendConst[y] += fMul * pExtendConst[uTradeFormula];
				}
			}


			// 基底変数に人為変数が含まれなくなった時点で終了させる
			{
				bContinue = FALSE;

				for( int y = 0 ; y<NUMofCOMPDIM ; ++y )
				{
					if( !pBasicVariableFlg[y][NUMofCOMPDIM + NUMofCOMPDIM] )
						continue;

					bContinue = TRUE;
					break;
				}
			}


			// 終了条件に達していなかったら Step 2 へもどる。
		}
		while( bContinue );


		// 最適解の答えは、pBasicVariableFlg と対応する pExtendConst に格納されている。
		{
			// 0 clear する
			memset( pCoeff, 0, sizeof(*pCoeff) * NUMofDIM );

			// 最適解のときのKKTベクトルを求める
			for( int u = 0 ; u<NUMofCOMPDIM ; ++u )
			{
				for( int v = 0 ; v<NUMofDIM ; ++v )
				{
					if( !pBasicVariableFlg[u][v] )
						continue;

					pCoeff[v] = pExtendConst[u];
					break;
				}
			}

			// 最適解の値を計算する
			{
				int uBase;

				*pMin = double(0);

				// 2次項
				for( int u = 0 ; u<NUMofDIM ; ++u )
				{
					*pMin += pTargetFunc[u] * (pCoeff[u] * pCoeff[u]);

					uBase  = NUMofDIM * (NUMofDIM+1) / 2;					// 2次項の全体数
					uBase -= (NUMofDIM-(u+1)) * (NUMofDIM-(u+1)+1) / 2;		// 現在参照している次元項（xαのxの部分）。階段で考えれば簡単。はじめは DIM 次は DIM + DIM-1 次は DIM + DIM-1 + DIM-2・・・・
					uBase -= u + 1;											// 次の式の基礎補正分を引く

					for( int v = u + 1 ; v<NUMofDIM ; ++v )
						*pMin += pTargetFunc[uBase + v] * (pCoeff[u] * pCoeff[v]);
				}

				// 1次項
				{
					uBase = NUMofDIM * (NUMofDIM+1) / 2;
					for( int u = 0 ; u<NUMofDIM ; ++u )
						*pMin += pTargetFunc[uBase + u] * pCoeff[u];
				}
			}
		}
	}

	return true;
}



void main( int argc, char *argv[] )
{

#define DIM	3
#define COL	3

	double	pTarget[(DIM * (DIM+1) / 2) + DIM];
	double	pConstrain[COL][DIM];
	double	pConstant[COL];

	// 目的関数
	pTarget[ 0] = 3;		// x^2 項
	pTarget[ 1] = 3;		// y^2 項
	pTarget[ 2] = 4;		// z^2 項
	pTarget[ 3] = -2;		// xy  項
	pTarget[ 4] = 1;		// xz  項
	pTarget[ 5] = 5;		// yz  項
	pTarget[ 6] = 0;		// x   項
	pTarget[ 7] = -2;		// y   項
	pTarget[ 8] = 4;		// z   項

	// 制約式
	{
		//  4x - 2y + z ≧ 15
		pConstrain[0][0]	= 4;
		pConstrain[0][1]	= -2;
		pConstrain[0][2]	= 1;
		pConstant[0]		= 15;

		//   x + 5z ≧ 17
		pConstrain[1][0]	= 1;
		pConstrain[1][1]	= 0;
		pConstrain[1][2]	= 5;
		pConstant[1]		= 17;

		//  2x + 4y - 2z ≧ 8
		pConstrain[2][0]	= 2;
		pConstrain[2][1]	= 4;
		pConstrain[2][2]	= -2;
		pConstant[2]		= 8;
	}

	double	pCoeff[DIM];
	double	fMin;

	if( !LemkeAlgorithm<DIM,COL>( &fMin, pCoeff, (double *)pTarget, (double *)pConstrain, (double *)pConstant ) )
	{
		printf( "失敗!\n" );
		return;
	}

	printf( "最小値 = %f\n", fMin );
	printf( "　係数 x = %f\n", pCoeff[0] );
	printf( "　係数 y = %f\n", pCoeff[1] );
	printf( "　係数 z = %f\n", pCoeff[2] );
}