#define __BSD_VISIBLE 1
#include "afterburner.h"
#include <azur/gint/render.h>
#include <math.h>

mat3 operator *(mat3 const &A, mat3 const &B)
{
    mat3 C;

    C.x11 = A.x11 * B.x11 + A.x12 * B.x21 + A.x13 * B.x31;
    C.x12 = A.x11 * B.x12 + A.x12 * B.x22 + A.x13 * B.x32;
    C.x13 = A.x11 * B.x13 + A.x12 * B.x23 + A.x13 * B.x33;

    C.x21 = A.x21 * B.x11 + A.x22 * B.x21 + A.x23 * B.x31;
    C.x22 = A.x21 * B.x12 + A.x22 * B.x22 + A.x23 * B.x32;
    C.x23 = A.x21 * B.x13 + A.x22 * B.x23 + A.x23 * B.x33;

    C.x31 = A.x31 * B.x11 + A.x32 * B.x21 + A.x33 * B.x31;
    C.x32 = A.x31 * B.x12 + A.x32 * B.x22 + A.x33 * B.x32;
    C.x33 = A.x31 * B.x13 + A.x32 * B.x23 + A.x33 * B.x33;

    return C;
}

vec3 operator * (mat3 const &M, vec3 const &u)
{
    vec3 v;

    v.x = M.x11 * u.x + M.x12 * u.y + M.x13 * u.z;
    v.y = M.x21 * u.x + M.x22 * u.y + M.x23 * u.z;
    v.z = M.x31 * u.x + M.x32 * u.y + M.x33 * u.z;

    return v;
}

num distance2(vec3 u, vec3 v)
{
    u -= v;
    return u.x * u.x + u.y * u.y + u.z * u.z;
}

num64 distance2_64(vec3 u, vec3 v)
{
    u -= v;
    return num32::dmul(u.x,u.x) + num32::dmul(u.y,u.y) + num32::dmul(u.z,u.z);
}

void plane_update(struct plane *plane)
{
    float c, s;

    sincosf(plane->roll, &s, &c);
    plane->sin_r = s;
    plane->cos_r = c;

    sincosf(plane->pitch, &s, &c);
    plane->sin_p = s;
    plane->cos_p = c;

    sincosf(plane->yaw, &s, &c);
    plane->sin_y = s;
    plane->cos_y = c;

    plane->forward = vec3(
        -plane->sin_y * plane->cos_p,
         plane->cos_y * plane->cos_p,
         plane->sin_p);
    plane->forward_ground = vec3(-plane->sin_y, plane->cos_y, 0);
    plane->right = vec3(plane->cos_y, plane->sin_y, 0);
}

void transform_world2camera(struct plane const *plane, vec3 camera_pos,
    struct ref_point *points, int size)
{
    num cos_r = plane->cos_r;
    num sin_r = plane->sin_r;
    num cos_p = plane->cos_p;
    num sin_p = plane->sin_p;
    num cos_y = plane->cos_y;
    num sin_y = plane->sin_y;

    mat3 m_roll = {
        cos_r,    0,    sin_r,
          0,      1,      0,
       -sin_r,    0,    cos_r,
    };
    mat3 m_pitch = {
          1,      0,      0,
          0,    cos_p, +sin_p,
          0,   -sin_p,  cos_p,
    };
    mat3 m_yaw = {
        cos_y, +sin_y,   0,
       -sin_y,  cos_y,   0,
          0,      0,     1,
    };

    mat3 M = m_roll * (m_pitch * m_yaw);

    for(int i = 0; i < size; i++)
        points[i].pos = M * (points[i].pos - camera_pos);
}

void transform_camera2screen(struct ref_point *points, int size)
{
    for(int i = 0; i < size; i++) {
        vec3 r = points[i].pos;
        r.x = num(azrp_width  / 2) + r.x * SCREEN_SCALING / r.y;
        r.z = num(azrp_height / 2) - r.z * SCREEN_SCALING / r.y;
        points[i].pos = r;
    }
}