#include "entity.hpp"
/*******************************************************************************
** EntityCreate
*******************************************************************************/
void
EntityCreate(
   Entity* entity,
   float xp, float yp, float zp,
   void  (*behaviour)(Entity*))
{
   memset(entity, 0, sizeof(Entity));
   
   VectorCreate(entity->pos,   xp, yp, zp);
   VectorCreate(entity->scale, 1.0f, 1.0f, 1.0f);
   
   entity->behaviour = behaviour;
   entity->issolid   = 0;
   entity->radius    = 0.0f;
}
/*******************************************************************************
** EntityRender
*******************************************************************************/
void
EntityRender(Entity* entity, Camera* camera)
{
   Vector pos;
   Vector poly[2];

   if (entity->texture)
   {
      VectorTransform(pos, entity->pos, camera->matrix);

      if (pos[2] > 1.0)
      {
         pos[1] *= camera->aspect;
         
         float w = (entity->texture->width  / 2.0f) * entity->scale[0];
         float h = (entity->texture->height / 2.0f) * entity->scale[1] * camera->aspect;
                     
         poly[0][0] = pos[0] - w;
         poly[0][1] = pos[1] + h;
         poly[0][2] = pos[2];
   
         poly[1][0] = pos[0] + w;
         poly[1][1] = pos[1] - h;
         poly[1][2] = pos[2];
   
         PolygonProject(poly, 2, camera);

         PictureCopy(
            PICTURE_FX_OR, 
            camera->fb,
               poly[0][0],
               poly[0][1],
               poly[1][0] + 0.9999f,
               poly[1][1] + 0.9999f,
            entity->texture,
               0, 0,
               entity->texture->width,
               entity->texture->height,
               camera->zb, PixelZDepth(poly[0][2]),
               entity->texture->pixels[0][0]);
      }
   }
}
/*******************************************************************************
** EntityToEntityCollision
*******************************************************************************/
void
EntityToEntityCollision(Entity* A, Entity* B)
{
   Vector vector;

   if (A->issolid && B->issolid)
   {
      VectorSubtract(vector, A->pos, B->pos);

      float dist = A->radius + B->radius - VectorMagnitude(vector);
      
      if (dist > 0.0)
      {
         VectorSetLength(vector, vector, dist / 2.0f);
         VectorAddition(A->vel, A->vel, vector);
         VectorAddition(A->pos, A->pos, vector);
         VectorSubtract(B->vel, B->vel, vector);
         VectorSubtract(B->pos, B->pos, vector);      
      }
   }
}
/*******************************************************************************
** EntityToWorldCollision
*******************************************************************************/
void
EntityToWorldCollision(Entity* entity, BSP* bsp, World* world)
{
   Vector   A;

   assert(bsp && world && entity);
   
   if (bsp->sides[0] && bsp->sides[1])
   {            
      float dist = PlaneDistanceTo(bsp->plane, entity->pos);         

      if (dist <  entity->radius) EntityToWorldCollision(entity, bsp->sides[1], world);      
      if (dist > -entity->radius) EntityToWorldCollision(entity, bsp->sides[0], world);
                  
      if (dist < entity->radius && dist > -entity->radius)
      {
         for (int i = 0; i < NumberOf(bsp->faces); i++)
         {
            if (bsp->faces[i].colour > 2)
            {
               dist = FaceDistanceTo(&bsp->faces[i], entity->pos, A);
               
               if (dist < entity->radius && dist > -entity->radius)
               {
                  entity->collision++;
   
                  VectorSubtract(A, entity->pos, A);
                  VectorNormalize(A, A);
                  
                  if (A[1] > 0.4f) entity->isonground++;
                  
                  VectorScale(A, A, entity->radius - dist);

                  VectorAddition(entity->pos, entity->pos, A);
                  VectorAddition(entity->vel, entity->vel, A);                                                                         
               }
            }
         }
      }                
   }
}
/*******************************************************************************
** EntityProcess
*******************************************************************************/
void
EntityCollisionProcess(Entity* entity, World* world)
{
   entity->collision    = 0;
   entity->isonground   = 0;   
   
   if (entity->behaviour)
   {            
      VectorAddition(entity->pos, entity->pos, entity->vel);
      
      if (entity->radius > 4.0f)
      {             
         VectorInverseTransform(entity->pos, entity->pos, world->matrix);
         
         EntityToWorldCollision(entity, world->bsp, world);

         VectorInverseTransform(entity->pos, entity->pos, world->matrix);
      } 
   }
}