#version 400

in vec3 v2f_pos;

out vec4 out_color;

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()
{
    vec3 pixel_coordinates;
    project_onto_camera(v2f_pos, pixel_coordinates);
    pixel_coordinates /= vec3(sensor_resolution, 1.0);

    // vec2 uv = (pixel_coordinates.xy - 0.5) * 2;

    //out_color = vec4(texture2D(camera_image_sampler, pixel_coordinates.xy).rgb, 1.0);
    out_color = vec4(pixel_coordinates.z, pixel_coordinates.z, pixel_coordinates.z, 1.0);
}
