#include "render_utils.h"
#include "utils.h"

inline void CameraState_Init(CameraState* camera_state) {
  // clang-format off
  static float mtx_identity[16] = {
      1.f, 0.f, 0.f, 0.f,
      0.f, 1.f, 0.f, 0.f,
      0.f, 0.f, 1.f, 0.f,
      0.f, 0.f, 0.f, 1.f
  };
  // clang-format on

  camera_state->forward[0] = 0.f;
  camera_state->forward[1] = 0.f;
  camera_state->forward[2] = -1.f;

  camera_state->right[0] = 1.f;
  camera_state->right[1] = 0.f;
  camera_state->right[2] = 0.f;

  camera_state->up[0] = 0.f;
  camera_state->up[1] = 1.f;
  camera_state->up[2] = 0.f;

  camera_state->pos[0] = 0.f;
  camera_state->pos[1] = 0.f;
  camera_state->pos[2] = 0.f;

  camera_state->vel[0] = 0.f;
  camera_state->vel[1] = 0.f;
  camera_state->vel[2] = 0.f;

  memcpy(&camera_state->mtxView, &mtx_identity, sizeof(camera_state->mtxView));
  CameraState_CalcFromMatrix(camera_state, &camera_state->mtxView);
}

void CameraState_CalcFromMatrix(CameraState* camera_state, simd4x4f* mtx) {

  camera_state->forward[0] = mtx->x[2];
  camera_state->forward[1] = mtx->y[2];
  camera_state->forward[2] = mtx->z[2];

  camera_state->right[0] = mtx->x[0];
  camera_state->right[1] = mtx->y[0];
  camera_state->right[2] = mtx->z[0];

  camera_state->heading = atan2(-camera_state->forward[2], camera_state->forward[0]);
  camera_state->pitch = asin(camera_state->forward[1]);

  simd4x4f rot_mat = *mtx;
  rot_mat.w = simd4f_create(0.f, 0.f, 0.f, 1.f);
  simd4x4f_transpose_inplace(&rot_mat);

  simd4f eye;
  simd4x4f_matrix_point3_mul(&rot_mat, &mtx->w, &eye);

  camera_state->pos[0] = -simd4f_get_x(eye);
  camera_state->pos[1] = -simd4f_get_y(eye);
  camera_state->pos[2] = -simd4f_get_z(eye);

//  gLog ("ViewMat");
//  gLog ("%f, %f, %f, %f", mtx->x[0], mtx->x[1], mtx->x[2], mtx->x[3]);
//  gLog ("%f, %f, %f, %f", mtx->y[0], mtx->y[1], mtx->y[2], mtx->y[3]);
//  gLog ("%f, %f, %f, %f", mtx->z[0], mtx->z[1], mtx->z[2], mtx->z[3]);
//  gLog ("%f, %f, %f, %f", mtx->w[0], mtx->w[1], mtx->w[2], mtx->w[3]);
}

void CameraState_CalcToMatrix(CameraState* camera_state, simd4x4f* mtx) {
  float sp = sin(camera_state->pitch);
  float cp = cos(camera_state->pitch);
  float ch = cos(camera_state->heading);
  float sh = sin(camera_state->heading);

  const float d = 10.0f;

  simd4f eye = simd4f_create (camera_state->pos[0], camera_state->pos[1], camera_state->pos[2], 1.f);
  simd4f forward = simd4f_create (-cp * ch, -sp, cp * sh, 0.f);
  simd4f right = simd4f_cross3 (forward, simd4f_create (0.f, 1.f, 0.f, 1.f));
  simd4f up = simd4f_cross3(right, forward);
  simd4f center = simd4f_add(simd4f_mul(forward, simd4f_splat(d)), eye);

  camera_state->forward[0] = -simd4f_get_x(forward);
  camera_state->forward[1] = -simd4f_get_y(forward);
  camera_state->forward[2] = -simd4f_get_z(forward);

  camera_state->right[0] = simd4f_get_x(right);
  camera_state->right[1] = simd4f_get_y(right);
  camera_state->right[2] = simd4f_get_z(right);

  simd4x4f_lookat(mtx, eye, center, up);
}

inline void CameraState_Update(
    CameraState* camera_state,
    float dt,
    float mouse_dx,
    float mouse_dy,
    float accel[3]) {
  const float mouse_sensitivity = 20.0f;

  camera_state->heading -= dt * mouse_dx * mouse_sensitivity * M_PI / 180.f;
  if (camera_state->heading < -M_PI) {
    camera_state->heading += M_PI * 2.f;
  } else if (camera_state->heading > M_PI) {
    camera_state->heading -= M_PI * 2.f;
  }
  camera_state->pitch += dt * mouse_dy * mouse_sensitivity * M_PI / 180.f;
  if (camera_state->pitch < -M_PI * 0.49) {
    camera_state->pitch = -M_PI * 0.49;
  } else if (camera_state->pitch > M_PI * 0.49) {
    camera_state->pitch = M_PI * 0.49;
  }

  for (int i = 0; i < 3; i++) {
    camera_state->vel[i] += dt * accel[0] * camera_state->forward[i] + dt * accel[2] * camera_state->right[i] + dt * accel[1] * camera_state->up[i];
    camera_state->pos[i] += dt * camera_state->vel[i];
    camera_state->vel[i] = camera_state->vel[i] * 0.1;
  }

  CameraState_CalcToMatrix(camera_state, &camera_state->mtxView);
}