#include <acknex.h>
#include <default.c>
#include <ackphysx.h>
#include <windows.h>

#define PRAGMA_ZER0

var twist, swing1, swing2, index;

typedef struct JT {
	var motion[6];
	var drive[6];
} JT;

JT jt[8];

TEXT* jtxt = {
	string("fixed joint","twist only", "swing1 only", "swing2only", "twist and swing1", 
	       "twist and swing2", "swing1 and swing2", "twist, swing1, and swing2");
}

STRING* jstr = "fixed joint";

PANEL* displaypanel={
	digits(10, 10, "twist (cul ,cur ): %3.0f", "Courier#12b",1, twist);
	digits(10, 30, "swing2(cuu ,cud ): %3.0f", "Courier#12b",1, swing2);
	digits(10, 50, "swing1(pgup,pgdn): %3.0f", "Courier#12b",1, swing1);
	//digits(10, 80, "index(home,end)  : %3.0f", "Courier#12b",1, index);
	digits(10, 80, "index(home,end)  : %s", "Courier#12b",1, jstr);

	flags = SHOW;	
}


function main()
{

// set up a simple level  
  shadow_stencil = 1;
  d3d_antialias = 1;
  physX_open();
  level_load(""); // load an empty level
  pXent_settype(NULL,PH_STATIC,PH_PLANE); // create a static plane at groundlevel zero
  ENTITY* ground = ent_createterrain(NULL,NULL,32,32,1000);
  bmap_fill(bmap_for_entity(ground,0),COLOR_RED,100);  
  vec_set(camera.x,vector(-150,0,50));
  
  //set this to see the different types of rotational only joints
  var jointtype=1;
    
  //joint axis set to default (0,0,-1)
  //Motion: 
  //3: twist  >> swing1?
  //4: swing1 >> swing2?
  //5: swing2 >> twist ?
  //Drive: 
  //3: swing (swing1 and swing2)
  //4: slerp
  //5: twist
  //pXcon_setmotor(ent,vecParam,0);
  //vecParam: (twist?, swing2?, swing1?)
   
   //indicate where joint is located
   you = ent_create(SPHERE_MDL,vector(0,0,16),NULL);
   you.scale_x = you.scale_y = you.scale_z = 0.5;
   
   you = ent_create(CUBE_MDL,vector(0,0,32),NULL);
   set(you, SHADOW);
	you.scale_z = 2;   pXent_settype(you,PH_RIGID,PH_BOX);// define a joint with a twist motor
	 
	pXcon_add(PH_6DJOINT,you,NULL,0); 
	pXcon_setparams1(you,vector(0,0,16),NULL, NULL); 
	pXcon_setparams2(you,vector(45,45,0),vector(-45,45,0), NULL); 
	//set swing1 and swing2 limits to 45, set twist limits between -45 and 45.  
  
	//1: pan? (twist)
	jt[1].motion[5] = NX_D6JOINT_MOTION_LIMITED;
	jt[1].drive[5] = NX_D6JOINT_DRIVE_VELOCITY;
	//2: tilt? (swing1)
	jt[2].motion[3] = NX_D6JOINT_MOTION_LIMITED;
	jt[2].drive[3] = NX_D6JOINT_DRIVE_VELOCITY;
	//3: roll? (swing2)
	jt[3].motion[4] = NX_D6JOINT_MOTION_LIMITED;
	jt[3].drive[3] = NX_D6JOINT_DRIVE_VELOCITY;
	//4: pan and tilt? (twist and swing1)
	jt[4].motion[3] = jt[4].motion[5] = NX_D6JOINT_MOTION_LIMITED;
	jt[4].drive[3]  = jt[4].drive[5]  = NX_D6JOINT_DRIVE_VELOCITY;
	//5: pan and roll? (twist and swing2)
	jt[5].motion[4] = jt[5].motion[5] = NX_D6JOINT_MOTION_LIMITED;
	jt[5].drive[3]  = jt[5].drive[5]  = NX_D6JOINT_DRIVE_VELOCITY;
	//6: tilt and roll? (swing1 and swing2)
	jt[6].motion[3] = jt[6].motion[4] = NX_D6JOINT_MOTION_LIMITED;
	jt[6].drive[3]  = NX_D6JOINT_DRIVE_VELOCITY;
	//7: pan, tilt and roll? (twist, swing1, ad swing2)
	jt[7].motion[3] = jt[7].motion[4] = jt[7].motion[5] = NX_D6JOINT_MOTION_LIMITED;
	jt[7].drive[3]  = jt[7].drive[5]  = NX_D6JOINT_DRIVE_VELOCITY;
	
  var NoDrive[6] = { 0,0,0,0,0,0};
  
  while(1)
  {
  		twist=swing2=swing1=0;
  		
  		if(key_end)
  		{	
  			while(key_end) wait(1);
  			index = cycle(index+1, 0, 8);
  		}	
  		if(key_home)
  		{	
  			while(key_home) wait(1);
  			index = cycle(index-1, 0, 8);
  		}	
  		str_cpy(jstr, (jtxt.pstring)[index]);
  		if(key_any)
  		{
  			pXcon_set6djoint(you,jt[index].motion,jt[index].drive);
	  		twist=(key_cul-key_cur)*100;
	  		swing2=(key_cuu-key_cud)*100;
	  		swing1=(key_pgup-key_pgdn)*100;
	  		pXcon_setmotor(you,vector(twist, swing2, swing1),0);
  		}
  		else
  			pXcon_set6djoint(you,jt[index].motion,NoDrive);
  		
  		wait(1);	
  }
  	
}