#include "Camera.h"

#include <assert.h>

#include "string.h"
#include "vectorial/simd4x4f.h"

inline void Camera_Init(Camera* camera) {
  // 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->near = 0.01;
  camera->far = 1000.0;
  camera->fov = 90 * M_PI / 180.f;

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

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

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

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

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

  camera->heading = -45.0 * M_PI / 180.0f;
  camera->pitch = 10 * M_PI / 180.0f;

  memcpy(&camera->mtxView, &mtx_identity, sizeof(camera->mtxView));
  Camera_CalcToMatrix(camera, &camera->mtxView);
  Camera_CalcFromMatrix(camera, &camera->mtxView);
}

void Camera_CalcFromMatrix(Camera* camera, float* mat) {
  simd4x4f mtx;
  simd4x4f_uload(&mtx, mat);

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

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

  camera->heading = atan2(-camera->forward[2], camera->forward[0]);
  camera->pitch = asin(camera->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->pos[0] = -simd4f_get_x(eye);
  camera->pos[1] = -simd4f_get_y(eye);
  camera->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 Camera_CalcToMatrix(Camera* camera, float* mat) {
  float sp = sin(camera->pitch);
  float cp = cos(camera->pitch);
  float ch = cos(camera->heading);
  float sh = sin(camera->heading);

  const float d = 10.0f;

  simd4f eye = simd4f_create (camera->pos[0], camera->pos[1], camera->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->forward[0] = -simd4f_get_x(forward);
  camera->forward[1] = -simd4f_get_y(forward);
  camera->forward[2] = -simd4f_get_z(forward);

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

  simd4x4f mtx;
  simd4x4f_lookat(&mtx, eye, center, up);

  simd4f_ustore4(mtx.x, mat);
  simd4f_ustore4(mtx.y, mat +4);
  simd4f_ustore4(mtx.z, mat +8);
  simd4f_ustore4(mtx.w, mat +12);
}

inline void Camera_Update(
    Camera* camera,
    int width,
    int height,
    float dt,
    float mouse_dx,
    float mouse_dy,
    float accel[3]) {
  assert(camera);
  assert((width > 0) && (height > 0));
  const float w = (float) width;
  const float h = (float) height;
  simd4x4f proj;
  simd4x4f_perspective(&proj, camera->fov, w/h, camera->near, camera->far);
  simd4f_ustore4(proj.x, camera->mtxProj);
  simd4f_ustore4(proj.y, camera->mtxProj +4);
  simd4f_ustore4(proj.z, camera->mtxProj +8);
  simd4f_ustore4(proj.w, camera->mtxProj +12);

  if (mouse_dx != 0.f || mouse_dy != 0.f || accel != NULL) {
    const float mouse_sensitivity = 20.0f;

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

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

    Camera_CalcToMatrix(camera, &camera->mtxView);
  }
}