#version 400

in vec3 pos;
in vec3 normal;


out vec3 v2f_pos;

// out vec3  v2f_pos;
// out vec3  v2f_normal;
// out float v2f_cos_camera_angle;

uniform vec3  camera_translation;
uniform mat3  camera_inv_mat_R;
uniform vec2  sensor_resolution;
uniform vec3  sensor_radial_distortion;
uniform vec2  sensor_tangential_distortion;
uniform vec2  sensor_skew;
uniform vec2  sensor_principal_point;
uniform float sensor_focal_length;


void project_onto_camera(in vec3 pos, out vec3 pixel_coordinates)
{
    vec3 camera_coordinates = camera_inv_mat_R * ( pos - camera_translation );
    float X = camera_coordinates.x / camera_coordinates.z;
    float Y = camera_coordinates.y / camera_coordinates.z;

    float X_2 = X * X;
    float Y_2 = Y * Y;
    float XY  = X * Y;

    float r = sqrt(X_2 + Y_2);
    float r_2 = r * r;
    float r_4 = r_2 * r_2;
    float r_6 = r_2 * r_4;

    vec3 k = sensor_radial_distortion;

    // radial distortion coefficient
    float K_term = 1 + k.x * r_2 + k.y * r_4 + k.z * r_6;

    // tangential distortion coefficient
    vec2 p = sensor_tangential_distortion;
    float P_term_x = p.x * (r_2 + 2 * X_2) + 2 * p.y * XY;
    float P_term_y = p.y * (r_2 + 2 * Y_2) + 2 * p.x * XY;

    float x = X * K_term + P_term_x ;
    float y = Y * K_term + P_term_y ;

    float f = sensor_focal_length;
    vec2  b = sensor_skew;
    vec2  c = sensor_principal_point;

    float u = c.x + f * x + x * b.x + y * b.y;
    float v = c.y + f * y;

    if (X < -1 || X > 1 || Y < -1 || Y > 1)
    {
        u = -10000;
        v = -10000;
    }

    // Set output
    pixel_coordinates = vec3(u, v, camera_coordinates.z);
}


void main()
{
    v2f_pos    = pos;
    // v2f_normal = normalize(normal);

    vec3 pixel_coordinates;
    project_onto_camera(pos, pixel_coordinates);

    pixel_coordinates /= vec3(sensor_resolution, 100.0);
    pixel_coordinates.xy = 2 * (pixel_coordinates.xy - 0.5);

    gl_Position = vec4(pixel_coordinates, 1.0);
}
