#include <math.h>
#include "mex.h"
#include "stdio.h"
#include "stdlib.h"
#include "matrix.h"

//---------------------------------------------------------------------------
// 定義輸入參數
#define XN prhs[0]        // 
#define DN prhs[1]        // 
#define TIMES prhs[2]           // 

// 定義輸出參數
#define HN plhs[0]              //

// 聲明 C 運算函數 (該函數名不能和本文件名重名)
void fir_mse();

//---------------------------------------------------------------------------
void 
mexFunction (int nlhs, mxArray *plhs[],			// 輸出參數個數，及輸出參數數組
			 int nrhs, const mxArray *prhs[])	// 輸入參數個數，及輸入參數數組
{
      double *pXn,*pDn,*pHn;
      int Times,M,N;

    // 取得輸入參數
    pXn = mxGetPr(XN);
    pDn = mxGetPr(DN); 
    M = mxGetM(XN);
    N = mxGetN(XN);
    Times = (int) *mxGetPr(TIMES);
	
    // 為輸出變量分配內存空間
	HN = mxCreateDoubleMatrix(M,1,mxREAL); 
	
	// 取得輸出參數指針
	pHn = mxGetPr(HN);

    // 調用 C 運算函數 (該函數名不能和本文件名重名)
    fir_mse(pXn,M,N,pDn,Times,pHn);
	return;
}

//---------------------------------------------------------------------------
// 定義 C 運算函數
void fir_mse(double *pXn,
             int M,
             int N,
             double *pDn,
             int Times,
             double *pHn)
// C中二維數組是行向量，Matlab矩陣是列向量
{
    double *pXi,*pR,*pP,*pHn0,trR,temp,mu;
    int i,j,k,l,t;
    
    //printf("M = %d, N = %d, Times = %d\n",M,N,Times);
    
	//----------------- 分配內存 ----------------
    pXi = malloc(M*sizeof(double));
    pR =  malloc(M*M*sizeof(double));
	pP =  malloc(M*sizeof(double));
	pHn0 =  malloc(M*sizeof(double));
	
	//------------------- 初始化 ----------------
	for (i=0;i<M;i++)
	{
	    *(pXi+i) = 0;
	    *(pP+i) = 0;
	}
	for (i=0;i<M*M;i++)
	{
	    *(pR+i) = 0;
	}
	trR = 0;
	
	//-------------------- 運算1 ----------------
	for (i=0;i<N;i++)
	{   for (j=0;j<M;j++)
	    {
            *(pXi+j)=*(pXn+i*M+j);      // pXi 為 pXn 的第i列
        }
        
        for (k=0;k<M;k++)
        {   for (l=0;l<M;l++)
            {
                (*(pR+k*M+l)) = (*(pR+k*M+l)) + (*(pXi+k))*(*(pXi+l));
            }
        }
        temp = 0;
        for (j=0;j<M;j++)
	    {
            (*(pP+j)) = (*(pP+j)) + (*(pDn+i))*(*(pXi+j));      // pXi 為 pXn 的第i列
            temp = temp + (*(pXi+j))*(*(pXi+j));
        }
        trR = trR + temp;
  	}

    for (k=0;k<M;k++)
    {   for (l=0;l<M;l++)
        {
            *(pR+k*M+l) = (*(pR+k*M+l))/N;
            //printf("%f ",*(pR+k*M+l));
        }
        //printf("\n");
    }

    for (j=0;j<M;j++)
    {
        *(pP+j) = (*(pP+j))/N;
         //printf("%f ",*(pP+j));
    }
    trR = trR/N;
    //printf("trR = %f ",trR);
        
  	//-------------------- 運算2 ----------------
  	mu = 0.95/trR;
  	for (i=0;i<M;i++)
	{
	    *(pHn0+i) = 0;
	}
	
	for (t=0;t<Times;t++)
	{
    	for (i=0;i<M;i++)
    	{   temp = 0; 	
        	for (j=0;j<M;j++)
        	{   
            	temp = temp + (*(pR+i*M+j))*(*(pHn0+j));
        	}
        	*(pHn+i) = *(pHn0+i) - 2*mu*(temp-(*(pP+i)));
    	}
    	
    	for (i=0;i<M;i++)
    	{
	        *(pHn0+i) = *(pHn+i);
    	}
	}
}