/* Last edited on 2015-01-20 16:44:54 by stolfilocal */

#include <nget.h>
#include <fget.h>
#include <pz_proc.h>
#include <pz_match.h>
#include <pz_segment.h>
#include <pz_candidate.h>
#include <pz_double_chain.h>
#include <pz_double_chain_match.h>

#include <math.h>
#include <stdio.h>
#include <pz_types.h>

void pz_double_chain_match_find_best
  ( pz_double_chain_t *a,
    pz_double_chain_t *b,
    double step,
    double maxDistSqr,
    double skipDistSqr,
    bool_t removeUnmatchedEnds,
    REF *pz_match_t mt  /* (OUT) */
    double *avgDisc, /* (OUT) */
    double *length, /* (OUT) */
    double *matchedLength, /* (OUT) */
    REF *pz_match_cost_matrix_t rCost, /* (WORK) */
  )

  TYPE Pair == pz_match_pair;


  double  StepDistSqr
  ( Pair *p,
    Pair *q 
  )
    /*
      Computes the mean squared chain distance for one pairing step "p -> q": */
    {

      { /* with */
        ??? dp = a[p[0]] - b[p[1]];
        ??? dq = a[q[0]] - b[q[1]];
        ??? mdSqr = 0.5e0 * (dp*dp + dq*dq);
        /* do */
        return min(mdSqr, maxDistSqr);
      }
    } /* StepDistSqr */


  double  StepIntgDiscSqr
  ( Pair *p,
    Pair *q 
  )
    /*
      Computes the integrated discrepancy squared
      for one pairing step "p -> q": */
    {

      { /* with */
        ??? d2 = StepDistSqr(p, q);
        /* do */
        if (( p[0] == q[0] ) || ( p[1] == q[1] )){
          return step * (d2 + skipDistSqr)/2.0e0
        }else{
          return step * d2;
        };
      }
    } /* StepIntgDiscSqr */


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

  VAR intgDiscSqr: double;
  {

    { /* with */
      ??? NA = (a.ne);
      ??? NB = (b.ne);
      /* do */
      pz_match_find_best
  (     NA, NB,
        stepCostFn = StepIntgDiscSqr,
        /*OUT*/ mt = mt, totCost = intgDiscSqr,
        /*WORK*/ rCost = rCost
      );
      if (( removeUnmatchedEnds )){
        mt = pz_match_remove_unmatched_ends(mt^, StepIsMatched);
        intgDiscSqr = pz_match_sum(mt^, stepFn = StepIntgDiscSqr);
      };
      affirm((mt^.ne) > 0 );

      { /* with */
        ??? m = (mt^.ne);
        ??? na = mt[m-1][0] - mt[0][0];
        ??? nb = mt[m-1][1] - mt[0][1];
        /* do */
        length = step * ((double)na + nb)/2.0e0;
        if (( length <= 0.0e0 )){
          avgDisc = sqrt(maxDistSqr)
        }else{
          avgDisc = sqrt(intgDiscSqr/length);
        };
      };

      { /* with */
        ??? msc = pz_match_matched_step_count(mt^, StepIsMatched);
        /* do */
        matchedLength = step * ((double)msc)/2.0e0;
      };
    };
  } /* pz_double_chain_match_find_best */


void pz_double_chain_match_refine
  ( pz_double_chain_t *a,
    pz_double_chain_t *b,
    double step,
    pz_match_t *mtOld,
    unsigned maxAdjust,
    double critDistSqr,
    double maxDistSqr,
    double skipDistSqr,
    REF *pz_match_t mt  /* (OUT) */
    double *mismatch, /* (OUT) */
    double *length, /* (OUT) */
    double *matchedLength, /* (OUT) */
    double_vec_t *minAvgDisc, /* (OUT) */
    REF *pz_match_cost_matrix_t rCost, /* (WORK) */
  )

  TYPE Pair == pz_match_pair;


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

      { /* with */
        ??? dp = a[p[0]] - b[p[1]];
        ??? dq = a[q[0]] - b[q[1]];
        ??? mdSqr = 0.5e0 * (dp*dp + dq*dq);
        /* do */
        return min(mdSqr, maxDistSqr);
      }
    } /* StepDistSqr */


  double  StepMismatch 
  ( 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 chain steps "a[p[0]]->a[q[0]]"
      and "b[p[1]]->b[q[1]]", plus the skip distance squared,
      minus the critical distance squared,
      all times the step length. */
    {

      { /* with */
        ??? d2 = StepDistSqr(p, q) - critDistSqr;
        /* do */
        if (( p[0] == q[0] ) || ( p[1] == q[1] )){
          return step * (d2 + skipDistSqr)/2.0e0
        }else{
          return step * d2;
        };
      }
    } /* StepMismatch */


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

  {

    { /* with */
      ??? NA = (a.ne);
      ??? NB = (b.ne);
      /* do */
      affirm(NA > 0 );
      affirm(NB > 0 );
      pz_match_refine
  (     NA = NA, NB = NB,
        stepCostFn = StepMismatch,
        mtOld = mtOld,
        maxAdjust = maxAdjust,
        /*OUT*/ mt = mt,
        /*OUT*/ totCost = mismatch,
        /*OUT*/ minTotCost = minAvgDisc,
        /*WORK*/ rCost = rCost
      );
      affirm( (mt^.ne) > 0 );

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

        { /* with */
          ??? Lk = step*((double)K)/2.0e0;
          /* do */
          minAvgDisc[K] = sqrt(max(0.0e0, minAvgDisc[K]/Lk + critDistSqr));
        };
      };
    };
  } /* pz_double_chain_match_refine */


pz_match_t *pz_double_chain_map_match
  ( pz_match_t *mOld,
    pz_segment_pair *sOld,
    pz_double_chain_t *t0Old,
    pz_double_chain_t *t1Old,
    pz_segment_pair *sNew,
    pz_double_chain_t *t0New,
    pz_double_chain_t *t1New,
    r2_t stride 
  )
  {

    auto pz_match_t *alloc_initial_match(void);
    pz_match_t *alloc_initial_match(void)
      {
        int nOld = (t0Old->nel) + (t1Old->nel);
        int nNew = (t0New->nel) + (t1New->nel);
        int mNewSizeEst = MAX((mOld->nel) * nNew / nOld, 1);
        return pz_match_new(mNewSizeEst);
      }

    auto pz_match_pair map_abs_pair ( pz_match_pair p_old );
    pz_match_pair map_abs_pair ( pz_match_pair p_old )
      /*
        Maps a pair of ABSOLUTE sample indices from the old
        chains to the new chains. */
      {
        double label0 = 
          pz_double_chain_value_from_arg(t0Old,stride[0],FLOAT(pOld[0],double));
        int pNew0 = pz_round(pz_double_chain_arg_from_value(t0New, stride[0], label0));

        double 
          label1 = pz_double_chain_value_from_arg(t1Old,stride[1],FLOAT(pOld[1],double));
        int pNew1 = pz_round(pz_double_chain_arg_from_value(t1New, stride[1], label1));
        return (pz_match_pair){pNew0, pNew1};
      }


    void EnsureSpace
    ( REF *pz_match_t mR, 
      unsigned k 
    )
      {
        if (( mR == NULL )){
          mR = pz_match_new(k)
        }else if (( k > (mR^.ne - 1) )){

          { /* with */
            int n = (mR^.ne), mT = NEW(REF pz_match_t, MAX(k+1, 2*n));
            /* do */
            SUBARRAY(mT^,0,n)  = mR^; mR = mT;
          };
        }
      } /* EnsureSpace */

    VAR mNewR: REF pz_match_t := alloc_initial_match();  /* The new pairing */
        kNew: unsigned; /* Number of new pairs in {mNewR^} */
        stop: bool_t;  /* TRUE after first interruption. */

    kNew = 0;
    stop = FALSE;
    for ( kOld = 0 TO (mOld->nel - 1) ){

      { /* with */
        ??? rOld = mOld[kOld];                      /* Old pair relative to {sOld} */
        ??? pOld = pz_match_abs_pair_clip(rOld, sOld); /* Old pair, absolute */
        ??? pNew = map_abs_pair(pOld);                /* New pair, absolute */
        ??? rNew = pz_match_rel_pair_clip(pNew, sNew); /* New pair, relative to {sNew} */
        /* do */
        if (( rNew[0] == (unsigned.ne - 1) ) || ( rNew[1] == (unsigned.ne - 1) )){
          /* Pair is outside {sNew}: */
          if (( kNew > 0 )){ stop = TRUE ;}
        }else{
          if (( stop )){ return NULL; }
          /* Pair lies inside {sNew} */
          if (( kNew == 0 )){
            /* Firstpair of new pairing, just store it: */
            EnsureSpace(mNewR, kNew);
            mNewR^[kNew] = rNew;
            kNew++
          }else{
            /* Interpolate, Bresenham style, as needed: */
            VAR
              aNew = mNewR^[kNew-1];
              p = aNew;
            {
              affirm(aNew[0] <= rNew[0] );
              affirm(aNew[1] <= rNew[1] );

              { /* with */
                ??? d0 = rNew[0] - aNew[0];
                ??? d1 = rNew[1] - aNew[1];
                ??? d = MAX(d0, d1);
                /* do */
                for (ib = 1; ib <= d ; ib++){
                  if (( d0 > d1 )){
                    p[0] = aNew[0] + ib;
                    p[1] = aNew[1] + (ib*d1) / d
                  }else{
                    p[0] = aNew[0] + (ib*d0) / d;
                    p[1] = aNew[1] + ib;
                  };
                  EnsureSpace(mNewR, kNew);
                  mNewR^[kNew] = p;
                  kNew++;
                };;
              };
            };
          };
        };;
      };
    };
    if (( kNew == 0 )){ return NULL; }

    { /* with */
      ??? mR = pz_match_new(kNew);
      /* do */
      mR^ = SUBARRAY(mNewR^, 0, kNew);
      return mR;
    }
  } /* pz_double_chain_map_match */


pz_match_packed_t *pz_double_chain_map_packed_match
  ( pz_match_packed_t *pm,
    pz_segment_pair *sOld,
    pz_double_chain_t *t0Old,
    pz_double_chain_t *t1Old,
    pz_segment_pair *sNew,
    pz_double_chain_t *t0New,
    pz_double_chain_t *t1New,
    r2_t stride 
  )
  {
    if (( pm == NULL )){
      return NULL
    }else{

      { /* with */
        ??? mOld = pz_match_unpack(pm^);
        ??? mNew = pz_double_chain_map_match(mOld^, sOld, t0Old, t1Old, sNew, t0New, t1New, stride);
        /* do */
        if (( mNew == NULL )){
          return NULL 
        }else{
          return pz_match_pack(mNew^);
        };
      };
    }
  } /* pz_double_chain_map_packed_match */