/* Last edited on 2012-12-26 17:38:23 by stolfilocal */
/* Procedures for filtering multidimensional signals with {float}-valued samples. */

package minetto.utils;

import java.lang.Math;
import java.lang.Float;
import java.util.Arrays;
import minetto.utils.FloatSignal;

public class FloatSignalFilter {

    /* === BINARY MULTISCALE DECOMPOSITION ==================================== */
    
    /* Multiscale signal processing is based on two basic procedures,
    /downsampling/ and /upsampling/ (roughly, halving and doubling the
    number of samples along each domain dimension). In each case one
    must use filters to avoid aliasing of high frequencies to low
    frequencies (in downsampling) and introduction of spurious high
    frequencies (in upsampling).
    
    A filter is just a list of {nw} "tap weights".  In a downsampling filter,
    these weights used to combine {nw} consecutive samples of the old signal into one 
    sample of the new signal. They must add to 1.
    
    An upsampling filter is used in the same way; however, for each new
    sample only half of the taps will have an old sample to connect to.
    Therefore the even tap weights must add to 1, and the odd tap weights
    must add to 1.
    
    In either case, the weights must be symmetrical under reversal.
    The resulting samples will be centered on the filter window.
    As a consequence, the sample slots of the coarser (small) sequence
    will be aligned with every other sample slot of the finer (large)
    sequence -- either at their centers or at their low ends, depending
    on the parity of the filter's size. See the following diagram:

      For filters with even number of taps (say, 4):

      | up filter tap indices (odd)      ............0.......2................    
      | up filter tap slots (odd)        ........+-------+-------+............
      | up filter tap indices (even)     ................1.......3............
      | up filter tap slots (even)       ............+-------+-------+........
      |                                                                    
      | small signal sample indices      ....0.......1.......2.......3........
      | small signal sample slots        +-------+-------+-------+-------+....
      | large signal sample slots        +---+---+---+---+---+---+---+---+....
      | large signal sample indices      ..0...1...2...3...4...5...6...7......
      |                                                                    
      | dn filter tap slots              ............+---+---+---+---+........
      | dn filter tap indices            ..............0...1...2...3..........
                        
      For filters with an odd number of taps (say, 5):
                        
      | up filter tap indices (odd)      ................1.......3............
      | up filter tap slots (odd)        ............+-------+-------+........
      | up filter tap indices (even)     ............0.......2.......4........
      | up filter tap slots (even)       ........+-------+-------+-------+....
      |                                                                    
      | small signal sample indices      ....0.......1.......2.......3........
      | small signal sample slots        +-------+-------+-------+-------+--..
      | large signal sample slots        ..+---+---+---+---+---+---+---+---+..
      | large signal sample indices      ....0...1...2...3...4...5...6...7....
      |                                                                    
      | dn filter tap slots              ..........+---+---+---+---+---+......
      | dn filter tap indices            ............0...1...2...3...4........

      Note that if the parity of the filter width does not match the
      parity of the large signal's length (number of samples) then the last
      sample of the large signal will be lost on downsampling, and
      cannot be decently reconstructed on upsampling.

    */

    /* === DOWNSAMPLING ===================================================== */
    
    /* Weights for downsampling with each filter size: */
    /* They must add to exactly 1.0. */
    /* !!! Not sure these are optimal !!! */
    static final double dn_weights[][] = {
        { },
        { 1.0/ 2,  1.0/ 2 },
        { 1.0/ 4,  2.0/ 4,  1.0/ 4 },
        { 1.0/ 8,  3.0/ 8,  3.0/ 8,  1.0/ 8 },
        { 1.0/16,  4.0/16,  6.0/16,  4.0/16,  1.0/16 },
        { 1.0/32,  5.0/32, 10.0/32, 10.0/32,  5.0/32,  1.0/32 },
    };
        
    /* ----------------------------------------------------------------------
    Downsamples a multidimensional signal {sig_old} along a specified
    dimension {rdn}, using a filter with {filter_size} taps.
    Returns the resulting signal {sig_new}.
    
    In principle, {sig_new} it will have one sample for every two
    samples of {sig_old} that are consecutive in the direction {rdn};
    but the result may be shaved or padded slightly on both extremes of
    that direction, and these adjustments may depend on the parity of
    {filter_size}.  More precisely, the size {n_new} of {sig_new}
    along that dimension will be  {downsampled_signal_size(n_old,filter_size)},
    where where {n_old} is {sig_old.size[rdn]}.
    
    If {avg_old} and {avg_new} are null, the procedure assumes that each
    sample of {sig_old} is some sort of `density' averaged over some
    neighborhood of the sample's nominal position. (For example, the
    light energy falling on an element of imaging sensor, divided by the
    element's area) Thus, each sample of the downsampled signal
    {sig_new} will be a weighted average of nearby samples in {sig_old}.
    
    On the other hand,if {avg_old} and {avg_new} are non-null, the
    procedure assumes that {sig_old} is a /variance signal/, where each
    sample slot is the weighted variance of the point-to-point density in that
    neighborhood. In that case, it expects {avg_old} to be the original
    intensity signal corresponding to {sig_old}, and {avg_new} to be the
    downsampled version of {avg_old}, obtained by {downsample} with the
    same filter order; and it will add to each sample of {sig_new} the
    weighted variance of the samples of {avg_old} with respect to the
    sample of {avg_new}.
    
    The signals {avg_old} and {avg_new}, if, given, must have the same
    size as {sig_old} (and {sig_new}) along every domain dimension
    except dimension {rdn}. In that direction, the size of {avg_old}
    must be {n_old}, and that of {avg_new} must be {n_new}. */
    
    public static FloatSignal downsample (
        FloatSignal sig_old,
        int rdn, 
        int filter_size,
        FloatSignal avg_old,
        FloatSignal avg_new
      ) {
      
        /* Decide if we are downsampling a variance signal instead of an intensity one: */
        boolean is_var_signal = (avg_old != null);
        assert(is_var_signal == (avg_new != null));

        /* Validate the array sizes and indexing parameters: */
        assert(sig_old.is_consistent());
        if (is_var_signal) { assert(avg_old.is_consistent() && avg_new.is_consistent()); }

        /* Get number of dimensions, validate {rdn}: */
        int nd = sig_old.nd;
        if (is_var_signal) { assert((avg_old.nd == nd) && (avg_new.nd == nd)); }
        assert((rdn >= 0) && (rdn < nd));
        
        /* Compute the size of the original and downsampled signal along the filtering axis: */
        int n_old = sig_old.size[rdn];
        int n_new = downsampled_signal_size(n_old, filter_size); 
        
        /* Allocate the new signal: */
        int[] size_new = Arrays.copyOf(sig_old.size, nd);
        size_new[rdn] = n_new;
        FloatSignal sig_new = new FloatSignal(nd, size_new);
        assert(sig_new.is_consistent()); /* Paranoia is good. */
                
        /* Check if sizes match and whether the result is empty: */
        boolean no_samples = false;
        for (int r = 0; r < nd; r++) {
            assert(sig_new.size[r] == (r == rdn ? n_new : sig_old.size[r]));
            if (sig_new.size[r] == 0) { 
                no_samples = true;
                if (is_var_signal) {
                    assert(avg_old.size[r] == sig_old.size[r]);
                    assert(avg_new.size[r] == sig_new.size[r]);
                }
            }
        }
        
        if (no_samples) { /* Nothing to do: */ return sig_new; }

        for (int xn = 0; xn < n_new; xn++) {
            System.err.println("  xn = " + xn);
            /* Compute the slice of {sig_new} where {ix[rdn] == xn}: */

            /* Select the filter to use for this particular slice (may be different near ends): */
            double[] weights = select_downsampling_filter(xn, filter_size, n_old);

            /* Get index of slice of {sig_old} corresponding to filter tap 0 for slice {xn} of {sig_new}: */
            int xo_min = downsampling_tapped_sample_index(xn, 0, weights.length);

            /* Get start positions and step vectors of those slices: */
            int[] pos = new int[4];        /* Positions in {sig_old,sig_new,avg_old,avg_new}. */
            int[][] step = new int[4][];   /* Positions increments of {sig_old,sig_new,avg_old,avg_new}. */
            step[0] = sig_old.step; pos[0] = sig_old.start + sig_old.step[rdn]*xo_min; 
            step[1] = sig_new.step; pos[1] = sig_new.start + sig_old.step[rdn]*xn;     
            if (is_var_signal) {
                step[2] = avg_old.step; pos[2] = avg_old.start + avg_old.step[rdn]*xo_min; 
                step[3] = avg_new.step; pos[3] = avg_new.start + avg_new.step[rdn]*xn;
            } else {
                step[2] =null; pos[2] = 0; 
                step[3] =null; pos[3] = 0;
            }
           
            /* Get the size of the slices where index {ix[kdn]} is fixed: */
            int[] slice_size = Arrays.copyOf(sig_old.size, nd);
            slice_size[rdn] = 1;

            /* Scan all valid combinations of the indices other than dimension {rdn}: */
            int ix[] = new int[nd]; Arrays.fill(ix,0); 
            
            boolean exhausted = false;
            while (! exhausted) {
                /* Let {ix!a} stand for the index vector {ix} with
                  {ix[rdn]} replaced by {a}. In this iteration we compute
                  one new sample whose indices are {ix!xn}. For that, we
                  combine the old samples whose indices are {ix!xo}
                  where {xo} ranges from {xo_min} to {xo_min +
                  w.length-1}. At this point, {pos[1],pos[3]} are the
                  positions of samples in {sig_new,avg_new} with indices
                  {ix!xn}, and {pos[0],pos[2]} are the positions of
                  samples in {sig_old,avg_old} with indices
                  {ix!xo_min}. */
                
                //System.err.println("    ix = [" + ix[0] + "," + ix[1] + "," + ix[2] + "]");
                int sig_old_p = pos[0];
                int avg_old_p = pos[2];
                
                /* If we are downsampling a variance signal, get the mean intensity {a_new} in the window: */
                double a_new = ( is_var_signal ?  (double)avg_new.smp[pos[3]] : 0.0 );

                double sumw = 0;
                double sumvw = 0;
                int xo = xo_min;
                for (int kw = 0; kw < weights.length; kw++) {
                    assert((xo >= 0) && (xo <n_old));
                    /* Compute the weight {w} from the window: */
                    double w = weights[kw];
                    /* Get the matching sample value from {sig_old}, advance {sig_old_p}: */
                    double v = sig_old.smp[sig_old_p]; 
                    sig_old_p += sig_old.step[rdn];
                    if (is_var_signal) {
                        /* Add the mean-shift term, advance {avg_old_p}: */
                        double a_old = avg_old.smp[avg_old_p];
                        avg_old_p += avg_old.step[rdn];
                        v += (a_old - a_new)*(a_old - a_new);
                    }
                    xo++; 
                    /* Accumulate the old sample term: */
                    sumw += w;
                    sumvw += v*w;
                }
                assert(sumw == 1.0); /* Yes, exactly. */
                /* Compute sew sample {s_new} and save in new signal: */
                float s_new = (float)(sumvw/sumw);
                sig_new.smp[pos[1]] = s_new;
                
                /* Move to the next {ix!0} combination, adjust positions of samples: */
                exhausted = FloatSignal.next_index_tuple(nd, ix, slice_size, pos, step);
            }
        }
        return sig_new;
    }

    /* ----------------------------------------------------------------------
    Assumes that a filter with {filter_size} taps is being used to
    compute sample {index_new} the downsampled version {sig_new} of some
    signal {sig_old}. This procedure returns the index of the sample in
    {sig_old} that corresponds to tap {k} of the filter. */
    public static int downsampling_tapped_sample_index(int index_new, int k, int filter_size) {
        /* The following formula should work in all cases: */
        return 2*index_new - (filter_size - 1)/2 + k;
    }
     
    /* === UPSAMPLING ======================================================= */

    /* Weights for upsampling: */
    /* They must add to exactly 1.0. */
    /* !!! Not sure these are the right ones to use !!! */
    static final double up_weights[][] = {
        { 1.0/ 2,  1.0/ 2 },
        { 1.0/ 4,  2.0/ 4,  1.0/ 4 },
        { 1.0/ 8,  3.0/ 8,  3.0/ 8,  1.0/ 8 },
        { 1.0/16,  4.0/16,  6.0/16,  4.0/16,  1.0/16 },
        { 1.0/32,  5.0/32, 10.0/32, 10.0/32,  5.0/32,  1.0/32 },
    };
        
    /* ----------------------------------------------------------------------
    Upsamples an signal {sig_old} along a specified dimension {rdn},
    using a filter with {filter_size} elements, yielding a new signal {sig_new}.  
    Can be used to expand both intensity signals (with linear scale) and
    variance signals. 
    
    The upsampled signal {sig_new} will have the same size along every
    domain dimension except dimension {rdn}. In that direction, it will
    have the size {n_new} requested by the user. That size must be
    compatible with {downsample}; namely, {n_new} must be
    {downsampled_signal_size(n_old, filter_size)}, where {n_old =
    sig_old.size[rdn]}.  Thus there are only two possible values for
    {n_new}, differing by 1 sample slot. 
    
    In principle, {sig_new} will have two samples, consecutive in the
    direction {rdn}, for every sample of {sig_old}; but it may
    be shaved or padded slightly on both extremes of that direction, and
    these adjustments may depend on the parity of {filter_size}. */
    public static FloatSignal upsample(
        FloatSignal sig_old,
        int n_new,
        int rdn, 
        int filter_size
      ) {
      
        /* Validate the array sizes and indexing parameters: */
        assert(sig_old.is_consistent());

        /* Get number of dimensions, validate {rdn}: */
        int nd = sig_old.nd;
        assert((rdn >= 0) && (rdn < nd));
        
        /* Compute the size of the original and validate requested size: */
        int n_old = sig_old.size[rdn];
        assert(n_old == downsampled_signal_size(n_new, filter_size)); 
        
        /* Allocate the new signal: */
        int[] size_new = Arrays.copyOf(sig_old.size, nd);
        size_new[rdn] = n_new;
        FloatSignal sig_new = new FloatSignal(nd, size_new);
        assert(sig_new.is_consistent()); /* Paranoia is good. */
                
        /* Check if sizes match and whether the result is empty: */
        boolean no_samples = false;
        for (int r = 0; r < nd; r++) {
            assert(sig_new.size[r] == (r == rdn ? n_new : sig_old.size[r]));
            if (sig_new.size[r] == 0) { no_samples = true; }
        }
        
        if (no_samples) { /* Nothing to do: */ return sig_new; }

        for (int xn = 0; xn < n_new; xn++) {
            /* Compute the slice of {sig_new} where {ix[rdn] == xn}: */

            /* Select the filter to use for this particular slice (may be different near ends): */
            double[] weights = select_upsampling_filter(xn, filter_size, n_old);

            /* Find the first tap {kwmin} of the filter that is used for sample {xn} of {sig_new}: */
            int xo_min_x2 = upsampling_tapped_sample_index_x2(xn, 0, weights.length);
            int kwmin = (xo_min_x2 % 2);
            /* Get index {xo_min} of slice of {sig_old} tapped by that tap: */
            int xo_min = (xo_min_x2 + kwmin)/2;

            /* Get start positions and step vectors of those slices: */
            int[] pos = new int[2];        /* Positions in {sig_old,sig_new,avg_old,avg_new}. */
            int[][] step = new int[2][];   /* Positions increments of {sig_old,sig_new,avg_old,avg_new}. */
            step[0] = sig_old.step; pos[0] = sig_old.start + sig_old.step[rdn]*xo_min; 
            step[1] = sig_new.step; pos[1] = sig_new.start + sig_old.step[rdn]*xn;     
            
            /* Get the size of the slices where index {ix[kdn]} is fixed: */
            int[] slice_size = Arrays.copyOf(sig_old.size, nd);
            slice_size[rdn] = 1;

            /* Scan all valid combinations of the indices other than dimension {rdn}: */
            int ix[] = new int[nd]; Arrays.fill(ix,0); 
            
            boolean exhausted = false;
            while (! exhausted) {
                /* Let {ix!a} stand for the index vector {ix} with
                  {ix[rdn]} replaced by {a}. In this iteration we compute
                  one new sample whose indices are {ix!xn}. For that, we
                  combine the old samples whose indices are {ix!xo}
                  where {xo} ranges from {xo_min} to {xo_min +
                  w.length-1}. At this point, {pos[1]} is the position
                  of samples in {sig_new} with indices {ix!xn}, and
                  {pos[0]} is the position of sample in {sig_old} with
                  indices {ix!xo_min}. */
                
                int sig_old_p = pos[0];
                
                double sumw = 0;
                double sumvw = 0;
                int xo = xo_min;
                for (int kw = kwmin; kw < weights.length; kw += 2) {
                    assert((xo >= 0) && (xo <n_old));
                    /* Compute the weight {w} from the window: */
                    double w = weights[kw];
                    /* Get the matching sample value from {sig_old}, advance to next one: */
                    double v = sig_old.smp[sig_old_p]; 
                    sig_old_p += sig_old.step[rdn];
                    xo++;
                    /* Accumulate it: */
                    sumw += w;
                    sumvw += v*w;
                }
                assert(sumw == 1.0); /* Yes, exactly. */
                /* Compute average and save in new signal: */
                float a_new = (float)(sumvw/sumw);
                sig_new.smp[pos[1]] = a_new;
                
                /* Move to the next {ix!0} combination, adjust positions of samples: */
                exhausted = FloatSignal.next_index_tuple(nd, ix, slice_size, pos, step);
            }
        }
        return sig_new;
    }
    
    /* ----------------------------------------------------------------------
    Returns the weight table for a symmetric upsampling filter that can
    be used to compute the new sample {index_new} from a signal with
    size {n_old} so that all taps of the appropriate parity fall on
    existing old samples. The filter size will be at most {filter_size}
    and will have the same parity as it. If there is no such filter,
    returns {null}. */
    public static double[] select_upsampling_filter(int index_new, int filter_size, int n_old) {
        int msz = max_upsampling_filter_size(index_new, filter_size, n_old);
        if (msz <= 0) { return null; }
        assert((msz <= filter_size) && (((filter_size - msz) % 2) == 0));
        return up_weights[msz];
    }
    
    /* ----------------------------------------------------------------------
    Assumes that a filter with {filter_size} taps is being used to
    compute sample {index_new} of the upsampled version {sig_new} of
    some signal {sig_old}. This procedure returns the index of the
    sample in {sig_old} that corresponds to tap {k} of the filter. Since
    that index may not be an integer, the procedure actually returns
    twice it. Thus the old sample exists, and the tap is used, only if
    the result is even. */
    public static int upsampling_tapped_sample_index_x2(int index_new, int k, int filter_size) {
        /* The following formula should work for both cases: */
        return index_new - (filter_size - 1) + 2*k;
    }
    
    /* === FILTER SIZE ADJUSTMENT NEAR EDGES ================================ */
    
    /*  The procedures in this section should be used to reduce the sizes of the
    downsampling and upsampling filters */

    /* ----------------------------------------------------------------------
    Returns the maximum downsampling filter size that can be used to compute sample
    {index_new} of the downsampled signal, given that the original signal had
    {n_old} samples, and that far from the edges one is using a filter
    with {filter_size} taps. */
    public static int max_downsampling_filter_size(int index_new, int filter_size, int n_old) {
        /* Get the min and max indices of samples in {sig_old} tapped by the requested filter: */
        int min_index_old = downsampling_tapped_sample_index(index_new, 0, filter_size);
        int max_index_old = min_index_old + filter_size - 1;
        /* Figure out how much we must clip the filter on both sides to keep it symmetrical: */
        int clip = Math.max(0, Math.max(0 - min_index_old, max_index_old - (n_old - 1)));
        return Math.max(0, filter_size - 2*clip);
    }

    /* ----------------------------------------------------------------------
    Returns the maximum upsampling filter size that can be used to compute sample
    {index_new} of the upsampled signal, given that the original signal had
    {n_old} samples, and that far from the edges one is using a filter
    with {filter_size} taps. */
    public static int max_upsampling_filter_size(int index_new, int filter_size, int n_old) {
        /* Get the min and max indices of samples in {sig_old} tapped by the requested filter: */
        int min_index_old_x2 = upsampling_tapped_sample_index_x2(index_new, 0, filter_size);
        int max_index_old_x2 = min_index_old_x2 + filter_size - 1;
        /* If these taps have the wrong parity, take the next ones with correct parity: */
        if ((min_index_old_x2 % 2) == 1) { min_index_old_x2++; }
        if ((max_index_old_x2 % 2) == 1) { max_index_old_x2--; }
        /* Figure out how much we must clip the filter on both sides to keep it symmetrical: */
        int clip_x2 = Math.max(0, Math.max(0 - min_index_old_x2, max_index_old_x2 - 2*(n_old - 1)));
        assert((clip_x2 % 2) == 0);
        return Math.max(0, filter_size - clip_x2);
    }

    /* === MAPPING BETWEEN UPSAMPLED/DOWNSAMPLED DOMAINS ======================== */
    
    /* The procedures in this section allow the correct mapping of
    points between the domains of two signals which are related by
    upsampling or downsampling. 
    
    For these procedures, the domain of a signal {sig} is a box in {\RR^{nd}} 
    extending along each each axis {r} from 0 to {sig.size[r]}. The sample with
    index tuple {ix[0..nd-1]} is interpreted as a hypercube with unit side,
    which in each axis {r} spans from {ix[r]} to {ix[r]+1}, centered
    at {ix[r]+0.5} Thus the vectors {(0,0,...,0)} and {sig.size}
    are the opposite corners of the signal's domain.
     
    The procedures in this section assume that one of the signals
    {small} was created from another signal {large} through the use
    {downsample} (or, alternatively, that {large} was created from
    {small} by {upsample}) with a given {filter_order}, along an axis
    {rop}. The mapping is always a scaling by 2 or 1/2 along the axis
    {rop}, possibly with a small shift of up to one sample slot. The
    shift depending on the details of the filtering method and on
    whether the original image size was even or odd. In any case a
    distance {d} along axis {rop} in the domain of {small} corresponds
    to distance exactly {2*d} in the domain of {large}. These procedures
    can be applied even to coordinates that lie outside the signal's
    domain. */

    /* ----------------------------------------------------------------------
    Maps the coordinate {z_small} on the the downsampling axis {rop} 
    from the domain of a downsampled signal {small} to the corresponding coordinate
    in the domain of the original signal {large}. Requires the sizes {n_small,n_large} (in
    sample slots) of the two signals, along that coordinate axis; and the
    size of the downsampling filter. */
    public static double map_coord_from_downsampled_to_original(double z_small, int n_small, int n_large, int filter_size) {
        assert(n_large >= 0);
        assert(n_small >= 0);
        assert(n_small == downsampled_signal_size(n_large, filter_size)); 
        double shift = 0.5*(filter_size % 2);
        return 2*z_small + shift;
    }
        
    /* ----------------------------------------------------------------------
    Maps a coordinate {z_large} (either X or Y) of a point in the domain of an original signal 
    {old} to the same coordinate of the corresponding point of the downsampled signal {small}.
    Requires the sizes {n_small,n_large} (in sample slots) of the two signals, along the same 
    coordinate axis. */
    public static double map_coord_from_original_to_downsampled(double z_large, int n_large, int n_small, int filter_size) {
        assert(n_large >= 0);
        assert(n_small >= 0);
        assert(n_small == downsampled_signal_size(n_large, filter_size)); 
        double shift = 0.5*(filter_size % 2);
        return (z_large - shift)/2;
    }
        
}