/**************************************************************************
 * Source File:   linkdist6.c                                             *
 * Description:   Compute distance to obstacle with many shapes, for link *
 *     class with any number of shapes per link, and output a matrix:     *
 *     with each row a vector from each obstacle to each shape in robot,  *
 *     then the near point on the robot, then the robot link number.      *
 * Author:        Juanita Albro (standing on the shoulders of giants)     *
 * Modifications:                                                         *
 * University of California at Irvine, 2002, 2005                         *
 * Questions/comments: juanita@eng.uci.edu                                *
 **************************************************************************/

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


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

  if (nrhs !=5){
    mexErrMsgTxt("five input arguments required: l,pos,g,tot_obs,vec_shapes"); 
  }else if (nlhs > 1){
    mexErrMsgTxt("Only one output argument allowed.");
  }

  // feeble attempt at error checking the input arguments
  if (!mxIsStruct(prhs[0]) || !mxIsStruct(prhs[2])){
     mexErrMsgTxt("Error with input arguments.");
  }


  Vector pos,vel,acc,tau,apmap;

  unsigned int n = mxGetN(prhs[0]);

  pos.Resize(n);
  vel.Resize(n);
  acc.Resize(n);
  tau.Resize(n);
  apmap.Resize(n);

  vel.Zero();
  acc.Zero();
  tau.Zero();

  pos = mattocvector(prhs[1]);

  Link *l1;
  l1 = mattoclink(prhs[0],pos,vel,acc,tau,apmap);

  Geometry g;
  g = mattocgeometry(prhs[2]);

  //JVC mod
  Vector vecshapes;
  vecshapes.Resize(n);
  vecshapes = mattocvector(prhs[4]);

  double *obsno = mxGetPr(prhs[3]);
  int totobs = (int) *obsno;
  //printf("tot obs = %d\n",totobs);
  //JVC mod end

  DistanceCollection d;

  l1->Distance(g,d);
  //l1->DistanceDerivative(g,d);
  //JVC mod
  //for now, just see what's in DistanceCollection
  
   n = d.NumPoints();
  printf("n is %d\n",n);
   
  for (int i = 0; i<n;i++){
      //min[2*i][0] = d[i].link_pt.x;
      //min[2*i][1] = d[i].link_pt.y;
      //min[2*i][2] = d[i].link_pt.z;
      //min[2*i+1][0] = d[i].obstacle_pt.x;
      //min[2*i+1][1] = d[i].obstacle_pt.y;
      //min[2*i+1][2] = d[i].obstacle_pt.z;
      printf("%f %f %f\n",d[i].link_pt.x,
		                  d[i].link_pt.y,
						  d[i].link_pt.z);
      printf("%f %f %f\n",d[i].obstacle_pt.x,
		                  d[i].obstacle_pt.y,
						  d[i].obstacle_pt.z);
	  for (int j = 0; j < 8; j++){
          printf("vertex %i: %f %f %f lambda%i %f\n",j, d[i].vertices[j].x,
		                                                d[i].vertices[j].y,
						                                d[i].vertices[j].z,
											         j, d[i].lambdas[j]);
	  }      
  }
 
 
//  Matrix mout(2*n,3);
  //n = d.NumPoints();

  Matrix min(2*n,3);

  for (i = 0; i<n;i++){
      min[2*i][0] = d[i].link_pt.x;
      min[2*i][1] = d[i].link_pt.y;
      min[2*i][2] = d[i].link_pt.z;
      min[2*i+1][0] = d[i].obstacle_pt.x;
      min[2*i+1][1] = d[i].obstacle_pt.y;
      min[2*i+1][2] = d[i].obstacle_pt.z;
      //printf("%f %f %f\n",min[2*i][0],min[2*i][1],min[2*i][2]);
      //printf("%f %f %f\n",min[2*i+1][0],min[2*i+1][1],min[2*i+1][2]);

  }

  //JVC mod, trying out ReorderDistanceToMatlab
  //Matrix mout(2*n,3);
  Matrix mout(n,7);

  //mout = l1->ReorderDistanceToMatlab(min,totobs,vecshapes);
  mout = l1->WhatDoICallThisFunction(min,totobs,vecshapes);

  //plhs[0] = mxCreateDoubleMatrix(2*n,3,mxREAL);
  plhs[0] = mxCreateDoubleMatrix(n,7,mxREAL);

  double *xout;
  xout = mxGetPr(plhs[0]);

  ctomatmatrix(mout,xout);

  delete l1;
  

}