/* originally written by Maxim Likhachev, simplified by Ben Cohen */

#include <math.h>
#include <vector>
#include "mex.h"
using namespace std;

typedef struct cell{
	int x;
	int y;
} CELL

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	// input
	double *map, *fstart, *fgoal;
	double * bpath_array;
	int width, height, startx, starty;
	vector<CELL> pathV;
	CELL bcell;

	if (nrhs < 3)
		mexErrMsgTxt("Need three input arguments.");

	//get dimensions of map
	map = mxGetPr(prhs[0]); 
	height = mxGetM(prhs[0]);	//mxGetM - gets number of rows of mxArray
	width = mxGetN(prhs[0]);  //mxGetN - gets number of columns of mxArray

	//get start position
	fstart = mxGetPr(prhs[1]);
	startx = ((int)fstart[0]);
	starty = ((int)fstart[1]);

	//get goal position
	fgoal = mxGetPr(prhs[1]);
	goalx = ((int)goal[0]);
	goaly = ((int)goal[1]);

	// get path from planner
	printf("calling planner\n");
	/*
	 * Call your planner here with these parameters (or your params):
	 * bool myPlanner(int *map, int width, int height, int startx, int starty, int goalx, int goaly,std::vector<cell> &pathV) 
	 *  return 1 if success and not 1 if planning failed (so exit cleanly)
	 */
	printf("call to planner finished\n");

	// output to matlab
	int x, y, npath;
	npath = pathV.size();
	plhs[0] = mxCreateDoubleMatrix(npath,1,mxREAL);
	plhs[1] = mxCreateDoubleMatrix(npath,1,mxREAL);
	double *xpath = mxGetPr(plhs[0]);
	double *ypath = mxGetPr(plhs[1]);

	for (int i = 0; i < npath; i++) {
		xpath[i] = pathV[i].x;
		ypath[i] = pathV[i].y;
	}
}