#include <math.h>
#include "mex.h"
#include <string>
#include <iostream>
#include <fstream>

#include <opencv2/opencv.hpp>

// small value, used to avoid division by zero
#define eps 0.0001

static inline float min(float x, float y) { return (x <= y ? x : y); }
static inline float max(float x, float y) { return (x <= y ? y : x); }

static inline int min(int x, int y) { return (x <= y ? x : y); }
static inline int max(int x, int y) { return (x <= y ? y : x); }

using namespace std;
using namespace cv;

enum MappingType
{
	LBP_MAPPING_NONE = 0,
	LBP_MAPPING_U2,
	LBP_MAPPING_RI,
	LBP_MAPPING_RIU2,
	LBP_MAPPING_HF
};

static const string MappingTypeStr[] = {"none", "u2", "ri", "riu2", "hf" };

class Results
{
	
	
};

class Mapping
{
	
public:
	
	MappingType strToType( string s )
	{
		if( s.compare( "u2" ) == 0 )
			return LBP_MAPPING_U2;
		else if( s.compare("ri") == 0 )
			return LBP_MAPPING_RI;
		else if ( s.compare("riu2") == 0 )
			return LBP_MAPPING_RIU2;
		else if( s.compare("hf")  == 0 )
			return LBP_MAPPING_HF;
		else
			return LBP_MAPPING_NONE;
	}
	
	bool loadMapping( string fileName )
	{
		ifstream ifs( fileName.c_str(), ios::in );
		
		if( ! ifs )
		{
			cerr << "File \'" << fileName << "\' could not be opened" << endl;
			return false;
		}

		string s; int i;
		// Get file type
		ifs >> s;
		if( s.compare("LBPMapping") )
		{
			cerr << fileName << " is not a LBPMapping file." << endl;
			return false;
		}

		// Get verion
		ifs >> s >> i;

		// Get mapping type
		ifs >> s >> s;
		this->type = strToType(s);

		// Get samples
		ifs >> s >> this->samples;

		// Get maxnum
		ifs >> s >> this->num;

		// Get table
		ifs >> s;
		this->table.clear();
		for (int j = 0; j < pow(2., (double)samples); j++ ) 
		{
			ifs >> i;
			table.push_back( i );
		}

		if ( type != LBP_MAPPING_HF )
		{
			return true;
		}
		
		// Get orbits for HF
		this->orbits.clear();
		ifs >> s;
		vector<int> o;
		
		while( ifs >> i ) 
		{
			if( i < 0 )
			{ // -1 are used as separators
				orbits.push_back(o);
				o.clear();
				continue;
			}
			
			o.push_back(i);
		}

		return true;
	}
	
	MappingType type;
	vector<int> table;
	unsigned int samples;
	unsigned int num;
		// Fourier Histogram variables
	vector< vector<int> > orbits;
};

mxArray* clbp(const mxArray* image,Mapping& mapping, int radius, int neighbors, int sbin)
{
	double *image_ptr = (double *)mxGetPr(image);
	const int *dims = mxGetDimensions(image);
	
	int spoints_size[2];
	spoints_size[0] = neighbors;
	spoints_size[1] = 2;
	
	double *spoints= (double *)mxCalloc(spoints_size[0]*spoints_size[1], sizeof(double));

    //Angle step.
    double a = 2.*M_PI/(double)neighbors;
    
	double miny=1e6;
	double maxy=-1e6;
	double minx=1e6;
	double maxx=-1e6;
	
    for(int i = 0; i<neighbors;i++)
	{
        double* sx = spoints + i*spoints_size[1];
		double* sy = sx+1;
		
		*sy = radius*cos(i*a);
		
		if(*sy<miny)
			miny=*sy;
		if(*sy>maxy)
			maxy=*sy;
		
        *sx = -radius*sin(i*a);
		if(*sx<minx)
			minx=*sx;
		if(*sx>maxx)
			maxx=*sx;
	}
    
	int bsizey = ceil(max(maxy,0))-floor(min(miny,0))+1;
	int bsizex = ceil(max(maxx,0))-floor(min(minx,0))+1;
    
	int origy = 0-floor(min(miny,0));
	int origx = 0-floor(min(minx,0));
	
	//Minimum allowed size for the input image depends
	//on the radius of the used LBP operator.
	if(dims[0] < bsizex || dims[1] < bsizey)
		mexErrMsgTxt("Too small input image. Should be at least (2*radius+1) x (2*radius+1)");
	
	// Calculate dx and dy;
	int dx = dims[0] - bsizex +1;
	int dy = dims[1] - bsizey +1;
	int dxdy = dx*dy;
	
	//C is a partial copy of the input image
	double* Cp;
	double* Np;
	
	double* Diff = (double *)mxCalloc(dxdy*neighbors, sizeof(double));
	double* Diffp;
	
	int* CLBP_S = (int*)mxCalloc(dxdy, sizeof(int));
	int* CLBP_Sp;
	
	int* CLBP_M = (int*)mxCalloc(dxdy, sizeof(int));
	int* CLBP_Mp;
		
	double sumg=0;
	
	double* sx;
	double* sy;
	
	double iy;
	double ix;
	
	int elements = dxdy;
	
	//Compute the LBP code image
	for(int n=0;n<neighbors;n++)
	{
		sx = spoints + n*spoints_size[1];
		sy = sx+1;
		
		iy = *sx + origy;
		ix = *sy + origx;
						
		double sum = 0;
		
		int rx = round(ix);
		int ry = round(iy);
		
		double v = pow(2,n);
		
// 		cout<<"C=["<<endl;
		//Check if interpolation is needed
		if( abs(ix-rx)<eps && abs(iy-ry)<eps)
		{
			//Interpolation is not needed, use original datatypes
			//Get Image N and compare it to matrix C			
			for(int x=0;x<dx;x++)
			{
				for(int y=0;y<dy;y++)
				{
					Np = image_ptr + (x+ry) + (y+rx)*dims[0];
					Cp = image_ptr + (x+origy) + (y+origx)*dims[0];
					
					CLBP_Sp = CLBP_S + x*dy + y;
					Diffp = Diff + x*dy + y +n*dxdy;
					
					if(*Np>=*Cp)
						*CLBP_Sp += v;
					
					*Diffp = abs(*Np-*Cp);
					
					sum+=*Diffp;
				}
			}			
		}else
		{
			//Calculate floors, ceils and rounds for the x and y
			int fy = floor(iy);
			int cy = ceil(iy);
			
			int fx = floor(ix);
			int cx = ceil(ix);

			double ty = iy - fy;
			double tx = ix - fx;
		
			double w1 = (1 - tx) * (1 - ty);
			double w2 =      tx  * (1 - ty);
			double w3 = (1 - tx) *      ty;
			double w4 =      tx  *      ty;

			for(int x=0;x<dx;x++)
			{
				for(int y=0;y<dy;y++)
				{
					double* ip1 = image_ptr + (x+fy) + (y+fx)*dims[0];
					double* ip2 = image_ptr + (x+fy) + (y+cx)*dims[0];
					double* ip3 = image_ptr + (x+cy) + (y+fx)*dims[0];
					double* ip4 = image_ptr + (x+cy) + (y+cx)*dims[0];
					
					double Np;
					CLBP_Sp = CLBP_S + x*dy + y;
					Cp = image_ptr + (x+origy) + (y+origx)*dims[0];
					Np = *ip1*w1 + *ip2*w2 + *ip3*w3 + *ip4*w4;
					Diffp = Diff + x*dy + y +n*dxdy;	
					
					if(Np>=*Cp)
						*CLBP_Sp += v;

					*Diffp = abs(Np-*Cp);
					
					sum+=*Diffp;

				}
			}
		}
		
		//Sum of the average in the whole image for this neighbor
		sumg+= sum/(double)elements;
	}

	double DiffThreshold = sumg/(double)neighbors;
	
	//Calculate CLBP_Mp
	for(int n=0;n<neighbors;n++)
	{
		double v = pow(2,n);
		
		for(int x=0;x<dx;x++)
		{
			for(int y=0;y<dy;y++)
			{
				CLBP_Mp = CLBP_M + x*dy + y;
				
				Diffp = Diff + x*dy + y +n*dxdy;

				if(*Diffp>=DiffThreshold)
					*CLBP_Mp += v;
			}
		}
	}
	
	//Apply mapping
	for(int i=0;i<dxdy;i++)
	{
		*(CLBP_S + i) = mapping.table[*(CLBP_S + i)];
		*(CLBP_M + i) = mapping.table[*(CLBP_M + i)];
	}
	
	//Create a unique histogram for the whole image
// 	if(1)
// 	{
// 		int out[3];
// 		out[0]=1;
// 		out[1]=1;
// 		out[2]=20;
// 		
// 		mxArray *mxfeat = mxCreateNumericArray(3, out, mxSINGLE_CLASS, mxREAL);
// 		float *feat = (float *)mxGetPr(mxfeat);
// 		
// 		for(int x=0;x<dx;x++)
// 		{
// 			for(int y=0;y<dy;y++)
// 			{
// 				CLBP_Sp = CLBP_S + x*dy + y;	
// 				CLBP_Mp = CLBP_M + x*dy + y;
// 				
// 				
// 				float*featSp = feat + *CLBP_Sp;
// 				*featSp+=1;
// 				
// 				float*featMp = feat + *CLBP_Mp + 10;
// 				*featMp+=1;
// 			}
// 		}
// 		
// 		return mxfeat;
// 	}
	
	
	//Create histograms
	//The image will be divided into sbin*sbin blocks
	
	//Number of blocks
	int blocks[2];
	blocks[0] = (int)round((double)dims[0]/(double)sbin);
	blocks[1] = (int)round((double)dims[1]/(double)sbin);
	
	//Output dimensions
	int out[3];
	out[0] = max(blocks[0]-2,0);
	out[1] = max(blocks[1]-2,0);
	out[2] = 20;
	
	mxArray *mxfeat = mxCreateNumericArray(3, out, mxSINGLE_CLASS, mxREAL);
	float *feat = (float *)mxGetPr(mxfeat);
	
// 	if(!out[0] || !out[1])
// 		mexErrMsgTxt("Image too small, zero bins");
	
	int xblock;
	int yblock;
	
	float* featSp;
	float* featMp;
	
	for(int x=0;x<dx;x++)
	{
		for(int y=0;y<dy;y++)
		{
			//s blocks
			xblock = (int)floor((double)x/(double)sbin);
			yblock = (int)floor((double)y/(double)sbin);
			
			CLBP_Sp = CLBP_S + x*dy + y;	
			CLBP_Mp = CLBP_M + x*dy + y;
			
			//Do not count bordering blocks
			if(xblock<1 || xblock>=out[0] || yblock<1 || yblock>=out[1])
				continue;
			
			//Out blocks
			xblock-=1;
			yblock-=1;

			
			featSp = feat + xblock*out[1] + yblock + (*CLBP_Sp)*out[0]*out[1];
			*featSp+=1;
			
			featMp = feat + xblock*out[1] + yblock + (*CLBP_Mp+10)*out[0]*out[1];
			*featMp+=1;
		}
	}
	
	//Normalise histograms
	for(int x=0;x<out[0];x++)
	{
		for(int y=0;y<out[1];y++)
		{
			double sum = 0;
			
			//Get sum over block
			for(int f=0;f<10;f++)
			{
				featSp = feat + x*out[1] + y + f*out[0]*out[1];
				sum+=*featSp;
			}
			
			//Normalise elements
			if(sum>0)
			{
				for(int f=0;f<10;f++)
				{
					featSp = feat + x*out[1] + y + f*out[0]*out[1];
					(*featSp)/=sum;
				}
			}
			
			sum = 0;
			
			//Get sum for Magnitude
			for(int f=10;f<20;f++)
			{
				featMp = feat + x*out[1] + y + f*out[0]*out[1];
				sum+=*featMp;
			}
			
			//Normalise
			if(sum>0)
			{
				for(int f=10;f<20;f++)
				{
					featMp = feat + x*out[1] + y + f*out[0]*out[1];
					(*featMp)/=sum;
				}
			}
		}
	}
	
	
	mxFree(spoints);
	mxFree(Diff);
	mxFree(CLBP_S);
	mxFree(CLBP_M);
	
	return mxfeat;
}

// main function:
// takes a double color image and a bin size 
// returns HOG features
mxArray *process(const mxArray *mximage, const mxArray *mxsbin) {
	if (mxGetNumberOfDimensions(mximage) != 2 || mxGetClassID(mximage) != mxDOUBLE_CLASS)
		mexErrMsgTxt("Invalid input");
	
	Mapping mapping;
	mapping.loadMapping("riu2.mapping");
	
	int sbin = (int)mxGetScalar(mxsbin);
	
	return clbp(mximage,mapping,1, 8,sbin);
}

// matlab entry point
// F = lbp_simple(image, bin)
// image should be grayscale with double values
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { 
	if (nrhs != 2)
		mexErrMsgTxt("Wrong number of inputs"); 
	if (nlhs != 1)
		mexErrMsgTxt("Wrong number of outputs");

	plhs[0] = process(prhs[0], prhs[1]);
}