#include "cg_local.h"
#include "cg_vehicles.h"

/*
=============
VectorToString

This is just a convenience function
for printing vectors
=============
*/
char	*vtos( const vec3_t v ) {
	static	int		index;
	static	char	str[8][32];
	char	*s;

	// use an array so that multiple vtos won't collide
	s = str[index];
	index = (index + 1)&7;

	Com_sprintf (s, 32, "(%i %i %i)", (int)v[0], (int)v[1], (int)v[2]);

	return s;
}

void Cmd_ResetRotation_f(void) {
   if(trap_Argc()) {
      quat_t q, qtemp;
      vec3_t angles;
      
      int num = atoi(CG_Argv(1));
      
      Com_Printf("cgame: Cmd_ResetRotation_f");

      // special case: if the client being reset is the current client
      if(num == cg.snap->ps.clientNum) {
         QuaternionConjugate(cg.Q, &q);
         QuaternionMultiply(&cg.Q, &q, &qtemp);
         QuaternionCopy(qtemp, &(cg.Q));
      }

      /*
      if(num == cg.snap->ps.clientNum) {
         QuaternionConjugate(cg.Q, &q);
         QuaternionMultiply(&cg.Q, &q, &qtemp);
         QuaternionCopy(qtemp, &(cg.Q));
         QuaternionToTieFloats(cg.snap->ps.tieFloats, &(cg.Q));
      } else {
         QuaternionConjugate(cg_quats[num], &q);
         QuaternionMultiply(&cg_quats[num], &q, &qtemp);
         QuaternionCopy(qtemp, &(cg_quats[num]));
      }
      */

      VectorCopy(cg.snap->ps.viewangles, angles);

      if(num == cg.snap->ps.clientNum) {
         VectorCopy(angles, cg.Rangles);
         VectorCopy(cg.Rangles, cg.snap->ps.viewangles);
         VectorCopy(cg.Rangles, cg.predictedPlayerState.viewangles);
         VectorCopy(cg.Rangles, cg_angles[cg.snap->ps.clientNum]);
      } else {
         VectorCopy(angles, cg_angles[num]);
      }

      /*
      cg_entities[cg.snap->ps.clientNum].currentState.apos.trBase[0] = 0;
      cg_entities[cg.snap->ps.clientNum].currentState.apos.trBase[1] = 0;
      cg_entities[cg.snap->ps.clientNum].currentState.apos.trBase[2] = 0;
      */
   }
}

void Cmd_SpawnVehicle_f(void) {
   if(trap_Argc() == 4) {
      //char vehiclename[16];
      int vehicleTemplateNum, vehicleNum, entityNum;

      //Q_strncpyz (cg.testModelName, CG_Argv( 1 ), MAX_QPATH );
      vehicleTemplateNum = atoi(CG_Argv(1));
      vehicleNum = atoi(CG_Argv(2));
      entityNum = atoi(CG_Argv(3));

      Com_Printf("CGAME: got template for %s, template num %d, vehicle num %d, entityNum %d\n", cg_vehicleTemplates[vehicleTemplateNum].vehicleInfo.vehicleName, vehicleTemplateNum, vehicleNum, entityNum);

      if((vehicleTemplateNum >= 0) && (vehicleNum >= 0)) {
         vehicle_t v;
         int newVid;
         
         //v.entityId = entityNum;
         Vehicle_CopyVehicle( &v, &(cg_vehicleTemplates[vehicleTemplateNum]) );
         
         /*
         v.templateNum = vehicleTemplateNum;
         v.vehicleId = vehicleNum;
         v.entityId = entityNum;
         */

         //VehicleArray_SetVehicleForId(v.vehicleId, &v);
         newVid = VehicleArray_AddVehicle( &v );

         //cg_vehicles[newVid].templateNum = vehicleTemplateNum;
         cg_vehicles[newVid].vehicleId = vehicleNum;
         cg_vehicles[newVid].entityId = entityNum;

         CG_Printf("CGAME: Spawned vehicle: %s with template num %d, vehicle num %d, entity num %d", cg_vehicles[v.vehicleId].vehicleInfo.vehicleName, cg_vehicles[v.vehicleId].templateNum, cg_vehicles[v.vehicleId].vehicleId, cg_vehicles[v.vehicleId].entityId);

         /* = ent->client->ps.stats[VEHICLE_STAT] */
      }
   }
}

static float identity[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};

void CG_Vehicle( centity_t *cent ) 
{
   int vehicleTemplateNum, vehicleNum;
   vehicle_t vehicleTemplate;

	refEntity_t			vehicleEnt;
   //refEntity_t       cockpitEnt;

   //float           R[3][3];
   vec3_t            R[3];
   vec3_t            currentAngles, previousAngles, deltaAngles;

   quat_t            q, qtemp, q_previous;

	entityState_t		*s1;

	s1 = &cent->currentState;

	// create the render entity
	memset (&vehicleEnt, 0, sizeof(vehicleEnt));
   //memset (&cockpitEnt, 0, sizeof(cockpitEnt));

	vehicleEnt.frame = 0;
	vehicleEnt.oldframe = 0;
	vehicleEnt.backlerp = 0;

	/*
	cockpitEnt.frame = 0;
	cockpitEnt.oldframe = 0;
	cockpitEnt.backlerp = 0;
	*/

   // calculate the axis
	VectorCopy( s1->angles, cent->lerpAngles);
	VectorCopy( cent->lerpOrigin, vehicleEnt.origin);
	VectorCopy( cent->lerpOrigin, vehicleEnt.oldorigin);

	/*
	VectorCopy( s1->angles, cent->lerpAngles);
	VectorCopy( cent->lerpOrigin, cockpitEnt.origin);
	VectorCopy( cent->lerpOrigin, cockpitEnt.oldorigin);
	*/

	// set up current and previous angles
   if(cent->currentState.number == cg.predictedPlayerEntity.currentState.number) {
      VectorCopy(cg.refdefViewAngles, currentAngles);
   } else {
		VectorCopy(cent->currentState.angles, currentAngles);
		//QuaternionCopy(cg_quats[cent->currentState.number], &q_previous);
   }
   VectorCopy(cg_angles[cent->currentState.number], previousAngles);
	
	//Com_Printf("%d ", cent->currentState.number);
	
	// check to see if we're an NPV (non player vehicle)
	if( ((cent->currentState.number < 0) || (cent->currentState.number >= MAX_CLIENTS) ) && (cent->currentState.apos.trTime)){
		//Com_Printf("drawing an npv.\n");
		
		// slerp...
		// if cg.time > trtime + duration
		//  - copy new a and make new b
		//  - set a and b into cg_lerpquats
		//  - begin to lerp
		//  - copy the quat to q
		
		// did the slerp just restart?
		if(s1->apos.trType && !cg_slerping[s1->number] ) {
			cg_slerping[s1->number] = qtrue;

			//QuaternionFromApos(&(cent->currentState.apos), &((cg_lerpquats[s1->number])[0]));
			
			// copy new A
			QuaternionCopy(cg_quats[s1->number], &(cg_lerpquats[s1->number][0]));

         // get delta angles and convert to quat
			VectorSubtract(currentAngles, previousAngles, deltaAngles);
			QuaternionFromAngles(deltaAngles, &q);

         // concatenate quat with previous
			//IF (relative orientation) (or NOT absolute)
			QuaternionCopy(q, &qtemp);
			QuaternionMultiply(&(cg_lerpquats[s1->number][0]), &qtemp, &(cg_lerpquats[s1->number][1]));

		   // store old angles
			VectorCopy(currentAngles, cg_angles[cent->currentState.number]);
		
		// if we're currently starting to do some slerping (only do this once (in previous case))
		} else if(s1->apos.trType && cg_slerping[s1->number] ) {
			// dont do anything
		
		// if the server says we just finished starting our slerping
		} else if(!s1->apos.trType && cg_slerping[s1->number] ) {
			cg_slerping[s1->number] = qfalse;
		}

		// interpolate
		QuaternionIdentity(&q);
		QuaternionSlerp(cg_lerpquats[s1->number][0], cg_lerpquats[s1->number][1], s1->apos.trTime, s1->apos.trDuration, cg.time, &q);

		//Com_Printf("q: %f,%f,%f,%f\n", q.x, q.y, q.z, q.w);
		//Com_Printf("lerped with %f\n", (float)(cg.time - s1->apos.trTime)/(float)s1->apos.trDuration);
	
	// if we're a player controlled vehicle
	} else {
		
		// get the previous orientation
		QuaternionCopy(cg_quats[cent->currentState.number], &q_previous);
		
		if(QuaternionIsEmpty(&q_previous)){
			Com_Printf("quat prev is empty\n");
			QuaternionIdentity(&q_previous);
		}

		// get delta angles and convert to a quat
		VectorSubtract(currentAngles, previousAngles, deltaAngles);
		QuaternionFromAngles(deltaAngles, &q);

		// concatenate with previous
		// IF (relative orientation) (NOT absolute orientation)
		QuaternionCopy(q, &qtemp);
		QuaternionMultiply(&qtemp, &q_previous, &q);	
	}

	// normalize our orientation
	QuaternionNormalize(&q);

   // generate rotation matrix
   QuaternionToAxis(q, R);

   // apply rotation to entity axes
	//AxisCopy(R, cockpitEnt.axis);
	AxisCopy(R, vehicleEnt.axis);
   //MatrixMultiply(R, identity, turretEnt.axis);
   //MatrixMultiply(R, identity, baseEnt.axis);
   
	// store old angles if we're a player controlled vehicle
	if(!( ((cent->currentState.number < 0) || (cent->currentState.number >= MAX_CLIENTS) ) && (cent->currentState.apos.trTime) )) {
		VectorCopy(currentAngles, cg_angles[cent->currentState.number]);
	}

   // store old orientation for everyone
   QuaternionCopy(q, &(cg_quats[cent->currentState.number]));

   vehicleTemplateNum = cent->currentState.otherEntityNum;
   vehicleNum = cent->currentState.otherEntityNum2;
   Vehicle_CopyVehicle(&vehicleTemplate, &(cg_vehicleTemplates[vehicleTemplateNum]));

   // this is a hack for the current client
   /*
	if(cent->currentState.number == cg.predictedPlayerEntity.currentState.number) {
      Vehicle_CopyVehicle(&vehicleTemplate, &(cg_vehicleTemplates[VehicleTemplateArray_GetTemplateIndexForName("tiefighter")]));
   }
	*/

	// does the current vehicle have a basemodel assigned?
	if(vehicleTemplate.vehicleInfo.displayInfo.models[MODEL_VEHICLE].model != -1) {
      vehicleEnt.hModel = vehicleTemplate.vehicleInfo.displayInfo.models[MODEL_VEHICLE].model;
	   
      vehicleEnt.reType = RT_MODEL;
      
		// does it have a base shader assigned to this base model?
		if(vehicleTemplate.vehicleInfo.displayInfo.models[MODEL_VEHICLE].shader != -1) {
         //if(cent->currentState.number != cg.predictedPlayerEntity.currentState.number)
            //Com_Printf("base customShader: %d\n", cg_vehicleTemplates[cent->currentState.otherEntityNum].vehicleInfo.displayInfo.baseshader);
			vehicleEnt.customShader = vehicleTemplate.vehicleInfo.displayInfo.models[MODEL_VEHICLE].shader;
      }
      
		// does it have a skin assigned to this base model?
		if(vehicleTemplate.vehicleInfo.displayInfo.models[MODEL_VEHICLE].skin != -1) {
         //if(cent->currentState.number != cg.predictedPlayerEntity.currentState.number)
            //Com_Printf("base customSkin: %d\n", cg_vehicleTemplates[cent->currentState.otherEntityNum].vehicleInfo.displayInfo.baseskin);
         vehicleEnt.customSkin = vehicleTemplate.vehicleInfo.displayInfo.models[MODEL_VEHICLE].skin;
      }
      
		// add the offset to the origin
		VectorAdd(vehicleEnt.origin, vehicleTemplate.vehicleInfo.displayInfo.models[MODEL_VEHICLE].offset, vehicleEnt.origin);

		// scale the axes
		AxisScale(vehicleEnt.axis, vehicleTemplate.vehicleInfo.displayInfo.models[MODEL_VEHICLE].scale, vehicleEnt.axis);

      //baseEnt.skinNum = baseEnt.customSkin;
	   //baseEnt.renderfx = RF_MINLIGHT | RF_FIRST_PERSON;
      vehicleEnt.renderfx = RF_FIRST_PERSON;
      vehicleEnt.radius = 4096;

      trap_R_AddRefEntityToScene(&vehicleEnt);
   }

	/*
	// does the current vehicle have a cockpit model assigned?
	if(vehicleTemplate.vehicleInfo.displayInfo.models[MODEL_COCKPIT].model >= 0) {
		cockpitEnt.hModel = vehicleTemplate.vehicleInfo.displayInfo.models[MODEL_COCKPIT].model;

	   cockpitEnt.reType = RT_MODEL;
   
		// does it have a base shader assigned to this base model?
		if(vehicleTemplate.vehicleInfo.displayInfo.models[MODEL_COCKPIT].shader >= 0) {
         if(cent->currentState.number != cg.predictedPlayerEntity.currentState.number)  
            //Com_Printf("turret customShader: %d\n", cg_vehicleTemplates[cent->currentState.otherEntityNum].vehicleInfo.displayInfo.turretshader);
				cockpitEnt.customShader = vehicleTemplate.vehicleInfo.displayInfo.models[MODEL_COCKPIT].shader;   
      }

		// does it have a skin assigned to this base model?
		if(vehicleTemplate.vehicleInfo.displayInfo.models[MODEL_COCKPIT].skin >= 0) {
         if(cent->currentState.number != cg.predictedPlayerEntity.currentState.number)
            //Com_Printf("turret customSkin: %d\n", cg_vehicleTemplates[cent->currentState.otherEntityNum].vehicleInfo.displayInfo.turretskin);
				cockpitEnt.customSkin = vehicleTemplate.vehicleInfo.displayInfo.models[MODEL_COCKPIT].skin;
      }

		// scale the axes
		AxisScale(cockpitEnt.axis, vehicleTemplate.vehicleInfo.displayInfo.models[MODEL_COCKPIT].scale, cockpitEnt.axis);

      //turretEnt.renderfx = RF_MINLIGHT | RF_FIRST_PERSON;
      cockpitEnt.renderfx = RF_FIRST_PERSON;
      cockpitEnt.radius = 4096;
      trap_R_AddRefEntityToScene(&cockpitEnt);
   }
	*/
}


////////////////////////////////////
// vehicle array helper functions //
////////////////////////////////////
int VehicleArray_InitVehicleArray() {
   int i;
//   for(i = 0; i < MAX_CLIENTS; i++) {
   for(i = 0; i < MAX_GENTITIES; i++) {
      Vehicle_InitVehicle(&(cg_vehicles[i]));
   }

   return 1;
}

vehicle_t *VehicleArray_GetVehicleForId(int vehicleId) {
   int i;
   if(vehicleId >= 0) {
      //for(i = 0; i < MAX_CLIENTS; i++) {
      for(i = 0; i < MAX_GENTITIES; i++) {
         if(cg_vehicles[i].vehicleId == vehicleId)
            return &cg_vehicles[i];
      }
   }
   return (vehicle_t*)0;
}

int VehicleArray_SetVehicleForId(int vehicleId, vehicle_t *vehicle) {
   int i;
   if(vehicle && vehicleId) {
      //for(i = 0; i < MAX_CLIENTS; i++) {
      for(i = 0; i < MAX_GENTITIES; i++) {
         if(cg_vehicles[i].vehicleId == vehicleId) {
            Vehicle_CopyVehicle(&(cg_vehicles[i]), vehicle);
            return 1;
         }
      }
   }
   return 0;
}


int VehicleArray_AddVehicle(vehicle_t *vehicle) {
   int i;
   if(vehicle) {
      //for(i = 0; i < MAX_CLIENTS; i++) {
      for(i = 0; i < MAX_GENTITIES; i++) {
         if(cg_vehicles[i].vehicleId == -1) {
            vehicle->vehicleId = i;
            return Vehicle_CopyVehicle(&(cg_vehicles[i]), vehicle);
         }
      }
   }
   return 0;
}

int VehicleArray_RemoveVehicleForId(int vehicleId) {
   int i;
   if(vehicleId) {
      //for(i = 0; i < MAX_CLIENTS; i++) {
      for(i = 0; i < MAX_GENTITIES; i++) {
         if(cg_vehicles[i].vehicleId == vehicleId) {
            return Vehicle_InitVehicle(&(cg_vehicles[i]));
         }
      }
   }
   return 0;
}

int VehicleArray_OutputVehicles() {
   int i;
   //char buffer[64];

   //clear_string(buffer, 64);

   CG_Printf("Vehicles:\n\n" );

   //for(i = 0; i < MAX_CLIENTS; i++) {
   for(i = 0; i < MAX_GENTITIES; i++) {
      if(cg_vehicles[i].vehicleId != -1) {
         CG_Printf("  Vehicle: %d\n", i);
         CG_Printf("    Name:    %s\n", cg_vehicles[i].vehicleInfo.vehicleName );
      }
   }

	return 1;
}

//////////////////////////////
// vehicle template helpers //
//////////////////////////////

int VehicleTemplateArray_InitVehicleTemplateArray() {
   int i;
   //for(i = 0; i < MAX_CLIENTS; i++) {
   for(i = 0; i < MAX_GENTITIES; i++) {
      Vehicle_InitVehicle(&(cg_vehicleTemplates[i]));
   }

   return 1;
}


int VehicleTemplateArray_GetTemplateIndexForName(char *name) {
   int i;
   //for(i = 0; i < MAX_CLIENTS; i++) {
   for(i = 0; i < MAX_GENTITIES; i++) {
      if(Q_stricmp(cg_vehicleTemplates[i].vehicleInfo.vehicleName, name) == 0)
         return i;
   }

   return -1;
}

int VehicleTemplateArray_OutputTemplates(int clientNum) {
   int i;
   //char buffer[64];

   //clear_string(buffer, 64);

   CG_Printf("Vehicle Templates:\n\n" );

   //for(i = 0; i < MAX_CLIENTS; i++) {
   for(i = 0; i < MAX_GENTITIES; i++) {
      if(cg_vehicleTemplates[i].templateNum != -1) {
         CG_Printf("  Vehicle: %d\n", i);
         CG_Printf("    Name:    %s\n", cg_vehicleTemplates[i].vehicleInfo.vehicleName );
      }
   }

	return 1;
}

void Cmd_VehicleTemplates_f(void) {
   VehicleTemplateArray_OutputTemplates();
}

void Cmd_Vehicles_f(void) {
   VehicleArray_OutputVehicles();
}