/***************************************************************************
                          player.cpp  -  description
                             -------------------
    begin                : Mit Sep 25 13:11:41 CEST 2002
    copyright            : (C) 2002 by Harald Krippel
    email                : neuro.harald@surfeu.at
 ***************************************************************************/

/***************************************************************************
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 ***************************************************************************/
#include <player.h>
#include "main.h"

Player::Player(void)
{
  int a=0;
  
  for(a=0;a<MAX_PLAYER_OBJ;a++)
  {
    objtrans[a]=NULL;
  }
	rotspeed=380.0f;
	transspeed=10.0f;
  fmax2=0;
  tmprotspeed=0;
  tmptransspeed=0;
  netrotspeed=0;
  nettransspeed=0;

  sgZeroCoord(&pos);
  kbforward=0;
  kbback=0;
  kbleft=0;
  kbright=0;

  // camera
  camtyp=2;
  kbcamrotadd='4';
  kbcamrotsub='6';
  camrotw=-90.0f;
  kbcamheightadd='9';
  kbcamheightsub='7';
  camhight= 5.0f;
  kbcamradadd='-';
  kbcamradsub='+';
  camradius=-10.0f;
  
  gox=0.0f;
  goy=0.0f;
  controller=0;
  axis[0][0]=0;
  axis[0][1]=1;
  axis[1][0]=3;
  axis[1][1]=4;

}

void Player::createOdeBody(dWorldID odworld, dSpaceID odspace)
{
  int a=0;
  const dReal *odp=NULL;

   // some constants
 
#define LENGTH 3.5	// 0.7 chassis length
#define WIDTH 3.8	// 0.5 chassis width
#define HEIGHT 0.2	// 0.2 chassis height
#define RADIUS 0.65	// 0.18wheel radius
#define STARTZ 1.0	// 0.5 starting height of chassis
#define CMASS 1		//   1 chassis mass
#define WMASS 0.1	// wheel mass
#define VRAD  WIDTH*0.5
#define HRAD  WIDTH*0.6




    // chassis body

   odbody[0] = dBodyCreate (odworld);
   dBodySetPosition (odbody[0],0,0,STARTZ);
   dMassSetBox(&odm,1,LENGTH,WIDTH,HEIGHT);
   dMassTranslate (&odm, 0, 0, 0);  // HEIGHT
   dMassAdjust(&odm,CMASS); //Mass of player
   dBodySetMass (odbody[0],&odm);
   
   odbox[0] = dCreateBox (0,LENGTH,WIDTH,HEIGHT);
   dGeomSetBody (odbox[0],odbody[0]);

     // wheel bodies
  for (a=1; a<=4; a++)
  {
    odbody[a] = dBodyCreate (odworld);
    dQuaternion q;
    dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
    dBodySetQuaternion (odbody[a],q);
    dMassSetSphere (&odm,1,RADIUS);
    dMassAdjust (&odm,WMASS);
    dBodySetMass (odbody[a],&odm);
    odsphere[a-1] = dCreateSphere (0,RADIUS);
    dGeomSetBody (odsphere[a-1],odbody[a]);
  }
  dBodySetPosition (odbody[1],0.5*LENGTH,VRAD,STARTZ-HEIGHT*0.5);
  dBodySetPosition (odbody[2],0.5*LENGTH,-VRAD,STARTZ-HEIGHT*0.5);
  dBodySetPosition (odbody[3],-0.5*LENGTH,HRAD,STARTZ-HEIGHT*0.5);
  dBodySetPosition (odbody[4],-0.5*LENGTH,-HRAD,STARTZ-HEIGHT*0.5);

    // front and back wheel hinges

  for (a=0; a<4; a++)
  {
    odjoint[a] = dJointCreateHinge2 (odworld,0);
    dJointAttach (odjoint[a],odbody[0],odbody[a+1]);
    odp = dBodyGetPosition (odbody[a+1]);
    dJointSetHinge2Anchor (odjoint[a],odp[0],odp[1],odp[2]);
    dJointSetHinge2Axis1 (odjoint[a],0,0,1);
    dJointSetHinge2Axis2 (odjoint[a],0,1,0);
  }
    // set joint suspension

  for (a=0; a<4; a++)
  {
    dJointSetHinge2Param (odjoint[a],dParamSuspensionERP,0.4);     // 0.4
    dJointSetHinge2Param (odjoint[a],dParamSuspensionCFM,0.8);     // 0.8
  }

  // lock back wheels along the steering axis
  for (a=2; a<4; a++)
  {
    // set stops to make sure wheels always stay in alignment
    dJointSetHinge2Param (odjoint[a],dParamLoStop,0);
    dJointSetHinge2Param (odjoint[a],dParamHiStop,0);

  }

  // create car space and add it to the top level space
  car_space = dSimpleSpaceCreate (odspace);
  dSpaceSetCleanup (car_space,0);
  dSpaceAdd (car_space,odbox[0]);
  dSpaceAdd (car_space,odsphere[0]);
  dSpaceAdd (car_space,odsphere[1]);
  dSpaceAdd (car_space,odsphere[2]);
  dSpaceAdd (car_space,odsphere[3]);

}

void Player::update(ssgRoot *scene, float dt)
{
  dReal campos[3];
  float fcampos[3];
  sgVec3 up;
  const dReal *cpos=NULL;
  const dReal *cw=NULL;

  
//  int buttons ;
  int a=0;
  
  if(controller == 1)
  {
	  // walk kbd
 	  tmprotspeed -= goy * rotspeed * dt;
	  tmptransspeed -=   gox * transspeed * dt;
  }
    

  if(controller == 2)
  {
	  // walk maus
 	  tmprotspeed =  gmoy * rotspeed;
	  tmptransspeed = - gmox * transspeed;
  }

  if(controller == 3)
  {
    // walk js0
    tmprotspeed = (g_ax[0][(axis[0][0])]) * rotspeed;
    tmptransspeed = - (g_ax[0][(axis[0][1])]) * transspeed;
  }

  if(controller == 4)
  {
    // walk js1
    tmprotspeed = (g_ax[1][(axis[0][0])]) * rotspeed;
    tmptransspeed = - (g_ax[1][(axis[0][1])]) * transspeed;
  }

  if(controller == 5)
  {
    // network
    tmprotspeed = netrotspeed;
    tmptransspeed = nettransspeed;
  }
//  printf("%d %f %f \n",controller,tmprotspeed,tmptransspeed);    // debug

  for(a=0;a<2;a++)
  {
    // motor  front
    dJointSetHinge2Param (odjoint[a],dParamVel2,-tmptransspeed);
    dJointSetHinge2Param (odjoint[a],dParamFMax2,fmax2);     // FMax

    // steering
    dReal v = (SG_DEGREES_TO_RADIANS * tmprotspeed) - dJointGetHinge2Angle1 (odjoint[a]);
    if (v > 0.1) v = 0.1;
    if (v < -0.1) v = -0.1;
    v *= 10.0;
    dJointSetHinge2Param (odjoint[a],dParamVel,v);
    dJointSetHinge2Param (odjoint[a],dParamFMax,0.4);  // 0.2
    dJointSetHinge2Param (odjoint[a],dParamLoStop,-0.75);
    dJointSetHinge2Param (odjoint[a],dParamHiStop,0.75);
    dJointSetHinge2Param (odjoint[a],dParamFudgeFactor,0.1); // 0.1
  }
    // motor back
    dJointSetHinge2Param (odjoint[2],dParamVel2,-tmptransspeed);
    dJointSetHinge2Param (odjoint[2],dParamFMax2,fmax2);     // 0.1
    dJointSetHinge2Param (odjoint[3],dParamVel2,-tmptransspeed);
    dJointSetHinge2Param (odjoint[3],dParamFMax2,fmax2);     // 0.1

  // positon player from ode
  for(a=0;a<MAX_PLAYER_OBJ;a++)
  {
    if(objtrans[a] != NULL)
    {
      setTransform(objtrans[a],dBodyGetPosition(odbody[a]),dBodyGetRotation(odbody[a]));
    }
  }
  
  // positon camera
  
	cpos=dBodyGetPosition(odbody[0]);
  cw=dBodyGetRotation(odbody[0]);

  pos.xyz[0]= cpos[0];
  pos.xyz[1]= cpos[1];
  pos.xyz[2]= cpos[2];
  pos.hpr[0]= cw[0];
  pos.hpr[1]= cw[1];
  pos.hpr[2]= cw[2];
  
  campos[0] = cpos[0];
	campos[1] = cpos[1];
	campos[2] = cpos[2];

  
   if(camtyp==1)  // TV
   {
	    fcampos[0] = 0 + camradius * sin(camrotw*SG_DEGREES_TO_RADIANS);
	    fcampos[1] = 0 + camradius * cos(camrotw*SG_DEGREES_TO_RADIANS);
	    fcampos[2] = 0 + camhight;
      sgSetVec3(up,0.0f,0.0f,1.0f);
      sgMakeLookAtMat4(cmat, fcampos, pos.xyz,up);
   }
   if(camtyp==2)   // 2D
   {
	    fcampos[0] = cpos[0] + camradius * sin(camrotw*SG_DEGREES_TO_RADIANS);
	    fcampos[1] = cpos[1] + camradius * cos(camrotw*SG_DEGREES_TO_RADIANS);
	    fcampos[2] = cpos[2] + camhight;
      sgSetVec3(up,0.0f,0.0f,1.0f);
      sgMakeLookAtMat4(cmat,fcampos, pos.xyz,up);
   }
   if(camtyp==3)   // EGO
   {
      setCamera ( campos, cw);
   }

   gox=0;
   goy=0;
}

void Player::setTransform (ssgTransform *objtrans, const dReal pos[3], const dReal R[12])
{
  sgMat4 m;
  
//  SGfloat matrix[16];
//  matrix[0]=R[0];
//  matrix[1]=R[4];
//  matrix[2]=R[8];
//  matrix[3]=0;
//  matrix[4]=R[1];
//  matrix[5]=R[5];
//  matrix[6]=R[9];
//  matrix[7]=0;
//  matrix[8]=R[2];
//  matrix[9]=R[6];
//  matrix[10]=R[10];
//  matrix[11]=0;
//  matrix[12]=pos[0];
//  matrix[13]=pos[1];
//  matrix[14]=pos[2];
//  matrix[15]=1;


  m[0][0] = R[0] ;
  m[0][1] = R[4] ;
  m[0][2] = R[8] ;
  m[0][3] = SG_ZERO ; //x

  m[1][0] = R[1] ;
  m[1][1] = R[5] ;
  m[1][2] = R[9] ;
  m[1][3] = SG_ZERO ;  // y

  m[2][0] = R[2] ;
  m[2][1] = R[6] ;
  m[2][2] = R[10] ;
  m[2][3] = SG_ZERO ;  // z

  m[3][0] =  pos [0] ;
  m[3][1] =  pos [1] ;
  m[3][2] =  pos [2] ;
  m[3][3] =  SG_ONE ;


          
  objtrans->setTransform(m);
}

void Player::setCamera (const dReal pos[3], const dReal R[12])
{
  sgMat4 mt;
  sgMat4 m;
  sgMat4 m1;
  sgMat4 m2;


  m[0][0] = R[0] ;
  m[0][1] = R[4] ;
  m[0][2] = R[8] ;
  m[0][3] = SG_ZERO ; //x

  m[1][0] = R[1] ;
  m[1][1] = R[5] ;
  m[1][2] = R[9] ;
  m[1][3] = SG_ZERO ;  // y

  m[2][0] = R[2] ;
  m[2][1] = R[6] ;
  m[2][2] = R[10] ;
  m[2][3] = SG_ZERO ;  // z

  m[3][0] =  pos [0] ;
  m[3][1] =  pos [1] ;
  m[3][2] =  pos [2] ;

  m[3][3] =  SG_ONE ;

  sgMakeTransMat4(mt,0.0f,camradius,camhight);
  sgMakeRotMat4(m1,camrotw,0.0f,0.0f);
  sgMultMat4(m2,m,m1);
  sgMultMat4(cmat,m2,mt);
}

void Player::kbd ( char key )
{

//        printf("kbd %c %x %c\n",key,key,kbforward);
        // Keyboard Play
        if( key == kbforward)
          gox = -1;
          
        if( key == kbback)
          gox = 1;
          
        if( key == kbleft)
          goy = -1;
          
        if( key == kbright)
          goy = 1;
          
        // camera  
        if( key == kbcamrotadd)
          camrotw += 4;

        if( key == kbcamrotsub)
          camrotw -= 4;
          
        if( key == kbcamheightadd)
          camhight += 1;

        if( key == kbcamheightsub)
          camhight -= 1;

        if( key == kbcamradadd)
          camradius += 1;

        if( key == kbcamradsub)
          camradius -= 1;
}

void Player::maus ( float x, float y )
{
//       printf("maus %f %f\n",x,y);
       gmox=y;
       gmoy=x;
}

void Player::GetPosition ( sgCoord  *bodypos )
{
   sgCopyCoord ( bodypos, &pos ) ;
}

void Player::SetPosition ()
{
  int a=0;
  dMatrix3 odR;
  
  dBodySetPosition (odbody[0],pos.xyz[0],pos.xyz[1],pos.xyz[2]);
  dRFromEulerAngles(odR,-pos.hpr[1] * SG_DEGREES_TO_RADIANS
                      ,-pos.hpr[2] * SG_DEGREES_TO_RADIANS
                      ,-pos.hpr[0] * SG_DEGREES_TO_RADIANS);
      dBodySetRotation (odbody[0],odR);

  dBodySetPosition (odbody[1], 0.5*LENGTH + pos.xyz[0], VRAD + pos.xyz[1],STARTZ-HEIGHT*0.5 + pos.xyz[2]);
  dBodySetPosition (odbody[2], 0.5*LENGTH + pos.xyz[0],-VRAD + pos.xyz[1],STARTZ-HEIGHT*0.5 + pos.xyz[2]);
  dBodySetPosition (odbody[3],-0.5*LENGTH + pos.xyz[0], HRAD + pos.xyz[1],STARTZ-HEIGHT*0.5 + pos.xyz[2]);
  dBodySetPosition (odbody[4],-0.5*LENGTH + pos.xyz[0],-HRAD + pos.xyz[1],STARTZ-HEIGHT*0.5 + pos.xyz[2]);
  
  for (a=0; a<=4; a++)
  {
      dBodySetLinearVel (odbody[a],0.0f,0.0f,0.0f);
      dBodySetAngularVel (odbody[a],0.0f,0.0f,0.0f);
  }
}

Player::~Player()
{
}
