//     This code implements the recoloring method described in the paper:
//     Musialski, P., Cui, M., Ye, J., Razdan, A., & Wonka, P. (2012). 
//     A Framework for Interactive Image Color Editing. 
//     The Visual Computer, 29(11), 1173–1186. doi:10.1007/s00371-012-0761-5
// 
//     Copyright (C) 2011-2013, Przemyslaw Musialski, pm@cg.tuwien.ac.at
// 
//     This program is free software: you can redistribute it and/or modify
//     it under the terms of the GNU General Public License as published by
//     the Free Software Foundation, either version 3 of the License, or
//     (at your option) any later version.
// -------------------------------------------------------------------------

#include "mex.h"

#include <vector>
#include <iostream>
using namespace std;

template <class T>
void GetPointFromBarycentric3D(T a[], T b[], T c[], T d[], T bc[], T p[])
{
    p[0] = a[0] * bc[0] + b[0] * bc[1] + c[0]*bc[2] + d[0]*bc[3];
    p[1] = a[1] * bc[0] + b[1] * bc[1] + c[1]*bc[2] + d[1]*bc[3];
    p[2] = a[2] * bc[0] + b[2] * bc[1] + c[2]*bc[2] + d[2]*bc[3];
}

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

  
  /*  check for proper number of arguments */
  /* NOTE: You do not need an else statement when using mexErrMsgTxt
     within an if statement, because it will never get to the else
     statement if mexErrMsgTxt is executed. (mexErrMsgTxt breaks you out of
     the MEX-file) */
  if(nrhs!=3) 
    mexErrMsgTxt("Three inputs matrices required. a 4*M barycentric coord list,a D*N reference point set, and a 4*M index set.");
  if(nlhs!=1) 
    mexErrMsgTxt("One output required. D*M coordinates");
  
  
  int M, N , D;
  
  M = mxGetN(prhs[0]);
  if (M != mxGetN(prhs[2]))
   mexErrMsgTxt("the number of points in the first and the third input matrix must be same");
  D = mxGetM(prhs[1]);
  N = mxGetN(prhs[1]);
  if (mxGetM(prhs[0])!=4 || mxGetM(prhs[2])!=4)
    mexErrMsgTxt("the dimension has problems!");  
  double * query, * ref, * ind, *bary;
  /*  get the scalar input x */
  bary = mxGetPr(prhs[0]);
  ref = mxGetPr(prhs[1]);
  ind = mxGetPr(prhs[2]);
  
  /*  set the output pointer to the output matrix */
  plhs[0] = mxCreateDoubleMatrix(D,M, mxREAL);
  /*  create a C pointer to a copy of the output matrix */
  query = mxGetPr(plhs[0]);
  
  int i;
  
  for(i = 0; i < M; i ++)
  {
  	double * a, *b, *c, *d, *p, *bc;
  	a = ref + ((int)(ind[i*4+0]) - 1)*D;
  	b = ref + ((int)(ind[i*4+1]) - 1)*D;
  	c = ref + ((int)(ind[i*4+2]) - 1)*D;
  	d = ref + ((int)(ind[i*4+3]) - 1)*D;
  	p = query + i*D;
  	bc = bary + i*4;
  	//printf("the first reference point is %f, %f\n",a[0],a[1]);
  	//printf("the second reference point is %f, %f\n",b[0],b[1]);
  	//printf("the third reference point is %f, %f\n",c[0],c[1]);
  	//printf("the bary centric is %f, %f, %f\n",bc[0],bc[1],bc[2]);
  	GetPointFromBarycentric3D(a,b,c,d,bc,p);
  }    
}