package com.elphel.imagej.cuas.rt;

public class TemporalConsolidator {

    /**
     * 1D Cumulative Distribution Function for a Trapezoid centered at 0.
     * Guarantees exact fractional area calculations.
     */
    private static double trapezoidCDF(double x, double w_top, double w_bot) {
        double x0 = -w_bot / 2.0;
        double x1 = -w_top / 2.0;
        double x2 =  w_top / 2.0;
        double x3 =  w_bot / 2.0;

        if (x <= x0) return 0.0;

        // Handle t=0 (Boxcar, no velocity blur)
        if (Math.abs(w_bot - w_top) < 1e-9) {
            if (x >= x3) return w_bot; 
            return (x - x0);
        }

        double m = 1.0 / (x1 - x0); // Rising slope
        double area_rising = 0.5 * m * (x1 - x0) * (x1 - x0);
        double area_flat = 1.0 * (x2 - x1);

        if (x <= x1) {
            return 0.5 * m * (x - x0) * (x - x0);
        } else if (x <= x2) {
            return area_rising + 1.0 * (x - x1);
        } else if (x <= x3) {
            double dx = x - x2;
            return area_rising + area_flat + (1.0 * dx - 0.5 * m * dx * dx);
        }
        
        return area_rising + area_flat + 0.5 * m * (x3 - x2) * (x3 - x2);
    }

    /**
     * Consolidates coarse temporal velocity outputs into a high-resolution grid.
     */
    public static double[][][] consolidateTemporalLayers(
            double[][][] historic_pixel_vel, // [historic_t][pixel][9_velocities]
            double[] weights,                // [historic_t] sums to 1.0
            int width, int height,
            int pixel_decimate, 
            int velocity_decimate, 
            int velocity_radius
    ) {
        int num_pixels = width * height;
        int num_subpixels = pixel_decimate * pixel_decimate;
        int v_dim = 2 * velocity_radius + 1;
        int num_velocities = v_dim * v_dim;

        // Output: [pixel][subpixel][high_res_velocity]
        double[][][] output = new double[num_pixels][num_subpixels][num_velocities];
        int num_history = weights.length;

        for (int p = 0; p < num_pixels; p++) {
            int px = p % width;
            int py = p / width;

            for (int t = 0; t < num_history; t++) {
                double w_t = weights[t];
                if (w_t == 0) continue;

                // Trapezoid width scales with time and velocity decimation
                double blur_width = (double) t / velocity_decimate;
                double w_bot = 1.0 + blur_width;
                double w_top = Math.abs(1.0 - blur_width);
                double total_area = 0.5 * (w_bot + w_top); // Used to normalize area

                for (int vx = -velocity_radius; vx <= velocity_radius; vx++) {
                    for (int vy = -velocity_radius; vy <= velocity_radius; vy++) {
                        
                        // 1. Map High-Res Velocity to parent Coarse Velocity (-1, 0, 1)
                        int coarse_vx = (int) Math.round((double) vx / velocity_decimate);
                        int coarse_vy = (int) Math.round((double) vy / velocity_decimate);
                        
                        // If outside the 3x3 coarse grid, target exceeds tracking limit
                        if (Math.abs(coarse_vx) > 1 || Math.abs(coarse_vy) > 1) continue;
                        
                        int coarse_v_idx = (coarse_vy + 1) * 3 + (coarse_vx + 1);
                        int v_out_idx = (vy + velocity_radius) * v_dim + (vx + velocity_radius);

                        // 2. Calculate the projected center of the past pixel
                        double proj_x = ((double) vx / velocity_decimate) * t;
                        double proj_y = ((double) vy / velocity_decimate) * t;

                        // Identify the source macro-pixel (Simplified: no border checks)
                        int src_px = px - (int) Math.round(proj_x);
                        int src_py = py - (int) Math.round(proj_y);
                        int src_p = src_py * width + src_px;

                        // Calculate the sub-pixel shift within the target macro-pixel
                        double shift_x = proj_x - Math.round(proj_x);
                        double shift_y = proj_y - Math.round(proj_y);

                        // 3. Integrate over Output Spatial Subpixels
                        for (int sx = 0; sx < pixel_decimate; sx++) {
                            // Subpixel boundaries relative to the projected beam center
                            double left_x = ((double) sx / pixel_decimate) - 0.5 - shift_x;
                            double right_x = ((double) (sx + 1) / pixel_decimate) - 0.5 - shift_x;
                            
                            double weight_x = (trapezoidCDF(right_x, w_top, w_bot) - trapezoidCDF(left_x, w_top, w_bot)) / total_area;

                            if (weight_x <= 0) continue;

                            for (int sy = 0; sy < pixel_decimate; sy++) {
                                double top_y = ((double) sy / pixel_decimate) - 0.5 - shift_y;
                                double bottom_y = ((double) (sy + 1) / pixel_decimate) - 0.5 - shift_y;
                                
                                double weight_y = (trapezoidCDF(bottom_y, w_top, w_bot) - trapezoidCDF(top_y, w_top, w_bot)) / total_area;

                                double total_weight = weight_x * weight_y * w_t;

                                if (total_weight > 0) {
                                    int sub_idx = sy * pixel_decimate + sx;
                                    output[p][sub_idx][v_out_idx] += total_weight * historic_pixel_vel[t][src_p][coarse_v_idx];
                                }
                            }
                        }
                    }
                }
            }
        }
        return output;
    }
}



