/*************************************************************************

 *                                                                       *

 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *

 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *

 *                                                                       *

 * This library is free software; you can redistribute it and/or         *

 * modify it under the terms of EITHER:                                  *

 *   (1) The GNU Lesser General Public License as published by the Free  *

 *       Software Foundation; either version 2.1 of the License, or (at  *

 *       your option) any later version. The text of the GNU Lesser      *

 *       General Public License is included with this library in the     *

 *       file LICENSE.TXT.                                               *

 *   (2) The BSD-style license that is included with this library in     *

 *       the file LICENSE-BSD.TXT.                                       *

 *                                                                       *

 * This library is distributed in the hope that it will be useful,       *

 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *

 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *

 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *

 *                                                                       *

 *************************************************************************/

// this source file is mostly concerned with the data structures, not the

// numerics.

#include <e32std.h>

#include <pls.h>

#include "object.h"

#include <ode/ode.h>

#include "joint.h"

#include <ode/odemath.h>

#include <ode/matrix.h>

#include "step.h"

#include "quickstep.h"

#include "util.h"

#include <ode/memory.h>

#include <ode/error.h>

#ifdef __WINSCW__

TInt InitializeGlobaData( TOdeStaticData* aData )

    {

    Mem::FillZ( aData, sizeof( TOdeStaticData ) );

    aData->autoEnableDepth = 2;

    return KErrNone;

    }

struct TOdeStaticData* GetGlobalData()

    {

    return Pls<TOdeStaticData>( TUid::Uid( 0x2001CBE3 ), InitializeGlobaData );

    }

#else

static struct TOdeStaticData odeStaticData;

struct TOdeStaticData* GetGlobalData()

    {

    return &odeStaticData;

    }

#endif // __WINSCW__

// misc defines

//#define ALLOCA dALLOCA16

//****************************************************************************

// utility

static inline void initObject (dObject *obj, dxWorld *w)

{

  obj->world = w;

  obj->next = 0;

  obj->tome = 0;

  obj->userdata = 0;

  obj->tag = 0;

}

// add an object `obj' to the list who's head pointer is pointed to by `first'.

static inline void addObjectToList (dObject *obj, dObject **first)

{

  obj->next = *first;

  obj->tome = first;

  if (*first) (*first)->tome = &obj->next;

  (*first) = obj;

}

// remove the object from the linked list

static inline void removeObjectFromList (dObject *obj)

{

  if (obj->next) obj->next->tome = obj->tome;

  *(obj->tome) = obj->next;

  // safeguard

  obj->next = 0;

  obj->tome = 0;

}

// remove the joint from neighbour lists of all connected bodies

static void removeJointReferencesFromAttachedBodies (dxJoint *j)

{

  for (int i=0; i<2; i++) {

    dxBody *body = j->node[i].body;

    if (body) {

      dxJointNode *n = body->firstjoint;

      dxJointNode *last = 0;

      while (n) {

	if (n->joint == j) {

	  if (last) last->next = n->next;

	  else body->firstjoint = n->next;

	  break;

	}

	last = n;

	n = n->next;

      }

    }

  }

  j->node[0].body = 0;

  j->node[0].next = 0;

  j->node[1].body = 0;

  j->node[1].next = 0;

}

//****************************************************************************

// debugging

// check the validity of the world data structures

static void checkWorld (dxWorld */*w*/)

{

  //dxBody *b;

  //dxJoint *j;

 // TBD

}

void dWorldCheck (dxWorld *w)

{

  checkWorld (w);

}

//****************************************************************************

// body

EXPORT_C dxWorld* dBodyGetWorld (dxBody* b)

{

  return b->world;

}

EXPORT_C dxBody *dBodyCreate (dxWorld *w)

{

  dxBody *b = new dxBody;

  initObject (b,w);

  b->firstjoint = 0;

  b->flags = 0;

  b->geom = 0;

  b->average_lvel_buffer = 0;

  b->average_avel_buffer = 0;

  dMassSetParameters (&b->mass,REAL(1.0),0,0,0,REAL(1.0),REAL(1.0),REAL(1.0),0,0,0);

  dSetZero (b->invI,4*3);

  b->invI[0] = REAL(1.0);

  b->invI[5] = REAL(1.0);

  b->invI[10] = REAL(1.0);

  b->invMass = REAL(1.0);

  dSetZero (b->posr.pos,4);

  dSetZero (b->q,4);

  b->q[0] = REAL(1.0);

  dRSetIdentity (b->posr.R);

  dSetZero (b->lvel,4);

  dSetZero (b->avel,4);

  dSetZero (b->facc,4);

  dSetZero (b->tacc,4);

  dSetZero (b->finite_rot_axis,4);

  addObjectToList (b,(dObject **) &w->firstbody);

  w->nb++;

  // set auto-disable parameters

  b->average_avel_buffer = b->average_lvel_buffer = 0; // no buffer at beginnin

  dBodySetAutoDisableDefaults (b);	// must do this after adding to world

  b->adis_stepsleft = b->adis.idle_steps;

  b->adis_timeleft = b->adis.idle_time;

  b->average_counter = 0;

  b->average_ready = 0; // average buffer not filled on the beginning

  dBodySetAutoDisableAverageSamplesCount(b, b->adis.average_samples);

  return b;

}

EXPORT_C void dBodyDestroy (dxBody *b)

{

  // all geoms that link to this body must be notified that the body is about

  // to disappear. note that the call to dGeomSetBody(geom,0) will result in

  // dGeomGetBodyNext() returning 0 for the body, so we must get the next body

  // before setting the body to 0.

  dxGeom *next_geom = 0;

  for (dxGeom *geom = b->geom; geom; geom = next_geom) {

    next_geom = dGeomGetBodyNext (geom);

    dGeomSetBody (geom,0);

  }

  // detach all neighbouring joints, then delete this body.

  dxJointNode *n = b->firstjoint;

  while (n) {

    // sneaky trick to speed up removal of joint references (black magic)

    n->joint->node[(n == n->joint->node)].body = 0;

    dxJointNode *next = n->next;

    n->next = 0;

    removeJointReferencesFromAttachedBodies (n->joint);

    n = next;

  }

  removeObjectFromList (b);

  b->world->nb--;

  // delete the average buffers

  if(b->average_lvel_buffer)

  {

	  delete[] (b->average_lvel_buffer);

	  b->average_lvel_buffer = 0;

  }

  if(b->average_avel_buffer)

  {

	  delete[] (b->average_avel_buffer);

	  b->average_avel_buffer = 0;

  }

  delete b;

}

EXPORT_C void dBodySetData (dBodyID b, void *data)

{

  b->userdata = data;

}

EXPORT_C void *dBodyGetData (dBodyID b)

{

  return b->userdata;

}

EXPORT_C void dBodySetPosition (dBodyID b, dReal x, dReal y, dReal z)

{

  b->posr.pos[0] = x;

  b->posr.pos[1] = y;

  b->posr.pos[2] = z;

  // notify all attached geoms that this body has moved

  for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))

    dGeomMoved (geom);

}

EXPORT_C void dBodySetRotation (dBodyID b, const dMatrix3 R)

{

  dQuaternion q;

  dRtoQ (R,q);

  dNormalize4 (q);

  b->q[0] = q[0];

  b->q[1] = q[1];

  b->q[2] = q[2];

  b->q[3] = q[3];

  dQtoR (b->q,b->posr.R);

  // notify all attached geoms that this body has moved

  for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))

    dGeomMoved (geom);

}

EXPORT_C void dBodySetQuaternion (dBodyID b, const dQuaternion q)

{

  b->q[0] = q[0];

  b->q[1] = q[1];

  b->q[2] = q[2];

  b->q[3] = q[3];

  dNormalize4 (b->q);

  dQtoR (b->q,b->posr.R);

  // notify all attached geoms that this body has moved

  for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))

    dGeomMoved (geom);

}

EXPORT_C void dBodySetLinearVel  (dBodyID b, dReal x, dReal y, dReal z)

{

  b->lvel[0] = x;

  b->lvel[1] = y;

  b->lvel[2] = z;

}

EXPORT_C void dBodySetAngularVel (dBodyID b, dReal x, dReal y, dReal z)

{

  b->avel[0] = x;

  b->avel[1] = y;

  b->avel[2] = z;

}

EXPORT_C const dReal * dBodyGetPosition (dBodyID b)

{

  return b->posr.pos;

}

EXPORT_C void dBodyCopyPosition (dBodyID b, dVector3 pos)

{

	dReal* src = b->posr.pos;

	pos[0] = src[0];

	pos[1] = src[1];

	pos[2] = src[2];

}

EXPORT_C const dReal * dBodyGetRotation (dBodyID b)

{

  return b->posr.R;

}

EXPORT_C void dBodyCopyRotation (dBodyID b, dMatrix3 R)

{

	const dReal* src = b->posr.R;

	R[0] = src[0];

	R[1] = src[1];

	R[2] = src[2];

	R[3] = src[3];

	R[4] = src[4];

	R[5] = src[5];

	R[6] = src[6];

	R[7] = src[7];

	R[8] = src[8];

	R[9] = src[9];

	R[10] = src[10];

	R[11] = src[11];

}

EXPORT_C const dReal * dBodyGetQuaternion (dBodyID b)

{

  return b->q;

}

EXPORT_C void dBodyCopyQuaternion (dBodyID b, dQuaternion quat)

{

	dReal* src = b->q;

	quat[0] = src[0];

	quat[1] = src[1];

	quat[2] = src[2];

	quat[3] = src[3];

}

EXPORT_C const dReal * dBodyGetLinearVel (dBodyID b)

{

  return b->lvel;

}

EXPORT_C const dReal * dBodyGetAngularVel (dBodyID b)

{

  return b->avel;

}

EXPORT_C void dBodySetMass (dBodyID b, const dMass *mass)

{

  // The centre of mass must be at the origin.

  // Use dMassTranslate( mass, -mass->c[0], -mass->c[1], -mass->c[2] ) to correct it.

  memcpy (&b->mass,mass,sizeof(dMass));

  if (dInvertPDMatrix (b->mass.I,b->invI,3)==0) {

    dRSetIdentity (b->invI);

  }

  b->invMass = dRecip(b->mass.mass);

}

EXPORT_C void dBodyGetMass (dBodyID b, dMass *mass)

{

  memcpy (mass,&b->mass,sizeof(dMass));

}

EXPORT_C void dBodyAddForce (dBodyID b, dReal fx, dReal fy, dReal fz)

{

  b->facc[0] += fx;

  b->facc[1] += fy;

  b->facc[2] += fz;

}

EXPORT_C void dBodyAddTorque (dBodyID b, dReal fx, dReal fy, dReal fz)

{

  b->tacc[0] += fx;

  b->tacc[1] += fy;

  b->tacc[2] += fz;

}

EXPORT_C void dBodyAddRelForce (dBodyID b, dReal fx, dReal fy, dReal fz)

{

  dVector3 t1,t2;

  t1[0] = fx;

  t1[1] = fy;

  t1[2] = fz;

  t1[3] = 0;

  dMULTIPLY0_331 (t2,b->posr.R,t1);

  b->facc[0] += t2[0];

  b->facc[1] += t2[1];

  b->facc[2] += t2[2];

}

EXPORT_C void dBodyAddRelTorque (dBodyID b, dReal fx, dReal fy, dReal fz)

{

  dVector3 t1,t2;

  t1[0] = fx;

  t1[1] = fy;

  t1[2] = fz;

  t1[3] = 0;

  dMULTIPLY0_331 (t2,b->posr.R,t1);

  b->tacc[0] += t2[0];

  b->tacc[1] += t2[1];

  b->tacc[2] += t2[2];

}

EXPORT_C void dBodyAddForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,

			 dReal px, dReal py, dReal pz)

{

  b->facc[0] += fx;

  b->facc[1] += fy;

  b->facc[2] += fz;

  dVector3 f,q;

  f[0] = fx;

  f[1] = fy;

  f[2] = fz;

  q[0] = px - b->posr.pos[0];

  q[1] = py - b->posr.pos[1];

  q[2] = pz - b->posr.pos[2];

  dCROSS (b->tacc,+=,q,f);

}

EXPORT_C void dBodyAddForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,

			    dReal px, dReal py, dReal pz)

{

  dVector3 prel,f,p;

  f[0] = fx;

  f[1] = fy;

  f[2] = fz;

  f[3] = 0;

  prel[0] = px;

  prel[1] = py;

  prel[2] = pz;

  prel[3] = 0;

  dMULTIPLY0_331 (p,b->posr.R,prel);

  b->facc[0] += f[0];

  b->facc[1] += f[1];

  b->facc[2] += f[2];

  dCROSS (b->tacc,+=,p,f);

}

EXPORT_C void dBodyAddRelForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,

			    dReal px, dReal py, dReal pz)

{

  dVector3 frel,f;

  frel[0] = fx;

  frel[1] = fy;

  frel[2] = fz;

  frel[3] = 0;

  dMULTIPLY0_331 (f,b->posr.R,frel);

  b->facc[0] += f[0];

  b->facc[1] += f[1];

  b->facc[2] += f[2];

  dVector3 q;

  q[0] = px - b->posr.pos[0];

  q[1] = py - b->posr.pos[1];

  q[2] = pz - b->posr.pos[2];

  dCROSS (b->tacc,+=,q,f);

}

EXPORT_C void dBodyAddRelForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,

			       dReal px, dReal py, dReal pz)

{

  dVector3 frel,prel,f,p;

  frel[0] = fx;

  frel[1] = fy;

  frel[2] = fz;

  frel[3] = 0;

  prel[0] = px;

  prel[1] = py;

  prel[2] = pz;

  prel[3] = 0;

  dMULTIPLY0_331 (f,b->posr.R,frel);

  dMULTIPLY0_331 (p,b->posr.R,prel);

  b->facc[0] += f[0];

  b->facc[1] += f[1];

  b->facc[2] += f[2];

  dCROSS (b->tacc,+=,p,f);

}

EXPORT_C const dReal * dBodyGetForce (dBodyID b)

{

  return b->facc;

}