#include "stdafx.h"
#include "GMM.h"
#include "cv.h"
#include "highgui.h"
#include "cxcore.h"
#include "iplImageProcess.h"
#include <fstream.h>
#include <math.h>


GMM::GMM(CvCapture* pCapture)
{
	ASSERT(pCapture!=NULL);
	pImage=NULL;
	pResultImage=NULL;
	gmm=NULL;
	originalImgMat=NULL;
	resultImgMat=NULL;

    this->pCapture=pCapture;
    pImage = cvQueryFrame(pCapture);
	height=pImage->height;
	width=pImage->width;
	gmm=new GMS[height*width];
    originalImgMat=new char[height*width*3];
    resultImgMat=new char[height*width*3];
    pResultImage=cvCreateImage(cvSize(width,height), IPL_DEPTH_8U,3);
	pResultImage->origin=1;
	
	for(int y=0;y<height;y++)
		for(int x=0;x<width;x++)
		{
			iplGetPixelColor(pImage,x,y,xt);//得到當前像素值并放于xt中
			//xt[0]=iplGetPixelRed(pImage,x,y);
			//xt[1]=iplGetPixelGreen(pImage,x,y);
			//xt[2]=iplGetPixelBlue(pImage,x,y);

			for(int k=0;k<K;k++)
			{
				gmm[y*width+x].gm[k].u[0]=xt[0];
				gmm[y*width+x].gm[k].u[1]=xt[1];
				gmm[y*width+x].gm[k].u[2]=xt[2];
			}
		}
		
}
GMM::~GMM()
{
	if(pImage!=NULL)
	{
		cvReleaseImage(&pImage);
	}
	if(pResultImage!=NULL)
	{
		cvReleaseImage(&pResultImage);
	}
	if(gmm!=NULL)
	{
		delete [] gmm;
	}
	if(originalImgMat!=NULL)
	{
		delete [] originalImgMat;
	}
	if(resultImgMat!=NULL)
	{
		delete [] resultImgMat;
	}
	

}

int GMM::findMatch(int* xt,GMS& g)
{
	ASSERT(xt!=NULL && g.gm!=NULL);
	for(int i=0;i<K;i++)
	{
		if(fabs((double)xt[0]-g.gm[i].u[0])<2.5*sqrt(g.gm[i].gama[0]) &&
			fabs((double)xt[1]-g.gm[i].u[1])<2.5*sqrt(g.gm[i].gama[1]) &&
			fabs((double)xt[2]-g.gm[i].u[2])<2.5*sqrt(g.gm[i].gama[2]))
		{
			return i;
		}
	}
	return K+1;
}

void GMM::reorderGMM(GMS& g)
{
	ASSERT(g.gm!=NULL);
	GM g_max;
	int loc_max;
	for(int i=0;i<K;i++)
	{
		g_max=g.gm[i];
		loc_max=i;
		for(int j=i;j<K;j++)
		{
			if(g_max.w<g.gm[j].w)
			{
				g_max=g.gm[j];
				loc_max=j;
			}
		}
		g.gm[loc_max]=g.gm[i];
		g.gm[i]=g_max;
		
	}
		
}


/*
 *用當前像素創建一個新的GM模型
*/
GM GMM:: createNewGM(int* xt)
{
	ASSERT(xt!=NULL);
	GM g(xt);
	return g;

}
void GMM::processImage()
{
	ASSERT(pImage);
	cvNamedWindow("pImage", CV_WINDOW_AUTOSIZE);  
	cvNamedWindow("pResultImage",CV_WINDOW_AUTOSIZE);
    cvMoveWindow("pImage", 120, 200);  
    cvMoveWindow("pResultImage",130+width, 200);

	int location=0;
    //ofstream output("order.txt",ios::ate);
	

	while(pImage = cvQueryFrame(pCapture))
	{
		
		for(int x=0;x<width;x++)
			for(int y=0;y<height;y++)
			{
				iplGetPixelColor(pImage,x,y,xt);//得到一個像素的三個分量

				//output<<xt[0]<<","<<xt[1]<<","<<xt[2]<<endl;

				if((location=findMatch(xt,gmm[y*width+x]))<K)
				{
					for(int i=0;i<location;i++)
					{
						gmm[y*width+x].gm[i].updateData();
					}
					gmm[y*width+x].gm[location].updateData(xt);
					gmm[y*width+x].gm[location].newGMM=true;
					for(int j=location+1;j<K;j++)
					{
						gmm[y*width+x].gm[j].updateData();
					}
				}
				else //no match found
				{
					for(int i=0;i<K-1;i++)
					{
						gmm[y*width+x].gm[i].updateData();
					}
					gmm[y*width+x].gm[K-1]=createNewGM(xt);

					//output<<gmm[y*width+x].gm[K-1].w<<endl;	
					
				}
				 //reorder the GMM
 		    reorderGMM(gmm[y*width+x]);

			

 		    for(int i=0;i<K;i++)
 		    {
 		    	if(gmm[y*width+x].gm[i].newGMM==true)
 		    	{
 		    		location=i;//location 的新位置
 		    		gmm[y*width+x].gm[i].newGMM=false;
 		    		break;
 		    	}
 		    }
 		    
 		    int B=0;
 		   double w_tmp=0;
 		   double w_all=0;
 		  for(i=0;i<K;i++)
 		  {
 		   	w_all+=gmm[y*width+x].gm[i].w;
 		  }
 		  for( i=0;i<K;i++)
 		  {
 		  	gmm[y*width+x].gm[i].w/=w_all;
 		  	//gmm[y][x][i].get_ratio;
 		   	
 		  }
 		    for(i=0;i<K;i++)
 		    {
 		    	if(w_tmp<T)
 		    	{	
 		    		w_tmp+=gmm[y*width+x].gm[i].w;
 		    	}
 		    	else
 		    	{
 		    		B=i;
 		    		break;
 		    	}
 		    }
 		    if(location<=B)
 		    {
 		    	iplSetPixelColor(pResultImage,x,y,0xff000000);
 		    }
 		    else
			{
 		    	double bgPixel[]={0,0,0};//stand for a background pixel
	            double gamaBgPixel[]={0,0,0};//stand for a background pixel gama.
	        
 		    	////////////////////////////////////shadow detection in RGB color space
 		    	for(int m=0;m<K;m++)
 		    	{
					bgPixel[0]+=gmm[y*width+x].gm[m].w*gmm[y*width+x].gm[m].u[0];
					bgPixel[1]+=gmm[y*width+x].gm[m].w*gmm[y*width+x].gm[m].u[1];
					bgPixel[2]+=gmm[y*width+x].gm[m].w*gmm[y*width+x].gm[m].u[2];

					gamaBgPixel[0]+=gmm[y*width+x].gm[m].w*sqrt(gmm[y+width+x].gm[m].gama[0]);
					gamaBgPixel[1]+=gmm[y*width+x].gm[m].w*sqrt(gmm[y+width+x].gm[m].gama[1]);
					gamaBgPixel[2]+=gmm[y*width+x].gm[m].w*sqrt(gmm[y+width+x].gm[m].gama[2]);

 		    		//bgPixel=g.vectorAdd(bgPixel,g.getDoubleVector(g.vectorMulNum(gmm[y][x][m].w,gmm[y][x][m].u)));//calculate the background value	
 		    	    //gamaBgPixel=g.vectorAdd(gamaBgPixel,g.vectorMulNum(gmm[y][x][m].w,g.vectorSqrt(gmm[y][x][m].gama)));
 		    	}
 		    	
 		    	if(fabs(xt[0]-bgPixel[0])/(double)(bgPixel[0])<ALPHA_RGB && 
 		    		fabs(xt[1]-bgPixel[1])/(double)(bgPixel[1])<ALPHA_RGB &&
 		    	    fabs(xt[2]-bgPixel[2])/(double)(bgPixel[2])<ALPHA_RGB)
 		    	{
 		    		iplSetPixelColor(pResultImage,x,y,0xff000000);
				}
 		    	else
 		    	{
 		    		iplSetPixelColor(pResultImage,x,y,0xffffffff);
 		    	}
			}
		}

	
		/*for(int num=0;num<K;num++)
		{
			output<<gmm[100*width+100].gm[num].w<<",";	
		}
 		output<<endl;*/
		//output.close();	


		cvShowImage("pImage",pImage);	  
		cvShowImage("pResultImage",pResultImage);
		//如果有按鍵事件，則跳出循環
		//此等待也為cvShowImage函數提供時間完成顯示
		//等待時間可以根據CPU速度調整
		if(cvWaitKey(1) >= 0)
		{
			pImage=0;
			break;
		}
	}
	
    cvReleaseImage(&pImage);  
	pImage=NULL;
	cvReleaseImage(&pResultImage);  
	pResultImage=NULL;
	cvDestroyWindow("pImage");  
	cvDestroyWindow("pResultImage");
	//output.close();

}