#include "mattoc.h"


Vector3d mattocvector3d(const mxArray *v)
{
  // Convert a matlab vector3d class into a C++ Vector3d class
  //  see @vector3d/vector3d.m for description of the matlab vector3d class
  //  see include/matrix3d.h for a description of the C++ vector3d class

  mxArray *tmp;
  const char * fname;
  double *x,*y,*z;

  fname = mxGetFieldNameByNumber(v,0);
  tmp = mxGetField(v,0,fname);
#if MEXDEBUG
  if (!mxIsDouble(tmp) && !(mxGetM(tmp) == 1) && !(mxGetN(tmp) == 1)){
    mexErrMsgTxt("Error in conversion of Vector3d.");
  }
#endif
  x = mxGetPr(tmp);

  fname = mxGetFieldNameByNumber(v,1);
  tmp = mxGetField(v,0,fname);
#if MEXDEBUG
  if (!mxIsDouble(tmp) && !(mxGetM(tmp) == 1) && !(mxGetN(tmp) == 1)){
    mexErrMsgTxt("Error in conversion of Vector3d.");
  }
#endif
  y = mxGetPr(tmp);

  fname = mxGetFieldNameByNumber(v,2);
  tmp = mxGetField(v,0,fname);
#if MEXDEBUG
  if (!mxIsDouble(tmp) && !(mxGetM(tmp) == 1) && !(mxGetN(tmp) == 1)){
    mexErrMsgTxt("Error in conversion of Vector3d.");
  }
#endif
  z = mxGetPr(tmp);

  return Vector3d(x[0],y[0],z[0]);
}

Vector3d mattocvector3d(const mxArray *v,int idx)
{
  // Convert a matlab vector3d class into a C++ Vector3d class
  //  see @vector3d/vector3d.m for description of the matlab vector3d class
  //  see include/matrix3d.h for a description of the C++ vector3d class

  mxArray *tmp;
  const char * fname;
  double *x,*y,*z;

  fname = mxGetFieldNameByNumber(v,0);
  tmp = mxGetField(v,idx,fname);
#if MEXDEBUG
  if (!mxIsDouble(tmp) && !(mxGetM(tmp) == 1) && !(mxGetN(tmp) == 1)){
    mexErrMsgTxt("Error in conversion of Vector3d.");
  }
#endif
  x = mxGetPr(tmp);

  fname = mxGetFieldNameByNumber(v,1);
  tmp = mxGetField(v,idx,fname);
#if MEXDEBUG
  if (!mxIsDouble(tmp) && !(mxGetM(tmp) == 1) && !(mxGetN(tmp) == 1)){
    mexErrMsgTxt("Error in conversion of Vector3d.");
  }
#endif
  y = mxGetPr(tmp);

  fname = mxGetFieldNameByNumber(v,2);
  tmp = mxGetField(v,idx,fname);
#if MEXDEBUG
  if (!mxIsDouble(tmp) && !(mxGetM(tmp) == 1) && !(mxGetN(tmp) == 1)){
    mexErrMsgTxt("Error in conversion of Vector3d.");
  }
#endif
  z = mxGetPr(tmp);

  return Vector3d(x[0],y[0],z[0]);
}


Vector mattocvector(const mxArray *v)
{
  // Convert a matlab vector into a C++ Vector class
  //  matlab vectors are normal (not a class) arrays
  //  see include/matrix.h for a description of the C++ vector class

  //printf("inside mattocvector\n");
  double *x;
  
  //printf("inside mattocvector\n");
  x = mxGetPr(v);
  
  //printf("inside mattocvector\n");
  unsigned int n = mxGetN(v);
  //printf("mxGetN = %d\n",n);
  unsigned int len = mxGetN(v) * mxGetM(v);

  //printf("inside mattocvector\n");
  if (len == 0) return Vector();
  else return Vector(x,len);

}



void ctomatvector(const Vector &v, double *x)
{
  // make sure memory is allocated for x correctly...

  for (int i = 0;i<v.size();i++){
    x[i] = v[i];
  }
}

void ctomatmatrix(const Matrix &m, double *x)
{
  // make sure memory is allocated for x!!!

  unsigned int r = m.Nrows();
  unsigned int c = m.Ncols();

  for (int j=0;j<c;j++){
    for (int i=0;i<r;i++){
      x[i + j*r] = m[i][j];
    }
  }

}

//JVC
void ctomatmatrixarray(Matrix **matrixarray, double *x, const int *dims)
{
  // make sure memory is allocated for x!!!
  //Matrix &m = *matrixarray[0]; 
  //unsigned int r = m.Nrows();
  //unsigned int c = m.Ncols();

  int r = dims[0];
  int c = dims[1];

  for (int k = 0; k < dims[2]; ++k)  {
    for (int j = 0; j < c; ++j)   {
      for (int i = 0; i < r; ++i)  {
        x[i + j*r + k*(r*c)] = (*matrixarray[k])[i][j];
      }
    }
  }

}

void ctomatmatrix4d(const Matrix4d &m, double *x)
{
  // make sure memory is allocated for x before calling me!!!

  for (int j=0;j<3;j++){
    for (int i=0;i<3;i++){
      x[i + j*4] = m.GetRotation(i+1,j+1);
    }
    x[3+j*4] = 0.0;
  }

  x[12] = m.GetTranslation().x;
  x[13] = m.GetTranslation().y;
  x[14] = m.GetTranslation().z;
  x[15] = 1.0;

}

Matrix mattocmatrix(const mxArray *m)
{
  double *x;
  
  x = mxGetPr(m);

  unsigned int r = mxGetM(m);
  unsigned int c = mxGetN(m);

  // wouldn't ya know it, matlab stores things in a weird way...
  //   we need to swap the order here...
  double *y;
  y = new double[r*c];
  for (int i=0;i<r;i++){
    for (int j=0;j<c;j++){
      y[c*i+j] = x[i+r*j];
    }
  } 

  Matrix out(r,c,y);

  delete [] y;

  return out;

}

Bspline* mattocpatharray(const mxArray *pth)
{
  mxArray *tmp;
  const char * fname;
  
  int np = mxGetN(pth); // number of paths to convert
  Bspline *bsp;
  bsp = new Bspline[np];
  Vector ts;
  Matrix xp;

  for (int i=0;i<np;i++){
    // third field is a spline
    fname = mxGetFieldNameByNumber(pth,2);
    tmp = mxGetField(pth,i,fname);
    bsp[i] = mattocbspline(tmp);

    // 6th field is Tspan (information from matlab integration)
    fname = mxGetFieldNameByNumber(pth,5);
    tmp = mxGetField(pth,i,fname);
    ts = mattocvector(tmp);

    // 7th field is Xpass (information from matlab integration)
    fname = mxGetFieldNameByNumber(pth,6);
    tmp = mxGetField(pth,i,fname);
    xp = mattocmatrix(tmp);
 
    // set the data
    bsp[i].Xpass(xp);
    bsp[i].Tspan(ts);

  }
  
  return bsp;
}



#if 1
Bspline mattocpath(const mxArray *pth)
{
  mxArray *tmp;
  const char * fname;

#if 0
  // first field is interpolating points
  fname = mxGetFieldNameByNumber(pth,0);
  tmp = mxGetField(pth,0,fname);
  unsigned int r = mxGetM(tmp);
  unsigned int c = mxGetN(tmp);
  Matrix m = mattocmatrix(tmp);
#endif

#if 0
  // second field is matching time values
  fname = mxGetFieldNameByNumber(pth,1);
  tmp = mxGetField(pth,0,fname);
  Vector t = mattocvector(tmp);
#endif

#if 0
  Vector *pt; // interpolation points
  pt = new Vector[c];
  for (int i=0;i<c;i++) {
    pt[i].Resize(r);
    m.GetColumn(i,pt[i]);
  }
#endif

  // third field is a spline
  fname = mxGetFieldNameByNumber(pth,2);
  tmp = mxGetField(pth,0,fname);
  Bspline bsp = mattocbspline(tmp);

#if 0
  // 4th field is dimension
  fname = mxGetFieldNameByNumber(pth,3);
  tmp = mxGetField(pth,0,fname);
  double *dd = mxGetPr(tmp);
  unsigned int dim = (int)dd[0];
#endif

  // 5th field is order

  // 6th field is Tspan (information from matlab integration)
  fname = mxGetFieldNameByNumber(pth,5);
  tmp = mxGetField(pth,0,fname);
  Vector ts = mattocvector(tmp);

  // 7th field is Xpass (information from matlab integration)
  fname = mxGetFieldNameByNumber(pth,6);
  tmp = mxGetField(pth,0,fname);
  Matrix xp = mattocmatrix(tmp);

  bsp.Xpass(xp);
  bsp.Tspan(ts);
  
  
#if 0
  Path out;
  out.Interpolate(r,c,pt,t);
  delete [] pt;
#endif
  

  return bsp;
}

Bspline mattocbspline(const mxArray *bs)
{
  mxArray *tmp;
  const char * fname;

  // first field is multipliers
  fname = mxGetFieldNameByNumber(bs,0);
  tmp = mxGetField(bs,0,fname);
  Matrix mult = mattocmatrix(tmp);

  unsigned int dim = mxGetM(tmp);
  unsigned int num_points = mxGetN(tmp);

  //printf("converting knots_without\n");
  // second field is knots (without multiplicity)
  fname = mxGetFieldNameByNumber(bs,1);
  tmp = mxGetField(bs,0,fname); 
  Vector knots = mattocvector(tmp);

  // third field is dimension of spline
  
  //printf("converting order\n");
  // fourth field is the order
  fname = mxGetFieldNameByNumber(bs,3);
  tmp = mxGetField(bs,0,fname);
  double *d_order = mxGetPr(tmp);   
  unsigned int order = (int)d_order[0];

  // now use the order information to set up the end point multiplicity
  unsigned int i,j;
  unsigned int num_knots = knots.size();
  // we now have the correct multiplicities in 'knots'


  Vector *ptv;
  ptv = new Vector[num_points];
  for (i=0;i<num_points;i++){
    ptv[i].Resize(dim);
    mult.GetColumn(i+1,ptv[i]);
  }

  //printf("out of mex crap\n");

  Bspline out;

  out.SetT(knots);
  //printf("out of SetT\n");
  out.SetP(num_points,ptv,dim);
  //printf("out of SetP\n");

  delete [] ptv;


  return out;

}

  
#endif

se3 mattocse3(const mxArray *s)
{
  mxArray *tmp;
  const char * fname;
  Vector3d w,v;

  fname = mxGetFieldNameByNumber(s,0);
  tmp = mxGetField(s,0,fname);
#if MEXDEBUG
  if (!mxIsStruct(tmp)){
    mexErrMsgTxt("Error in conversion of se3.");
  }
#endif
  w = mattocvector3d(tmp);

  fname = mxGetFieldNameByNumber(s,1);
  tmp = mxGetField(s,0,fname);
#if MEXDEBUG
  if (!mxIsStruct(tmp)){
    mexErrMsgTxt("Error in conversion of se3.");
  }
#endif
  v = mattocvector3d(tmp);

  return se3(w,v);
}

//Put the ForceApp Stuff in...
ForceApp mattocforceapp(const mxArray *fa,int i)
{
  mxArray *tmp;
  const char * fname;
  se3 force;
  Matrix4d frame;

  fname = mxGetFieldNameByNumber(fa,0);
  tmp = mxGetField(fa,i,fname);
#if MEXDEBUG
  if (!mxIsStruct(tmp)){
    mexErrMsgTxt("Error in conversion of ForceApp.");
  }
#endif

  force = mattocse3(tmp);

  fname = mxGetFieldNameByNumber(fa,1);
  tmp = mxGetField(fa,i,fname);
#if MEXDEBUG
  if (!mxIsStruct(tmp)){
    mexErrMsgTxt("Error in conversion of ForceApp.");
  }
#endif
  
  frame = mattocmatrix4d(tmp);
//  f.F = force;
//  f.frame = frame;
//  f.next = NULL;
  return ForceApp(force,frame,NULL);

}
//end of ForceApp Stuff...JVC

Matrix4d mattocmatrix4d(const mxArray *m)
{
  mxArray *tmp;
  const char * fname;
  double d[3][3];
  double v[3];
  double *dd;

  fname = mxGetFieldNameByNumber(m,0);
  tmp = mxGetField(m,0,fname);
#if MEXDEBUG
  if (!mxIsDouble(tmp) || (mxGetM(tmp) != 4) || (mxGetN(tmp) != 4)){
    mexErrMsgTxt("Error in conversion of Matrix4d.");
  }
#endif
  dd = mxGetPr(tmp);

  int i,j,k=0;

  for (j=0;j<3;j++){
    for (i=0;i<3;i++){
      d[i][j] = dd[j*3 + i + k];
    }
    k++ ;
  }

  for (j=0;j<3;j++){
    v[j] = dd[12+j];
  }

  return Matrix4d(d,Vector3d(v[0],v[1],v[2]));
}

GenMass mattocgenmass(const mxArray *gm)
{
  mxArray *tmp;
  const char * fname;
  Vector3d v;
  double *mass;
  double *matmat;

//  printf ("inside mattocgenmass\n");
  fname = mxGetFieldNameByNumber(gm,0);
  tmp = mxGetField(gm,0,fname);
#if MEXDEBUG
  if (!mxIsDouble(tmp) || (mxGetM(tmp) != 3) || (mxGetN(tmp) != 3)){
    mexErrMsgTxt("Error in conversion of General Mass Matrix.");
  }
#endif
  matmat = mxGetPr(tmp); // this is the 3x3 matlab matrix 
  
  fname = mxGetFieldNameByNumber(gm,1);
  tmp = mxGetField(gm,0,fname);
#if MEXDEBUG
  if (!mxIsStruct(tmp)){
    mexErrMsgTxt("Error in conversion of General Mass Matrix.");
  }
#endif
  v = mattocvector3d(tmp);

  fname = mxGetFieldNameByNumber(gm,2);  
  tmp = mxGetField(gm,0,fname);
#if MEXDEBUG
  if (!mxIsDouble(tmp) || (mxGetM(tmp) != 1) || (mxGetN(tmp) != 1)){
    mexErrMsgTxt("Error in conversion of General Mass Matrix.");
  }
#endif
  mass = mxGetPr(tmp);

  //printf("ready to leave mattocgenmass\n");
  return GenMass(matmat, v, mass[0]);

}


Sphere mattocsphere(const mxArray *sp, int i )
{
  mxArray *tmp;
  const char * fname;
  
  // first field is center point
  fname = mxGetFieldNameByNumber(sp,0);
  tmp = mxGetField(sp,i,fname);
  Vector3d ctr = mattocvector3d(tmp);

  // second field is radius
  fname = mxGetFieldNameByNumber(sp,1);
  tmp = mxGetField(sp,i,fname);
#if MEXDEBUG
  if (!mxIsDouble(tmp) || (mxGetM(tmp) != 1) || (mxGetN(tmp) != 1)){
    mexErrMsgTxt("Error in conversion of Sphere.radius");
  }
#endif
  double *r = mxGetPr(tmp);

  // third field is color
  fname = mxGetFieldNameByNumber(sp,2);
  tmp = mxGetField(sp,i,fname);
#if MEXDEBUG
  if (!mxIsDouble(tmp) || (mxGetM(tmp) != 1) || (mxGetN(tmp) != 1)){
    mexErrMsgTxt("Error in conversion of Sphere.color");
  }
#endif
  double *c = mxGetPr(tmp);
  
  // ignore color for now....GAS
  // no longer should I ignore color...GAS!
 
  Sphere out(ctr,r[0]);
  unsigned long mycolor;
  mattoccolor(*c,mycolor);
  out.SetColor(mycolor);
  return out;

}

MyPolygon mattocpoly(const mxArray *poly, int i)
{
  //printf("trying to convert a poly2d\n");
  mxArray * tmp;
  const char * fname;
  int nvert;

  // first field is x coordinates
  fname = mxGetFieldNameByNumber(poly,0);
  tmp = mxGetField(poly,i,fname);
  Vector x = mattocvector(tmp);
  nvert = x.size();

  // second field is y coordinates
  fname = mxGetFieldNameByNumber(poly,1);
  tmp = mxGetField(poly,i,fname);
  Vector y = mattocvector(tmp);

  // third field is z coordinates
  fname = mxGetFieldNameByNumber(poly,2);
  tmp = mxGetField(poly,i,fname);
  Vector z = mattocvector(tmp);

  // fourth field is color
  fname = mxGetFieldNameByNumber(poly,3);
  tmp = mxGetField(poly,i,fname);
  double *c = mxGetPr(tmp);
  //printf("matlab color = %f\n",*c);
  unsigned long mycolor;
  mattoccolor(*c,mycolor);
  //printf("matlab color = %f\n",*c);

  Vector3d *vert;
  vert = new Vector3d[nvert];
  
  for (int j=0;j<nvert;j++){
    vert[j].x = x[j];
    vert[j].y = y[j];
    vert[j].z = z[j];
  }
  
  //printf("my color = %ld\n",mycolor);
  MyPolygon out(nvert,vert,mycolor);

  delete [] vert;

  return out;
}


Rod mattocrod(const mxArray *rd, int i )
{
  mxArray *tmp;
  const char * fname;
  //printf("hey, slower\n");

  // first field is p1
  fname = mxGetFieldNameByNumber(rd,0);
  tmp = mxGetField(rd,i,fname);
  Vector3d p1 = mattocvector3d(tmp);

  // second field is p2
  fname = mxGetFieldNameByNumber(rd,1);
  tmp = mxGetField(rd,i,fname);
  Vector3d p2 = mattocvector3d(tmp);

  // third field is radius
  fname = mxGetFieldNameByNumber(rd,2);
  tmp = mxGetField(rd,i,fname);
#if MEXDEBUG
  if (!mxIsDouble(tmp) || (mxGetM(tmp) != 1) || (mxGetN(tmp) != 1)){
    mexErrMsgTxt("Error in conversion of Rod.radius");
  }
#endif
  double *r = mxGetPr(tmp);
  
  // fourth field is color
  fname = mxGetFieldNameByNumber(rd,3);
  tmp = mxGetField(rd,i,fname);
#if MEXDEBUG
  if (!mxIsDouble(tmp) || (mxGetM(tmp) != 1) || (mxGetN(tmp) != 1)){
    mexErrMsgTxt("Error in conversion of Rod.color");
  }
#endif
  double *c = mxGetPr(tmp);
  
  Rod out(p1,p2,r[0]); 
  // no longer should I ignore color...GAS!
  unsigned long mycolor;
  mattoccolor(*c,mycolor);
  out.SetColor(mycolor);
  delete r;
  delete c;
  return out;
}

void mattoccolor(const double c, unsigned long &hexcolor)
{
  double red,green,blue;
  if (c < (23.0/63.0)) red = 0;
  else if (c < (39.0/63.0)) red = (255 / (16.0/63.0)) * (c-(23.0/63.0));
  else if (c < (56.0/63.0)) red = 255;
  else red = 255 + ((-255.0 / (16.0/63.0))* (c-(56.0/63.0)));

  if (c < (7.0/63.0)) green = 0;
  else if (c < (23.0/63.0)) green = (255 / (16.0/63.0)) * (c-(7.0/63.0));
  else if (c < (40.0/63.0)) green = 255;
  else if (c < (56.0/63.0)) green = 255 + ((-255.0 / (16.0/63.0)) * (c-(40.0/63.0)));
  else green = 0;

  if (c < (7.0/63.0)) blue = (255 / (16.0/63.0)) * (c + (9.0/63.0));
  else if (c < (24.0/63.0)) blue = 255;
  else if (c < (40.0/63.0)) blue = 255 + ((-255.0/(16.0/63.0)) * (c-(24.0/63.0)));
  else blue = 0;
  
  unsigned long nred,ngreen,nblue;
  nred = (unsigned long)(red);
  ngreen = 256* (unsigned long) green;
  nblue = 256*256* (unsigned long)blue;

  hexcolor = 0;
  hexcolor = nred+ngreen+nblue;
  //printf("hexcolor = %ld\n",hexcolor);

}


GeneralGeometry mattocgengeom(const mxArray *gg)
{
  mxArray *tmp;
  const char * fname;
  GeneralGeometry out;


#if 1
  //printf("field 1 of gengeom\n");
  // first field is draw
  fname = mxGetFieldNameByNumber(gg,0);
  tmp = mxGetField(gg,0,fname);
  Geometry drawg = mattocgeometry(tmp);
  //printf("**************************\n");
  //printf("returned %ld\n",&drawg);
  //printf("**************************\n");

  //printf("field 2 of gengeom\n");
  // second field is distance
  fname = mxGetFieldNameByNumber(gg,1);
  tmp = mxGetField(gg,0,fname);
  Geometry distg = mattocgeometry(tmp);
  //printf("**************************\n");
  //printf("returned %ld\n",&distg);
  //printf("**************************\n");
#else
  Geometry drawg;
  Geometry distg;
#endif


  //printf("calling setgeom for draw\n");
  out.SetGeometry(drawg,DrawGeometry);
  //printf("calling setgeom for dist\n");
  out.SetGeometry(distg,DistGeometry);

  //printf("returning from mattocgengeom\n");
  return out;
}

#if 0
Geometry mattocgeometry(const mxArray *g)
{
  mxArray *tmp;
  const char * fname;
  Geometry out;
  //printf("***************************\n");
  //printf("out = %ld\n",&out);
  //printf("***************************\n");


  //printf("inside mattocgeometry\n");

  // first field is array of cspheres
  fname = mxGetFieldNameByNumber(g,0);
  tmp = mxGetField(g,0,fname);
  
  int ns = mxGetN(tmp);

  Sphere sp;

  for (int i = 0;i<ns;i++){
    sp = mattocsphere(tmp,i);
// Memory leak here: (this has been fixed?), yes it is OK...
    out.AddShape(sp);
  }

  // second field is array of rods
  fname = mxGetFieldNameByNumber(g,1);
  tmp = mxGetField(g,0,fname);
  int nr = mxGetN(tmp);

  //printf("number of rods = %d\n",nr);
  Rod rd;

  for (i = 0;i<nr;i++){
    rd = mattocrod(tmp,i);
// Memory leak here: (this has been fixed?), yes i think it is OK now...
    out.AddShape(rd);
  }

  // third field is handles to matlab objects...only used in matlab

  // fourth field is an array of polygons
  fname = mxGetFieldNameByNumber(g,3);
  tmp = mxGetField(g,0,fname);
  int npoly = mxGetN(tmp);

  MyPolygon poly;
  for (i = 0;i<npoly;i++){
    poly = mattocpoly(tmp,i);
    out.AddShape(poly);
  }

  // fifth field is an array of polyhedron!
  fname = mxGetFieldNameByNumber(g,4);
  tmp = mxGetField(g,0,fname);
  int npoly3d = mxGetN(tmp);
  
  Polyhedron p3d;
  for (i=0;i<npoly3d;i++){
    p3d = mattocpolyhedron(tmp,i);
    //printf("trying to add a polyhedron\n");
    out.AddShape(p3d);
  }


  //printf("returning a geometry in mattoc...\n");
  //out.debug();
  //printf("done printing in mattoc...\n");


  //printf("returning from mattocgeom\n");
  return out;
}
#endif

//JVC mod, new mattocgeometry
Geometry mattocgeometry(const mxArray *g)
{
  mxArray *tmp;
  const char * fname;
  Geometry out;
  //printf("***************************\n");
  //printf("out = %ld\n",&out);
  //printf("***************************\n");


  //printf("inside mattocgeometry\n");

  //JVC mod, making it work with new field, indexflag
  //which is the seventh field
 
#if 0
  fname = mxGetFieldNameByNumber(g,6);
  if (fname != NULL)
    {
    //Note, the number in this next line is *0*.  That was the bug.
    //Squish!  But it's already been squished another way, sigh....
    tmp = mxGetField(g,0,fname);
    printf("fname exists\n");
    Vector indexflagvector = mattocvector(tmp);
    //let's figure out why it crashes here... 
    //printf("data is double: %d\n",mxIsDouble(tmp));
    //printf("Data is numeric: %d\n",mxIsNumeric(tmp));
    //printf("Data is char: %d\n",mxIsChar(tmp));
    //int nstuff = mxGetClassID(tmp);
    //printf("data class is %d\n",nstuff);
    }
  fname = mxGetFieldNameByNumber(g,6);
  if (fname)  //has the field indexflag
    {
    tmp = mxGetField(g,6,fname);
    Vector indexflagvector = mattocvector(tmp);

    if (indexflagvector[0] > 0.0)
      { 
      // first field is array of cspheres
      fname = mxGetFieldNameByNumber(g,0);
      tmp = mxGetField(g,0,fname);
    
      int ns = mxGetN(tmp);
  
      printf("number of spheres = %d\n",ns);
      Sphere sp;
  
      for (int i = 0;i<ns;i++)
        {
        sp = mattocsphere(tmp,i);
        // Memory leak here: (this has been fixed?), yes it is OK...
        out.AddShape(sp);
        printf("just added a sphere\n");
        }
      }

    if (indexflagvector[1] > 0.0)
      { 
      // second field is array of rods
      fname = mxGetFieldNameByNumber(g,1);
      tmp = mxGetField(g,0,fname);
      int nr = mxGetN(tmp);
      
      printf("number of rods = %d\n",nr);
      Rod rd;
      for (int i = 0;i<nr;i++)
        {
        rd = mattocrod(tmp,i);
        // Memory leak here: (this has been fixed?), yes i think it is OK now...
        out.AddShape(rd);
        printf("just added a rod\n");
        }
      }
  
    if (indexflagvector[2] > 0.0)
      {
      // fourth field is an array of polygons
      fname = mxGetFieldNameByNumber(g,3);
      tmp = mxGetField(g,0,fname);
      int npoly = mxGetN(tmp);

      MyPolygon poly;
      for (int i = 0;i<npoly;i++)
        {
        poly = mattocpoly(tmp,i);
        out.AddShape(poly);
        }
      }

    if (indexflagvector[3] > 0.0)
      {
      // fifth field is an array of polyhedron!
      fname = mxGetFieldNameByNumber(g,4);
      tmp = mxGetField(g,0,fname);
      int npoly3d = mxGetN(tmp);
    
      Polyhedron p3d;
      for (int i=0;i<npoly3d;i++)
        {
        p3d = mattocpolyhedron(tmp,i);
        //printf("trying to add a polyhedron\n");
        out.AddShape(p3d);
        }
      }
    }
  else //do what it did before the addition of indexflag
#endif
    { 
    // first field is array of cspheres
    fname = mxGetFieldNameByNumber(g,0);
    tmp = mxGetField(g,0,fname);
    int nstuff = mxGetClassID(tmp);
    //printf("got class id cspere array %d\n",nstuff);
  
    const char *fname2;
    mxArray *tmp2;

    int ns = mxGetN(tmp);
    //printf("number of spheres: %d\n",ns);

    Sphere sp;

    for (int i = 0;i<ns;i++){
      sp = mattocsphere(tmp,i);
      // Memory leak here: (this has been fixed?), yes it is OK...
      double radius = sp.Radius();
      if (radius > 0.0)
        {
        out.AddShape(sp);
        //printf("just added a sphere radius %f\n",radius);
        }
    }

    // second field is array of rods
    fname = mxGetFieldNameByNumber(g,1);
    tmp = mxGetField(g,0,fname);
    int nr = mxGetN(tmp);
  
    //printf("number of rods = %d\n",nr);
    Rod rd;

    for (i = 0;i<nr;i++){
      rd = mattocrod(tmp,i);
      // Memory leak here: (this has been fixed?), yes i think it is OK now...
      double rradius = rd.Radius();
      if (rradius > 0.0)
        {
        out.AddShape(rd);
        //printf("just added a rod radius %f\n",rradius);
        }
    }

    // third field is handles to matlab objects...only used in matlab

    // fourth field is an array of polygons
    fname = mxGetFieldNameByNumber(g,3);
    tmp = mxGetField(g,0,fname);
    int npoly = mxGetN(tmp);
    //printf("number of poly2d = %d\n",npoly);


    MyPolygon poly;
    for (i = 0;i<npoly;i++){
      if (npoly == 1)
        {
        fname2 = mxGetFieldNameByNumber(tmp,3);
        tmp2 = mxGetField(tmp,0,fname2);
        double *c = mxGetPr(tmp2);
        //printf("color is %f\n",*c);
        double colorjj = *c;
        //printf("inside poly2d loop\n"); 
        if (colorjj > 0.0)
          {
          poly = mattocpoly(tmp,i);
          out.AddShape(poly);
          //printf("just added a poly2d\n");
          }
        }  
      else
        {
        poly = mattocpoly(tmp,i);
        out.AddShape(poly);
        //printf("just added a poly2d\n");
        }
    }
  
    // fifth field is an array of polyhedron!
    fname = mxGetFieldNameByNumber(g,4);
    tmp = mxGetField(g,0,fname);
    int npoly3d = mxGetN(tmp);
    //printf("number of poly3d = %d\n",npoly3d);
    
    Polyhedron p3d;
    for (i=0;i<npoly3d;i++){
      if (npoly3d == 1)
        {
        fname2 = mxGetFieldNameByNumber(tmp,3);
        tmp2 = mxGetField(tmp,0,fname2);
        double *c = mxGetPr(tmp2);
        //printf("poly3d color is %f\n",*c);
        double jcolor = *c;
        //printf("poly3d color is %f\n",jcolor);
        
        if (jcolor > 0.0)
          {
          p3d = mattocpolyhedron(tmp,i);
          out.AddShape(p3d);
          //printf("just added a poly3d color %f\n",jcolor);
          }
        }  
      else 
        {
        p3d = mattocpolyhedron(tmp,i);
        //printf("trying to add a polyhedron\n");
        out.AddShape(p3d);
        //printf("just added a poly3d\n");
        }
    }
  } //end else

  //printf("returning a geometry in mattoc...\n");
  //out.debug();
  //printf("done printing in mattoc...\n");


  //printf("returning from mattocgeom\n");
  return out;
}

Polyhedron mattocpolyhedron(const mxArray *p, int idx)
{
  //printf("trying to convert a polyhedron\n");
  mxArray *tmp;
  const char * fname;
  int i = 0;

  // first field is an array of verteces
  fname = mxGetFieldNameByNumber(p,0);
  tmp = mxGetField(p,idx,fname);
  int nvert = mxGetNumberOfElements(tmp); // number of verteces

  Vector3d *verts;
  verts = new Vector3d[nvert];
  for (i=0;i<nvert;i++){
    verts[i] = mattocvector3d(tmp,i);
  }

  // second field is vertex index listings for each facet
  fname = mxGetFieldNameByNumber(p,1);
  tmp = mxGetField(p,idx,fname);
  int nfacet = mxGetNumberOfElements(tmp); // number of facets
  //printf("nfacet = %d\n",nfacet);

  mxArray *facet_tmp;
  int nfv;

  int *fc;
  fc = new int[nfacet];
  int **fi;
  fi = new int*[nfacet];
  int j;
  Vector vv;

  for (i=0;i<nfacet;i++){
    facet_tmp = mxGetCell(tmp,i);
    nfv = mxGetNumberOfElements(facet_tmp);
    fc[i] = nfv;
    fi[i] = new int[nfv];
    vv = mattocvector(facet_tmp);
    for (j=0;j<nfv;j++) {
      fi[i][j] = (int)vv[j]-1; // ARG! i wish Matlab had a integer type
    }
  }

  // third field is array of adjacent verteces...ignore for now

  // fourth field is color
  fname = mxGetFieldNameByNumber(p,3);
  tmp = mxGetField(p,idx,fname);
  double *c = mxGetPr(tmp);
  //printf("in mattocpolyhedron poly3d color is %f\n",*c);

  unsigned long mycolor;
  mattoccolor(*c,mycolor);
    
  Polyhedron out(nvert,verts,nfacet,fc,fi);
  out.SetColor(mycolor);

  for (i=0;i<nfacet;i++) delete [] fi[i];
  delete [] fi;
  delete [] verts;
  delete [] fc;

  return out;


}


Joint mattocjoint(const mxArray *j, int i)
{
  //printf("inside mattocjoint (%d) \n",i);
  mxArray *tmp;
  const char * fname;
  
  // first field is min
  fname = mxGetFieldNameByNumber(j,0);
  tmp = mxGetField(j,i,fname);
#if MEXDEBUG
  if (!mxIsDouble(tmp) || (mxGetM(tmp) != 1) || (mxGetN(tmp) != 1)){
    mexErrMsgTxt("Error in conversion of Joint.");
  }
#endif
  double *min = mxGetPr(tmp);
  //printf("done with min\n");
 
  // second field is max
  fname = mxGetFieldNameByNumber(j,1);
  tmp = mxGetField(j,i,fname);
#if MEXDEBUG
  if (!mxIsDouble(tmp) || (mxGetM(tmp) != 1) || (mxGetN(tmp) != 1)){  
    mexErrMsgTxt("Error in conversion of Joint.");
  }
#endif
  double *max = mxGetPr(tmp);
  //printf("done with max\n");
  
  // third field is value
  fname = mxGetFieldNameByNumber(j,2);
  tmp = mxGetField(j,i,fname);
#if MEXDEBUG
  if (!mxIsDouble(tmp) || (mxGetM(tmp) != 1) || (mxGetN(tmp) != 1)){  
    mexErrMsgTxt("Error in conversion of Joint.");
  }
#endif
  double *val = mxGetPr(tmp);
  //printf("done with val\n");

#if 1
  // fourth field is general geometry
  fname = mxGetFieldNameByNumber(j,3);
  tmp = mxGetField(j,i,fname);
  //printf("converting a general geometry for joint %d\n",i);
  GeneralGeometry gg = mattocgengeom(tmp);
  //printf("done with mattocgengeom for joint %d\n",i);
#else
  GeneralGeometry gg;
#endif


  // fifth field is position
  fname = mxGetFieldNameByNumber(j,4);
  tmp = mxGetField(j,i,fname);
#if MEXDEBUG
  if (!mxIsStruct(tmp)){
    mexErrMsgTxt("Error in conversion of Joint.");
  }
#endif
  Vector3d pos = mattocvector3d(tmp);
  //printf("done with pos\n");
   
  // sixth field is mass
  fname = mxGetFieldNameByNumber(j,5);
  tmp = mxGetField(j,i,fname);
#if MEXDEBUG
  if (!mxIsDouble(tmp) || (mxGetM(tmp) != 1) || (mxGetN(tmp) != 1)){
    mexErrMsgTxt("Error in conversion of Joint.");
  }
#endif
  double *mass = mxGetPr(tmp);
  //printf("done with mass\n");

  // 7th is center of mass
  fname = mxGetFieldNameByNumber(j,6);
  tmp = mxGetField(j,i,fname);
#if MEXDEBUG
  if (!mxIsStruct(tmp)){
    mexErrMsgTxt("Error in conversion of Joint.");
  }
#endif
  Vector3d cent_mass = mattocvector3d(tmp);
  //printf("done with c_mass\n");

  // 8th is exp  
  fname = mxGetFieldNameByNumber(j,7);
  tmp = mxGetField(j,i,fname);
#if MEXDEBUG
  if (!mxIsStruct(tmp)){
    mexErrMsgTxt("Error in conversion of Joint.");
  }
#endif
  Matrix4d exp1 = mattocmatrix4d(tmp);
  //printf("done with exp\n");

  // 9th is endeffector frame
  fname = mxGetFieldNameByNumber(j,8); 
  tmp = mxGetField(j,i,fname); 
#if MEXDEBUG
  if (!mxIsStruct(tmp)){
    mexErrMsgTxt("Error in conversion of Joint.");
  }
#endif
  Matrix4d endeff = mattocmatrix4d(tmp);
  //printf("done with end\n");

  // 10th is inertia matrix
  fname = mxGetFieldNameByNumber(j,9);
  tmp = mxGetField(j,i,fname);
#if MEXDEBUG
  if (!mxIsDouble(tmp) || (mxGetM(tmp) != 3) || (mxGetN(tmp) != 3)){
    mexErrMsgTxt("Error in conversion of Joint.");
  }
#endif
  double *in = mxGetPr(tmp);
  //printf("done with inertia\n");

  // 11th is general mass matrix
  fname = mxGetFieldNameByNumber(j,10);
  tmp = mxGetField(j,i,fname);
#if MEXDEBUG
  if (!mxIsStruct(tmp)){
    mexErrMsgTxt("Error in conversion of Joint.");
  }
#endif
  GenMass gm = mattocgenmass(tmp); 
  //printf("done with gm\n");
 
  // 12th is joint type
  fname = mxGetFieldNameByNumber(j,11);
  tmp = mxGetField(j,i,fname);
  // do SOMETHING HERE...GAS
  char *buf;
  int buflen;
  int status;
  buflen = (mxGetM(tmp) * mxGetN(tmp)) + 1;         
  buf = new char[buflen];

  status = mxGetString(tmp,buf,buflen);

  //printf("The converted string is: %s\n",buf);
  //if (!strcmp(buf,"prismatic")) printf ("GEE YOUR DUMB!\n");
  


  // 13th is se3 which contains joint parameters
  fname = mxGetFieldNameByNumber(j,12);
  tmp = mxGetField(j,i,fname);
#if MEXDEBUG
  if (!mxIsStruct(tmp)){
    mexErrMsgTxt("Error in conversion of Joint.");
  }
#endif
  se3 s1 = mattocse3(tmp);
  //printf("done with s1\n");

//  return Joint(min[0],max[0],val[0],pos,mass[0],cent_mass,exp1,endeff,in,gm,s1);
  
  JointType jt;
  if (!strcmp(buf,"prismatic")) {
    jt = Prismatic;
    //printf("attempting prismatic joint..not well tested yet....\n");
  }else jt = Revolute;

  delete [] buf;

#if 1
  Joint j1(min[0],max[0],val[0],pos,mass[0],cent_mass,
             exp1,endeff,in,gm,gg,s1,jt);
#else
  Joint j1(min[0],max[0],val[0],pos,mass[0],cent_mass,
             exp1,endeff,in,gm,s1,jt);
#endif
  //printf ("leaving mattocjoint for joint %d\n",i);
//  Joint j1;
  return j1;

}

Link * mattoclink(const mxArray *lnk, Vector &pos, Vector &vel, 
                   Vector &acc, Vector &tau, Vector &apmap, int link_no)
{
  mxArray *tmp;
  const char * fname;

  // get the number of links:
  int nl = mxGetN(lnk);
  apmap.Resize(nl);

  Link *tmp_link;
  tmp_link = new Link[nl];

  int *child_link;
  int *next_link;
  int *parent_link;

  child_link = new int[nl];
  next_link = new int[nl];
  parent_link = new int[nl];

  unsigned int aidx = 0;
  unsigned int pidx = 0;

  //printf("doing the first loop\n");
  for (int i = 0;i < nl; i++){
    //printf("calling mattoclink with i = %d\n",i);
    tmp_link[i] = mattoclink(lnk,i,child_link[i],next_link[i],
                           parent_link[i]);
    //printf("back from mattoclink with i = %d\n",i);
    tmp_link[i].JointValue(pos[i]);
//    printf("joint value[%d] = %4.9f\n",i,pos[i]);
    tmp_link[i].JointVel(vel[i]);
    tmp_link[i].MatlabAbsoluteLoc(i);
    if (tmp_link[i].IsPassive()) {
      //printf("hey1\n");
      tmp_link[i].Torque(tau[i]);
      apmap[i] = 0;
      tmp_link[i].MatlabPassLoc(pidx);
      pidx++;
    }else{
      //printf("hey2\n");
      tmp_link[i].JointAcc(acc[i]);
      apmap[i] = 1;
      tmp_link[i].MatlabActLoc(aidx);
      aidx++;
    }
  }
  //printf("done with the first loop\n");

  Link **l1;
  l1 = new Link*[nl];

  //printf("doing the second loop\n");
  int chld;
  l1[0] = new Link(tmp_link[0]);
  for (i = 0; i < nl; i++){
    chld = child_link[i];
    while (chld >= 0){
      l1[chld] = l1[i]->AddLink(tmp_link[chld]);
      chld = next_link[chld];
    }
  }

  //printf("Setting outlink\n");
  Link *outlink = l1[link_no-1];

  delete [] tmp_link;
  delete [] child_link;
  delete [] next_link;
  delete [] parent_link;

  // delete the memory for the array of Link *, without deleting
  //   the links themselves: the links will be deleted in the mex file
  //   once you are done with them
  delete [] l1;

  return outlink;
}

Link * mattoclink(const mxArray *lnk, int link_no)
{
  // get the number of links:
  int nl = mxGetN(lnk);
  
  // create a joint angle vector of zeros:
  Vector pos;
  pos.Resize(nl);
  int i;
  for (i=0;i<nl;i++){
    pos[i] = 0.0;
  }
  return mattoclink(lnk,link_no,pos);

}

Link * mattoclink(const mxArray *lnk, int link_no, Vector &pos)
{
  mxArray *tmp;
  const char * fname;

  // get the number of links:
  int nl = mxGetN(lnk);

  Link *tmp_link;
  tmp_link = new Link[nl];

  int *child_link;
  int *next_link;
  int *parent_link;

  child_link = new int[nl];
  next_link = new int[nl];
  parent_link = new int[nl];

  unsigned int aidx = 0;
  unsigned int pidx = 0;

  //printf("staring the loop\n");
  for (int i = 0;i < nl; i++){
    tmp_link[i] = mattoclink(lnk,i,child_link[i],next_link[i],
                           parent_link[i]);
    if (tmp_link[i].IsPassive()){
      tmp_link[i].MatlabPassLoc(pidx);
      pidx++;
    }else{
      tmp_link[i].MatlabActLoc(aidx);
      aidx++;
    }
    tmp_link[i].MatlabAbsoluteLoc(i);
    tmp_link[i].JointValue(pos[i]);
    //printf("joint value[%d] = %4.9f\n",i,pos[i]);
  }

  Link **l1;
  l1 = new Link*[nl];
  //printf("nl = %d\n",nl);

  //printf("doing the second loop\n");
  int chld;
  l1[0] = new Link(tmp_link[0]);
  //printf("hey1\n");
#if 0
  for (i = 1; i < nl; i++){
    l1[i] = NULL; // this is done to make sure we catch initialized pointers...
  }
#endif
  for (i = 0; i < nl; i++){
    chld = child_link[i];
    //printf("i = %d, chld = %d\n",i,chld);
    while (chld >= 0){
#if 0
      if (l1[i])
        l1[chld] = l1[i]->AddLink(tmp_link[chld]);
      else{
        printf("*********************************************************\n");
        printf("Reference to unitialized parent in mexfiles/mattoc.cpp, mattoclink function\n");
        printf("Parents must come before children in array of links!\n");
        printf("This error message brought you by the pointer police.\n");
        printf("*********************************************************\n");
        mexErrMsgTxt("Please fix the ordering of your Matlab array!\n");
      }
#else 
      l1[chld] = l1[i]->AddLink(tmp_link[chld]);
#endif
      //printf("added the link\n");
      chld = next_link[chld];
      //printf("next chld = %d\n",chld);
    }
  }

  //printf("Setting outlink\n");
  Link *outlink = l1[link_no-1];

  delete [] tmp_link;
  delete [] child_link;
  delete [] next_link;
  delete [] parent_link;

  // delete the memory for the array of Link *, without deleting
  //   the links themselves: the links will be deleted in the mex file
  //   once you are done with them
  delete [] l1;

  return outlink;
}



Link mattoclink(const mxArray *lnk, const int i, int &child_link,
                int &next_link, int &parent_link)
{

  mxArray *tmp;
  const char * fname;

  // 1st is M

  // 2nd is frame

  //printf("converting the base frame\n");
  // 3rd is base_frame (*)
  fname = mxGetFieldNameByNumber(lnk,2);
  tmp = mxGetField(lnk,i,fname);
  Matrix4d bframe = mattocmatrix4d(tmp);

  // 4th is tip_frame

  // 5th is inv_base_frame (*)
  //fname = mxGetFieldNameByNumber(lnk,4);
  //tmp = mxGetField(lnk,i,fname);
  //Matrix4d inv_bframe = mattocmatrix4d(tmp);

  // 6th is inv_frame

  // 7th is inv_M

  //printf("converting the joint screw \n");
  // 8th is joint screw (*)
  fname = mxGetFieldNameByNumber(lnk,7);
  tmp = mxGetField(lnk,i,fname);
  se3 j_screw = mattocse3(tmp);

  // 9th is type

  //printf("converting the ap map\n");
  // 10th is active/passive
  fname = mxGetFieldNameByNumber(lnk,9);
  tmp = mxGetField(lnk,i,fname);
  double *atv = mxGetPr(tmp);
  int apmap = (int)atv[0];

  //printf("converting the position\n");
  // 11th is joint position (*)
  fname = mxGetFieldNameByNumber(lnk,10);
  tmp = mxGetField(lnk,i,fname);
  double *pos = mxGetPr(tmp);

  //printf("converting the velocity\n");
  // 12th is joint velocity (*)
  fname = mxGetFieldNameByNumber(lnk,11);
  tmp = mxGetField(lnk,i,fname);
  double *vel = mxGetPr(tmp);

  //printf("converting the acceleration\n");
  // 13th is joint acceleration (*)
  fname = mxGetFieldNameByNumber(lnk,12);
  tmp = mxGetField(lnk,i,fname);
  double *acc = mxGetPr(tmp);

  // 14th is min
  fname = mxGetFieldNameByNumber(lnk,13);
  tmp = mxGetField(lnk,i,fname);
  double *min = mxGetPr(tmp);

  // 15th is max
  fname = mxGetFieldNameByNumber(lnk,14);
  tmp = mxGetField(lnk,i,fname);
  double *max = mxGetPr(tmp);

  //printf("converting the inertia\n");
  // 16th is inertia (*)
  fname = mxGetFieldNameByNumber(lnk,15);
  tmp = mxGetField(lnk,i,fname);
  //double *in = mxGetPr(tmp);
  Matrix inert_mat = mattocmatrix(tmp);

  //printf("converting the mass matrix\n");
  // 17th is general mass matrix (*)
  //fname = mxGetFieldNameByNumber(lnk,16);
  //tmp = mxGetField(lnk,i,fname);
  //GenMass gm = mattocgenmass(tmp);

  //printf("converting the mass\n");
  // 18th is the mass (*)
  fname = mxGetFieldNameByNumber(lnk,17);
  tmp = mxGetField(lnk,i,fname);
  double *mass = mxGetPr(tmp);

  //printf("converting the cent_mass\n");
  // 19th is center of mass (*)
  fname = mxGetFieldNameByNumber(lnk,18);
  tmp = mxGetField(lnk,i,fname);
  Vector3d cent_mass = mattocvector3d(tmp);

#if 1
  // 20th is general geometry of link
  fname = mxGetFieldNameByNumber(lnk,19);
  tmp = mxGetField(lnk,i,fname);
  GeneralGeometry gg = mattocgengeom(tmp);
#endif
  
  // 21st is general geometry of base

  // 22nd is update_local
  // 23rd is update_global
  // 24th is update_inv_local
  // 25th is update_inv_global

  // 26th is child  (*)
  fname = mxGetFieldNameByNumber(lnk,25);
  tmp = mxGetField(lnk,i,fname);
  double *cl = mxGetPr(tmp);     
  child_link = (int)cl[0] - 1;
  //printf("child for link %d is %d\n",i,child_link);

  // 27th is next (*)
  fname = mxGetFieldNameByNumber(lnk,26);
  tmp = mxGetField(lnk,i,fname);
  double *nl = mxGetPr(tmp);     
  next_link = (int)nl[0] - 1;
  //printf("next for link %d is %d\n",i,next_link);

  // 28th is parent (*)
  fname = mxGetFieldNameByNumber(lnk,27);
  tmp = mxGetField(lnk,i,fname);
  double *pl = mxGetPr(tmp);     
  parent_link = (int)pl[0] - 1;
  //printf("parent for link %d is %d\n",i,parent_link);

  // 29th is baseaccel
  fname = mxGetFieldNameByNumber(lnk,28);
  tmp = mxGetField(lnk,i,fname);
  Vector3d base_acc = mattocvector3d(tmp);

  // 30th is joint torque
  fname = mxGetFieldNameByNumber(lnk,29);
  tmp = mxGetField(lnk,i,fname);
  double *tau = mxGetPr(tmp);

  //31st is applied force array
  fname = mxGetFieldNameByNumber(lnk,30);
  tmp = mxGetField(lnk,i,fname);
  ForceApp fa;

  Link l1(j_screw,pos[0]);
  l1.BaseFrame(bframe);
  l1.SetMassInfo(inert_mat, mass[0], cent_mass);
  l1.JointVel(vel[0]);
  l1.JointAcc(acc[0]);
  l1.BaseAcceleration(base_acc);
  l1.MinJointValue(min[0]);
  l1.MaxJointValue(max[0]);

  int nfa =mxGetN(tmp);
 //  printf("nfa = %d\n",nfa);
  for (int j = 0; j < nfa; j++) {
    fa = mattocforceapp(tmp,j);
//    printf("Adding force = %f %f %f %f %f %f\n",fa.F.w.x,fa.F.w.y,fa.F.w.z,
//             fa.F.v.x, fa.F.v.y, fa.F.v.z);
    l1.AddForce(fa.F,fa.frame);
  }  
  //printf("done with forceapp crap\n");

  if (apmap == 0){
    l1.IsPassive(1);
    l1.Torque(tau[0]);
  }

  l1.LinkGeneralGeometry(gg);

  return l1;

}


OpenChain mattocopenchain(const mxArray *oc_t)
{

  mxArray *tmp;
  const char * fname;


  //printf("inside mattocopenchain\n");

  // 1st is numjoints
  fname = mxGetFieldNameByNumber(oc_t,0);
  tmp = mxGetField(oc_t,0,fname);
  int numjoints;
#if MEXDEBUG
  if (!mxIsDouble(tmp) || (mxGetM(tmp) != 1) || (mxGetN(tmp) != 1)){
    mexErrMsgTxt("Error in conversion of OpenChain.");
  }
#endif
  double *temp = mxGetPr(tmp);
  numjoints = (int)temp[0];

  // 2nd is array of joints
  fname = mxGetFieldNameByNumber(oc_t,1);
  tmp = mxGetField(oc_t,0,fname);
#if MEXDEBUG
  if (!mxIsStruct(tmp)){
    mexErrMsgTxt("Error in conversion of OpenChain.");
  }
#endif

  //printf("allocating\n");
  Joint *cj;
  cj = new Joint[numjoints];
  //printf("done allocating\n");


  for (int i=0;i<numjoints;i++){
    //printf("calling mattojoint for joint %d\n",i);
    cj[i] = mattocjoint(tmp,i); 
    //printf("back from mattojoint for joint %d\n",i);
    //printf("joint %d value = %f\n",i,cj[i].GetValue());
  }
    


  // 3rd is base geometry

  // 4th is base acceleration/gravity
  fname = mxGetFieldNameByNumber(oc_t,3);
  tmp = mxGetField(oc_t,0,fname);
#if MEXDEBUG
  if (!mxIsStruct(tmp)){
    mexErrMsgTxt("Error in conversion of OpenChain.");
  }
#endif
  Vector3d ba = mattocvector3d(tmp);

#if 0
  // 5th is V
  fname = mxGetFieldNameByNumber(oc_t,4);
  tmp = mxGetField(oc_t,0,fname);
  se3 V = mattocse3(tmp);

  // 6th is Vd
  fname = mxGetFieldNameByNumber(oc_t,5);
  tmp = mxGetField(oc_t,0,fname);
  se3 Vd = mattocse3(tmp);

  // 7th is cV
  fname = mxGetFieldNameByNumber(oc_t,6);
  tmp = mxGetField(oc_t,0,fname);
  se3 cV = mattocse3(tmp);

  // 8th is cVd
  fname = mxGetFieldNameByNumber(oc_t,7);
  tmp = mxGetField(oc_t,0,fname);
  se3 cVd = mattocse3(tmp);

  // 9th is s
  fname = mxGetFieldNameByNumber(oc_t,8);
  tmp = mxGetField(oc_t,0,fname);
  se3 s = mattocse3(tmp);

  // 10th is fi
  fname = mxGetFieldNameByNumber(oc_t,9);
  tmp = mxGetField(oc_t,0,fname);
  Matrix4d fi = mattocmatrix4d(tmp);

  // 11th is J

  // 12th is m
#endif

  // 13th field is active/passive joint map
  fname = mxGetFieldNameByNumber(oc_t,12);
  tmp = mxGetField(oc_t,0,fname);
  Vector Hap = mattocvector(tmp);
  //printf("Converted Hap successfully\n");

  OpenChain coc_t(numjoints,cj,ba);
  coc_t.APMap(Hap);


  delete [] cj;

  return coc_t;

}