#include <mex.h>

#include <mat.h>

#include <matrix.h>

#define cimg_plugin "cimgmatlab.h"

#include "CImg.h"

#include <iostream>

#include <string>

using namespace cimg_library;

using namespace std;

// The lines below are necessary when using a non-standard compiler as visualcpp6.

#ifdef cimg_use_visualcpp6

#define std

#endif

#ifdef min

#undef min

#undef max

#endif

#ifndef cimg_imagepath

#define cimg_imagepath "img/"

#endif

//global constants

const bool normalize = true; //"Histogram normalization of the images"

const bool morph = true;//"Morphing mode"

const bool imode  = true;//"Complete interpolation (or last frame is missing)"

const bool dispflag = true;//"Visualization"

/////////////////////////////////////////////////////////////////////////////////

//Function Prototype

template<typename T> CImg<T> getwarp(const CImg<T>& src, const CImg<>& u);

//mex function

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

  if (nrhs < 2) mexErrMsgTxt("No enough input arguments.");

  if (nrhs > 2) mexErrMsgTxt("Too many input arguments.");

  if (nrhs == 2){

                CImg<> src(prhs[0],true), u(prhs[1],false);

                //Input images preprocessing

                 CImg<> src_blur(src.size());  

                 src_blur = normalize?src.get_blur(0.5f).equalize(256):src.get_blur(0.5f);

                 CImgDisplay disp(src_blur);

                 if (dispflag) {

                                unsigned int w = src.dimx(), h = src.dimy();

                                const unsigned int dmin = cimg::min(w,h), minsiz = 512;

                                if (dmin<minsiz) { w=w*minsiz/dmin; h=h*minsiz/dmin; }

                                const unsigned int dmax = cimg::max(w,h), maxsiz = 1024;

                                if (dmax>maxsiz) { w=w*maxsiz/dmax; h=h*maxsiz/dmax; }

                                disp.assign(w,h,"Estimated Motion",0);

                             }

                 CImg<> c  = src_blur.get_pointwise_norm(1).resize(src.dimx(),src.dimy()).normalize(0,180);

                 c.display(disp);

                // disp.wait();

                 const unsigned char white = 255;

                 c.draw_quiver(u,&white,0.7f,10,-14,0).display(disp);

                 disp.wait();

            }

  return;

}

// get_warp() : Return the image src warped by the motion field u.

//------------

template<typename T> CImg<T> getwarp(const CImg<T>& src, const CImg<>& u) {

  CImg<T> warp(src);

  cimg_forXY(warp,x,y) warp(x,y) = (T)src.linear_atXY(x - u(x,y,0), y - u(x,y,1));

  return warp;

}