/* See frb_geo.h */
/* Last edited on 2023-02-22 12:43:47 by stolfi */

#include <frb_geo.h>
#include <frb_types.h>

#include <affirm.h>

#include <math.h>
#include <r3.h>
#include <r4x4.h>
#include <stdio.h>

void frb_geo_debug ( char *msg, double_vec_t *x );

r3_t frb_linear_interpolate
  ( double t, 
    double a, 
    r3_t *pa, 
    double b, 
    r3_t *pb 
  )
  { 
    double Ca, Cb;
    if ((b - a) != 0.0)
      { Ca = (b - t) / (b - a);  Cb = 1.00 - Ca; }
    else
      { Ca = 0.5; Cb = 0.5; }
    return (r3_t){{
      Ca * pa->c[0] + Cb * pb->c[0],
      Ca * pa->c[1] + Cb * pb->c[1],
      Ca * pa->c[2] + Cb * pb->c[2]
    }};
  }

void frb_hermite_interpolate
  ( double t, 
    double a, 
    r3_t *pa, 
    r3_t *va, 
    double b, 
    r3_t *pb, 
    r3_t *vb,  
    r3_t *p,
    r3_t *v 
  )
  {
    affirm(a <= b, "arg order");
    /* 
      Test for degenerate intervals: If the interval has zero width,
      return the mean of the positions (which is an OK result) and the
      mean of the velocities (which is almost certainly wrong).
    */
    if (b - a == 0.0)
      { /* Interval is degenerate: */
        r3_mix(0.5, pa, 0.5, pb, p);
        r3_mix(0.5, va, 0.5, vb, v);
      }
    else
      {
        double tab = b - a;

        double tax = t - a;
        double txb = b - t;

        double rax = tax/tab;
        double rxb = txb/tab;

        double Cpa = rxb * rxb * (3.0 * rax + rxb);
        double Dpa = - 6.0 * rax * rxb / tab;

        double Cva = + rax * rxb * txb;
        double Dva = + rxb * (rxb - 2.0 * rax);

        double Cvb = - rax * rxb * tax;
        double Dvb = + rax * (rax - 2.0 * rxb);

        double Cpb = rax * rax * (rax + 3.0 * rxb);
        double Dpb = + 6.0 * rax * rxb / tab;

        *p = (r3_t){{
          Cpa*pa->c[0] + Cva*va->c[0] + Cvb*vb->c[0] + Cpb*pb->c[0],
          Cpa*pa->c[1] + Cva*va->c[1] + Cvb*vb->c[1] + Cpb*pb->c[1],
          Cpa*pa->c[2] + Cva*va->c[2] + Cvb*vb->c[2] + Cpb*pb->c[2]
        }};
        *v = (r3_t){{
          Dpa*pa->c[0] + Dva*va->c[0] + Dvb*vb->c[0] + Dpb*pb->c[0],
          Dpa*pa->c[1] + Dva*va->c[1] + Dvb*vb->c[1] + Dpb*pb->c[1],
          Dpa*pa->c[2] + Dva*va->c[2] + Dvb*vb->c[2] + Dpb*pb->c[2]
        }};
      }
  }

r3_t  frb_estimate_velocity_Q
  ( double a, 
    r3_t *pa, 
    double b, 
    r3_t *pb, 
    double c, 
    r3_t *pc 
  )
  {
    affirm((a <= b) && (b <= c), "arg order");
    
    /* Test for degenerate intervals: If intervals are degenerate, use
      linear interpolation, just to return something (but it will be
      almost surely wrong, and discontinuous).
    */
    double eps = 1.0e-7*(c - a);
    if (eps == 0.0)
      { /* Whole interval is degenerate: */
        return (r3_t){{0.0, 0.0, 0.0}};
      }
    else if ((b-a) < eps)
      { /* Left interval is degenerate; */
        r3_t pbc; r3_sub(pc, pb, &pbc);
        r3_scale(1.0/(c-b), &pbc, &pbc);
        return pbc;
      }
    else if ((c-b) < eps)
      { /* Right interval is degenerate; */
        r3_t pab; r3_sub(pb, pa, &pab);
        r3_scale(1.0/(b-a), &pab, &pab);
        return pab;
      }
    else
      { double fac = 1.0/(c - a);
        double fab = 1.0/(b - a);
        double fbc = 1.0/(c - b);

        double Ca = fac - fab;
        double Cb = fab - fbc;
        double Cc = fbc - fac;
        return (r3_t){{
          Ca*pa->c[0] + Cb*pb->c[0] + Cc*pc->c[0],
          Ca*pa->c[1] + Cb*pb->c[1] + Cc*pc->c[1],
          Ca*pa->c[2] + Cb*pb->c[2] + Cc*pc->c[2]
        }};
      }
  }

r3_t  frb_estimate_velocity_L
  ( double a, 
    r3_t *pa, 
    double b, 
    r3_t *pb,
    double c, 
    r3_t *pc 
  )
  { affirm((a <= b) && (b <= c), "arg order");
    /* Test for degenerate intervals: If whole interval is degenerate,
      return 0 (which is almost surely wrong). */
    if (c-a == 0.0)
      { return (r3_t){{0.0, 0.0, 0.0}}; }
    else
      { double Ca = 1.0/(c - a);
        return (r3_t){{
          Ca*(pc->c[0] - pa->c[0]),
          Ca*(pc->c[1] - pa->c[1]),
          Ca*(pc->c[2] - pa->c[2])
        }};
      }
  }

r3_t  frb_estimate_velocity_C
  ( double a, 
    r3_t *pa, 
    double b, 
    r3_t *pb, 
    double c, 
    r3_t *pc 
  )
  { affirm((a <= b) && (b <= c), "arg order");
    /* Test for degenerate intervals: */
    /* If whole interval is degenerate, return 0 (which is almost surely wrong). */
    if (c - a == 0.0)
      { /* Whole interval is degenerate: */
        return (r3_t){{0.0, 0.0, 0.0}};
      }
    else
      { double tab = b - a;
        double tbc = c - b;
        double den = tab*tab + tbc*tbc;
        
        double Ca = - tab/den;
        double Cb = (tab - tbc)/den;
        double Cc = + tbc/den;
        
        affirm(den > 0.0, "bug");
        return (r3_t){{
          Ca*pa->c[0] + Cb*pb->c[0] + Cc*pc->c[0],
          Ca*pa->c[1] + Cb*pb->c[1] + Cc*pc->c[1],
          Ca*pa->c[2] + Cb*pb->c[2] + Cc*pc->c[2]
        }};
      }
  }

double  frb_hermite_curve_length
  ( double a, 
    r3_t *pa, 
    r3_t *va, 
    double b, 
    r3_t *pb, 
    r3_t *vb 
  )
  {
    /* Shift time axis so that midpoint of {b} and {c} is zero: */
    { double m = 0.5*(a+b);
      a = a - m;
      b = b - m;
    }
    /* In case of degenerate interval, assume straight line: */
    affirm(a <= b, "arg order");
    if (b-a == 0.0)
      { return r3_dist(pa, pb); }
    else
      {
        double ab = b - a;

        /* Compute the derivative of the Hermite interpolant.
        
          Let the Hermite interpolant be {h(u) = h3*u^3 + ... + h0};
          where {u = 2*(t - a)/(b-a)-1} ranges from -1 to +1.  
          Note that {dh/du(-1) = va*(b-a)/2}, and {dh/du(1) = vb*(b-a)/2}.
          Denoting {b-a} by {ab}, the polynomial {h} is then
          
           | { h(u) = 
           |
           |     + pa*(+u^3 - 3*u + 2)/4
           |     + pb*(-u^3 + 3*u + 2)/4
           |     + va*ab*(u^3 - u^2 - u + 1)/8
           |     + vb*ab*(u^3 + u^2 - u - 1)/8
           |   = 
           |     + (1/4*(pa-pb) + 1/8*(va+vb)*ab)*u^3
           |     + ((vb-va)*ab/8)*u^2
           |     + (3/4*(pb-pa) - 1/8*(va+vb)*ab)*u
           |     + (1/2*(pa+pb) + 1/8*(va-vb)*ab
           | }

           Its derivative with respect to {u} is then

           | { dh/du(u) = 
           |     + (3/4*(pa-pb)+3/8*(va+vb)*ab) * u^2
           |     + (1/4*(vb-va)*ab) * u
           |     + (3/4*(pb-pa) - 1/8*(va+vb)*ab)
           | }
           
           Note that the curve depends only on the products
           {va*ab} and {vb*ab}, not on {va}, {vb}, or {ab}
           separately.
        */       

        double dxdu_2 = 0.750*(pa->c[0]-pb->c[0]) + 0.375*(va->c[0]+vb->c[0])*ab;
        double dydu_2 = 0.750*(pa->c[1]-pb->c[1]) + 0.375*(va->c[1]+vb->c[1])*ab;
        double dzdu_2 = 0.750*(pa->c[2]-pb->c[2]) + 0.375*(va->c[2]+vb->c[2])*ab;

        double dxdu_1 = 0.250*(vb->c[0] - va->c[0])*ab;
        double dydu_1 = 0.250*(vb->c[1] - va->c[1])*ab;
        double dzdu_1 = 0.250*(vb->c[2] - va->c[2])*ab;

        double dxdu_0 = 0.750*(pb->c[0]-pa->c[0]) - 0.125*(va->c[0]+vb->c[0])*ab;
        double dydu_0 = 0.750*(pb->c[1]-pa->c[1]) - 0.125*(va->c[1]+vb->c[1])*ab;
        double dzdu_0 = 0.750*(pb->c[2]-pa->c[2]) - 0.125*(va->c[2]+vb->c[2])*ab;

        /* Computes the speed squared {|dh/du|^2}, as a function of
          {u}. We only need the even powers: */
        
        double s_4 = dxdu_2*dxdu_2 + dydu_2*dydu_2 + dzdu_2*dzdu_2;
        double s_2 = 2.0 * (dxdu_2*dxdu_0 + dydu_2*dydu_0 + dzdu_2*dzdu_0) +
             (dxdu_1*dxdu_1 + dydu_1*dydu_1 + dzdu_1*dzdu_1);
        double s_0 = dxdu_0*dxdu_0 + dydu_0*dydu_0 + dzdu_0*dzdu_0;

        /* Computes the approximate square root of the polynomial
          Assumes the constant term {s0} dominates, so 
          {sqrt(U + s_0) ~ sqrt(s_0) + U/sqrt(s_0)/2} */
          
        double r_0 = sqrt(s_0 + 1.0e-50);
        double r_2 = 0.5 * s_2/r_0;
        double r_4 = 0.5 * s_4/r_0;

        /* Integrates the speed from -1 to +1 to get the length: */
        double length = 2.0*(r_4/5.0 + r_2/3.0 + r_0);
        return length;
      }
  }

void frb_geo_debug ( char *msg, double_vec_t *x )
  {
    fprintf(stderr, msg);
    int i;
    for (i = 0; i < x->nel; i++)
      { fprintf(stderr, " %12.8f", x->el[i]); }
    fprintf(stderr, "\n");
  }

r3_t frb_B_spline_approximation
  ( double t,
    double a, 
    double b, 
    r3_t *Pabc,
    double c, 
    r3_t *Pbcd,
    double d, 
    r3_t *Pcde,
    double e, 
    r3_t *Pdef,
    double f 
  )
  {
    r3_t Ptbc = frb_linear_interpolate(t, a, Pabc, d, Pbcd);
    r3_t Ptcd = frb_linear_interpolate(t, b, Pbcd, e, Pcde);
    r3_t Ptde = frb_linear_interpolate(t, c, Pcde, f, Pdef);
    r3_t Pttc = frb_linear_interpolate(t, b, &Ptbc, d, &Ptcd);
    r3_t Pttd = frb_linear_interpolate(t, c, &Ptcd, e, &Ptde);
    r3_t Pttt = frb_linear_interpolate(t, c, &Pttc, d, &Pttd);
    return Pttt;
  }

double frb_avg_seg_dist
  ( r3_t *a1,
    r3_t *a2,
    r3_t *b1,
    r3_t *b2 
  )
  {
    return sqrt(frb_avg_seg_dist_sqr(a1, a2, b1, b2));
  }

double frb_avg_seg_dist_sqr
  ( r3_t *a1,
    r3_t *a2,
    r3_t *b1,
    r3_t *b2 
  )
  {
    r3_t d1; r3_sub(a1, b1, &d1);
    r3_t d2; r3_sub(a2, b2, &d2);
    double d = ( 
      d1.c[0]*d1.c[0] + d1.c[0]*d2.c[0] + d2.c[0]*d2.c[0] +
      d1.c[1]*d1.c[1] + d1.c[1]*d2.c[1] + d2.c[1]*d2.c[1] +
      d1.c[2]*d1.c[2] + d1.c[2]*d2.c[2] + d2.c[2]*d2.c[2]
    )/3.0;
    return d;
  }

r4x4_t frb_translation_matrix ( r3_t *p ) 
  {
    r4x4_t m; 
    r4x4_ident(&m);
    m.c[0][1] = p->c[0];
    m.c[0][2] = p->c[1];
    m.c[0][3] = p->c[2];
    return m;
  }

r4x4_t frb_rotation_matrix ( r3_t *u, r3_t *v )
  {
    r4x4_t m; 
    r3_t e;
    double cos, sin;
    r4x4_ident(&m);
    cos = r3_dot(u, v);
    r3_cross(u,v, &e);
    sin = r3_norm(&e);
    r3_dir(&e, &e);
    affirm(fabs(cos*cos + sin*sin - 1.0) < 1.0e-10, "degen matrix");
    if (fabs(1.0 - abs(cos)) < 1.0e-10)
      { /* Vectors are practically collinear. */
        if (cos > 0.0) { return m; }
        /* Vectors are practically opposite; pick any {e} orthogonal to {u}. */
        /* Find smallest coordinate {u[k]}: */
        { 
          unsigned k = 0;
          if (fabs(u->c[1]) < fabs(u->c[k])) { k = 1; }
          if (fabs(u->c[2]) < fabs(u->c[k])) { k = 2; }
          e = (r3_t){{0.0, 0.0, 0.0}};
          e.c[k] = 1.0;;
        }
        r3_t ue; r3_cross(u,&e,&ue); r3_dir(&ue, &e);
        sin = 0.0;
      }
    double x = e.c[0], y = e.c[1], z = e.c[2];
    double xx = x*x, yy = y*y, zz = z*z;
    double xy = x*y, yz = y*z, zx = z*x;
    
    m.c[1][1] = xx + cos*(yy + zz);
    m.c[1][2] = (1.0 - cos)*xy + sin*z;
    m.c[1][3] = (1.0 - cos)*zx - sin*y;
    m.c[2][1] = (1.0 - cos)*xy - sin*z;
    m.c[2][2] = yy + cos*(xx + zz);
    m.c[2][3] = (1.0 - cos)*yz + sin*x;
    m.c[3][1] = (1.0 - cos)*zx + sin*y;
    m.c[3][2] = (1.0 - cos)*yz - sin*x;
    m.c[3][3] = zz + cos*(xx + yy);;
    return m;
  }

r3_t frb_seg_dir ( r3_t *p, r3_t *q )
  {
    r3_t r;
    r.c[0] = q->c[0] - p->c[0];
    r.c[1] = q->c[1] - p->c[1];
    r.c[2] = q->c[2] - p->c[2];
    r3_dir(&r, &r);
    return r;
  }

r3_t  frb_seg_mid ( r3_t *p, r3_t *q )
  { 
    r3_t o;
    int i;
    for (i = 0; i < 3; i++)
      { o.c[i] = (p->c[i] + q->c[i]) / 2.0; }
    return o;  
  }