/**
  racing module: implements the racing physics and logic.
*/

#ifndef _LCR_RACING_H
#define _LCR_RACING_H

typedef int32_t LCR_GameUnit;  ///< abstract game unit

#define LCR_GAME_UNIT 1024     ///< length of map square in LCR_GameUnits

#define LCR_RACING_INPUT_FORW  0x01
#define LCR_RACING_INPUT_RIGHT 0x02
#define LCR_RACING_INPUT_BACK  0x04
#define LCR_RACING_INPUT_LEFT  0x08

#define LCR_PHYSICS_UNIT 2048   ///< length of map square for physics engine

#include "map.h"
#include "tinyphysicsengine.h"

#define LCR_CAR_JOINTS 5
#define LCR_CAR_CONNECTIONS 10

#define LCR_GRAVITY (LCR_PHYSICS_UNIT / 100)
#define LCR_CAR_FORWARD_FRICTION (TPE_F / 11)
#define LCR_CAR_TURN_FRICTION (4 * TPE_F / 4)
#define LCR_CAR_ELASTICITY (TPE_F / 100)
#define LCR_CAR_ACCELERATION (LCR_PHYSICS_UNIT / 15)
#define LCR_CAR_TURN_SPEED (LCR_GAME_UNIT / 20)
#define LCR_CAR_TURN_MAX (LCR_GAME_UNIT / 4)

/*
#define LCR_GRAVITY (LCR_PHYSICS_UNIT / 100)
#define LCR_CAR_FORWARD_FRICTION (TPE_F / 14)
#define LCR_CAR_TURN_FRICTION (3 * TPE_F / 4)
#define LCR_CAR_ELASTICITY (TPE_F / 100)
#define LCR_CAR_ACCELERATION (LCR_PHYSICS_UNIT / 18)
#define LCR_CAR_TURN_SPEED (LCR_GAME_UNIT / 3)
#define LCR_CAR_TURN_MAX (LCR_GAME_UNIT / 4)
*/

struct
{
  TPE_World physicsWorld;
  TPE_Body carBody;
  TPE_Joint carJoints[LCR_CAR_JOINTS];
  TPE_Connection carConnections[LCR_CAR_CONNECTIONS];
  uint32_t tick;
  uint8_t wheelCollisions;  /**< In individual bits records for each car wheel
                                 whether it's currently touching the ground.
                                 Lower bits record current collisions, higher
                                 bits the previous state (for averaging). */


  TPE_Vec3 carPositions[2]; ///* Current and previous position.

  LCR_GameUnit wheelRotation;
  LCR_GameUnit wheelSteer;
} LCR_racing;

TPE_Vec3 _LCR_TPE_vec3DividePlain(TPE_Vec3 v, TPE_Unit d)
{
  v.x /= d;
  v.y /= d;
  v.z /= d;
  return v;
}

TPE_Vec3 _LCR_racingEnvironmentFunction(TPE_Vec3 point, TPE_Unit maxDist)
{
  return TPE_envAABoxInside(point,TPE_vec3(0,0,0),TPE_vec3(
      LCR_PHYSICS_UNIT * LCR_MAP_SIZE_BLOCKS,
      (LCR_PHYSICS_UNIT * LCR_MAP_SIZE_BLOCKS) / 2,
      LCR_PHYSICS_UNIT * LCR_MAP_SIZE_BLOCKS));
}

LCR_GameUnit LCR_racingGetCarSpeed(void)
{
  return (TPE_vec3Len(TPE_vec3(
    LCR_racing.carBody.joints[4].velocity[0],
    LCR_racing.carBody.joints[4].velocity[1],
    LCR_racing.carBody.joints[4].velocity[2])) * LCR_GAME_UNIT)
    / LCR_PHYSICS_UNIT;
}

uint8_t _LCR_racingCollisionHandler(uint16_t b1, uint16_t j1, uint16_t b2,
  uint16_t j2, TPE_Vec3 p)
{
  // check which wheels are touching the ground.

  if (j1 < 4) // wheel joint?
    LCR_racing.wheelCollisions |= 0x01 << j1;

  return 1;
}

/**
  Initializes new run.
*/
void LCR_racingRestart(void)
{
  LCR_racing.tick = 0;

  TPE_bodyActivate(&(LCR_racing.carBody));
  LCR_racing.wheelCollisions = 0;

  LCR_racing.wheelRotation = 0;
  LCR_racing.wheelSteer = 0;

  // TODO
}

/**
  Initializes the racing module, only call once.
*/
void LCR_racingInit(void)
{
  LCR_log("initializing racing engine");

  // make the car body:
  TPE_makeCenterRectFull(LCR_racing.carJoints,
    LCR_racing.carConnections,
    LCR_PHYSICS_UNIT / 2,
    (LCR_PHYSICS_UNIT * 3) / 4,
    LCR_PHYSICS_UNIT / 8);

  LCR_racing.carJoints[4].position.y += LCR_PHYSICS_UNIT / 6;
  LCR_racing.carJoints[4].sizeDivided *= 3;
  LCR_racing.carJoints[4].sizeDivided /= 2;

  TPE_bodyInit(&(LCR_racing.carBody), 
    LCR_racing.carJoints,LCR_CAR_JOINTS,
    LCR_racing.carConnections,LCR_CAR_CONNECTIONS,
    TPE_F);

  TPE_worldInit(&(LCR_racing.physicsWorld),
  &(LCR_racing.carBody),1,_LCR_racingEnvironmentFunction);

  LCR_racing.physicsWorld.collisionCallback = _LCR_racingCollisionHandler;

  LCR_racing.carBody.friction = LCR_CAR_FORWARD_FRICTION;
  LCR_racing.carBody.elasticity = LCR_CAR_ELASTICITY;
  LCR_racing.carBody.flags |= TPE_BODY_FLAG_ALWAYS_ACTIVE;

  LCR_racingRestart();
}

void LCR_racingGetCarTransform(LCR_GameUnit position[3],
  LCR_GameUnit rotation[3], LCR_GameUnit interpolationParam)
{
#if LCR_SETTING_SMOOTH_ANIMATIONS
  TPE_Vec3 v = TPE_vec3Plus(
      LCR_racing.carPositions[1],
        _LCR_TPE_vec3DividePlain(
          TPE_vec3TimesPlain(
            TPE_vec3Minus(
              LCR_racing.carPositions[0],LCR_racing.carPositions[1]),
              interpolationParam),LCR_GAME_UNIT));

  position[0] = v.x;
  position[1] = v.y;
  position[2] = v.z;
#else
  TPE_Vec3 v;

  position[0] = LCR_racing.carPositions[0].x;
  position[1] = LCR_racing.carPositions[0].y;
  position[2] = LCR_racing.carPositions[0].z;
#endif

  v = TPE_bodyGetRotation(&(LCR_racing.carBody),0,2,1);

  rotation[0] = (v.x * LCR_GAME_UNIT) / TPE_F;
  rotation[1] = (v.y * LCR_GAME_UNIT) / TPE_F;
  rotation[2] = (v.z * LCR_GAME_UNIT) / TPE_F;
}

void _LCR_drawPhysicsDebugPixel(uint16_t x, uint16_t y, uint8_t color)
{
  if (x > 1 && x < LCR_EFFECTIVE_RESOLUTION_X - 2 &&
      y > 1 && y < LCR_EFFECTIVE_RESOLUTION_Y - 2)
  {
    uint16_t c = 0x8101 | (0x8f1f << (2 * color));

    for (int j = -1; j <= 2; ++j)
      for (int i = -1; i <= 2; ++i)
        LCR_drawPixelXYUnsafe(x + i,y + j,c);
  }
}

int LCR_racingCarWheelTouchesGround(int wheel)
{
  return ((LCR_racing.wheelCollisions & (LCR_racing.wheelCollisions >> 4))
    >> wheel) & 0x01;
}
      
LCR_GameUnit LCR_racingGetWheelRotation(void)
{
  return LCR_racing.wheelRotation;
}

LCR_GameUnit LCR_racingGetWheelSteer(void)
{
  return LCR_racing.wheelSteer;
}

void _LCR_racingWheelAccelerate(unsigned int wheel, TPE_Vec3 dir)
{
  LCR_racing.carBody.joints[wheel].velocity[0] +=
    (dir.x * LCR_CAR_ACCELERATION) / TPE_F;
  LCR_racing.carBody.joints[wheel].velocity[1] +=
    (dir.y * LCR_CAR_ACCELERATION) / TPE_F;
  LCR_racing.carBody.joints[wheel].velocity[2] +=
    (dir.z * LCR_CAR_ACCELERATION) / TPE_F;
}

/**
  Updates the racing physics world, call every LCR_RACING_TICK_MS milliseconds.
*/
void LCR_racingStep(unsigned int input)
{
  TPE_Vec3 carForw, carRight, carUp;

  carForw = TPE_vec3Normalized(TPE_vec3Plus(
    TPE_vec3Minus(LCR_racing.carBody.joints[0].position,
                  LCR_racing.carBody.joints[2].position),
    TPE_vec3Minus(LCR_racing.carBody.joints[1].position,
                  LCR_racing.carBody.joints[3].position)));

  carRight = TPE_vec3Normalized(TPE_vec3Plus(
    TPE_vec3Minus(LCR_racing.carBody.joints[0].position,
                  LCR_racing.carBody.joints[1].position),
    TPE_vec3Minus(LCR_racing.carBody.joints[2].position,
                  LCR_racing.carBody.joints[3].position)));

  carUp = TPE_vec3Cross(carForw,carRight);

  if (input)
  {
    unsigned char steering = 0;

    // TODO: magic constants

    if (input & (LCR_RACING_INPUT_FORW | LCR_RACING_INPUT_BACK))
    {
      LCR_GameUnit rotateBy =
        (LCR_racing.wheelCollisions & 0x0f) ? // on ground slow down wheel rot.
        (LCR_racingGetCarSpeed() / 16) : LCR_GAME_UNIT / 32;

      if (!(input & LCR_RACING_INPUT_BACK))
        rotateBy *= -1;

      LCR_racing.wheelRotation =
        (LCR_racing.wheelRotation + rotateBy) % LCR_GAME_UNIT;

      if (LCR_racing.wheelRotation < 0)
        LCR_racing.wheelRotation += LCR_GAME_UNIT;
    }

    if (input & LCR_RACING_INPUT_RIGHT)
    {
      steering = 2;
      
      LCR_racing.wheelSteer = TPE_min(
        LCR_racing.wheelSteer + LCR_CAR_TURN_SPEED,
        LCR_CAR_TURN_MAX);
    }
    else if (input & LCR_RACING_INPUT_LEFT)
    {
      steering = 1;

      LCR_racing.wheelSteer = TPE_max(
        LCR_racing.wheelSteer - LCR_CAR_TURN_SPEED,
        -1 * LCR_CAR_TURN_MAX);
    }

    if ((LCR_racing.wheelCollisions & 0x0c)) // back wheel on ground?
    {
      if (input & LCR_RACING_INPUT_FORW)
      {
        _LCR_racingWheelAccelerate(0,carForw);
        _LCR_racingWheelAccelerate(1,carForw);
      }
      else if (input & LCR_RACING_INPUT_BACK)
      {
        _LCR_racingWheelAccelerate(0,TPE_vec3TimesPlain(carForw,-1));
        _LCR_racingWheelAccelerate(1,TPE_vec3TimesPlain(carForw,-1));
      }
    }

    for (int i = 0; i < 4; ++i)
      if (LCR_racing.wheelCollisions & (0x01 << i)) // wheel on ground?
      {
        TPE_Vec3 jv = TPE_vec3( // joint velocity 
          LCR_racing.carBody.joints[i].velocity[0],   
          LCR_racing.carBody.joints[i].velocity[1],   
          LCR_racing.carBody.joints[i].velocity[2]);   

        TPE_Vec3 ja = carRight; // wheel axis of rotation

        if (i >= 2 && steering)
        {
          // for front wheels with turning we tilt the wheel axis 45 degrees

          TPE_Unit steer =
            (LCR_racing.wheelSteer * TPE_F) / LCR_GAME_UNIT;
           
          ja = TPE_vec3Normalized(
            TPE_vec3Plus(TPE_vec3Times(carForw,steer),carRight));
        }

        /* friction is in the direction if the axis and its magnitude is
          determined by the dot product (angle) of the axis and velocity */
        TPE_Vec3 fric = TPE_vec3Times(ja,(TPE_vec3Dot(ja,jv) *
          LCR_CAR_TURN_FRICTION) / TPE_F);

        jv = TPE_vec3Minus(jv,fric); // subtract the friction

        LCR_racing.carBody.joints[i].velocity[0] = jv.x;
        LCR_racing.carBody.joints[i].velocity[1] = jv.y;
        LCR_racing.carBody.joints[i].velocity[2] = jv.z;
      }
  }

  if ((!(input & LCR_RACING_INPUT_LEFT)) &&
    (!(input & LCR_RACING_INPUT_RIGHT)))
    LCR_racing.wheelSteer /= 2;

  if ((LCR_racing.wheelCollisions & 0x0f) != 0x0f) // EXPERIMENTAL: don't apply gravity with all wheels on ground
    TPE_bodyApplyGravity(&(LCR_racing.carBody),LCR_GRAVITY);

  LCR_racing.wheelCollisions <<= 4;
  TPE_worldStep(&(LCR_racing.physicsWorld));

  if (TPE_vec3Dot(carUp,TPE_vec3Minus(LCR_racing.carBody.joints[4].position,
      LCR_racing.carBody.joints[0].position)) < 0)
  {
    /* if the car falls on its roof the center joint may flip to the other
       side, here we fix it */

// LCR_log("car flipped over, fixing");

    LCR_racing.carBody.joints[4].position = TPE_vec3Plus(TPE_vec3Times(carUp,
      LCR_GAME_UNIT / 4),LCR_racing.carBody.joints[4].position);
  }

  TPE_Vec3 tmpVec = LCR_racing.carPositions[0];

LCR_racing.carPositions[0] =
  _LCR_TPE_vec3DividePlain(
    TPE_vec3TimesPlain(
      LCR_racing.carBody.joints[4].position,LCR_GAME_UNIT),
          LCR_PHYSICS_UNIT);

LCR_racing.carPositions[0] =
  TPE_vec3KeepWithinBox(
    LCR_racing.carPositions[1],
    LCR_racing.carPositions[0],
      TPE_vec3(
        LCR_PHYSICS_UNIT / 50, // TODO: constant
        LCR_PHYSICS_UNIT / 50,
        LCR_PHYSICS_UNIT / 50
        )
    );

  LCR_racing.carPositions[1] = tmpVec;

  LCR_racing.tick++;
}

void LCR_physicsDebugDraw(LCR_GameUnit camPos[3], LCR_GameUnit camRot[2],
  LCR_GameUnit camFov)
{
  TPE_Vec3 cPos, cRot, cView;

  cPos.x = (camPos[0] * LCR_PHYSICS_UNIT) / LCR_GAME_UNIT;
  cPos.y = (camPos[1] * LCR_PHYSICS_UNIT) / LCR_GAME_UNIT;
  cPos.z = (camPos[2] * LCR_PHYSICS_UNIT) / LCR_GAME_UNIT;

  cRot.x = (camRot[0] * TPE_F) / LCR_GAME_UNIT;
  cRot.y = (camRot[1] * TPE_F) / LCR_GAME_UNIT;
  cRot.z = 0; 

  cView.x = LCR_EFFECTIVE_RESOLUTION_X;
  cView.y = LCR_EFFECTIVE_RESOLUTION_Y;
  cView.z = (camFov * TPE_F) / LCR_GAME_UNIT;

  TPE_worldDebugDraw(&(LCR_racing.physicsWorld),_LCR_drawPhysicsDebugPixel,
    cPos,cRot,cView,16,2 * LCR_PHYSICS_UNIT);
}

#endif // guard