/* Last edited on 2015-01-20 16:47:23 by stolfilocal */

#include <pz_match.h>
#include <pz_proc.h>
#include <pz_segment.h>
#include <pz_i2_chain.h>
#include <pz_double_chain.h>
#include <pz_fourier.h>
#include <pz_window.h>
#include <pz_geo.h>
#include <pz_types.h>
#include <pz_r3_chain.h>
#include <pz_r3_chain_match.h>

#include <nget.h>
#include <fget.h>
#include <r3.h>
#include <r4.h>
#include <r4x4.h>
#include <math.h>
#include <stdio.h>

void pz_r3_chain_match_find_best
  ( pz_r3_chain_t *a,
    pz_r3_chain_t *b,
    double maxDistSqr,
    bool_t removeUnmatchedEnds,
    REF *pz_match_t mt  /* (OUT) */
    double *avgDist, /* (OUT) */
    double *length, /* (OUT) */
    double *matchedLength, /* (OUT) */
    REF *pz_match_cost_matrix_t rCost, /* (WORK) */
  )

  TYPE Pair == pz_match_pair;


  double  pz_r3_chain_step_dist_sqr 
  ( Pair *p,
    Pair *q 
  )
    /*
      The mean square distance between the curve steps
      {a[p[0]]->a[q[0]]} and {b[p[1]]->b[q[1]]}. */
    {

      { /* with */
        r3_t a1 = a[p->el[0]], b1 = b[p->el[1]];
        r3_t a2 = a[q->el[0]], b2 = b[q->el[1]];
        double mdSqr = 0.5 * (r3_dist_sqr(a1,b1) + r3_dist_sqr(a2,b2));
        /* do */
        return min(mdSqr, maxDistSqr);
      }
    } /* pz_r3_chain_step_dist_sqr */


  double  pz_r3_chain_step_length 
  ( Pair *p,
    Pair *q 
  )
    /*
      The length of the step {p->q}, i.e. the mean length of
      {a[p[0]]--a[q[0]]} and {b[p[1]]--b[q[1]]}. */
    {

      { /* with */
        r3_t a1 = a[p->el[0]];
        r3_t a2 = a[q->el[0]];
        r3_t b1 = b[p->el[1]];
        r3_t b2 = b[q->el[1]];
        double La = r3_dist(a1,a2);
        double Lb = r3_dist(b1,b2);
        /* do */
        return  (La + Lb) * 0.5;;
      }
    } /* pz_r3_chain_step_length */


  double  pz_r3_chain_step_intg_dist_sqr 
  ( Pair *p,
    Pair *q 
  )
    /*
      A {pz_match_step_function_t} whose value is the integrated square distance
      of the step {p -> q}, i.e. between the curve steps
      {a[p[0]]->a[q[0]]} and {b[p[1]]->b[q[1]]}. */
    {
      return pz_r3_chain_step_dist_sqr(p, q) * pz_r3_chain_step_length(p, q)
    } /* pz_r3_chain_step_intg_dist_sqr */


  bool_t  pz_r3_chain_step_is_matched
  ( Pair *p,
    Pair *q 
  )
    /*
      A {pz_match_step_predicate_t} that returns TRUE if the step {p->q}
      has average curve distance less than {maxDist}. */
    {
      return pz_r3_chain_step_dist_sqr(p, q) < maxDistSqr
    } /* pz_r3_chain_step_is_matched */

  VAR intgDistSqr: double;
  {

    { /* with */
      int na = a->nel;
      int nb = b->nel;
      /* do */
      pz_match_find_best
  (     na, nb,
        stepCostFn = pz_r3_chain_step_intg_dist_sqr,
        /*OUT*/ mt = mt, totCost = intgDistSqr,
        /*WORK*/ rCost = rCost
      );
      if (( removeUnmatchedEnds )){
        mt = pz_match_remove_unmatched_ends(mt^, pz_r3_chain_step_is_matched);
        intgDistSqr = pz_match_integral(mt^,
          lengthFn = pz_r3_chain_step_length,
          stepFn = pz_r3_chain_step_dist_sqr
        );
      };
      affirm((mt^.ne) > 0 );
      length = pz_match_length(mt^, lengthFn = pz_r3_chain_step_length);
      if (( length <= 0.0 )){

        { /* with */
          ??? cp = (mt^.ne) / 2, ctr = mt[cp];
          /* do */
          avgDist = r3_dist_sqr(a[ctr[0]], b[ctr[1]]);
        }
      }else{
        avgDist = sqrt(intgDistSqr/length);
      };
      matchedLength = pz_match_matched_length(mt^,
        lengthFn = pz_r3_chain_step_length,
        isMatched = pz_r3_chain_step_is_matched
      );;
    }
  } /* pz_r3_chain_match_find_best */


void pz_r3_chain_match_refine
  ( pz_r3_chain_t *a,
    pz_r3_chain_t *b,
    double step,
    pz_match_t *mtOld,
    unsigned maxAdjust,
    double critDistSqr,
    double maxDistSqr,
    REF *pz_match_t mt  /* (OUT) */
    double *mismatch, /* (OUT) */
    double *length, /* (OUT) */
    double *matchedLength, /* (OUT) */
    double_vec_t *minAvgDist, /* (OUT) */
    REF *pz_match_cost_matrix_t rCost, /* (WORK) */
  )

  TYPE Pair == pz_match_pair;


  double  pz_r3_chain_step_dist_sqr 
  ( Pair *p,
    Pair *q 
  )
    /*
      The mean square distance between the curve steps
      {a[p[0]]->a[q[0]]} and {b[p[1]]->b[q[1]]}. */
    {

      { /* with */
        r3_t a1 = a[p->el[0]];
        r3_t a2 = a[q->el[0]];
        r3_t b1 = b[p->el[1]];
        r3_t b2 = b[q->el[1]];
        double mdSqr = 0.5 * (r3_dist_sqr(a1,b1) + r3_dist_sqr(a2,b2));
        /* do */
        return min(mdSqr, maxDistSqr);
      }
    } /* pz_r3_chain_step_dist_sqr */


  double  pz_r3_chain_step_mismatch 
  ( Pair *p,
    Pair *q 
  )
    /*
      A {pz_match_step_function_t} whose value is the mismatch
      of the step {p -> q}, i.e. the integrated square
      distance between the curve steps {a[p[0]]->a[q[0]]}
      and {b[p[1]]->b[q[1]]}, minus the critical distance squared,
      all times the step length. */
    {

      { /* with */
        double d2 = pz_r3_chain_step_dist_sqr(p, q) - critDistSqr;
        /* do */
        if ((p->el[0] == q->el[0]) || (p->el[1] == q->el[1])){
          return step * d2 / 2.0
        }else{
          return step * d2;
        };
      };
    } /* pz_r3_chain_step_mismatch */


  bool_t  pz_r3_chain_step_is_matched
  ( Pair *p,
    Pair *q 
  )
    /*
      A {pz_match_step_predicate_t} that returns TRUE if the step {p->q}
      has average curve distance less than {maxDist}. */
    {
      return pz_r3_chain_step_dist_sqr(p, q) < maxDistSqr
    } /* pz_r3_chain_step_is_matched */

  {

    { /* with */
      int na = a->nel;
      int nb = b->nel;
      /* do */
      affirm(na > 0 );
      affirm(nb > 0 );
      pz_match_refine(
        na = na, nb = nb,
        stepCostFn = pz_r3_chain_step_mismatch,
        mtOld = mtOld,
        maxAdjust = maxAdjust,
        /*OUT*/ mt = mt,
        /*OUT*/ totCost = mismatch,
        /*OUT*/ minTotCost = minAvgDist,
        /*WORK*/ rCost = rCost
      );
      affirm( (mt^.ne) > 0 );

      { /* with */
        ??? mt = mt^;
        int m = (mt.ne);
        int stepCount = (mt[m-1][0] - mt[0][0]) + (mt[m-1][1] - mt[0][1]);
        int matchedStepCount = pz_match_matched_step_count(mt, pz_r3_chain_step_is_matched);
        /* do */
        length = ((double)stepCount) * step / 2.0;
        matchedLength = step * ((double)matchedStepCount)/2.0;
      };
      /* Compute the minimum average distance for each pairing length {K}. */
      /* Note that {pz_match_refine} returns total mismatch, not just {intgDistSqr} */
      affirm( minAvgDist[0] == 0.0 );
      for ( K = 1 TO (minAvgDist.ne - 1) ){

        { /* with */
          double Lk = step*((double)K)/2.0;
          /* do */
          minAvgDist[K] = sqrt(max(0.0, minAvgDist[K]/Lk + critDistSqr));
        };
      };
    };
  } /* pz_r3_chain_match_refine */