#include <defocus/pinhole.h>
#include <defocus/base.h>
#include <defocus/image.h>

#include <math.h>

void df_pinhole(const df_image *in_image, float focal_length, float orig_z, float new_z, df_image *out_image)
{
    /* orig_z is the z-coordinate of the original image plane (=> in_image is located there)
     * new_z  is the z-coordinate of the output image plane (=> out_image is located there)
     *
     * We can map from image coordinates to world coordinates, because we know the z coordinate of
     * the input image plane and the focal length of the camera.
     *
     * Let x,y,z be the world coordinates and u, v the image coordintes.
     * The pinhole camera model gives us:
     * u = f/z * x;  v = f/z * y
     * => x = u * z / f
     * => y = v * z / f
     */

    int w, h;
    df_get_image_size(in_image, &w, &h);

    int out_w, out_h;
    df_get_image_size(out_image, &out_w, &out_h);

    double in_z_over_f = orig_z / focal_length;
    double out_f_over_z = focal_length / new_z;

    int prev_out_iy = -1;
    for (int iy = 0; iy < h; ++iy) {
        double v = (double)iy / (double)h;
        double y = in_z_over_f * v;
        double out_v = out_f_over_z * y;

        int out_iy = (int)floor(out_v * (double)out_h);

        int prev_out_ix = -1;
        for (int ix = 0; ix < w; ++ix) {
            double u = (double)ix / (double)w;
            double x = in_z_over_f * u;
            double out_u = out_f_over_z * x;

            int out_ix = (int)floor(out_u * (double)out_w);

            df_color px = df_get_image_pixel(in_image, ix, iy);
            df_set_image_pixel(out_image, out_ix, out_iy, px);

            /* Go back and interpolate between this pixel and the previous one. */
            if ((out_ix - prev_out_ix) > 1) {
                df_color prev_px = df_get_image_pixel(out_image, prev_out_ix, out_iy);
                for (int d = prev_out_ix + 1; d < out_ix; ++d) {
                    double t = (double)(d - prev_out_ix) / (double)(out_ix - prev_out_ix);
                    df_color color = df_lerp_color(prev_px, px, t);
                    df_set_image_pixel(out_image, d, out_iy, color);
                }
            }
            prev_out_ix = out_ix;
        }

        /* Go back and interpolate between this row and the previous one */
        if ((out_iy - prev_out_iy) > 1) {
            for (int y = prev_out_iy + 1; y < out_iy; ++y) {
                double t = (double)(y - prev_out_iy) / (double)(out_iy - prev_out_iy);
                for (int x = 0; x < out_w; ++x) {
                    df_color a = df_get_image_pixel(out_image, x, prev_out_iy);
                    df_color b = df_get_image_pixel(out_image, x, out_iy);
                    df_color color = df_lerp_color(a, b, t);
                    df_set_image_pixel(out_image, x, y, color);
                }
            }
        }
        prev_out_iy = out_iy;
    }
}