#include <math.h>
#include "mex.h"

#include <iostream>
#include <fstream>
#include <sstream>

#include <complex>
#include <string>
#include <vector>

#include <fftw3.h>

#include <opencv2/opencv.hpp>

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

// unit vectors used to compute gradient orientation
double uu[9] = {1.0000, 
		0.9397, 
		0.7660, 
		0.500, 
		0.1736, 
		-0.1736, 
		-0.5000, 
		-0.7660, 
		-0.9397};
double vv[9] = {0.0000, 
		0.3420, 
		0.6428, 
		0.8660, 
		0.9848, 
		0.9848, 
		0.8660, 
		0.6428, 
		0.3420};

		
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 LBP
{
	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 saveMapping( string fileName ) 
		{
			ofstream ofs( fileName.c_str(), ios::out );
			if( ! ofs ) {
				cerr << "File \'" << fileName << "\' could not be opened" << endl;
				return false;
			}
			
			ofs << "LBPMapping" << endl;
			ofs << "version " << 1 << endl;
			ofs << "type " << MappingTypeStr[ type ] << endl;
			ofs << "samples " << samples << endl;
			ofs << "maxnum " << num << endl;
			ofs << "table ";
			for( int i = 0; i < table.size(); i++ ) {
				ofs << table[i] << " ";
			}
			ofs << endl;
			if( type == LBP_MAPPING_HF ) {
				ofs << "orbits ";
				for( int i = 0; i < orbits.size(); i++ ) {
					for(int j = 0; j < orbits[i].size(); j++ ) {
						ofs << orbits[i][j] << " ";
					}
					ofs << "-1 ";
				}
				ofs << endl;
			}
			
			
			return true;
		}
		
		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;
		}
		
	private:
        // Mapping variables
        MappingType type;
        vector<int> table;
        unsigned int samples;
        unsigned int num;
        // Fourier Histogram variables
        vector< vector<int> > orbits;
        double *fftIn;
        complex<double> *fftOut;
        fftw_plan fftPlan;
        unsigned int fftN;
        unsigned int fftHermN;
        vector<double> hf;
        // Histogram
        vector<double> h;
        // Descriptor variables
        Mat lbpImage;
        MatND hist;
};

// main function:
// takes a double color image and a bin size 
// returns HOG features
mxArray *process(const mxArray *mximage, const mxArray *mxsbin) {
	double *im = (double *)mxGetPr(mximage);
	const int *dims = mxGetDimensions(mximage);
	if (mxGetNumberOfDimensions(mximage) != 2)
		mexErrMsgTxt("Invalid input, not a grayscale image");

	if(mxGetClassID(mximage) != mxDOUBLE_CLASS)
		mexErrMsgTxt("Invalid input, not double class");
	  
	int sbin = (int)mxGetScalar(mxsbin);
	
	

	
	
//   // memory for caching orientation histograms & their norms
//   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]*18, sizeof(float));
//   float *norm = (float *)mxCalloc(blocks[0]*blocks[1], sizeof(float));
// 
//   // memory for HOG features
//   int out[3];
//   out[0] = max(blocks[0]-2, 0);
//   out[1] = max(blocks[1]-2, 0);
//   out[2] = 27+4+1;
//   mxArray *mxfeat = mxCreateNumericArray(3, out, mxSINGLE_CLASS, mxREAL);
//   float *feat = (float *)mxGetPr(mxfeat);
//   
//   int visible[2];
//   visible[0] = blocks[0]*sbin;
//   visible[1] = blocks[1]*sbin;
//   
//   for (int x = 1; x < visible[1]-1; x++) {
//     for (int y = 1; y < visible[0]-1; y++) {
//       // first color channel
//       double *s = im + min(x, dims[1]-2)*dims[0] + min(y, dims[0]-2);
//       double dy = *(s+1) - *(s-1);
//       double dx = *(s+dims[0]) - *(s-dims[0]);
//       double v = dx*dx + dy*dy;
// 
//       // second color channel
//       s += dims[0]*dims[1];
//       double dy2 = *(s+1) - *(s-1);
//       double dx2 = *(s+dims[0]) - *(s-dims[0]);
//       double v2 = dx2*dx2 + dy2*dy2;
// 
//       // third color channel
//       s += dims[0]*dims[1];
//       double dy3 = *(s+1) - *(s-1);
//       double dx3 = *(s+dims[0]) - *(s-dims[0]);
//       double v3 = dx3*dx3 + dy3*dy3;
// 
//       // pick channel with strongest gradient
//       if (v2 > v) {
//         v = v2;
//         dx = dx2;
//         dy = dy2;
//       } 
//       if (v3 > v) {
//         v = v3;
//         dx = dx3;
//         dy = dy3;
//       }
// 
//       // snap to one of 18 orientations
//       double best_dot = 0;
//       int best_o = 0;
//       for (int o = 0; o < 9; o++) {
//         double dot = uu[o]*dx + vv[o]*dy;
//         if (dot > best_dot) {
//           best_dot = dot;
//           best_o = o;
//         } else if (-dot > best_dot) {
//           best_dot = -dot;
//           best_o = o+9;
//         }
//       }
//       
//       // add to 4 histograms around pixel using linear interpolation
//       double xp = ((double)x+0.5)/(double)sbin - 0.5;
//       double yp = ((double)y+0.5)/(double)sbin - 0.5;
//       int ixp = (int)floor(xp);
//       int iyp = (int)floor(yp);
//       double vx0 = xp-ixp;
//       double vy0 = yp-iyp;
//       double vx1 = 1.0-vx0;
//       double vy1 = 1.0-vy0;
//       v = sqrt(v);
// 
//       if (ixp >= 0 && iyp >= 0) {
//         *(hist + ixp*blocks[0] + iyp + best_o*blocks[0]*blocks[1]) += 
//           vx1*vy1*v;
//       }
// 
//       if (ixp+1 < blocks[1] && iyp >= 0) {
//         *(hist + (ixp+1)*blocks[0] + iyp + best_o*blocks[0]*blocks[1]) += 
//           vx0*vy1*v;
//       }
// 
//       if (ixp >= 0 && iyp+1 < blocks[0]) {
//         *(hist + ixp*blocks[0] + (iyp+1) + best_o*blocks[0]*blocks[1]) += 
//           vx1*vy0*v;
//       }
// 
//       if (ixp+1 < blocks[1] && iyp+1 < blocks[0]) {
//         *(hist + (ixp+1)*blocks[0] + (iyp+1) + best_o*blocks[0]*blocks[1]) += 
//           vx0*vy0*v;
//       }
//     }
//   }
// 
//   // compute energy in each block by summing over orientations
//   for (int o = 0; o < 9; o++) {
//     float *src1 = hist + o*blocks[0]*blocks[1];
//     float *src2 = hist + (o+9)*blocks[0]*blocks[1];
//     float *dst = norm;
//     float *end = norm + blocks[1]*blocks[0];
//     while (dst < end) {
//       *(dst++) += (*src1 + *src2) * (*src1 + *src2);
//       src1++;
//       src2++;
//     }
//   }
// 
//   // compute features
//   for (int x = 0; x < out[1]; x++) {
//     for (int y = 0; y < out[0]; y++) {
//       float *dst = feat + x*out[0] + y;      
//       float *src, *p, n1, n2, n3, n4;
// 
//       p = norm + (x+1)*blocks[0] + y+1;
//       n1 = 1.0 / sqrt(*p + *(p+1) + *(p+blocks[0]) + *(p+blocks[0]+1) + eps);
//       p = norm + (x+1)*blocks[0] + y;
//       n2 = 1.0 / sqrt(*p + *(p+1) + *(p+blocks[0]) + *(p+blocks[0]+1) + eps);
//       p = norm + x*blocks[0] + y+1;
//       n3 = 1.0 / sqrt(*p + *(p+1) + *(p+blocks[0]) + *(p+blocks[0]+1) + eps);
//       p = norm + x*blocks[0] + y;      
//       n4 = 1.0 / sqrt(*p + *(p+1) + *(p+blocks[0]) + *(p+blocks[0]+1) + eps);
// 
//       float t1 = 0;
//       float t2 = 0;
//       float t3 = 0;
//       float t4 = 0;
// 
//       // contrast-sensitive features
//       src = hist + (x+1)*blocks[0] + (y+1);
//       for (int o = 0; o < 18; o++) {
//         float h1 = min(*src * n1, 0.2);
//         float h2 = min(*src * n2, 0.2);
//         float h3 = min(*src * n3, 0.2);
//         float h4 = min(*src * n4, 0.2);
//         *dst = 0.5 * (h1 + h2 + h3 + h4);
//         t1 += h1;
//         t2 += h2;
//         t3 += h3;
//         t4 += h4;
//         dst += out[0]*out[1];
//         src += blocks[0]*blocks[1];
//       }
// 
//       // contrast-insensitive features
//       src = hist + (x+1)*blocks[0] + (y+1);
//       for (int o = 0; o < 9; o++) {
//         float sum = *src + *(src + 9*blocks[0]*blocks[1]);
//         float h1 = min(sum * n1, 0.2);
//         float h2 = min(sum * n2, 0.2);
//         float h3 = min(sum * n3, 0.2);
//         float h4 = min(sum * n4, 0.2);
//         *dst = 0.5 * (h1 + h2 + h3 + h4);
//         dst += out[0]*out[1];
//         src += blocks[0]*blocks[1];
//       }
// 
//       // texture features
//       *dst = 0.2357 * t1;
//       dst += out[0]*out[1];
//       *dst = 0.2357 * t2;
//       dst += out[0]*out[1];
//       *dst = 0.2357 * t3;
//       dst += out[0]*out[1];
//       *dst = 0.2357 * t4;
// 
//       // truncation feature
//       dst += out[0]*out[1];
//       *dst = 0;
//     }
//   }
// 
//   mxFree(hist);
//   mxFree(norm);
  return mxfeat;
}

// 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, 2 inputs needed"); 
  if (nlhs != 1)
    mexErrMsgTxt("Wrong number of outputs, 1 output needed");
  
  plhs[0] = process(prhs[0], prhs[1]);
}