#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);
		
		//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;
				}
			}			
		}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;
				}
			}
		}
		
		//Sum of the average in the whole image for this neighbor
		sumg+= sum/(double)elements;
	}
	
	//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);
	
	float *hist = (float *)mxCalloc(blocks[0]*blocks[1]*256, sizeof(float));
	float *norm = (float *)mxCalloc(blocks[0]*blocks[1], sizeof(float));
  
	//Output dimensions
	int out[3];
	out[0] = max(blocks[0]-2,0);
	out[1] = max(blocks[1]-2,0);
	out[2] = 256+1;
		
	mxArray *mxfeat = mxCreateNumericArray(3, out, mxSINGLE_CLASS, mxREAL);
	float *feat = (float *)mxGetPr(mxfeat);
	
	double xblock;
	double yblock;
	
	//Histograms
	for(int x=0;x<dx;x++)
	{
		for(int y=0;y<dy;y++)
		{
			//s blocks
			
			xblock = (double)(x+0.5)/(double)sbin - 0.5;
			yblock = (double)(y+0.5)/(double)sbin - 0.5;
			
			int ixp = min((int)floor(xblock),blocks[0]);
			int iyp = min((int)floor(yblock),blocks[1]);
			
			ixp = max(ixp,0);
			iyp = max(iyp,0);
			
			CLBP_Sp = CLBP_S + x*dy + y;
			
			*(hist + ixp*blocks[1] + iyp + (int)(*CLBP_Sp)*blocks[0]*blocks[1]) += 1;			
		}
	}
	
	// compute energy in each block by summing over bins
	for (int o = 0; o < 256; o++)
	{
		float *src = hist + o*blocks[0]*blocks[1];
		float *dst = norm;
		float *end = norm + blocks[0]*blocks[1];
		
		while (dst < end) 
		{
			*(dst++) += (*src)*(*src);
			src++;
		}
	}
		
	//Normalise histograms
	for(int x=0;x<out[0];x++)
	{
		for(int y=0;y<out[1];y++)
		{
			float *dst = feat + x*out[1] + y;      
			float *src, *p, n1, n2, n3, n4;
			
			p = norm + (x+1)*blocks[1] + (y+1);
			float n = 1.0 / sqrt(*p);
			
 			//Calculate features
			src = hist + (x+1)*blocks[1] + y+1;
			for (int o = 0; o < 256; o++)
			{
				float h = (*src) * n;
				
				*dst = h;
				
				dst += out[0]*out[1];
				src += blocks[0]*blocks[1];
			}
		}
	}
	
	mxFree(spoints);
	mxFree(Diff);
	mxFree(CLBP_S);
	mxFree(CLBP_M);
	mxFree(hist);
	mxFree(norm);

	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("u2.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]);
}