/* See mtv_valve_backloop_make.h */
/* Last edited on 2016-04-22 17:13:07 by stolfilocal */

#define _GNU_SOURCE
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <assert.h>

#include <bool.h>
#include <r3.h>
#include <r3x3.h>
#include <r3_path.h>
#include <affirm.h>
#include <ppv_array.h>

#include <voxm_path.h>
#include <voxm_splat.h>
#include <voxm_splat_tube.h>

#include <mtv_valve_backloop_make.h>

/* INTERNAL PROTOTYPES */

voxm_path_state_t mtv_valve_backloop_get_turn_state
  ( r3_path_state_t *beS, /* State at end of untwisted backflow channel. */
    double tm,    /* Nominal time at beginning of the turn. */
    double te,    /* Nominal time at end of turn. */
    double bfR,   /* Radius of turn. */ 
    double trD,   /* Angle spanned by the circular part of the backflow loop, before torsion. */
    double bmW,   /* Torsion angle of backflow loop at beginning of the turn. */
    double beW,   /* Torsion angle of backflow loop at end of the turn. */
    double t      /* Time argument. */
  );
  /* Computes the position and velocity for a time {t} along the turn part of the 
    backflow channel (the toroidal helix), assuming that the whole turn is
    traversed with time varying from {tm} to {te}.  */
  
r3_t mtv_valve_backloop_get_turn_position
  ( r3_path_state_t *beS, /* State at end of untwisted backflow channel. */
    double tm,    /* Nominal time at beginning of the turn. */
    double te,    /* Nominal time at end of turn. */
    double bfR,   /* Radius of turn. */ 
    double trD,   /* Angle spanned by the circular part of the backflow loop, before torsion. */
    double bmW,   /* Torsion angle of backflow loop at beginning of the turn. */
    double beW,   /* Torsion angle of backflow loop at end of the turn. */
    double t      /* Time argument. */
  );
  /* Computes the position for a time {t} along the turn part of the 
    backflow channel (the toroidal helix), assuming that the whole turn is
    traversed with time varying from {tm} to {te}. */

/* IMPLEMENTATION */

void mtv_valve_backloop_make 
  ( ppv_array_t *a, 
    double inR,   /* Inner radius of main tube. */
    double otR,   /* Outer radius or main tube. */
    r3_path_state_t *bbS, /* State at beginning of untwisted backflow channel. */
    r3_path_state_t *beS, /* State at end of untwisted backflow channel. */
    double bfR,   /* Radius of backflow loop. */ 
    double trD,   /* Angle spanned by the circular part of the backflow loop, before torsion. */
    double bmW,   /* Torsion angle of backflow loop at beginning of the turn. */
    double beW,   /* Torsion angle of backflow loop at end of the turn. */
    double fuzzR, /* Range of distance function. */
    bool_t sub,   /* FALSE to lay down the tubes,TRUE to clear out the hole. */
    r3_path_state_t *btS0, /* (OUT) State at beginning of backloop, with twist. */
    r3_path_state_t *btS1  /* (OUT) State at end of backflow loop,with twist. */
  )
  {
    bool_t debug = FALSE;
    if (debug) { fprintf(stderr, "enter %s\n", __FUNCTION__); }

    /* Allocate a list of intermediate states along the path: */
    int ns = 9; /* States in the stem part (including start, excluding joint). */
    int nt = 10; /* States in the turn part (including joint and end). */
    int n = ns + nt; /* Total number of states along backflow channel. */
    voxm_path_state_t S[n];

    /* Guess the torsion angle {bmW} at the joint between the stem and turn arcs: */
    double dW = beW - bmW; /* Total torsion angle of turn part. */
    double stL = r3_dist(&(bbS->p), &(beS->p)); /* Estimated length of stem arc. */
    double trL = bfR*hypot(trD, dW/2); /* Estimated length of turn arc. */

    /* The whole backflow channel is parametrized with {t} varying from {tb=0} to {te=1}. */
    double tb = 0.0; /* Nominal time at start of backflow channel. */
    double te = 1.0; /* Nominal time at end of backflow channel. */

    /* Iterative adjustment of lengths, times, and velocities: */ 
    int iter;
    int niter = 5; /* Adjustment iterations. */
    for (iter = 0; iter < niter; iter++)
      { 
        /* Estimate time at joint: */
        double tm = stL/(stL + trL); /* Nominal time at junction between the two arcs. */

       if (debug) { fprintf(stderr, "  stL = %8.2f  trL = %8.2f  tm = %8.4f\n", stL, trL, tm); }

        /* Generate states {S[ns..n-1]} along the circular part, with times from {tm} to {te}: */
        int it; /* Counts states in turn arc, from end. */
        for (it = 0; it <= nt-1; it++)
          { double f = ((double)it)/((double)nt-1); /* Fractional position along turn segment. */
            double t = (1-f)*tm + f*te; /* Nominal time at that point. */
            int i = ns + it;
            assert((i >= 0) && (i < n));
            S[i] = mtv_valve_backloop_get_turn_state(beS, tm,te, bfR,trD,bmW,beW, t);
            if (debug) 
              { fprintf(stderr, "i = %d ", i);
                voxm_path_state_debug(stderr, &(S[i]), "  ", "backloop turn"); 
              }
          }

        /* Interpolate some states in the stem arc: */
        double vb = stL/(tm - tb); /* Estimated velocity at start of stem arc. */
        S[0] = voxm_path_state_from_r3_path_state(tb, bbS, vb);
        voxm_path_interpolate_some(&(S[0]), ns+1);
        if (debug) 
          { int i;
            for (i = 0; i <= ns; i++)
              { fprintf(stderr, "i = %d ", i);
                voxm_path_state_debug(stderr, &(S[i]), "  ", "backloop stem"); 
              }
          }
        
        /* Smooth out the joint a little: */
        int nw = 1; /* Number of states to smooth on each side of joint. */
        assert((ns-1-nw >= 0) && (ns+1+nw < n));
        voxm_path_interpolate_some(&(S[ns-1-nw]), 2*nw+3);
        
        /* Recompute lengths {stL,trL}: */
        stL = 0.0; trL = 0.0;
        int k;
        for (k = 0; k <= n-2; k++)
          { assert(k+1 < n);
            voxm_path_state_t *S0 = &(S[k]);
            voxm_path_state_t *S1 = &(S[k+1]);
            double Lk = voxm_path_length_estimate(S0, S1, 2);
            if (k < ns) { stL += Lk; } else {trL += Lk; }
          }
    }

    /* Now splat them: */
    int k;
    for (k = 0; k <= n-2; k++)
       { assert(k+1 < n);
         voxm_path_state_t *S0 = &(S[k]);
         voxm_path_state_t *S1 = &(S[k+1]);
         voxm_splat_tube_round_segment(a, S0, S1, inR, otR, fuzzR, sub); 
       }

    if (debug) { fprintf(stderr, "exit %s\n", __FUNCTION__); }
  }

voxm_path_state_t mtv_valve_backloop_get_turn_state
  ( r3_path_state_t *beS, /* State at end of untwisted backflow channel. */
    double tm,    /* Nominal time at beginning of the turn. */
    double te,    /* Nominal time at end of turn. */
    double bfR,   /* Radius of turn. */ 
    double trD,   /* Angle spanned by the circular part of the backflow loop, before torsion. */
    double bmW,   /* Torsion angle of backflow loop at beginning of the turn. */
    double beW,   /* Torsion angle of backflow loop at end of the turn. */
    double t      /* Time argument. */
  )
  {
    voxm_path_state_t S;
    S.t = t;
    
    /* Compute position: */
    S.p = mtv_valve_backloop_get_turn_position(beS, tm,te, bfR,trD,bmW,beW, t);
    
    /* Compute velocity by numeric differentiation: */
    double dt = 0.001;
    r3_t pm = mtv_valve_backloop_get_turn_position(beS, tm,te, bfR,trD,bmW,beW, t-dt);
    r3_t pp = mtv_valve_backloop_get_turn_position(beS, tm,te, bfR,trD,bmW,beW, t+dt);
    r3_mix(-0.5/dt, &pm, +0.5/dt, &pp, &(S.v));
    
    return S;
  }
    
r3_t mtv_valve_backloop_get_turn_position
  ( r3_path_state_t *beS, /* State at end of untwisted backflow channel. */
    double tm,    /* Nominal time at beginning of the turn. */
    double te,    /* Nominal time at end of turn. */
    double bfR,   /* Radius of turn. */ 
    double trD,   /* Angle spanned by the circular part of the backflow loop, before torsion. */
    double bmW,   /* Torsion angle of backflow loop at beginning of the turn. */
    double beW,   /* Torsion angle of backflow loop at end of the turn. */
    double t      /* Time argument. */
  )
  {
    /* Compute the fractional position: */
    double f = (te - t)/(te - tm); /* 0 at end of turn, 1 at joint. */
    
    /* Coordinates before torsion, relative to the {beS} state: */
    double aA = f*trD;    /* Latitude on torus, from end. */
    double cA = cos(aA);
    double sA = sin(aA);
    double X1 = bfR*sA;          
    double Y1 = -bfR*(1 - cA);  
    /* double Z1 = 0.0; */  
    
    /* Coordinates after torsion, relative to the {beS} state: */
    double aW = f*bmW + (1-f)*beW; /* Longitude on torus, from main channel osculating plane. */
    double cW = cos(aW);
    double sW = sin(aW);
    double X2 = X1;    
    double Y2 = Y1*cW;     
    double Z2 = -Y1*sW;       
    
    /* Get absolute coordinates: */
    r3_t p = (r3_t){{ X2, Y2, Z2 }};
    r3x3_map_row(&p, &(beS->M), &p);
    r3_add(&(beS->p), &p, &p);
    
    return p;
  }
