/* See {float_image_align.h}. */
/* Last edited on 2009-07-03 17:10:07 by stolfi */

#define _GNU_SOURCE
#include <math.h>
#include <limits.h>
#include <assert.h>
#include <string.h>
 
#include <bool.h>
#include <r2.h>
#include <i2.h>
#include <jsmath.h>
#include <affirm.h>
#include <float_image.h>
#include <float_image_align.h>
#include <sve_minn.h>
#include <wt_table.h>

/* IMPLEMENTATIONS */

void float_image_align_single_scale
  ( int ni,                            /* Number fo images to align. */
    int scale,                         /* Image scale. */  
    float_image_align_mismatch_t *f2,  /* Function that evaluates the mismatch between the images. */
    int hwx,                           /* Half-width of comparison window. */
    double wx[],                       /* Horizontal weights for comparison. */
    int hwy,                           /* Half-height of comparison window. */
    double wy[],                       /* Vertical weights for comparison. */
    r2_t p[],                          /* (IN/OUT) Corresponding points in each image. */
    r2_t adj_rad                       /* Adjustment increments along each axis. */
  )
  {
    int maxEvals = 100;
    bool_t debug = TRUE;
    
    /* Trivial case: */
    if (ni < 2) { return; }
    
    r2_t p0[ni]; /* Saved initial guess. */

    int nv = 2*(ni-1); /* Number of variables to optimize. */

    /* The optimization variables {x[0..nv-1]} are the coordinates
      of the displacements from {p0[0..ni-2]} to {p[0..ni-2]},
      divided by {step}. The last displacement, from {p0[ni-1]} to
      {p[ni-1]}, is always such that the sum of the displacements is
      zero. */
    
    /* These functions assume that the initial guess was saved in {p0[0..ni-1]}: */ 
    
    auto void points_to_vars(r2_t q[], double y[]);
      /* Stores the displacements {q[0..ni-2]-p0[0..ni-2]} into {y[0..nv-1]}.
        Assumes that {SUM{q[i]-p0[i] : i in 0..ni-1} == (0,0)}. */
 
    auto void vars_to_points(double y[], r2_t q[]);
      /* Stores {y[0..nv-1]} plus {p0[0..ni-2]} into {q[0..ni-2]}
        Sets {q[ni-1]} so that {SUM{q[i]-q0[i] : i in 0..ni-1} == (0,0)}. */
      
    auto double sve_goal(int nx, double x[]);
      /* Computes the minimization goal function from the given argument {x[0..nv-1]}.
        Expects {nx == nv}. Also sets {p[0..ni-1]}. */
    
    /* Save initial guess {p} in {p0}: */
    { int i; for (i = 0; i < ni; i++) { p0[i] = p[i]; } }
    
    /* Compute the optimum displacements: */
    double z[nv];
    points_to_vars(p, z);
    sve_minn_iterate
      ( nv, &sve_goal, NULL, z, 
        /*rIni:*/ 0.5, 
        /*rMin:*/ 0.05, 
        /*rMax:*/ 1.0, 
        /*dMax:*/ 3.0,
        maxEvals,
        debug
      );
    vars_to_points(z, p);
    
    return;

    void points_to_vars(r2_t q[], double y[])
      { int i, j;
        for (i = 0; i < ni-1; i++)
          { for (j = 0; j < 2; j++)
              { y[2*i + j] = (q[i].c[j] - p0[i].c[j])/adj_rad.c[j]; }
          }
      }
 
    void vars_to_points(double y[], r2_t q[])
      { int i, j;
        q[ni-1] = p0[ni-1];
        for (i = 0; i < ni-1; i++)
          { for (j = 0; j < 2; j++)
              { double d = y[2*i + j]*adj_rad.c[j];
                q[i].c[j] = p0[i].c[j] + d;
                q[ni-1].c[j] -= d;
              }
          }
      }
      
    double sve_goal(int nx, double x[])
      { assert(nx == nv);
        /* Convert variables {x[0..nx-1]} to displacements {p[0..ni-1]}: */
        vars_to_points(x, p);
        /* Evaluate the client function: */
        double Q2 = f2(ni, scale, hwx, wx, hwy, wy, p);
        return Q2;
      }
  }

double float_image_align_rel_disp_sqr(int ni, r2_t p[], r2_t q[], r2_t *r)
  {
    double d2 = 0.0;
    int i, j;
    for (i = 0; i < ni; i++)
      { for (j = 0; j < 2; j++)
          { double dij = (p[i].c[j] - q[i].c[j])/r->c[j];
            d2 += dij*dij;
          }
      }
    return d2;
  }