/* load_shape.cc */

#include "body_def.h"
#include <fstream.h>

Shape *get_shape( istream &, double, double, double );
void name_shape( Shape &S, cstring );

/* only works on an unattached shape */
int check_consistency( Shape &s );

Shape *load_shape( cstring filename, double wid, double hgt, double dep,
		   cstring name_str = "" ) {
  ifstream infile( filename );

  if( !infile ) {
    cerr << "load_shape -- can't open file \"" << filename << "\"\n";
    return nil;
  }

  Shape *S = load_shape( infile, wid, hgt, dep, name_str );

  infile.close();

  cerr << "LOADED SHAPE " << filename << "\n";

  return S;
}

Shape *load_shape( istream &infile, double wid, double hgt, double dep,
		   cstring name_str = "" ) {
  int nshapes;
  
  if( !infile.scan( "nshapes %u", &nshapes ) ) {
    cerr << "get_shape -- bad nshapes\n";
    return nil;
  }

  Shape *S;
  if( nshapes == 1 ) {
    S = get_shape( infile, wid, hgt, dep );
  } else {
    MultiShape *M = new MultiShape;
    for( int i = 0 ; i < nshapes ; i++ ) {
      Shape *s = get_shape( infile, wid, hgt, dep );
      if( s == nil ) {
	cerr << "load_shape -- bad shape " << i << "\n";
	delete( M );
	return nil;
      }
      M->add( *s );
    }
    S = M;
  }

  if( S ) {
    name_shape( *S, name_str );
    check_consistency( *S );
  }

  return S;
}

Shape *get_shape( istream &infile, double wid, double hgt, double dep ) {
  int nphdrons;
  if( !infile.scan( "nphdrons %u", &nphdrons ) ) return nil;  

  MultiShape *mshape = new MultiShape;  
  for( int p = 0 ; p < nphdrons ; p++ ) {
    Polyhedron poly;

    if( !infile.scan( "nverts %u", &( poly.nvertices ) ) ) {
      delete( mshape );
      cerr << "get_shape -- bad poly " << p << " nverts\n";
      return nil;
    }
    poly.vertices = new triple[poly.nvertices];
    for( int v = 0 ; v < poly.nvertices ; v++ ) {
      double x, y, z;
      if( !infile.scan( "( %lf , %lf , %lf )", &x, &y, &z ) ) {
	delete( mshape );
	return nil;
	cerr << "get_shape -- bad poly " << p << " vertex " << v << "\n";
      }
      poly.vertices[v] = triple( x * wid, y * hgt, z * dep );
    }
    
    if( !infile.scan( "nfaces %u", &( poly.nfaces ) ) ) {
      cerr << "get_shape -- bad poly " << p << " nfaces\n";
      delete( mshape );
      return nil;
    }
    poly.faces = new PolyhedronFace[poly.nfaces];
    for( int f = 0 ; f < poly.nfaces ; f++ ) {
      if( !infile.scan( "npts %u", &( poly.faces[f].npts ) ) ) {
	cerr << "get_shape -- bad poly " << p << " face " << f << " npts\n";
	delete( mshape );
	return nil;
      }
      poly.faces[f].indices = new int[poly.faces[f].npts];
      for( int i = 0 ; i < poly.faces[f].npts ; i++ ) {
	if( !infile.scan( "%u", &( poly.faces[f].indices[i] ) ) ||
	    poly.faces[f].indices[i] >= poly.nvertices ) {
	  cerr << "get_shape -- bad poly " << p << " face " << f << " index " 
	       << i << "\n";
	  delete( mshape );
	  return nil;
	}
      }
    }
    
    poly.process();
    mshape->add( *( poly.collect() ) );
  }
  return mshape;
}

void name_shape( Shape &s, cstring name ) {
  int nlen = name.length();

  if( s.entity_name ) delete( s.entity_name );
  s.entity_name = new char[nlen + 1];
  for( int i = 0 ; i < nlen ; i++ ) {
    s.entity_name[i] = name[i];
  }
  s.entity_name[nlen] = 0;

  EntityHook::EntityType etype = s.type();
  if( etype == EntityHook::MULTISHAPE ) {
    llist_iter<Shape *>iter = ( ( MultiShape * )&s )->children;
    int n = 0;
    while( iter ) {
      name_shape( *( iter() ), name + string( "_%d", n++ ) );
      iter++;
    }
  }
}

int check_consistency( Shape &s ) {
  int nerrors = 0;

  EntityHook::EntityType etype = s.type();
  if( etype == EntityHook::MULTISHAPE ) {
    llist_iter<Shape *>iter = ( ( MultiShape * )&s )->children;
    if( s.body ) s.body->unset_geometry();
    RigidBody rb;
    rb.set_geometry( s );
    while( iter ) {
      nerrors += check_consistency( *( iter() ) );
      iter++;
      if( !s.body ) {
	rb.set_geometry( s );
      }
    }
    if( nerrors ) {
      cerr << "check_consistency -- " << nerrors 
	   << " errors in MultiShape \"" << s.entity_name << "\"\n";
    }
    rb.unset_geometry();
  } else if( etype == EntityHook::POLY3D ) {
    /* assign shape to body and check that outward normal away from com */
    if( s.body ) {
      triple outward_normal = ( ( Poly3d * )&s )->boutward_normal();
      triple com = ( ( RigidBody * )s.body )->com;
      triple bp = ( ( Poly3d * )&s )->bp(0);
      /* if the distance between the bodies com and a point on the poly >
	 the distance between the com and that point + the outward normal then
	 the shape is inconsistant */
      if( abs( com - bp ) > abs( com - ( bp + outward_normal ) ) ) { 
	dc_trace( TRACE_ERROR ) {
	  cerr << "check_consistency -- Shape \"" << s.entity_name
	       << "\" is inconsistent\n";
	}
	return 1;
      }
    }
  }

  return nerrors;
}
