#include <stdlib.h>

/*---------------------------------------------------------------------------
  Compute spherical harmonic expansion up to order 2 (quadratic terms)
  npt     : number of points to evaluate at
  wt      : coefficient vector [9]  (r**2 = x**2+y**2)
            [0] = 1
            [1] = x  [2] = y   [3] = z
            [4] = 2*z**2-r**2  [5] = xz [6] = yz  [7] = x**2-y**2 [8] = xy
  x, y, z : vectors of points at which to evaluate [npt]
  v       : output vector [npt]
-----------------------------------------------------------------------------*/

void warp3D_sharm2( int npt , float *wt ,
                    float *x , float *y , float *z , float *v )
{
   int i ;
   float xq,yq,zq , a , b1,b2,b3 , c4,c5,c6,c7,c8 ;

   if( npt < 1 || wt == NULL || x == NULL ||
                  y  == NULL || z == NULL || v == NULL ) return ;

   a  = wt[0] ; b1 = wt[1] ; b2 = wt[2] ; b3 = wt[3] ;
   c4 = wt[4] ; c5 = wt[5] ; c6 = wt[6] ; c7 = wt[7] ; c8 = wt[8] ;

   for( i=0 ; i < npt ; i++ ){
      xq = x[i]*x[i] ; yq = y[i]*y[i] ; zq = z[i]*z[i] ;
      v[i] =  a + b2*y[i]
            + c4 * (2.0*zq-xq-yq)
            + c7 * (xq-yq)
            + (c5*x[i] + c6*y[i] + b3) * z[i]
            + (c8*y[i] + b1) * x[i] ;
   }
}

/*---------------------------------------------------------------------------*/

void warp3D_sharm2_grad( int npt , float *wt ,
                         float *x , float *y , float *z ,
                         float *gx, float *gy, float *gz )
{
   int i ;
   float b1,b2,b3 , c4,c5,c6,c7,c8 ;
   float gg,hh ;

   if( npt < 1   || wt == NULL || x == NULL ||
                    y  == NULL || z == NULL ||
      gx == NULL || gy == NULL || gz == NULL  ) return ;

                b1 = wt[1] ; b2 = wt[2] ; b3 = wt[3] ;
   c4 = wt[4] ; c5 = wt[5] ; c6 = wt[6] ; c7 = wt[7] ; c8 = wt[8] ;

   gg = 2.0*(c7-c4) ; hh = -2.0*(c4+c7) ;

   for( i=0 ; i < npt ; i++ ){
      gx[i] = b1 + gg*x[i] + c8*y[i] + c5*z[i] ;
      gy[i] = b2 + c8*x[i] + hh*y[i] + c6*z[i] ;
      gz[i] = b3 + c5*x[i] + c6*y[i] + c4*z[i] ;
   }
}


/*---------------------------------------------------------------------------
  Compute spherical harmonic expansion up to order 3 (cubic terms)
  npt     : number of points to evaluate at
  wt      : coefficient vector [16]  (r**2 = x**2+y**2)
            [0] = 1
            [1] = x  [2] = y   [3] = z
            [4] = 2*z**2-r**2  [5] = xz [6] = yz  [7] = x**2-y**2 [8] = xy
            [9]  = z*(2z**2-3*r**2)
            [10] = x*(4*z**2-r**2)
            [11] = y*(4*z**2-r**2)
            [12] = z*(x**2-y**2)
            [13] = xyz
            [14] = x*(x**2-3*y**2)
            [15] = y*(3*x**2-y**2)
  x, y, z : vectors of points at which to evaluate [npt]
  v       : output vector [npt]
-----------------------------------------------------------------------------*/

void warp3D_sharm3( int npt , float *wt ,
                    float *x , float *y , float *z , float *v )
{
   int i ;
   float xq,yq,zq,rr , a , b1,b2,b3 , c4,c5,c6,c7,c8 , d9,d10,d11,d12,d13,d14,d15;

   if( npt < 1 || wt == NULL || x == NULL ||
                  y  == NULL || z == NULL || v == NULL ) return ;

   a  = wt[0] ; b1 = wt[1] ; b2 = wt[2] ; b3 = wt[3] ;
   c4 = wt[4] ; c5 = wt[5] ; c6 = wt[6] ; c7 = wt[7] ; c8 = wt[8] ;

   d9  = wt[9]  ; d10 = wt[10] ; d11 = wt[11] ; d12 = wt[12] ;
   d13 = wt[13] ; d14 = wt[14] ; d15 = wt[15] ;

   for( i=0 ; i < npt ; i++ ){
      xq = x[i]*x[i] ; yq = y[i]*y[i] ; zq = z[i]*z[i] ; rr = xq+yq ;
      v[i] =  a
            + ( b2 + d15 * (3.0*xq-yq) ) * y[i]
            + c4 * (2.0*zq-xq-yq)
            + (c7 + d12*z[i] ) * (xq-yq)
            + (c5*x[i] + c6*y[i] + b3 + d9*(2.0*zq-3.0*rr) ) * z[i]
            + (c8*y[i] + b1 + d13*y[i]*z[i] + d14*(xq-3.0*yq) ) * x[i]
            + (d10*x[i] + d11*y[i])*(4.0*zq-rr) ;
   }
}

/*---------------------------------------------------------------------------
  Code for this was generated by Maple's codegen facility
-----------------------------------------------------------------------------*/

void warp3D_sharm3_grad( int npt , float *wt ,
                         float *x , float *y , float *z ,
                         float *gx, float *gy, float *gz )
{
   int i ;
   float xq,yq,zq,rr ,  b1,b2,b3 , c4,c5,c6,c7,c8 , d9,d10,d11,d12,d13,d14,d15;
   float df0,df1,df2,df3 , t9,t21,t28,t29,t32,t34,t40,t41,t43 ;

   if( npt < 1 || wt == NULL || x  == NULL ||
                  y  == NULL || z  == NULL ||
                  gx == NULL || gy == NULL || gz == NULL ) return ;

                b1 = wt[1] ; b2 = wt[2] ; b3 = wt[3] ;
   c4 = wt[4] ; c5 = wt[5] ; c6 = wt[6] ; c7 = wt[7] ; c8 = wt[8] ;

   d9  = wt[9]  ; d10 = wt[10] ; d11 = wt[11] ; d12 = wt[12] ;
   d13 = wt[13] ; d14 = wt[14] ; d15 = wt[15] ;

   for( i=0 ; i < npt ; i++ ){
     xq = x[i]*x[i];
     yq = y[i]*y[i];
     zq = z[i]*z[i];
     rr = xq+yq;
     t9 = d12*z[i];
     t21 = d13*y[i];
     t28 = d10*x[i];
     t29 = d11*y[i];
     t32 = 4.0*zq-rr;
     t34 = d9*z[i];
     df3 = 3.0*t34-t28-t29;
     df2 = 2.0*(c4+t34)+4.0*(t28+t29);
     t40 = d15*y[i];
     t41 = d14*x[i];
     t43 = df3;
     df1 = -t40-c4-c7-t9-3.0*t41+t43;
     df0 = 3.0*t40-c4+c7+t9+t41+t43;
     gx[i] = c5*z[i]+c8*y[i]+b1+t21*z[i]+d14*(xq-3.0*yq)+d10*t32+2.0*df0*x[i];
     gy[i] = b2+d15*(3.0*xq-yq)+c6*z[i]+(c8+d13*z[i])*x[i]+d11*t32+2.0*df1*y[i];
     gz[i] = d12*(xq-yq)+c5*x[i]+c6*y[i]+b3+d9*(2.0*zq-3.0*rr)+t21*x[i]+2.0*df2*z[i];
  }
}