/*
 * Copyright (c) 2014, Dignity Health
 *
 *     The GPI core node library is licensed under
 * either the BSD 3-clause or the LGPL v. 3.
 *
 *     Under either license, the following additional term applies:
 *
 *         NO CLINICAL USE.  THE SOFTWARE IS NOT INTENDED FOR COMMERCIAL
 * PURPOSES AND SHOULD BE USED ONLY FOR NON-COMMERCIAL RESEARCH PURPOSES.  THE
 * SOFTWARE MAY NOT IN ANY EVENT BE USED FOR ANY CLINICAL OR DIAGNOSTIC
 * PURPOSES.  YOU ACKNOWLEDGE AND AGREE THAT THE SOFTWARE IS NOT INTENDED FOR
 * USE IN ANY HIGH RISK OR STRICT LIABILITY ACTIVITY, INCLUDING BUT NOT LIMITED
 * TO LIFE SUPPORT OR EMERGENCY MEDICAL OPERATIONS OR USES.  LICENSOR MAKES NO
 * WARRANTY AND HAS NOR LIABILITY ARISING FROM ANY USE OF THE SOFTWARE IN ANY
 * HIGH RISK OR STRICT LIABILITY ACTIVITIES.
 *
 *     If you elect to license the GPI core node library under the LGPL the
 * following applies:
 *
 *         This file is part of the GPI core node library.
 *
 *         The GPI core node library is free software: you can redistribute it
 * and/or modify it under the terms of the GNU Lesser General Public License as
 * published by the Free Software Foundation, either version 3 of the License,
 * or (at your option) any later version. GPI core node library is distributed
 * in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even
 * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU Lesser General Public License for more details.
 *
 *         You should have received a copy of the GNU Lesser General Public
 * License along with the GPI core node library. If not, see
 * <http://www.gnu.org/licenses/>.
 */


/* bnispiralfill.c
 *
 * Author: Jim Pipe
 * Date: 2013 jul
 * Brief: takes coords generated by bnispiralgen.c and rotates them to their final
 *          3D orientations.  This could be SoS, FLORET, etc...
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>

extern "C"
{
    /* for parameter definitions */
    #include "bnispiralgen.c"
}

/* the base spiral sequence */

#ifndef MAX
#define UNDEFMAX
#define MAX(a,b) (((a)<(b))?(b):(a))
#endif

#ifndef MIN
#define UNDEFMIN
#define MIN(a,b) (((a)>(b))?(b):(a))
#endif

#define Phi_PI 3.141592653589793
void bnispiralfill(double* spparams, int maxarray, float* gxarray, float* gyarray, float* gzarray,
                 Array<double> &garray, Array<double> &karray, Array<double> &ktmp, double alpha0,
                 int rebin, double xdel, double ydel, double zdel, double tread0, int spinout
                )
  {

  uint64_t i, j, k;
  double betastep, beta, cb, sb;
  double alphastep, alpha, ca, sa;
  double kx,ky,kz;
  double jscale;
  double goldangle;
  // AGA - unused, 2015-10-13
  // double jstep;
  double kxyscale, kzscale;

  double ix,iy,iz;
  int fix, fiy, fiz;
  int ix1, ix2, ix3, ix4;
  int iy1, iy2, iy3, iy4;
  int iz1, iz2, iz3, iz4;
  double delx, dely, delz;
  double delx_sq, dely_sq, delz_sq;
  double dgrast;
  double a0,a1,a2;
  double f1,f2,f3,f4;

  // DHW to mathch sequence goldangle = (3.-sqrt(5.))*M_PI;
  goldangle  = 137.508*Phi_PI/180;
  dgrast = 1./PHGRAST;

// kscales are gamma dt res, to convert gradients to k-space
// These kscales will also make the default zeropadded resolution
// 0.8 * prescribed resolution for cylinder (inplane) and sphere
// and 1.0 * prescribed resolution for cylinder (through-plane).
// This should be easier to deal with to calculate the FOV
  if (spparams[spSTYPE] == 0) { // ARCH
    kxyscale = spparams[spGAMMA]*PHGRAST*spparams[spRESXY]*0.8*2./sqrt(M_PI);
    kzscale  = spparams[spGAMMA]*PHGRAST*spparams[spRESZ];
    }
  if (spparams[spSTYPE] == 1) { // CYL DST
    kxyscale = spparams[spGAMMA]*PHGRAST*spparams[spRESXY]*0.8*2./sqrt(M_PI);
    kzscale  = spparams[spGAMMA]*PHGRAST*spparams[spRESZ];
    }
  if (spparams[spSTYPE] == 2) { // SPH DST
    kxyscale = spparams[spGAMMA]*PHGRAST*spparams[spRESXY]*0.8*pow(6./M_PI,1./3.);
    kzscale  = spparams[spGAMMA]*PHGRAST* spparams[spRESZ]*0.8*pow(6./M_PI,1./3.);
    }
  if (spparams[spSTYPE] == 3) { // FLORET
    kxyscale = spparams[spGAMMA]*PHGRAST*spparams[spRESXY]*0.8*pow(6./M_PI,1./3.);
    kzscale  = spparams[spGAMMA]*PHGRAST* spparams[spRESZ]*0.8*pow(6./M_PI,1./3.);
    }

//***************
// ARCH
//***************
  if (spparams[spSTYPE] == 0) { // ARCH
    betastep = -2.*M_PI/(double)(garray.size(2)); //DHW match psd add -
    for(j=0;j<garray.size(2);j++) { // arms
      beta = (double)(j)*betastep;
      cb = cos(beta);
      sb = sin(beta);
      for(i=0;i<garray.size(1);i++) { // points
        garray(0,i,j) = cb*gxarray[i] - sb*gyarray[i];
        garray(1,i,j) = cb*gyarray[i] + sb*gxarray[i];
        garray(2,i,j) = 0.;
        } // i
      } // j

// Now integrate the gradients and scale to k-space
    for(j=0;j<garray.size(2);j++) { // arms
      ktmp(0,0,j,0) = kxyscale*garray(0,0,j);
      ktmp(1,0,j,0) = kxyscale*garray(1,0,j);
      ktmp(2,0,j,0) = 0.;
      for(i=1;i<garray.size(1);i++) { // points
        ktmp(0,i,j,0) = ktmp(0,i-1,j,0) + kxyscale*garray(0,i,j);
        ktmp(1,i,j,0) = ktmp(1,i-1,j,0) + kxyscale*garray(1,i,j);
        ktmp(2,i,j,0) = 0.;
        }
      }

    } // ARCH

//***************
// CYL DST
//***************
// (Note all arms in in dimensions[2])

  if (spparams[spSTYPE] == 1) { // CYL DST
    int jc = round(garray.size(2)/2); //DHW
    for(j=0;j<garray.size(2);j++) { // arms
      beta = -(((double) j)-jc)*goldangle; //DHW
      cb = cos(beta);
      sb = sin(beta);
      jscale = (2.*(double)(j)/(double)(garray.size(2)))-1.;
      for(i=0;i<garray.size(1);i++) { // points
        garray(0,i,j) = cb*gxarray[i] - sb*gyarray[i];
        garray(1,i,j) = cb*gyarray[i] + sb*gxarray[i];
        garray(2,i,j) = jscale * gzarray[i];
        } // i
      } // j

// Now integrate the gradients and scale to k-space
    for(j=0;j<garray.size(2);j++) { // arms
      jscale = (2.*(double)(j)/(double)(garray.size(2)))-1.;
      ktmp(0,0,j,0) = kxyscale*garray(0,0,j);
      ktmp(1,0,j,0) = kxyscale*garray(1,0,j);
      ktmp(2,0,j,0) = jscale*0.5*(1.-spparams[spTAPER]);
      for(i=1;i<garray.size(1);i++) { // points
        ktmp(0,i,j,0) = ktmp(0,i-1,j,0) + kxyscale*garray(0,i,j);
        ktmp(1,i,j,0) = ktmp(1,i-1,j,0) + kxyscale*garray(1,i,j);
        ktmp(2,i,j,0) = ktmp(2,i-1,j,0) +  kzscale*garray(2,i,j);
        } // i
      } // j

    } // CYL DST

//***************
// SPH DST
//***************
// (Note all arms are in dimensions[2])

  if (spparams[spSTYPE] == 2) { // SPH DST
    int jc = round(garray.size(2)/2); //DHW
    for(j=0;j<garray.size(2);j++) { // arms
      beta = -(((double) j)-jc)*goldangle; //DHW
      cb = cos(beta);
      sb = sin(beta);
      jscale = (2.*(double)(j)/(double)(garray.size(2)))-1.;
      for(i=0;i<garray.size(1);i++) { // points
        garray(0,i,j) = cb*gxarray[i] - sb*gyarray[i];
        garray(1,i,j) = cb*gyarray[i] + sb*gxarray[i];
        garray(2,i,j) = jscale * gzarray[i];
        } // i
      } // j

// Now integrate the gradients and scale to k-space
    for(j=0;j<garray.size(2);j++) { // arms
      jscale = (2.*(double)(j)/(double)(garray.size(2)))-1.;
      ktmp(0,0,j,0) = kxyscale*garray(0,0,j);
      ktmp(1,0,j,0) = kxyscale*garray(1,0,j);
      if(spinout == 0)
        ktmp(2,0,j,0) = jscale*0.5*(0.8*pow(6./M_PI,1./3.));
      else
        ktmp(2,0,j,0) = 0;
      for(i=1;i<garray.size(1);i++) { // points
        ktmp(0,i,j,0) = ktmp(0,i-1,j,0) + kxyscale*garray(0,i,j);
        ktmp(1,i,j,0) = ktmp(1,i-1,j,0) + kxyscale*garray(1,i,j);
        ktmp(2,i,j,0) = ktmp(2,i-1,j,0) +  kzscale*garray(2,i,j);
        } // i
      } // j

    } // SPH DST

//***************
// FLORET
//***************
// (Note all arms for each hub are in dimensions[2])
// hubs are in dimensions[3]

  if (spparams[spSTYPE] == 3) { // FLORET
    int hubs = ktmp.size(3);
    unsigned int arms = garray.size(2) / hubs;
    // alpha0 = 0.25*M_PI; // hardcoded for hubs to cover +/- 45 degrees
    alphastep = 2.*alpha0/(double)(arms);
    int jc = round(garray.size(2)/2);
    /* re-order interleaves */
    uint64_t setlength = arms / 34;
    uint64_t setind = 0;
    uint64_t curset;
    uint64_t curind;
    uint64_t subind;

    for(j=0;j<arms;j++) { // arms
      /* RKR re-order the FLORET arms to allow smooth transitions between
      * interleaves */
      if(rebin)
      {
          subind = j % setlength;
          setind = j / setlength;
          curset = (setind * 13) % 34;
          if (setind % 2 == 1)
          {
              subind = setlength - 1 - subind;
          }
          curind = subind * 34 + curset;
        beta = -((double) curind)*goldangle;
      }
      else
      {
          curind = j;
          beta = -(((double) j)-jc)*goldangle;
      }
      cb = cos(beta);
      sb = sin(beta);
      alpha = -alpha0 + ((double)(curind)*alphastep);
      ca = cos(alpha);
      sa = sin(alpha);
      for(i=0;i<garray.size(1);i++) { // points
        garray(0,i,j) = ca*(cb*gxarray[i] - sb*gyarray[i]);
        garray(1,i,j) = ca*(cb*gyarray[i] + sb*gxarray[i]);
        garray(2,i,j) = sa*gzarray[i];
        } // i
      } // j

// Now integrate the x-y gradients, scale to k-space
// kz = tan(alpha) * |{kx,ky}|
// also calculate gz = dkz/kscale
    for(j=0;j<arms;j++) { // arms
      // alpha = -alpha0 + ((double)(j)*alphastep);
      // ta = tan(alpha);
      kx = kxyscale*garray(0,0,j);
      ky = kxyscale*garray(1,0,j);
      kz = kzscale*garray(2,0,j);
      ktmp(0,0,j,0) = kx;
      ktmp(1,0,j,0) = ky;
      ktmp(2,0,j,0) = kz;
      // fill in other 2 hubs
      if(hubs > 1)
      {
        ktmp(0,0,j,1) = ktmp(1,0,j,0);
        ktmp(1,0,j,1) = ktmp(2,0,j,0);
        ktmp(2,0,j,1) = ktmp(0,0,j,0);
        garray(0,0,j+arms) = garray(1,0,j);
        garray(1,0,j+arms) = garray(2,0,j);
        garray(2,0,j+arms) = garray(0,0,j);
      }
      if(hubs > 2)
      {
        ktmp(0,0,j,2) = -ktmp(2,0,j,0);
        ktmp(1,0,j,2) = ktmp(1,0,j,0);
        ktmp(2,0,j,2) = ktmp(0,0,j,0);
        garray(0,0,j+2*arms) = -garray(2,0,j);
        garray(1,0,j+2*arms) = garray(1,0,j);
        garray(2,0,j+2*arms) = garray(0,0,j);
      }
      for(i=1;i<garray.size(1);i++) { // points
        kx = ktmp(0,i-1,j,0) + kxyscale*garray(0,i,j);
        ky = ktmp(1,i-1,j,0) + kxyscale*garray(1,i,j);
        kz = ktmp(2,i-1,j,0) + kzscale*garray(2,i,j);
        ktmp(0,i,j,0) = kx;
        ktmp(1,i,j,0) = ky;
        ktmp(2,i,j,0) = kz;
        // fill in other 2 hubs
        if(hubs > 1)
        {
            ktmp(0,i,j,1) = ktmp(1,i,j,0);
            ktmp(1,i,j,1) = ktmp(2,i,j,0);
            ktmp(2,i,j,1) = ktmp(0,i,j,0);
            garray(0,i,j+arms) = garray(1,i,j);
            garray(1,i,j+arms) = garray(2,i,j);
            garray(2,i,j+arms) = garray(0,i,j);
        }
        if(hubs > 2)
        {
            ktmp(0,i,j,2) = -ktmp(2,i,j,0);
            ktmp(1,i,j,2) = ktmp(1,i,j,0);
            ktmp(2,i,j,2) = ktmp(0,i,j,0);
            garray(0,i,j+2*arms) = -garray(2,i,j);
            garray(1,i,j+2*arms) = garray(1,i,j);
            garray(2,i,j+2*arms) = garray(0,i,j);
        }
      } // i
    } // j

  } // FLORET

//*************************************************************************************
// interpolate ktmp to kout
//*************************************************************************************
// consider simplifying kz interp for ARCH and CYL DST (it's just a constant for fixed k)
// also, consider greatly simplifying for ARCH (calculate over 1 kz plane, copy to rest)

// For interpoloating along kx:
// We first find ix where kout(i,...) = ktmp(ix,....).
// ix is a real number between ix2 = floor(ix) and ix3 = ceiling (ix)
// and delx = ix - ix2, i.e.
//
//                  delx
//                |<---->|
//    *           *      O    *           *
//   ix1         ix2     ix  ix3         ix4
//
// Now if
// f1 = ktmp(ix1,...)
// f2 = ktmp(ix2,...)
// f3 = ktmp(ix3,...)
// f4 = ktmp(ix4,...)
//
// We do a simple quadratic interopolation based on f1, f2, and f3, i.e.

// f(ix) = 0.5*(f1+f3-2f2)*delx^2 + 0.5*(f3-f1)*delx + f2;

// and another similar quadratic interopolation based on f2, f3, and f4.
// We then take their average. This average gives the coeffificents a0, a1, and a2
// as a function of f1, f2, f3, and f4.  (simple math :-)
// This interpolation has the nice
// feature that it passes exactly through f2 and f3
// It's only piecewise smooth, but I think that's OK.

// for interpolating along ky and kz, do the same (but with iy1, iz1, etc.)

  int64_t M = ktmp.size(1); //DHW
  for(i=0;i<karray.size(1);i++) { // points
// Note these times are in usec
    ix = (((double)(i)*spparams[spDWELL])-xdel+tread0)*dgrast;
    iy = (((double)(i)*spparams[spDWELL])-ydel+tread0)*dgrast;
    iz =  ((double)(i)*spparams[spDWELL]-zdel+tread0)*dgrast;

    fix = floor(ix);
    fiy = floor(iy);
    fiz = floor(iz);

    ix1 = MAX(0,fix - 1);
    ix2 = MAX(0,fix);
    ix3 = MAX(0,fix + 1);
    ix4 = MAX(0,fix + 2);
    //DHW (begin)
    ix1 = MIN(ix1, M-1);
    ix2 = MIN(ix2, M-1);
    ix3 = MIN(ix3, M-1);
    ix4 = MIN(ix4, M-1);
    //DHW (end)
    delx = MAX(0.,ix-(double)(ix2));
    delx_sq = delx*delx;

    iy1 = MAX(0,fiy - 1);
    iy2 = MAX(0,fiy);
    iy3 = MAX(0,fiy + 1);
    iy4 = MAX(0,fiy + 2);
    //DHW (begin)
    iy1 = MIN(iy1, M-1);
    iy2 = MIN(iy2, M-1);
    iy3 = MIN(iy3, M-1);
    iy4 = MIN(iy4, M-1);
    //DHW (end)
    dely = MAX(0.,iy-(double)(iy2));
    dely_sq = dely*dely;

    iz1 = MAX(0,fiz - 1);
    iz2 = MAX(0,fiz);
    iz3 = MAX(0,fiz + 1);
    iz4 = MAX(0,fiz + 2);
    //DHW (begin)
    iz1 = MIN(iz1, M-1);
    iz2 = MIN(iz2, M-1);
    iz3 = MIN(iz3, M-1);
    iz4 = MIN(iz4, M-1);
    //DHW (end)
    delz = MAX(0.,iz-(double)(iz2));
    delz_sq = delz*delz;

    for(j=0;j<karray.size(2);j++) { // arms
      for(k=0;k<karray.size(3);k++) { // planes
// X first
        f1 = ktmp(0,ix1,j,k);
        f2 = ktmp(0,ix2,j,k);
        f3 = ktmp(0,ix3,j,k);
        f4 = ktmp(0,ix4,j,k);
        a2 = 0.25*(f1-f2-f3+f4);
        a1 = 0.25*(-f1 - 3.*f2 + 5.*f3 - f4);
        a0 = f2;
        karray(0,i,j,k) = a2*delx_sq + a1*delx + a0;
// Y second
        f1 = ktmp(1,iy1,j,k);
        f2 = ktmp(1,iy2,j,k);
        f3 = ktmp(1,iy3,j,k);
        f4 = ktmp(1,iy4,j,k);
        a2 = 0.25*(f1-f2-f3+f4);
        a1 = 0.25*(-f1 - 3.*f2 + 5.*f3 - f4);
        a0 = f2;
        karray(1,i,j,k) = a2*dely_sq + a1*dely + a0;
// Z third
        f1 = ktmp(2,iz1,j,k);
        f2 = ktmp(2,iz2,j,k);
        f3 = ktmp(2,iz3,j,k);
        f4 = ktmp(2,iz4,j,k);
        a2 = 0.25*(f1-f2-f3+f4);
        a1 = 0.25*(-f1 - 3.*f2 + 5.*f3 - f4);
        a0 = f2;
        karray(2,i,j,k) = a2*delz_sq + a1*delz + a0;
    } } } // i j k

  } // bnispiralfill


/* undo common macro names */
#ifdef UNDEFMAX
#undef MAX
#endif

#ifdef UNDEFMIN
#undef MIN
#endif
