/* See frb_curve.h */
/* Last edited on 2023-02-12 07:53:58 by stolfi */

#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 <jsfile.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] = +INFINITY;
        w.r[k].end[1] = -INFINITY;
      }
    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_t 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 = INFINITY;
    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);
    int ind = 0; /* Comment indentation. */
    filefmt_write_comment(wr, cmt, ind, '|');
    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_t header_only,  
    bool_t 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_int32(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_int32(rd)) * unit;
            fget_skip_spaces(rd); 
            ci->c[1] = frb_curve_to_double(fget_int32(rd)) * unit;
            fget_skip_spaces(rd); 
            ci->c[2] = frb_curve_to_double(fget_int32(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_t header_only,
    bool_t centralize
  )
  { double unitMin = +INFINITY;
    double unitMax = -INFINITY;
    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;
            char *full_name = jsprintf("%s/%04d/%s.flc", dir, k, fileName); 
            FILE *rd = open_read(full_name, TRUE);
            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; 
    char *buf = jsprintf(
      "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
      };
  }