/* See frb_curve.h */
/* Last edited on 2005-01-16 14:20:42 by stolfi */
/* Copyright © 2005 Universidade Estadual de Campinas. See note at end of file. */

#include <frb_curve.h>
#include <frb_match.h>
#include <frb_proc.h>
#include <frb_segment.h>
#include <frb_signal.h>
#include <frb_fourier.h>
#include <frb_window.h>
#include <frb_geo.h>
#include <frb_types.h>

#include <nget.h>
#include <fget.h>
#include <filefmt.h>
#include <r3.h>
#include <r4.h>
#include <r4x4.h>

#include <libiberty.h>
#include <math.h>
#include <values.h>
#include <stdio.h>

frb_curve_t frb_curve_from_line_segment ( r3_t *p, r3_t *q, int n )
  {
    r3_t delta;
    r3_sub(q, p, &delta);
    r3_scale(1.0/((double)n-1), &delta, &delta);
    frb_curve_t c = r3_vec_new(n);
    int i;
    for (i = 0; i <= n-1 ; i++)
      {
        double f = (double)i;
        r3_t *ci = &(c.el[i]);
        ci->c[0] = delta.c[0] * f + p->c[0];
        ci->c[1] = delta.c[1] * f + p->c[1];
        ci->c[2] = delta.c[2] * f + p->c[2];
      }
    return c;
  }

frb_curve_t frb_curve_trim ( frb_curve_t *c, int start, int length )
  {
    frb_curve_t x = r3_vec_new(length);
    frb_curve_do_trim(c, start, length, &x);
    return x;
  }

void frb_curve_do_trim
  ( frb_curve_t *c, 
    int start,
    int length, 
    frb_curve_t *x 
  )
  {
    int n = c->nel;
    int i, j;
    for (i = 0, j = frb_imod(start, n); i < length; i++, j++)
      { if (j >= n) { j -= n; }
        x->el[i] = c->el[j];
      }
  }

void frb_curve_reverse ( frb_curve_t *c )
  {
    r3_t aux;
    int M = c->nel/2, i, j;
    for (i = 0, j = c->nel-1; i < M; i++, j--)
      { aux = c->el[i]; c->el[i] = c->el[j]; c->el[j] = aux; }
  }

frb_curve_t frb_curve_extract_segment ( frb_curve_t *c, frb_segment_t *s )
  {
    affirm(c->nel == s->tot, "wrong tot size");
    frb_curve_t t = r3_vec_new(s->ns);
    frb_curve_do_extract_segment(c, s, &t);
    return t;
  }

void frb_curve_do_extract_segment
  ( frb_curve_t *c, 
    frb_segment_t *s, 
    frb_curve_t *t 
  )
  {
    affirm(c->nel == s->tot, "wrong tot size");
    frb_curve_do_trim(c, s->ini, s->ns, t);
    if (s->rev) { frb_curve_reverse(t); }
  }

frb_window_t frb_curve_get_window ( frb_curve_t *c )
  {
    int n = c->nel;
    int i, k;
    frb_window_t w;
    for (k = 0; k <= 2 ; k++)
      { w.r[k].end[0] = +MAXDOUBLE;
        w.r[k].end[1] = -MAXDOUBLE;
      }
    for (i = 0; i < n; i++)
      { 
        r3_t *ci = &(c->el[i]);
        for (k = 0; k <= 2 ; k++)
          {
            double cik = ci->c[k];
            double *lo = &(w.r[k].end[0]), *hi = &(w.r[k].end[1]);
            if (cik < *lo) { *lo = cik; }
            if (cik > *hi) { *hi = cik; }
          }
      }
    return w;
  }

r3_t frb_curve_sample_barycenter ( frb_curve_t *c )
  {
    int n = c->nel;
    r3_t p = (r3_t){{0.0, 0.0, 0.0}};
    int i;
    for (i = 0; i < n ; i++)
      {
        r3_t *ci = &(c->el[i]);
        p.c[0] += ci->c[0];
        p.c[1] += ci->c[1];
        p.c[2] += ci->c[2];
      }
    double fn = n;
    p.c[0] /= fn;
    p.c[1] /= fn;
    p.c[2] /= fn;
    return p;
  }

r3_t frb_curve_area_barycenter ( frb_curve_t *c )
  {
    int n = c->nel;
    double totArea  = 0.0;  /* Twice the area, actually. */
    int iPrev = n-1;
    r3_t p = (r3_t){{0.0, 0.0, 0.0}};
    int i;
    for (i = 0; i < n ; i++)
      {
        r3_t *ai = &(c->el[iPrev]);
        r3_t *bi = &(c->el[i]);
        double area = ai->c[0]*bi->c[1] - ai->c[1]*bi->c[0]; /* 2× triang area. */
        p.c[0] += area*(ai->c[0] + bi->c[0]);  /* Postpone the {/ 3}. */
        p.c[1] += area*(ai->c[1] + bi->c[1]);  /* Postpone the {/ 3}. */
        totArea += area;
        iPrev = i;
      }
    if (totArea == 0.0)
      { p = frb_curve_sample_barycenter(c); }
    else
      { totArea *= 3;
        p.c[0] /= totArea;
        p.c[1] /= totArea;
      }
    p.c[2] = 0.0;
    return p;
  }

frb_curve_t frb_curve_translate ( frb_curve_t *c, r3_t t )
  {
    int n = c->nel;
    frb_curve_t d = r3_vec_new(n);
    int i;
    for (i = 0; i < n ; i++) { d.el[i] = c->el[i]; }
    frb_curve_do_translate(&d, t);
    return d;
  }

void frb_curve_do_translate ( frb_curve_t *c, r3_t t )
  {
    int n = c->nel;
    int i;
    for (i = 0; i < n ; i++)
      { r3_t *ci = &(c->el[i]);
        ci->c[0] += t.c[0];
        ci->c[1] += t.c[1];
        ci->c[2] += t.c[2];
      }
  }

frb_curve_t frb_curve_map ( frb_curve_t *c, r4x4_t *M )
  {
    int n = c->nel;
    frb_curve_t d = r3_vec_new(n);
    int i;
    for (i = 0; i < n ; i++) { d.el[i] = c->el[i]; }
    frb_curve_do_map(&d, M);
    return d;
  }

void frb_curve_do_map ( frb_curve_t *c, r4x4_t *M )
  {
    int n = c->nel;
    int i;
    for (i = 0; i < n ; i++)
      { r3_t *ci = &(c->el[i]);
        r4_t u = (r4_t){{1.0, ci->c[0], ci->c[1], ci->c[2]}};
        r4_t v; r4x4_map_row(&u, M, &v);
        *ci = (r3_t){{v.c[1]/v.c[0], v.c[2]/v.c[0], v.c[3]/v.c[0]}};
      }
  }

r4x4_t frb_curve_alignment_matrix (frb_curve_t *a, frb_curve_t *b, double pos)
  {
    int na = a->nel;
    int ma = (na - 1)/2;
    int ai = frb_imax(0, frb_imin((int)(pos*na + 0.5), ma - 1));
    int af = frb_imin(na - 1, frb_imax((int)((1-pos)*na + 0.5), ma + 1));

    r3_t *pi = &(a->el[ai]);
    r3_t *pf = &(a->el[af]);
    r3_t pc = frb_curve_sample_barycenter(a);

    int nb = b->nel;
    int mb = (nb - 1)/2;
    int bi = frb_imax(0, frb_imin((int)(pos*nb + 0.5), mb - 1));
    int bf = frb_imin(nb - 1, frb_imax((int)((1-pos)*nb + 0.5), mb + 1));

    r3_t *qi = &(b->el[bi]);
    r3_t *qf = &(b->el[bf]);
    r3_t qc = frb_curve_sample_barycenter(b);

    r3_t u = frb_seg_dir(pi, pf);
    r3_t v = frb_seg_dir(qi, qf);
    
    r3_t t;
    r3_sub(&qc, &pc, &t);
    r4x4_t T = frb_translation_matrix(&t);
    r4x4_t R = frb_rotation_matrix(&u, &v);

    r4x4_t M;
    r4x4_mul(&T, &R, &M);
    return M;
  }

r4x4_t frb_curve_normalization_matrix ( frb_curve_t *c, bool reverse )
  {
    int n = c->nel;
    r3_t *pi = &(c->el[0]);
    r3_t *pf = &(c->el[n-1]);

    r3_t *pa = ( reverse ? pf : pi );
    r3_t *pz = ( reverse ? pi : pf );
    
    r3_t t;
    r3_scale(-1.0, pa, &t); 
    r4x4_t T = frb_translation_matrix(&t);
    
    r3_t u = frb_seg_dir(pa, pz);
    r3_t v = (r3_t){{1.0, 0.0, 0.0}};
    r4x4_t R = frb_rotation_matrix(&u, &v);

    r4x4_t M;
    r4x4_mul(&T, &R, &M);
    return M;
  }

double frb_curve_linear_lengths (
    frb_curve_t *p,    /* {p[i]} is curve position at time {t[i]}. */
    frb_signal_t *s /* {s[j]} is the length from {p[0]} to {p[j]}. */
  )
  {
    double length = 0.0;
    int n = p->nel;
    int i;
    affirm((s->nel) == n, "wrong size");
    s->el[0] = 0.0;
    for (i = 1; i < n; i++)
      {
        length += r3_dist(&(p->el[i]), &(p->el[i-1]));
        s->el[i] = length;
      }
    return length + r3_dist(&(p->el[n-1]), &(p->el[0]));
  }

void frb_curve_linear_uniform_sample (
    frb_signal_t *t,
    double tPeriod,
    frb_curve_t *p,
    double tStart,
    frb_curve_t *q 
  )
  {
    int ib;
    double ta, tb;
    int k; 
    int np = p->nel;
    int nq = q->nel;
    double tLo = t->el[0], tHi = t->el[0] + tPeriod;
    affirm(t->nel == np, "wrong size");
    frb_reduce_to_period(&tStart, tLo, tHi, &k);
    ta = t->el[0]; ib = 1; tb = t->el[1];
    int j;
    for (j = 0; j < nq; j++)
      { double tau = tStart + tPeriod * (((double)j) / ((double)nq));
        affirm(tau >= t->el[ib-1], "interp bug");
        affirm(tau < tHi, "interp bug");
        while ((ib < np) && (tau > t->el[ib]))
          { ta = tb; ib = ib+1;
            tb = (ib == np ? tHi : t->el[ib]);
          }
        q->el[j] = frb_linear_interpolate(
          tau, 
          ta, &(p->el[ib-1]), 
          tb, &(p->el[ib % np])
        );
      }
  }

void frb_curve_linear_equidistant_sample 
  ( frb_curve_t *p, 
    double tStart,  
    frb_curve_t *q 
  )
  {
    int np = p->nel;
    frb_signal_t t = double_vec_new(np);
    double length = frb_curve_linear_lengths(p, &t);
    frb_curve_linear_uniform_sample(&t, length, p, tStart*length, q);
  }

void frb_curve_linear_time_sample
  ( frb_signal_t *t,
    double tPeriod,
    frb_curve_t *p,
    frb_signal_t *r,
    frb_curve_t *q 
  )
  {
    int ia, ib; 
    int i, k;
    double ta, tb;

    int np = p->nel;
    int nq = q->nel;
    double tLo = t->el[0];
    double tHi = t->el[0] + tPeriod;
    
    affirm(t->nel == np, "wrong size");
    affirm(r->nel == nq, "wrong size");
    ia = 0; ta = t->el[0];
    ib = 1; tb = t->el[1];
    for (i = 0; i <= nq-1 ; i++)
      { /* Invariant: {ia IN [0..np-1]}, {ib == ia+1} */
        double tau = r->el[i];
        frb_reduce_to_period(&tau, tLo, tHi, &k);
        affirm((tLo <= tau) && (tau < tHi), "bug");
        while (tau < t->el[ia]){ ib = ia; ia--; }
        while ((ib < np) && (tau > t->el[ib])) { ia = ib; ib++; }
        double ta = t->el[ia];
        double tb = t->el[ib % np] + tPeriod * ((double)ib / np);
        q->el[i] = frb_linear_interpolate(tau, ta, &(p->el[ia % np]), tb, &(p->el[ib % np]));
      }
  }

void frb_curve_hermite_uniform_sample
  ( frb_signal_t *t,
    double tPeriod,
    frb_curve_t *p,
    frb_curve_t *dp,
    double tStart,
    frb_curve_t *q,
    frb_curve_t *dq 
  )
  {
    int ib;
    double ta, tb;
    int i, k;
    int np = p->nel;
    int nq = q->nel;
    double tLo = t->el[0];
    double tHi = tLo + tPeriod;
    affirm(dp->nel == np, "dp wrong size");
    affirm(t->nel == np, "t wrong size");
    frb_reduce_to_period(&tStart, tLo, tHi, &k);
    ta = t->el[0]; ib = 1; tb = t->el[1];
    for (i = 0; i < nq; i++)
      { double tau = tStart + tPeriod * (((double)i) / ((double)nq));
        affirm(tau >= t->el[ib-1], "bug");
        affirm(tau < tHi, "bug");
        while ((ib < np) && (tau > t->el[ib]))
          { ta = tb; ib = ib + 1;
            if (ib == np) 
              { tb = tHi; }
            else
              { tb = t->el[ib]; }
          }
        affirm((tau >= ta) && (tau <= tb), "bug");
        frb_hermite_interpolate(
          tau,
          ta,
          &(p->el[ib-1]), &(dp->el[ib-1]),
          tb,
          &(p->el[ib % np]), &(dp->el[ib % np]),
          &(q->el[i]), &(dq->el[i])
        );
      }
  }

void frb_curve_hermite_equidistant_sample
  ( frb_curve_t *p,
    double tStart,
    frb_curve_t *q,
    frb_curve_t *dq 
  )
  {
    int np = p->nel;
    int i;
    if (np <= 1)
      { for (i = 0; i < q->nel; i++)
          { q->el[i] = p->el[0]; dq->el[i] = (r3_t){{0.0, 0.0, 0.0}}; }
        return;
      }
    frb_signal_t t = double_vec_new(np);
    double length = frb_curve_linear_lengths(p, &t);
    frb_curve_t dp = r3_vec_new(np);
    frb_curve_estimate_velocities(&t, length, p, &dp, frb_estimate_velocity_L);
    frb_curve_hermite_uniform_sample(&t, length, p, &dp, tStart*length, q, dq);
  }

void frb_curve_hermite_time_sample
  ( frb_signal_t *t,
    double tPeriod,
    frb_curve_t *p,
    frb_curve_t *dp,
    frb_signal_t *r,
    frb_curve_t *q,
    frb_curve_t *dq 
  )
  {
    int ia, ib;
    int i, k;
    int np = p->nel;
    int nq = q->nel;
    double tLo = t->el[0];
    double tHi = tLo + tPeriod;
    affirm(t->nel == np, "t wrong size");
    affirm(r->nel >= nq, "r wrong size");
    ia = 0; ib = 1;
    for (i = 0; i < nq; i++)
      { /* Invariant: {ia IN [0..np-1]}, {ib == ia+1} */
        double tau = r->el[i];
        frb_reduce_to_period(&tau, tLo, tHi, &k);
        while (tau < t->el[ia]) { ib = ia; ia--; }
        while ((ib < np) && (tau > t->el[ib])) { ia = ib; ib++; }
        double ta = t->el[ia];
        double tb = t->el[ib % np] + tPeriod * ((double)ib / np);
        affirm((ta <= tau) && (tau <= tb), "bug");
        frb_hermite_interpolate(
          tau,
          ta, &(p->el[ia]),          &(dp->el[ia]),
          tb, &(p->el[(ia+1) % np]), &(dp->el[(ia+1) % np]),
          &(q->el[i]), &(dq->el[i])
        );
      }
  }

void frb_curve_estimate_velocities
  ( frb_signal_t *t,
    double tPeriod,
    frb_curve_t *p,
    frb_curve_t *dp,
    frb_curve_vel_estimator_t est 
  )
  {
    int np = p->nel;
    affirm(dp->nel == np, "wrong size");
    affirm(t->nel == np, "wrong size");
    int i;
    if (np <= 2)
      { for (i = 0; i < np; i++) 
          { dp->el[i] = (r3_t){{0.0, 0.0, 0.0}}; }
      }
    else
      {
        /* Estimate the velocities by {L} method: */
        int ka = np-2, kb = np-1;
        double sa = t->el[np-2] - tPeriod, sb = t->el[np-1] - tPeriod;
        int i;
        for (i = 0; i <= np-1 ; i++)
          {
            dp->el[kb] = 
              est(sa, &(p->el[ka]), sb, &(p->el[kb]), t->el[i], &(p->el[i]));
            ka = kb; kb = i;
            sa = sb; sb = t->el[i];
          }
      }
  }

double frb_curve_adjust_unit ( double givenUnit, frb_curve_t *c )
  {
    double maxBig = 0.0, minDev = MAXDOUBLE;
    int n = c->nel;
    int i, k;
    for (k = 0; k < 3; k++)
      {
        double avg = 0.0, dev = 0.0, big = 0.0;
        /* Compute average and max-abs value: */
        if (n > 0)
          { for (i = 0; i < n; i++)
              { r3_t *ci = &(c->el[i]);
                avg = avg + ci->c[k];
                big = frb_dmax(big, fabs(ci->c[k]));
              }
            avg = avg/(double)n;
          }
        maxBig = frb_dmax(maxBig, big);
        /* Compute standard deviation: */
        if (n > 1)
          { for (i = 0; i < n ; i++)
              {
                double d = c->el[i].c[k] - avg;
                dev += d*d;
              }
            dev = sqrt(dev/((double)(n-1)));
            if (dev > 1.0e-10 * big){ minDev = frb_dmin(minDev, dev); }
          }
      }
    if (minDev > maxBig){ minDev = maxBig; }
    return frb_adjust_unit(givenUnit, minDev, maxBig);
  }

#define frb_curve_file_version "2004-04-30"

void frb_curve_write
  ( FILE *wr, 
    char *cmt, 
    frb_curve_t *c, 
    double unit ,
    double scale
  )
  {
    auto int to_int ( double x );
    int to_int ( double x )
      {
        affirm(x > (double)MININT, "overflow");
        affirm(x < (double)MAXINT, "overflow");
        return frb_round(x);
      }

    int n = c->nel;
    filefmt_write_header(wr, "frb_curve_t", frb_curve_file_version);
    filefmt_write_comment(wr, cmt, '|');
    unit /= scale;
    fprintf(wr, "unit = %g\n", unit);
    fprintf(wr, "samples = %d\n", n);
    int i;
    for (i = 0; i < n; i++)
      {
        r3_t *ci = &(c->el[i]);
        fprintf(wr, "%d ",  to_int(ci->c[0] / unit));
        fprintf(wr, "%d ",  to_int(ci->c[1] / unit));
        fprintf(wr, "%d\n", to_int(ci->c[2] / unit));
      }
    filefmt_write_footer(wr, "frb_curve_t");
    fflush(wr);
  }

frb_curve_read_data_t  frb_curve_read 
  ( FILE *rd,
    double scale, 
    bool header_only,  
    bool centralize 
  )
  {
    auto double  frb_curve_to_double ( int n );
    double frb_curve_to_double ( int n )
      {
        affirm(n > MININT, "overflow");
        affirm(n < MAXINT, "overflow");
        return (double)n;
      }

    frb_curve_read_data_t d;

    filefmt_read_header(rd, "frb_curve_t", frb_curve_file_version);
    d.cmt = filefmt_read_comment(rd, '|');
    double unit = nget_double(rd, "unit") * scale;
    d.unit = unit;
    fget_skip_formatting_chars(rd);
    int n = nget_int(rd, "samples");
    d.samples = n;
    if (header_only)
      { d.c = r3_vec_new(0); }
    else
      { d.c = r3_vec_new(n);
        int i;
        for (i = 0; i < n; i++)
          {
            r3_t *ci = &(d.c.el[i]);
            fget_skip_formatting_chars(rd); 
            ci->c[0] = frb_curve_to_double(fget_int(rd)) * unit;
            fget_skip_spaces(rd); 
            ci->c[1] = frb_curve_to_double(fget_int(rd)) * unit;
            fget_skip_spaces(rd); 
            ci->c[2] = frb_curve_to_double(fget_int(rd)) * unit;;
          }
        if (centralize)
          { r3_t bar = frb_curve_area_barycenter(&(d.c));
            r3_neg(&bar, &bar);
            frb_curve_do_translate(&(d.c), bar);
          }
        filefmt_read_footer(rd, "frb_curve_t");
      }
    return d;
  }

#define frb_curve_NO_DATA \
  (frb_curve_read_data_t) \
    { \
      /*cmt*/ NULL, \
      /*samples*/ 0, \
      /*c*/ (r3_vec_t){0, NULL}, \
      /*unit*/ 0 \
    }

frb_curve_list_read_data_t frb_curve_list_read
  ( char *dir,
    char *fileName,
    double scale,
    bool_vec_t *sel,
    bool header_only,
    bool centralize
  )
  { double unitMin = +MAXDOUBLE;
    double unitMax = -MAXDOUBLE;
    char *cmt = ""; 
    fprintf(stderr, "reading geometric curves:\n");
    int ncv = sel->nel;
    frb_curve_read_data_t **dd = 
      (frb_curve_read_data_t **)
        notnull(malloc(ncv * sizeof(frb_curve_read_data_t *)), "no mem");
    int k;
    for (k = 0; k < ncv; k++)
      { dd[k] = NULL;
        if (sel->el[k])
          { fprintf(stderr, "  %04d: ", k);
            char *full_name;
            asprintf(&full_name, "%s/%04d/%s.flc", dir, k, fileName); 
            FILE *rd = open_read(full_name);
            if (rd != NULL)
              { frb_curve_read_data_t *ddk = 
                  (frb_curve_read_data_t *)
                    notnull(malloc(sizeof(frb_curve_read_data_t)), "no mem");
                (*ddk) = frb_curve_read(rd, scale, header_only, centralize);
                dd[k] = ddk;
                /* fprintf(stderr, "read from %s\n", full_name ); */
                affirm(ddk->unit > 0.0, "bad unit");
                if (unitMin > unitMax) { cmt = ddk->cmt; };
                unitMin = frb_dmin(unitMin, ddk->unit);
                unitMax = frb_dmax(unitMax, ddk->unit);;
                fclose(rd);
              }
            else
              { fprintf(stderr, "file %s not found.\n", full_name ); }
          }
      }
    char *buf = NULL; 
    asprintf(&buf, 
      "Curves read from %s/NNNN/%s\nLast entry:\n%s", 
      dir, fileName, cmt
    );
    return (frb_curve_list_read_data_t) \
      { /* sgData */  dd,
        /* ncv */     ncv,
        /* unitMin */ unitMin,
        /* unitMax */ unitMax,
        /* cmt */     buf
      };
  }

/* COPYRIGHT AND AUTHORSHIP NOTICE

  Copyright © 2005 Universidade Estadual de Campinas (UNICAMP).
  Created by Helena C. G. Leitão and Jorge Stolfi in 1995--2005.
  
  This source file can be freely distributed, used, and modified,
  provided that this copyright and authorship notice is preserved in
  all copies, and that any modified versions of this file are clearly
  marked as such.
  
  This software has NO WARRANTY of correctness or applicability for
  any purpose. Neither the authors nor their employers shall be held
  responsible for any losses or damages that may result from its use.

  END OF NOTICE */