#include "MRegression.h"

//-------------------------------------------------------------------------------
// コンストラクタ
CMRegression::CMRegression(const int dim)
{
	m_Coef = NULL;
	m_Error = NULL;
	m_Dim = dim;
}

//-------------------------------------------------------------------------------
// デストラクタ
CMRegression::~CMRegression(void)
{
	SAFE_RELEASEMAT(m_Coef);
	SAFE_RELEASEMAT(m_Error);
}

//-------------------------------------------------------------------------------
// 係数の重回帰推定
bool CMRegression::CalcCoef(const CvMat *data, const CvMat *observ)
{
	int numSample = data->rows;		// サンプル数
	int numCh     = observ->cols;	// 観測値チャネル数

	// 行列の確保
	CvMat *matData    = cvCreateMat(m_Dim, m_Dim, CV_64FC1);
	CvMat *matDataInv = cvCreateMat(m_Dim, m_Dim, CV_64FC1);
	CvMat *matObsv    = cvCreateMat(m_Dim, numCh, CV_64FC1);

	// データ行列を求める
	for (int row = 0; row < m_Dim; row ++)
	{
		for (int col = 0; col < m_Dim; col ++)
		{
			double val = 0;
			for (int sample = 0; sample < numSample; sample ++)
				val += (GetX(data, sample, row) * GetX(data, sample, col));
			cvmSet(matData, row, col, val);
		}
	}

	// データ行列の逆行列を求める
	cvInvert(matData, matDataInv, CV_LU);

	// 右辺行列を求める
	for (int row = 0; row < m_Dim; row ++)
	{
		for (int col = 0; col < numCh; col ++)
		{
			double val = 0;
			for (int sample = 0; sample < numSample; sample ++)
				val += (GetX(data, sample, row) * GcvmGet(observ, sample, col));
			cvmSet(matObsv, row, col, val);
		}
	}

	// 係数行列を求める
	SAFE_RELEASEMAT(m_Coef);
	m_Coef = cvCreateMat(m_Dim, numCh, CV_64FC1);
	cvMatMul(matDataInv, matObsv, m_Coef);

	// 計算に使用した行列の解放
	SAFE_RELEASEMAT(matData);
	SAFE_RELEASEMAT(matDataInv);
	SAFE_RELEASEMAT(matObsv);

	return true;
}

//-------------------------------------------------------------------------------
// データ行列を生成する
CvMat * CMRegression::GenDataMat(const CvMat *data)
{
	// データ行列作成
	CvMat *matData = cvCreateMat(data->rows, m_Dim, CV_64FC1);

#ifdef _OPENMP
#pragma omp parallel for schedule(dynamic)
#endif
	for (int sample = 0; sample < data->rows; sample ++)
	{
		for (int i = 0; i < m_Dim; i++)
		{
			cvmSet(matData, sample, i, GetX(data, sample, i));
		}
	}
	return matData;
}

//-------------------------------------------------------------------------------
// 推定誤差を計算する
bool CMRegression::CalcError(const CvMat *data, const CvMat *observ)
{
	int numSample = data->rows;		// サンプル数
	int numCh     = observ->cols;	// 観測値チャネル数

	// 推定値の算出
	CvMat *estim = GenConvert(data);

	// RMSEの算出
	SAFE_RELEASEMAT(m_Error);
	m_Error = cvCreateMat(1, numCh, CV_64FC1);
	for (int col = 0; col < numCh; col ++)
	{
		double err = 0;
		for (int sample = 0; sample < numSample; sample ++)
		{
			err += pow(GcvmGet(observ, sample, col) - GcvmGet(estim, sample, col), 
				2.0);
		}
		cvmSet(m_Error, 0, col, sqrt(err / (double)numSample));
	}

	// 計算に使用した行列の解放
	SAFE_RELEASEMAT(estim);

	return true;
}

//-------------------------------------------------------------------------------
// 変換した行列データを生成する（係数行列が計算済みであることが前提）
CvMat *CMRegression::GenConvert(const CvMat *data)
{
//	CHQTime timer;
	// データ行列作成
	CvMat *matData = GenDataMat(data);
//	timer.LapTime("GenDataMat");

	// 推定値の算出
	CvMat *convert = cvCreateMat(data->rows, data->cols, CV_64FC1);
	cvMatMul(matData, m_Coef, convert);
//	timer.LapTime("cvMatMul  ");

	// 計算に使用した行列の解放
	SAFE_RELEASEMAT(matData);
//	timer.LapTime("RELEASEMAT");

	return convert;
}

//-------------------------------------------------------------------------------
// 画像を変換しデータを生成する（係数行列が計算済みであることが前提）
IplImage *CMRegression::GenConvert(const IplImage *dataImg)
{
	// 画像を行列に変換
	CvMat dataMat = cvMat(dataImg->width * dataImg->height, COLOR, 
		(dataImg->depth == IPL_DEPTH_8U ? CV_8UC1 : CV_64F),
		dataImg->imageData);

	// 重回帰変換
	CvMat *convMat = this->GenConvert(&dataMat);

	// 算出された行列を画像に戻す
	IplImage *convImg = 
		cvCreateImage(cvGetSize(dataImg), IPL_DEPTH_64F, COLOR);
	memcpy(convImg->imageData, convMat->data.ptr, convImg->imageSize);

	// 行列を解放
	cvReleaseMat(&convMat);

	return convImg;
}

bool CMRegression::DrawGraph4913(CvMat *rgb, CvMat *xyz)
{
	IplImage *graph = cvCreateImage(cvSize(DISP_W, DISP_H), IPL_DEPTH_8U, COLOR);
	cvSet(graph, cvScalarAll(255.0));

	CvMat *data = cvCreateMat(256, COLOR, CV_64F);
	for (int b = 0; b < 256; b ++) cvmSet(data, b, 0, pow((double)b, 2.2) / 1000.0);
	cvCreateMat(256, COLOR, CV_64F);

	double yScale = 10.0;
	double xScale = 2.0;
	double xMin, xMax;

	for (int z = 0; z < 1; z ++)
	{
//		double zv = (z > 15 ? 255.0 : z * 16.0);
		int iRGB = 0;
		for (int y = 0; y < 17; y ++)
		{
//			double yv = (y > 15 ? 255.0 : y * 16.0);
			for (int x = 0; x < 17; x ++)
			{
				double x1 = cvmGet(xyz, x + y * 17 + z * (17 * 17), 0);
				double y1 = cvmGet(rgb, x + y * 17 + z * (17 * 17), iRGB);
				if (x ==  0) xMin = x1;
				if (x == 16) xMax = x1;
				int dx1 = 100 + (int)(x1 * xScale);
				int dy1 = 412 - (int)(y1 * yScale);
				cvDrawCircle(graph, cvPoint(dx1, dy1), 3, 
					CV_RGB(0, (x > 15 ? 255.0 : x * 16.0), 0), CV_FILLED);
			}

			//for (int b = 0; b < 256; b ++)
			//{
			//	cvmSet(data, b, 1, pow((double)gv, 2.2) / 1000.0);
			//	cvmSet(data, b, 2, pow((double)rv, 2.2) / 1000.0);
			//}
			//GShowMat(data, "data for graph");
			//CvMat *est = this->GenConvert(data);
			//for (int b = 0; b < 255; b ++)
			//{
			//	double y1 = cvmGet(est, b, iXYZ);
			//	int dx1 = 100 + b * 2;
			//	int dy1 = 412 - (int)(y1 * yScale);
			//	double y2 = cvmGet(est, b + 1, iXYZ);
			//	int dx2 = 100 + (b + 1) * 2;
			//	int dy2 = 412 - (int)(y2 * yScale);
			//	cvDrawLine(graph, cvPoint(dx1, dy1), cvPoint(dx2, dy2), CV_RGB(0, (g>15 ? 255 : g*16), 0));
			//}
			//SAFE_RELEASEMAT(est);
		}
		GShowImage(graph, 1, "X : r = 0", 0);
	}

	SAFE_RELEASEIMG(graph);
	SAFE_RELEASEMAT(data);

	return true;
}

//-------------------------------------------------------------------------------
// Scalar値を変換する（係数行列が計算済みであることが前提）
CvScalar CMRegression::ScalarConvert(const CvScalar data)
{
	double dataA[3];
	dataA[0] = data.val[0];
	dataA[1] = data.val[1];
	dataA[2] = data.val[2];

	CvMat dataMat = cvMat(1, COLOR, CV_64F, dataA);

	CvMat *convMat = this->GenConvert(&dataMat);

	return cvScalar(cvmGet(convMat, 0, 0), cvmGet(convMat, 0, 1), cvmGet(convMat, 0, 2));
}