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

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] */
      }
   }
}

void Cmd_VehicleTemplates_f(void) {
   VehicleTemplateArray_OutputTemplates();
}

void Cmd_Vehicles_f(void) {
   VehicleArray_OutputVehicles();
}

static int degrees;
static float eye[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
static float prevR[3][3];
static vec3_t prevAngles;
static quat_t prevQ;

void CG_Vehicle( centity_t *cent )
{
   int i, j;
	int rotatedYet = 1;
   int zeros = 0;

	refEntity_t			baseEnt;
   refEntity_t       turretEnt;

   //float             R[3][3];
   vec3_t               Ry[3], Rp[3], Rr[3], R[3], Rinv[3];
   vec3_t               dir;
   double            rads;

   float             yaw, pitch, roll;
   quat_t            qy, qp, qr, q, qinv;

	entityState_t		*s1;

	s1 = &cent->currentState;

	// create the render entity
	memset (&baseEnt, 0, sizeof(baseEnt));
   memset (&turretEnt, 0, sizeof(turretEnt));

	baseEnt.frame = 0;
	baseEnt.oldframe = 0;
	baseEnt.backlerp = 0;

   turretEnt.frame = 0;
	turretEnt.oldframe = 0;
	turretEnt.backlerp = 0;

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

   VectorCopy( s1->angles, cent->lerpAngles);
	AnglesToAxis(cent->lerpAngles, turretEnt.axis);
	VectorCopy( cent->lerpOrigin, turretEnt.origin);
	VectorCopy( cent->lerpOrigin, turretEnt.oldorigin);

	// flicker between two skins
	//ent.skinNum = cg.clientFrame & 1;
   
   degrees++;
   if(degrees < 0)
      degrees = 0;
   if(degrees > 360)
      degrees -= 360;
   if(degrees == 360)
      degrees = 0;

   rads = degrees * M_PI/180.0f;
   
   // check for initial rotation
   for(i = 0; i < 3, rotatedYet; i++) {
      for(j = 0; j < 3; j++) {
         // bad logic...
         /*
         if(i == j) {
            if(prevR[i][j] <= 0.001f) {
               rotatedYet = 0;
            } else {
               rotatedYet = 1;
            }
         }
         */
         if(cg.R[i][j] <= 0.001f)
            zeros++;
      }
      if(zeros == 3) {
         rotatedYet = 0;
      } else
         zeros = 0;
   }

   // set up axes
   if(!rotatedYet) {
      MatrixMultiply(eye, eye, cg.R);
      VectorSet(cg.Rangles, 0, 0, 0);
      QuaternionIdentity(&cg.Q);
      rotatedYet = 1;
   }
   
   /*
   // rotation about dir
   VectorSet(dir, 1, 1, 1);
   RotationMatrixAroundDir(dir, rads, R);
   VectorNormalize(R[0]);
   VectorNormalize(R[1]);
   VectorNormalize(R[2]);
   MatrixMultiply(R, eye, baseEnt.axis);
   */

   /*
   VectorNormalize(baseEnt.axis[0]);
   VectorNormalize(baseEnt.axis[1]);
   VectorNormalize(baseEnt.axis[2]);
   */
   
   // get delta angles...
   if(rotatedYet) {
      pitch = cg.refdefViewAngles[0] - cg.Rangles[0];
      yaw = cg.refdefViewAngles[1] - cg.Rangles[1];
      roll = cg.refdefViewAngles[2] - cg.Rangles[2];
   } else {
      pitch = cg.refdefViewAngles[0];
      yaw = cg.refdefViewAngles[1];
      roll = cg.refdefViewAngles[2];
   }

   pitch = DEG2RAD(pitch);
   yaw = DEG2RAD(yaw);
   roll = DEG2RAD(roll);

   // RELATIVE UP, RELATIVE FORWARD, FIXED RIGHT
   
   // generate rotation quat from delta angles
   VectorSet(dir, pitch, yaw, roll);
   AnglesToQuat(dir, &qr);
   QuaternionNormalize(&qr);
   //if(rotatedYet) {
      QuaternionMultiply(cg.Q, qr, &q);
      QuaternionNormalize(&q);
   //}
   //else {
   //   QuaternionCopy(qr, &q);
   //   QuaternionNormalize(&q);
   //}

   // generate rotation matrix
   // q
   QuaternionToAxis(q, R);
   
   // q'
   QuaternionConjugate(q, &qinv);
   QuaternionToAxis(qinv, Rinv);
   
   /*
   // generate rotation quat from delta angles
   VectorSet(dir, pitch, yaw, roll);
   AnglesToQuat(dir, &qr);
   QuaternionNormalize(&qr);

   // concatenate to previous quat
   QuaternionMultiply(cg.Q, qr, &q);
   QuaternionNormalize(&q);
   */
   
   // v_rot = q * R * q'

   // apply quat
   MatrixMultiply(R, eye, turretEnt.axis);
   MatrixMultiply(R, eye, baseEnt.axis);
   
   // apply quat conjugate
   //MatrixMultiply(turretEnt.axis, Rinv, turretEnt.axis);
   //MatrixMultiply(baseEnt.axis, Rinv, baseEnt.axis);

   // normalize axes so we have no scaling
   VectorNormalize(baseEnt.axis[0]);
   VectorNormalize(baseEnt.axis[1]);
   VectorNormalize(baseEnt.axis[2]);
   VectorNormalize(turretEnt.axis[0]);
   VectorNormalize(turretEnt.axis[1]);
   VectorNormalize(turretEnt.axis[2]);
   VectorNormalize(R[0]);
   VectorNormalize(R[1]);
   VectorNormalize(R[2]);
   
   // store old orientation and angles and quat
   MatrixMultiply(R, eye, cg.R);
   VectorCopy(cg.refdefViewAngles, cg.Rangles);
   QuaternionCopy(q, &prevQ);
   QuaternionCopy(q, &cg.Q);
   
   /*
   baseEnt.hModel = cg_vehicles[cent->currentState.otherEntityNum].vehicleInfo.displayInfo.basemodel;
	baseEnt.reType = RT_MODEL;
   baseEnt.customShader = cg_vehicles[cent->currentState.otherEntityNum].vehicleInfo.displayInfo.baseshader;
	baseEnt.renderfx = RF_NOSHADOW;

   turretEnt.hModel = cg_vehicles[cent->currentState.otherEntityNum].vehicleInfo.displayInfo.turretmodel;
	turretEnt.reType = RT_MODEL;
   turretEnt.customShader = cg_vehicles[cent->currentState.otherEntityNum].vehicleInfo.displayInfo.turretshader;
	turretEnt.renderfx = RF_NOSHADOW;
   */

   /*
   otherEntityNum is templateNum
   otherEntityNum2 is vehicleNum
   */

   /*
   baseEnt.hModel = cg_vehicles[cent->currentState.otherEntityNum2].vehicleInfo.displayInfo.basemodel;
	baseEnt.reType = RT_MODEL;
   baseEnt.customShader = cg_vehicles[cent->currentState.otherEntityNum2].vehicleInfo.displayInfo.baseshader;
	baseEnt.renderfx = RF_MINLIGHT | RF_DEPTHHACK | RF_FIRST_PERSON;
   */

   baseEnt.hModel = cg_vehicleTemplates[cent->currentState.otherEntityNum].vehicleInfo.displayInfo.basemodel;
	baseEnt.reType = RT_MODEL;
   baseEnt.customShader = cg_vehicleTemplates[cent->currentState.otherEntityNum].vehicleInfo.displayInfo.baseshader;
	baseEnt.renderfx = RF_MINLIGHT | RF_FIRST_PERSON;
   baseEnt.radius = 4096;

   turretEnt.hModel = cg_vehicleTemplates[cent->currentState.otherEntityNum].vehicleInfo.displayInfo.turretmodel;
	turretEnt.reType = RT_MODEL;
   turretEnt.customShader = cg_vehicleTemplates[cent->currentState.otherEntityNum].vehicleInfo.displayInfo.turretshader;
	turretEnt.renderfx = RF_MINLIGHT | RF_FIRST_PERSON;
   turretEnt.radius = 4096;

	//Com_Printf("rendering klonky... %f, %f, %f\n", ent.origin[0], ent.origin[1], ent.origin[2]);

   trap_R_AddRefEntityToScene(&baseEnt);
   trap_R_AddRefEntityToScene(&turretEnt);
	

   /*
	BG_EvaluateTrajectory( &cent->currentState.pos, cg.time, cent->lerpOrigin );

	memset( &ent, 0, sizeof( ent ) );
	ent.reType = RT_SPRITE;
	VectorCopy( cent->lerpOrigin, ent.origin );
	ent.radius = 14;
	ent.customShader = cg_items[s1->modelindex].icons[0];
	ent.shaderRGBA[0] = 255;
	ent.shaderRGBA[1] = 255;
	ent.shaderRGBA[2] = 255;
	ent.shaderRGBA[3] = 255;
	trap_R_AddRefEntityToScene(&ent);

	//trap_R_AddRefEntityToScene((const)&ent);
   */
}


////////////////////////////////////
// vehicle array helper functions //
////////////////////////////////////
int VehicleArray_InitVehicleArray() {
   int i;
   for(i = 0; i < MAX_CLIENTS; 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++) {
         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++) {
         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++) {
         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++) {
         if(cg_vehicles[i].vehicleId == vehicleId) {
            return Vehicle_InitVehicle(&(cg_vehicles[i]));
         }
      }
   }
   return 0;
}

int VehicleArray_OutputVehicles(int clientNum) {
   int i, j, k;
   //char buffer[64];

   //clear_string(buffer, 64);

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

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

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

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

   return 1;
}


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

   return -1;
}

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

   //clear_string(buffer, 64);

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

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