// for license information, see the accompanying LICENSE file

#include <stdio.h>

#include <stdlib.h>

#include <math.h>

#include <assert.h>

// CBLAS, LAPACKE are f2c translated Netlib F77 reference routines.
// 'cblas.h' is widely supported, and is the f2c translated Netlib F77 refernce version.
// There are known issues with name mangling schemes regarding wrapped code and inter-language operability between Fortran and C.
//
// Netlib CBLAS / LAPACKE target
// #define LISE_LA_REF
// #include <lapacke.h>
// #include <cblas.h>
//
// IBM ESSL target
// #define LISE_LA_ESSL
// #include <essl.h>
//
// Intel MKL target
// #define LISE_LA_MKL
// #include <mkl.h>

#ifdef LISE_LA_MKL
#include <mkl.h>
double ddot(const int *, const double *, const int *, const double *, const int *);
void daxpy(const int *, const double *, const double *, const int *, double *, const int *);
void dgemv(const char *, const int *, const int *, const double *, const double *, const int *, const double *, const int *, const double *, double *, const int *);
void dcopy(const int *, const double *, const int *, double *, const int *);
void dgemm(const char *, const char *, const int *, const int *, const int *, const double *, const double *, const int *, const double *, const int *, const double *, double *, const int *);
void dgesdd(const char *, const int *, const int *, double *, const int *, double *, double *, const int *, double *, const int *, double *, const int *, int *, int *);
void dger(const int *, const int *, const double *, const double *, const int *, const double *, const int *, double *, const int *);
#endif

#ifdef LISE_LA_ESSL
#include <essl.h>
double ddot(int, const double *, int, const double *,int);
void daxpy(int,  double,  double *, int,  double *,  int);
void dgemv(const char *, int, int,  double, const void *, int, const  double *, int,  double,  double *, int);
void dcopy(int,  double *, int,  double *, int);
void dgemm(const char *, const char *, int, int, int,  double, const void *, int, const void *, int,  double, void *, int);
void dgesdd(const char *, int, int, void *, int, double *, void *, int, void *, int, double *, int, int *, int *);
void dger(int, int,  double, const  double *, int, const  double *, int, void *, int);
// void dger1(int, int,  double, const  double *, int, const  double *, int, void *, int);
#endif

#ifdef LISE_LA_REF
#include <lapacke.h>
#include <cblas.h>
double cblas_ddot(const int N, const double *X, const int incX, const double *Y, const int incY);
void cblas_daxpy(const int N, const double alpha, const double *X, const int incX, double *Y, const int incY);
void cblas_dgemv(CBLAS_LAYOUT layout, CBLAS_TRANSPOSE TransA, const int M, const int N, const double alpha, const double *A, const int lda, const double *X, const int incX, const double beta, double *Y, const int incY);
void cblas_dcopy(const int N, const double *X, const int incX, double *Y, const int incY);
void cblas_dgemm(CBLAS_LAYOUT layout, CBLAS_TRANSPOSE TransA, CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const double alpha, const double *A, const int lda, const double *B, const int ldb, const double beta, double *C, const int ldc);
void cblas_dger(CBLAS_LAYOUT layout, const int M, const int N, const double alpha, const double *X, const int incX, const double *Y, const int incY, double *A, const int lda);
lapack_int LAPACKE_dgesdd( int matrix_layout, char jobz, lapack_int m, lapack_int n, double* a, lapack_int lda, double* s, double* u, lapack_int ldu, double* vt, lapack_int ldvt );
#endif

double ddotII( int * , double * , int * , double * , int * ) ;
void daxpyII( int * , double * , double * , int * , double * , int * ) ;
int broyden_minB( double * v_in , double * v_in_old , double * f_old , double * b , double * v_out , const int n , int * it_out , const double alpha ) ;
int broyden_min( double * v_in , double * v_in_old , double * f_old , double * b_bra , double * b_ket , double * d , double * v_out , const int n , int * it_out , const double alpha ) ;
int broydenMod_min( double * v_in , double * v_in_old , double * f_old , double * b_bra , double * b_ket , double * d , double * v_out , const int n , int * it_out , const double alpha ) ;

void broyden_min_( double * v_in , double * v_in_old , double * f_old , double * b_bra , double * b_ket , double * d , double * v_out , int * n , int * it_out , double * alpha )

{
  broydenMod_min( v_in , v_in_old , f_old , b_bra , b_ket , d , v_out , * n , it_out , * alpha ) ;
}

void broyden_minb_( double * v_in , double * v_in_old , double * f_old , double * b_bra , double * v_out , int * n , int * it_out , double * alpha )

{

  broyden_minB( v_in , v_in_old , f_old , b_bra , v_out , * n , it_out , * alpha ) ;

}

int broyden_min( double * v_in , double * v_in_old , double * f_old , double * b_bra , double * b_ket , double * d , double * v_out , const int n , int * it_out , const double alpha )

{

  double * f , * dv , * df ;

  int i , ii , it1 ;

  int ione = 1 ;

  double norm , norm1 , norm2 ;

  int n1 , ishift ;

  int it ;

  it = * it_out ;

  n1 = n ;

  if( it == 0 )

    {

      * it_out = 1 ;

      for( i = 0 ; i < n ; ++i ) {

	v_in_old[ i ] = v_in[ i ] ;

	f_old[ i ] = alpha * ( v_in[ i ] - v_out[ i ] ) ;

	v_in[ i ]  -= f_old[ i ] ;

	v_out[ i ] = v_in[ i ] ;

      }

      return( 0 ) ;

    }

  assert( f = malloc( n * sizeof( double ) ) ) ;

  assert( df = malloc( n * sizeof( double ) ) ) ;

  assert( dv = malloc( n * sizeof( double ) ) ) ;

  it1 = it - 1 ;

  ishift = it1 * n ;

  norm = 0. ; /* || F || */

  norm1 = 0. ; /* || F_old || */

  for( i = 0 ; i < n ; i++ )

    {

      f[ i ] =  alpha * ( v_in[ i ] - v_out[ i ] ) ;

      norm += f[ i ] * f[ i ] ;

      norm1 += f_old[ i ] * f_old[ i ] ;

      dv[ i ] = v_in[ i ] - v_in_old[ i ] ;

      df[ i ] = f[ i ] - f_old[ i ] ;

      b_ket[ ishift + i ] = df[ i ] ;

      b_bra[ ishift + i ] = dv[ i ] ;

    }

  for( ii = 0 ; ii < it1 ; ++ii )

    {

#ifdef LISE_LA_MKL
      norm =  d[ ii ] * ddot( &n1 , df , &ione , b_bra + ii * n , &ione ) ;
      daxpy( &n1 , &norm , b_ket + ii * n , &ione , b_ket + ishift , &ione ) ;
      norm = d[ ii ] * ddot( &n1 , dv , &ione , b_ket + ii * n , &ione ) ;
      daxpy( &n1 , &norm , b_bra + ii * n , &ione , b_bra + ishift ,&ione ) ;
#endif
#ifdef LISE_LA_ESSL
      norm =  d[ ii ] * ddot( n1 , df , ione , b_bra + ii * n , ione ) ;
      daxpy( n1 , norm , b_ket + ii * n , ione , b_ket + ishift , ione ) ;
      norm = d[ ii ] * ddot( n1 , dv , ione , b_ket + ii * n , ione ) ;
      daxpy( n1 , norm , b_bra + ii * n , ione , b_bra + ishift ,ione ) ;
#endif
#ifdef LISE_LA_REF
//double cblas_ddot(const int N, const double *X, const int incX, const double *Y, const int incY);
//void cblas_daxpy(const int N, const double alpha, const double *X, const int incX, double *Y, const int incY);
      norm =  d[ ii ] * cblas_ddot( n1 , df , ione , b_bra + ii * n , ione ) ;
      cblas_daxpy( n1 , norm , b_ket + ii * n , ione , b_ket + ishift , ione ) ;
      norm = d[ ii ] * cblas_ddot( n1 , dv , ione , b_ket + ii * n , ione ) ;
      cblas_daxpy( n1 , norm , b_bra + ii * n , ione , b_bra + ishift ,ione ) ;
#endif

    }

#ifdef LISE_LA_MKL
  norm = ddot(  &n1 , dv , &ione , b_ket + ishift , &ione ) ;
#endif
#ifdef LISE_LA_ESSL
  norm = ddot(  n1 , dv , ione , b_ket + ishift , ione ) ;
#endif
#ifdef LISE_LA_REF
  norm = cblas_ddot(  n1 , dv , ione , b_ket + ishift , ione ) ;
#endif

  printf( "<dv|B|df> =%12.6e\n", norm ) ;

  if( fabs( norm ) < 1.e-15 ) return( 0 ) ;

  for( i = 0 ; i < n ; ++i )

    b_ket[ ishift + i ] = ( dv[ i ] - b_ket[ ishift + i ] ) / norm ;

#ifdef LISE_LA_MKL
  norm1 = sqrt( ddot( &n1 , b_ket + ishift , &ione , b_ket + ishift , &ione ) ) ;
  norm2 = sqrt( ddot( &n1 , b_bra + ishift , &ione , b_bra + ishift , &ione ) ) ;
#endif
#ifdef LISE_LA_ESSL
  norm1 = sqrt( ddot( n1 , b_ket + ishift , ione , b_ket + ishift , ione ) ) ;
  norm2 = sqrt( ddot( n1 , b_bra + ishift , ione , b_bra + ishift , ione ) ) ;
#endif
#ifdef LISE_LA_REF
  norm1 = sqrt( cblas_ddot( n1 , b_ket + ishift , ione , b_ket + ishift , ione ) ) ;
  norm2 = sqrt( cblas_ddot( n1 , b_bra + ishift , ione , b_bra + ishift , ione ) ) ;
#endif

  for( i = 0 ; i < n ; i++ )

    {

      b_ket[ ishift + i ] = b_ket[ ishift + i ] / norm1 ;

      b_bra[ ishift + i ] = b_bra[ ishift + i ] / norm2 ;

    }

  d[ it1 ] = norm1 * norm2 ;

  for( i = 0 ; i < n ; i++)
    {

      v_in_old[ i ] = v_in[ i ] ;

      v_in[ i ] -= f[ i ] ;

    }

  for( ii = 0 ; ii < it ; ii++ )

    {

#ifdef LISE_LA_MKL
      norm = - d[ ii ] * ddot( &n1 , b_bra + ii * n , &ione , f , &ione ) ;
      daxpy( &n1 , &norm , b_ket + ii * n , &ione , v_in , &ione ) ;
#endif
#ifdef LISE_LA_ESSL
      norm = - d[ ii ] * ddot( n1 , b_bra + ii * n , ione , f , ione ) ;
      daxpy( n1 , norm , b_ket + ii * n , ione , v_in , ione ) ;
#endif
#ifdef LISE_LA_REF
      norm = - d[ ii ] * cblas_ddot( n1 , b_bra + ii * n , ione , f , ione ) ;
      cblas_daxpy( n1 , norm , b_ket + ii * n , ione , v_in , ione ) ;
#endif

    }

  for( i = 0 ; i < n ; i++ )

    {

      f_old[ i ] = f[ i ] ;

      v_out[ i ] = v_in[ i ] ;

    }

  ( * it_out ) ++ ;

  free( f ) ; free( dv ) ; free( df ) ;

  return( 0 ) ;

}

int broyden_minB( double * v_in , double * v_in_old , double * f_old , double * b , double * v_out , const int n , int * it_out , const double alpha )

{

  double * f , * dv , * df , * bdf , * bdv ;

  int i , ii , it1 ;

  int ione = 1 ;

  double norm , norm1 , norm2 ;

  int n1 , ishift ;

  int it , j ;

  double * u , * vt , * s , * work , wk_[ 2 ] ;

  int lwork = -1 , info , * iwork ;

  it = * it_out ;

  printf( "Starting Broyden %d \n\n" , it ) ;

  n1 = n ;

  if( it == 0 )

    {

      * it_out = 1 ;

      for( i = 0 ; i < n ; ++i ) {

	v_in_old[ i ] = v_in[ i ] ;

	f_old[ i ] = alpha * ( v_in[ i ] - v_out[ i ] ) ;

	v_in[ i ]  -= f_old[ i ] ;

	v_out[ i ] = v_in[ i ] ;

	for( j = 0 ; j < n ; j++ )

	  b[ i + n * j ] = 0. ;

	b[ i + n * i ] = 1. ;

      }

      return( 0 ) ;

    }

  assert( f = malloc( n * sizeof( double ) ) ) ;

  assert( df = malloc( n * sizeof( double ) ) ) ;

  assert( dv = malloc( n * sizeof( double ) ) ) ;

  assert( bdv = malloc( n * sizeof( double ) ) ) ;

  assert( bdf = malloc( n * sizeof( double ) ) ) ;

  it1 = it - 1 ;

  ishift = it1 * n ;

  for( i = 0 ; i < n ; i++ )

    {

      f[ i ] =  alpha * ( v_in[ i ] - v_out[ i ] ) ;

      dv[ i ] = v_in[ i ] - v_in_old[ i ] ;

      df[ i ] = f[ i ] - f_old[ i ] ;

    }

  norm = 0. ;

  for( i = 0 ; i < n ; i ++ ) {

    bdv[ i ] = 0. ;

    bdf[ i ] = dv[ i ] ;

    for( j = 0 ; j < n ; j++ ){

      bdv[ i ] += b[ j + n * i ] * dv[ j ] ;

      bdf[ i ] -= b[ i + n * j ] * df[ j ] ;

    }

    norm += df[ i ] * bdv[ i ] ;

  }

  printf( "<dv|B|df> =%12.6e\n", norm ) ;

  norm = 1./norm ;

#ifdef LISE_LA_REF
  cblas_dger( CBLASColMajor, n1 , n1 , norm , bdf , ione , bdv , ione , b , n1 ) ;
#endif
#ifdef LISE_LA_ESSL
  dger( n1 , n1 , norm , bdf , ione , bdv , ione , b , n1 ) ;
#endif
#ifdef LISE_LA_MKL
// void dger(const int *, const int *, const double *, const double *, const int *, const double *, const int *, void *, const int *);
  dger( &n1 , &n1 , &norm , bdf , &ione , bdv , &ione , b , &n1 ) ;
#endif

  if( it > 5 ){

  /* add a SVD decomposition of matrix B */

    assert( s = malloc( n * sizeof( double ) ) ) ;

    assert( u = malloc( n * n  * sizeof( double ) ) ) ;

    assert( vt = malloc( n * n * sizeof( double ) ) ) ;

    assert( iwork = malloc( 8 * n * sizeof( double ) ) ) ;

#ifdef LISE_LA_MKL
    dgesdd( "A", &n1 , &n1 , b , &n1 , s , u , &n1 , vt , &n1 , wk_ , &lwork , iwork , &info ) ;
#endif
#ifdef LISE_LA_ESSL
    dgesdd( "A", n1 , n1 , b , n1 , s , u , n1 , vt , n1 , wk_ , lwork , iwork , &info ) ;
#endif
#ifdef LISE_LA_REF
    info = LAPACKE_dgesdd(LAPACK_COL_MAJOR, 'A', n1 , n1 , b , n1 , s , u , n1 , vt , n1 ) ;
#endif

    lwork = ( int ) wk_[ 0 ] ;

    assert( work = malloc( lwork * sizeof( double ) ) ) ;

#ifdef LISE_LA_ESSL
    dgesdd( "A" , n1 , n1 , b , n1 , s , u , n1 , vt , n1 , work , lwork , iwork , &info ) ;
#endif
#ifdef LISE_LA_REF
    info = LAPACKE_dgesdd(LAPACK_COL_MAJOR, 'A', n1 , n1 , b , n1 , s , u , n1 , vt , n1 ) ;
#endif
#ifdef LISE_LA_MKL
    dgesdd( "A", &n1 , &n1 , b , &n1 , s , u , &n1 , vt , &n1 , work , &lwork , iwork , &info ) ;
#endif

    free( work ) ; free( iwork ) ;

    for( i = 0 ; i < n ; i++ ){

      if( s[ i ] < 1.e-6 ) {

	printf( " s[ %d ] = %g \n" , i , s[ i ] ) ;

	s[ i ] = 0. ;

      }

      for( j = 0 ; j < n ; j++ )

	vt[ i + j * n ] *= s[ i ] ;

    }

    free( s ) ;

    norm = 0. ;

    norm1 = 1. ;

#ifdef LISE_LA_ESSL
    dgemm( "N" , "N" , n1 , n1 , n1 , norm1 , u , n1 , vt , n1 , norm , b , n1 ) ;
#endif
#ifdef LISE_LA_MKL
// void dgemm(const char *, const char *, const int *, const int *, const int *, const double *, const double *, const int *, const double *, const int *, const double *, double *, const int *);
    dgemm( "N" , "N" , &n1 , &n1 , &n1 , &norm1 , u , &n1 , vt , &n1 , &norm , b , &n1 ) ;
#endif
#ifdef LISE_LA_REF
    cblas_dgemm(CBLAS_COL_MAJOR,"N" , "N" , n1 , n1 , n1 , norm1 , u , n1 , vt , n1 , norm , b , n1);
#endif

    free( u ) ; free( vt ) ;

  }

#ifdef LISE_LA_ESSL
  dcopy( n1 , v_in , ione , v_in_old , ione ) ;
#endif
#ifdef LISE_LA_MKL
//void dcopy(const int *, const double *, const int *, double *, const int *);
  dcopy( &n1 , v_in , &ione , v_in_old , &ione ) ;
#endif
#ifdef LISE_LA_REF
  cblas_dcopy( n1 , v_in , ione , v_in_old , ione ) ;
#endif

  norm1 = -1. ;

  norm = 1. ;

#ifdef LISE_LA_MKL
//void dgemv(const char *, const int *, const int *, const double *, const double *, const int *, const double *, const int *, const double *, double *, const int *);
  dgemv( "N" , &n1 , &n1 , &norm1 , b , &n1 , f , &ione , &norm , v_in , &ione ) ;
#endif
#ifdef LISE_LA_ESSL
  dgemv( "N" , n1 , n1 , norm1 , b , n1 , f , ione , norm , v_in , ione ) ;
#endif
#ifdef LISE_LA_REF
  cblas_dgemv(CBLAS_COL_MAJOR, "N" , n1 , n1 , norm1 , b , n1 , f , ione , norm , v_in , ione ) ;
#endif

  for( i = 0 ; i < n ; i++ )

    {

      f_old[ i ] = f[ i ] ;

      v_out[ i ] = v_in[ i ] ;

    }

  ( * it_out ) ++ ;

  free( f ) ; free( dv ) ; free( df ) ; free( bdf ) ; free( bdv ) ;

  printf( "Done with Broyden \n\n" ) ;

  return( 0 ) ;

}

int broydenMod_min( double * v_in , double * v_in_old , double * f_old , double * b_bra , double * b_ket , double * d , double * v_out , const int n , int * it_out , const double alpha )
/*

Change in the formula. Use Eq. (10) in J.Chem.Phys. 134 (2011) 134109

 */

{

  double * f , * dv , * df ;

  int i , ii , it1 ;

  int ione = 1 ;

  double norm , norm1 , norm2 ;

  int n1 , ishift ;

  int it ;

  it = * it_out ;

  printf( "Starting Broyden %d \n\n" , it ) ;

  n1 = n ;

  if( it == 0 )

    {

      * it_out = 1 ;

      for( i = 0 ; i < n ; ++i ) {

	v_in_old[ i ] = v_in[ i ] ;

	f_old[ i ] = alpha * ( v_in[ i ] - v_out[ i ] ) ;

	v_in[ i ]  -= f_old[ i ] ;

	v_out[ i ] = v_in[ i ] ;

      }

      return( 0 ) ;

    }

  assert( f = malloc( n * sizeof( double ) ) ) ;

  assert( df = malloc( n * sizeof( double ) ) ) ;

  assert( dv = malloc( n * sizeof( double ) ) ) ;

  it1 = it - 1 ;

  ishift = it1 * n ;

  norm = 0. ; /* || F || */

  norm1 = 0. ; /* || F_old || */

  for( i = 0 ; i < n ; i++ )

    {

      f[ i ] =  alpha * ( v_in[ i ] - v_out[ i ] ) ;

      norm += f[ i ] * f[ i ] ;

      norm1 += f_old[ i ] * f_old[ i ] ;

      dv[ i ] = v_in[ i ] - v_in_old[ i ] ;

      df[ i ] = f[ i ] - f_old[ i ] ;

      b_ket[ ishift + i ] = df[ i ] ;

      b_bra[ ishift + i ] = df[ i ] ;

    }

  for( ii = 0 ; ii < it1 ; ++ii )

    {

#ifdef LISE_LA_MKL
      norm =  d[ ii ] * ddot( &n1 , df , &ione , b_bra + ii * n , &ione ) ;
      daxpy( &n1 , &norm , b_ket + ii * n , &ione , b_ket + ishift , &ione ) ;
#endif
#ifdef LISE_LA_ESSL
      norm =  d[ ii ] * ddot( n1 , df , ione , b_bra + ii * n , ione ) ;
      daxpy( n1 , norm , b_ket + ii * n , ione , b_ket + ishift , ione ) ;
#endif
#ifdef LISE_LA_REF
      norm =  d[ ii ] * cblas_ddot( n1 , df , ione , b_bra + ii * n , ione ) ;
      cblas_daxpy( n1 , norm , b_ket + ii * n , ione , b_ket + ishift , ione ) ;
#endif

    }

#ifdef LISE_LA_MKL
  norm = ddot( &n1 , df , &ione , df , &ione ) ;
#endif
#ifdef LISE_LA_ESSL
  norm = ddot( n1 , df , ione , df , ione ) ;
#endif
#ifdef LISE_LA_REF
  norm = cblas_ddot( n1 , df , ione , df , ione ) ;
#endif

  printf( "<df|df> =%12.6e\n", norm ) ;

  for( i = 0 ; i < n ; ++i )

    b_ket[ ishift + i ] = ( dv[ i ] - b_ket[ ishift + i ] ) / norm ;

#ifdef LISE_LA_MKL
  norm1 = sqrt( ddot( &n1 , b_ket + ishift , &ione , b_ket + ishift , &ione ) ) ;
  norm2 = sqrt( ddot( &n1 , b_bra + ishift , &ione , b_bra + ishift ,&ione ) ) ;
#endif
#ifdef LISE_LA_ESSL
  norm1 = sqrt( ddot( n1 , b_ket + ishift , ione , b_ket + ishift , ione ) ) ;
  norm2 = sqrt( ddot( n1 , b_bra + ishift , ione , b_bra + ishift ,ione ) ) ;
#endif
#ifdef LISE_LA_REF
  norm1 = sqrt( cblas_ddot( n1 , b_ket + ishift , ione , b_ket + ishift , ione ) ) ;
  norm2 = sqrt( cblas_ddot( n1 , b_bra + ishift , ione , b_bra + ishift ,ione ) ) ;
#endif

  for( i = 0 ; i < n ; i++ )

    {

      b_ket[ ishift + i ] = b_ket[ ishift + i ] / norm1 ;

      b_bra[ ishift + i ] = b_bra[ ishift + i ] / norm2 ;

    }

  d[ it1 ] = norm1 * norm2 ;

  for( i = 0 ; i < n ; i++)
    {

      v_in_old[ i ] = v_in[ i ] ;

      v_in[ i ] -= f[ i ] ;

    }

  for( ii = 0 ; ii < it ; ii++ )

    {

#ifdef LISE_LA_MKL
      norm = - d[ ii ] * ddot( &n1 , b_bra + ii * n , &ione , f , &ione ) ;
      daxpy( &n1 , &norm , b_ket + ii * n , &ione , v_in , &ione ) ;
#endif
#ifdef LISE_LA_ESSL
      norm = - d[ ii ] * ddot( n1 , b_bra + ii * n , ione , f , ione ) ;
      daxpy( n1 , norm , b_ket + ii * n , ione , v_in , ione ) ;
#endif
#ifdef LISE_LA_REF
      norm = - d[ ii ] * cblas_ddot( n1 , b_bra + ii * n , ione , f , ione ) ;
      cblas_daxpy( n1 , norm , b_ket + ii * n , ione , v_in , ione ) ;
#endif

    }

  for( i = 0 ; i < n ; i++ )

    {

      f_old[ i ] = f[ i ] ;

      v_out[ i ] = v_in[ i ] ;

    }

  ( * it_out ) ++ ;

  free( f ) ; free( dv ) ; free( df ) ;

  printf( "Done with Broyden \n\n" ) ;

  return( 0 ) ;

}

double ddotII( int * n , double *v1 , int * i1 , double * v2 , int * i2 )

{

  double sum = 0. ;

  int i ;

  for( i = 0 ; i < * n ; i++ )

    sum += v1[ i ] * v2[ i ] ;

  return( sum ) ;

}

void daxpyII( int * n , double * alpha , double * x , int * i1 , double * y , int * i2 )

{

  int i ;

  for( i = 0 ; i < * n ; i++ )

    y[ i ] += *alpha * x[ i ] ;

}


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <math.h>
#include <float.h>
#include "parmt_postProcess.h"
#include "beachball.h"
#ifdef PARMT_USE_INTEL
#include <mkl_cblas.h>
#else
#include <cblas.h>
#endif
#include "compearth.h"

static void argsort3(const double *__restrict__ x, int *__restrict__ iperm);
static void sortUL(const double *__restrict__ lam,
                   const double *__restrict__ U,
                   double *__restrict__ lamSort,
                   double *__restrict__ Usort);
static void eigenvector2PrincipalAxis(const double eig,
                                      const double *__restrict__ ev,
                                      double *__restrict__ paxis);
static void vecstrd(const double str, const double dip, double r[3]);
static double dot3(const double *__restrict__ x,
                   const double *__restrict__ y);
static void gemv3(const double *__restrict__ A, const double *__restrict__ x,
                  double *__restrict__ y);
static void crossp(const double *__restrict__ u,
                   const double *__restrict__ v,
                   double *__restrict__ n);
static void strdvec(double r[3], double *str, double *dip);

int postprocess_tnp2beachballPolarity(const int nxp,
                                      const double xc, const double yc,
                                      const double rad,
                                      const double *__restrict__ pAxis,
                                      const double *__restrict__ nAxis,
                                      const double *__restrict__ tAxis,
                                      double *__restrict__ xw1,
                                      double *__restrict__ yw1,
                                      int8_t *__restrict__ pn1)
{
    double ax1[3] __attribute__ ((aligned(64)));
    double ax2[3] __attribute__ ((aligned(64)));
    double ax3[3] __attribute__ ((aligned(64)));
    double rm[9] __attribute__ ((aligned(64)));
    double rmt[9] __attribute__ ((aligned(64)));
    double tm[9] __attribute__ ((aligned(64)));
    double pt[3] __attribute__ ((aligned(64)));
    double ptnew[3] __attribute__ ((aligned(64)));
    double ampMax, az, baz, baz1, bdp, bdp1, bev, bevI, dp,
           dx, dxc, dy, dyc, evI,
           fclvd, fIso, paz, paz1, pdp, pdp1, pev, pevI, plusMin,
           rad2, rpt, rpt2,
           taz, taz1, tdp, tdp1, tev, tevI, theta, x, x0, y, y0;
    int ix, iy, k, nx, ny;
    const double degradi = 180.0/M_PI;
    const double third = 1.0/3.0;
    const double sqrt2 = 1.4142135623730951;
    const double sqrt2i = 1.0/sqrt2;
    taz1 = tAxis[0];
    tdp1 = tAxis[1];
    tevI = tAxis[2];
    baz1 = nAxis[0];
    bdp1 = nAxis[1];
    bevI = nAxis[2];
    paz1 = pAxis[0];
    pdp1 = pAxis[1];
    pevI = pAxis[2];
    // Avoid overwriting memory
    taz = taz1;
    tdp = tdp1;
    baz = baz1;
    bdp = bdp1;
    paz = paz1;
    pdp = pdp1;
    // Step 1: set up axes:
    //  - D axis ('dominant' 1-axis - with largest absolute eigenvalue)
    //  - M axis ('minor'    3-axis)
    //  - B axis (           2-axis)
    evI = (tevI + bevI + pevI)*third;
    tev = tevI - evI;
    bev = bevI - evI;
    pev = pevI - evI;
    vecstrd(paz, pdp, ax1);
    vecstrd(taz, tdp, ax3);
    fclvd = fabs(bev/pev);
    fIso = evI/pev;
    if (fabs(tev) > fabs(pev))
    {
        vecstrd(taz, tdp, ax1);
        vecstrd(paz, pdp, ax3);
        fclvd = fabs(bev/tev);
        fIso = evI/tev;
    }
    //if (fabs(evI) > .03)
    //{
    //    write(6,901) evI
    //}
    crossp(ax1, ax3, ax2);
    strdvec(ax2, &baz, &bdp);
    cliffsNodes_mtensor(taz, tdp, paz, pdp, tevI, bevI, pevI,
                        tm, rm, rmt, &ampMax);
    nx = nxp;
    ny = nxp;
    memset(xw1, 0, (size_t) (nx*ny)*sizeof(double));
    memset(yw1, 0, (size_t) (nx*ny)*sizeof(double));
    memset(pn1, 0, (size_t) (nx*ny)*sizeof(int8_t));
    dx = 2.0*rad/(double) (nx - 1);
    dy = 2.0*rad/(double) (ny - 1);
    x0 = xc - rad;
    y0 = yc - rad;
    rad2 = rad*rad; // saves a sqrt computation when comparing distance
    // loop on pixel grid
    for (iy=0; iy<ny; iy++)
    {
        for (ix=0; ix<nx; ix++)
        {
            x = x0 + (double) ix*dx;
            y = y0 + (double) iy*dy;
            k = iy*nx + ix;
            pn1[k] = 0;
            xw1[k] = x;
            yw1[k] = y;
            // require (x,y) be in the focal sphere
            dxc = x - xc;
            dyc = y - yc;
            rpt2 = dxc*dxc + dyc*dyc;
            if (rpt2 < rad2)
            {
                rpt = sqrt(rpt2);
                // compute the azimuth
                theta = atan2(y - yc, x - xc);  // measure from x axis
                az = (M_PI_2 - theta)*degradi;  // az is measured from north
                if (az < 0.0){az = az + 360.0;} // convention is [0,360]
                // compute the dip
                dp = (M_PI_2 - 2.0*asin(rpt*sqrt2i))*degradi;
                vecstrd(az, dp, pt);
                gemv3(tm, pt, ptnew);
                plusMin = dot3(pt, ptnew);
                pn1[k] =-1;
                if (plusMin > 100.0*DBL_EPSILON*ampMax){pn1[k] = 1;}
            } // end check on location
        } // loop on x
    } // loop on y
    return 0;
}
/*!
 * @brief Converts a Tape and Tape 2015 tensor to tension, null, and
 *        plunge eigenvectors in USE coordinates for plotting with
 *        cliffsnodes
 * @param[in] beta    colatitude (radians)
 * @param[in] gamma   longitude (radians)
 * @param[in] kappa   strike angle (radians)
 * @param[in] sigma   slip angle (radians)
 * @param[in] theta   dip angle (radians)
 *
 * @param[out] pAxis  (azimuth, plunge, eigenvalue) of pressure axis [3]
 * @param[out] nAxis  (azimuth, plunge, eigenvalue) of null axis [3]
 * @param[out] tAxis  (azimuth, plunge, eigenvalue) of tension axis [3]
 *
 * @result 0 indicate success
 *
 * @author Ben Baker
 *
 * @copyright ISTI distributed under Apache 2
 *
 */
int postprocess_tt2tnp(const double beta, const double gamma,
                       const double kappa, const double sigma,
                       const double theta,
                       double *__restrict__ pAxis,
                       double *__restrict__ nAxis,
                       double *__restrict__ tAxis)
{
    const double R[9] = {
             0.7071067811865476, 0.0,                -0.7071067811865476,
            -0.4082482904638631, 0.8164965809277261, -0.4082482904638631,
             0.5773502691896258, 0.5773502691896258,  0.5773502691896258};
    const double Yrot[9] = {
             0.7071067811865475, 0.0, 0.7071067811865475,
             0.0, 1.0, 0.0,
            -0.7071067811865475, 0.0, 0.7071067811865475};
    // Convert NWU to USE
    const double Rot[9] = {0, -1, 0, 0, 0, -1, 1, 0, 0};
    //const double RotInv[9] = {0, 0, 1, -1, 0, 0, 0, -1, 0};
    double Zsigma[9], Xtheta[9], Zkappa[9], ZX[9], V[9],
           U[9], Uuse[9], Usort[9], Uw[9],
           lambda[3], v3[3], lamSort[3],
           cosb, cosg, sinb, sing, nKappaDeg, sigmaDeg, thetaDeg;
    const double toDeg = 180.0/M_PI;
    // Compute the eigenvalues from Eqn 7
    sinb = sin(beta);
    sing = sin(gamma);
    cosb = cos(beta);
    cosg = cos(gamma);
    v3[0] = sinb*cosg;
    v3[1] = sinb*sing;
    v3[2] = cosb;
    // Compute R*lam
    lambda[0] = R[0]*v3[0] + R[3]*v3[1] + R[6]*v3[2];
    lambda[1] = R[1]*v3[0] + R[4]*v3[1] + R[7]*v3[2];
    lambda[2] = R[2]*v3[0] + R[5]*v3[1] + R[8]*v3[2];
    // Eqn 9-10
    sigmaDeg  = sigma*toDeg;
    thetaDeg  = theta*toDeg;
    nKappaDeg =-kappa*toDeg;
    compearth_eulerUtil_rotmat(1, &sigmaDeg, 3, Zsigma);
    compearth_eulerUtil_rotmat(1, &thetaDeg, 1, Xtheta);
    compearth_eulerUtil_rotmat(1, &nKappaDeg, 3, Zkappa);
    //compearth_eulerUtil_rotmatGen(1,-45.0, 2, Yrot);
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
                3, 3, 3, 1.0, Zkappa, 3, Xtheta, 3, 0.0, ZX, 3);
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
                3, 3, 3, 1.0, ZX, 3, Zsigma, 3, 0.0, V, 3);
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
                3, 3, 3, 1.0, V, 3, Yrot, 3, 0.0, U, 3);
    // M could be computed by computing M = U*Lambda*inv(U) hence
    // U corresponds to the eigenvectors and lambda the scaling.
    // However, the current basis is NWU which must be converted to USE.
    // If performing a proper rotation of M to M_{use} we would compute:
    //   R M inv(R) = R U Lambda inv(U) inv(R)
    // thus what we must be do is transform the eigenvectors into the
    // apropriate frame.  Intuitively it is the rotation of each
    // eigenvector (column) of the U matrix which is indeed the matrix
    // matrix multiplication of R*U.
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
                3, 3, 3, 1.0, Rot, 3, U, 3, 0.0, Uuse, 3);
    // If I want to finish this I could compute:
    //   R U Lambda inv(U) inv(R) = R U Lambda U^T R^T
/*
    double lamMat[9], Temp[9], M[9], UtRt[9];
    memset(lamMat, 0, 9*sizeof(double));
    lamMat[0] = lambda[0]; lamMat[4] = lambda[1]; lamMat[8] = lambda[2];
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
                3, 3, 3, 1.0, Uuse, 3, lamMat, 3, 0.0, Temp, 3);
    cblas_dgemm(CblasColMajor, CblasTrans, CblasTrans,
                3, 3, 3, 1.0, U, 3, Rot, 3, 0.0, UtRt, 3);
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
                3, 3, 3, 1.0, Temp, 3, UtRt, 3, 0.0, M, 3);
    for (int i=0; i<3; i++)
    {
        for (int j=0; j<3; j++)
        {
            printf("%f ", M[3*j+i]);
        }
        printf("\n");
     }
*/
    sortUL(lambda, Uuse, lamSort, Usort);
//printf("%f %f %f\n", lamSort[0], lamSort[1], lamSort[2]);
    // Compute pressure, null, and tension axes
    eigenvector2PrincipalAxis(lamSort[0], &Usort[0], pAxis);
    eigenvector2PrincipalAxis(lamSort[1], &Usort[3], nAxis);
    eigenvector2PrincipalAxis(lamSort[2], &Usort[6], tAxis);
    // Explosion - simplification (all positive eigenvalues)
    if (lamSort[0] > 0.0)
    {
        pAxis[2] = 1.0;
        nAxis[2] = 1.0;
        tAxis[2] = 1.0;
    }
    // Implosion - simplification (all negative eigenvalues)
    if (lamSort[2] < 0.0)
    {
        pAxis[0] =-1.0;
        nAxis[1] =-1.0;
        tAxis[2] =-1.0;
    }
    //printf("%f %f %f\n", pAxis[0], pAxis[1], pAxis[2]);
    //printf("%f %f %f\n", nAxis[0], nAxis[1], nAxis[2]);
    //printf("%f %f %f\n", tAxis[0], tAxis[1], tAxis[2]);
    return 0;
}

/*!
 * @brief Convert pressure, null, and tension principal axes to azimuth,
 *        plunge, and length.
 */
static void eigenvector2PrincipalAxis(const double eig,
                                      const double *__restrict__ ev,
                                      double *__restrict__ paxis)
{
    const char *fcnm = "cmopad_Eigenvector2PrincipalAxis\0";
    double v[3], az, plunge;
    double twopi = 2.0*M_PI;
    double pi180i = 180.0/M_PI;
    int ierr;
    //------------------------------------------------------------------------//    //
    // Compute azimuth and plunge angles where ev is USE coordinates
    plunge = asin(-ev[0]);        //arcsin(-z/r)
    az     = atan2(ev[2],-ev[1]); //atan(x/y)
    if (plunge <= 0.0)
    {
        plunge =-plunge;
        az = az + M_PI;
    }
    if (az < 0.0){az = az + twopi;}   //Shift to [0,360]
    if (az > twopi){az = az - twopi;} //Shift to [0,360]
    // Convert to degrees
    plunge = plunge*pi180i;
    az = az*pi180i;
    // Copy back result
    paxis[0] = az;     //First value is azimuth (degrees)
    paxis[1] = plunge; //Second value is plunge (degrees)
    paxis[2] = eig;    //Final value is eigenvalue (distance)
    return;
}
//===========================================================================//
static void sortUL(const double *__restrict__ lam,
                   const double *__restrict__ U,
                   double *__restrict__ lamSort,
                   double *__restrict__ Usort)
{
    int perm[3], j;
    argsort3(lam, perm);
    lamSort[0] = lam[perm[0]];
    lamSort[1] = lam[perm[1]];
    lamSort[2] = lam[perm[2]];
    for (j=0; j<3; j++)
    {
        Usort[3*j+0] = U[3*perm[j]+0];
        Usort[3*j+1] = U[3*perm[j]+1];
        Usort[3*j+2] = U[3*perm[j]+2];
    }
    return;
}

//============================================================================//
/*!
 * @brief Permutation to sort length 3 array into ascending order
 */
static void argsort3(const double *__restrict__ x, int *__restrict__ iperm)
{
    const char *fcnm = "__cmopad_argsort3\0";
    int i, temp;
    const int a = 0;
    const int b = 1;
    const int c = 2;
    // Copy
    iperm[a] = a;
    iperm[b] = b;
    iperm[c] = c;
    if (x[iperm[a]] > x[iperm[c]])
    {
        temp = iperm[c];
        iperm[c] = iperm[a];
        iperm[a] = temp;
    }
    if (x[iperm[a]] > x[iperm[b]])
    {
        temp = iperm[b];
        iperm[b] = iperm[a];
        iperm[a] = temp;
    }
    //Now the smallest element is the first one. Just check the 2-nd and 3-rd
    if (x[iperm[b]] > x[iperm[c]])
    {
        temp = iperm[c];
        iperm[c] = iperm[b];
        iperm[b] = temp;
    }
    return;
}
//============================================================================//
/*!
 * @brief Find components of downward pointing unit vector pole from the
 *        strike and dip angles.  This is based on Cliff Frolich's vecstrd.
 *
 * @param[in] str    strike angle of pole (degrees)
 * @param[in] dip    dip angle of pole (degrees)
 *
 * @param[out] r     corresponding unit vector pole (3)
 *
 * @author Ben Baker
 *
 * @copyright BSD
 *
 */
static void vecstrd(const double str, const double dip, double r[3])
{
    double cosdip, cosstr, diprad, sindip, sinstr, strrad;
    const double degrad = M_PI/180.0;
    // convert to radians
    strrad = str*degrad;
    diprad = dip*degrad;
    // compute cosines and sines
    sinstr = sin(strrad);
    cosstr = cos(strrad);
    cosdip = cos(diprad);
    sindip = sin(diprad);
    // compute (x,y,z) from strike and dip for unit radius
    r[0] = sinstr*cosdip;
    r[1] = cosstr*cosdip;
    r[2] =-sindip;
    return;
}
/*!
 * @brief Utility function for computing dot product of length 3 vector
 */
static double dot3(const double *__restrict__ x,
                   const double *__restrict__ y)
{
    double dot;
    dot = x[0]*y[0] + x[1]*y[1] + x[2]*y[2];
    return dot;
}
/*!
 * @brief Utility function for computing y = Ax for 3 x 3 matrix.
 */
static void gemv3(const double *__restrict__ A, const double *__restrict__ x,
                  double *__restrict__ y)
{
    y[0] = A[0]*x[0] + A[3]*x[1] + A[6]*x[2];
    y[1] = A[1]*x[0] + A[4]*x[1] + A[7]*x[2];
    y[2] = A[2]*x[0] + A[5]*x[1] + A[8]*x[2];
    return;
}
/*!
 * @brief Cross product of 2 length 3 vectors u and v.  Returns normal vector n.
 *
 * @param[in] u    first vector in cross product u x v [3]
 * @param[in] v    second vector in cross product u x v [3]
 *
 * @param[out] n   normal vector n = u x v [3]
 *
 * @author Ben Baker
 *
 * @copyright BSD
 *
 */
static void crossp(const double *__restrict__ u,
                   const double *__restrict__ v,
                   double *__restrict__ n)
{
    n[0] = u[1]*v[2] - u[2]*v[1];
    n[1] = u[2]*v[0] - u[0]*v[2];
    n[2] = u[0]*v[1] - u[1]*v[0];
    return;
}
/*!
 * @brief Finds the strike and dip of downward pointing unit vector.  This
 *        is based on Cliff Frolich's strdvec.
 *
 * @param[in,out] r      on input contains the downward pointing unit
 *                       vector.
 *                       if r[2] is > 0 then the unit vector is pointing
 *                       updward.  in this case then on output r3 will
 *                       be negated.
 *
 * @param[out] str       strike angle (degrees) \f$ s.t. \phi \in [0,360] \f$.
 * @param[out] dip       dip angle (degrees)
 *
 * @author Ben Baker
 *
 * @copyright BSD
 *
 */
static void strdvec(double r[3], double *str, double *dip)
{
    double rs;
    const double degradi = 180.0/M_PI;
    if (r[2] > 0.0)
    {
        r[0] =-r[0];
        r[1] =-r[1];
        r[2] =-r[2];
    }
    *str = atan2(r[0], r[1])*degradi;
    if (*str < 0.){*str = *str + 360.;}
    rs = sqrt(r[0]*r[0] + r[1]*r[1]);
    *dip=atan2(-r[2], rs)*degradi;
    return;
}


/*
 * SPDX-License-Identifier:  BSD-3-Clause
 *
 * example_10-StructOfArrays-CellLinkedList-OuterLoop-SymmetricalLoadBalancing.c :
 *      Example of SPH Density Calculation using
 *      fast neighbor search the main density loop via
 *      Cell Linked List method, Struct of Arrays (SoA)
 *      data layout, OpenMP parallelization at the
 *      cell-pair level, SIMD directives in the kernel
 *      and in the inner-most loop. It also implements
 *      symmetrical load balancing by moving the parallelism
 *      from iterating over cells to iterate over
 *      unique pairs of cell (i.e. i<j) and recyling the
 *      intermediary calculations.
 *
 * (C) Copyright 2021 José Hugo Elsas
 * Author: José Hugo Elsas <jhelsas@gmail.com>
 *
 * Command Line Options:
 *   -runs  <int>   : Set the number of repetitions (runs) for
 *                      calculating the density. The value of
 *                      the density is based on the last
 *                      iteration.
 *                    Default value: 1
 *   -run_seed <int>: Flag to set an alternative seed use for
 *                      for the PRNG. Instead of feeding seed
 *                      to the PRNG directly, it feeds
 *                      seed + iteration, as to generate different
 *                      configurations for each iteration.
 *                    Default value: 0 - (possible 0/1)
 *   -seed     <int>: Set the seed to use for the SPH particles
 *                      uniform position generation in the box
 *                    Default value: 123123123
 *
 *   -N        <int>: Set the number of SPH particles to be used
 *                    Default value: 1e5 = 100,000
 *   -h      <float>: Set the value of the smoothing kernel
 *                      parameter h, which corresponds to half
 *                      of the support of the kernel.
 *                    Default value: 0.05
 *
 *   -Nx       <int>: Set the number of Cells in the X direction
 *                    Default value: 10
 *   -Ny       <int>: Set the number of Cells in the Y direction
 *                    Default value: 10
 *   -Nz       <int>: Set the number of Cells in the Z direction
 *                    Default value: 10
 *
 *   -Xmin   <float>: Set the lower bound in the X direction for
 *                      the Cell Linked List box
 *                    Default value: 0.0
 *   -Ymin   <float>: Set the lower bound in the Y direction for
 *                    the Cell Linked List box
 *                    Default value: 0.0
 *   -Ymin   <float>: Set the lower bound in the Z direction for
 *                      the Cell Linked List box
 *                    Default value: 0.0
 *
 *   -Xmax   <float>: Set the lower bound in the X direction for
 *                      the Cell Linked List box
 *                    Default value: 1.0
 *   -Ymax   <float>: Set the lower bound in the Y direction for
 *                      the Cell Linked List box
 *                    Default value: 1.0
 *   -Zmax   <float>: Set the lower bound in the Z direction for
 *                      the Cell Linked List box
 *                    Default value: 1.0
 */

#include <math.h>
#include <ctype.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <limits.h>
#include <unistd.h>
#include <stdbool.h>
#include <sys/time.h>
#include <inttypes.h>

#include <omp.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_heapsort.h>

#include "sph_data_types.h"
#include "sph_linked_list.h"
#include "sph_utils.h"

#ifndef M_PI
#define M_PI (3.14159265358979323846)
#endif

#define COMPUTE_BLOCKS 5

#define dbg false

int main_loop(int run, bool run_seed, int64_t N, double h, long int seed,
              void *swap_arr,int64_t *temp_hash, linkedListBox *box, SPHparticle *lsph, double *times);

int compute_density_3d_symmetrical_load_ballance(int N, double h, SPHparticle *lsph, linkedListBox *box);

int compute_density_3d_chunk_symmetrical(int64_t node_begin, int64_t node_end,
                                         int64_t nb_begin, int64_t nb_end,double h,
                                         double* restrict x, double* restrict y,
                                         double* restrict z, double* restrict nu,
                                         double* restrict rhoi, double* restrict rhoj);

//void quickSort_int64_t2(int64_t *arr, int64_t low, int64_t high);

void quickSort_int64_t_2(int64_t *arr, int64_t low, int64_t high);

void mergeSort(int64_t arr[], int64_t l, int64_t r);

double w_bspline_3d_constant(double h);

#pragma omp declare simd
double w_bspline_3d_simd(double q);

#define ip_swap(a,b) (b)^=((a)^=((b)^=(a)))

KHASH_MAP_INIT_INT64(2, int64_t)

void counting_sort(int64_t N, linkedListBox *box, SPHparticle *lsph,void *swap_arr,int64_t *temp_hash);

int main(int argc, char **argv){
  bool run_seed = false;       // By default the behavior is is to use the same seed
  int runs = 1,err;            // it only runs once
  long int seed = 123123123;   // The default seed is 123123123
  int64_t N = 100000;          // The default number of particles is N = 1e5 = 100,000
  double h=0.05;               // The default kernel smoothing length is h = 0.05
  linkedListBox *box;          // Uninitialized Box containing the cells for the cell linked list method
  SPHparticle *lsph;           // Uninitialized array of SPH particles

  box = (linkedListBox*)malloc(1*sizeof(linkedListBox)); // Create a box representing the entire 3d domain

  // allow for command line customization of the run
  arg_parse(argc,argv,&N,&h,&seed,&runs,&run_seed,box);  // Parse the command line options
                                                         // line arguments and override default values

  err = SPHparticle_SoA_malloc(N,&lsph);
  if(err)
    fprintf(stderr,"error in SPHparticle_SoA_malloc\n");

  void *swap_arr = malloc(N*sizeof(double));
  int64_t *temp_hash = (int64_t*)malloc(2*N*sizeof(int64_t));
  double times[runs*COMPUTE_BLOCKS];

  for(int run=0;run<runs;run+=1)
    main_loop(run,run_seed,N,h,seed,swap_arr,temp_hash,box,lsph,times);

  bool is_cll = true;
  const char *prefix = "ex11,cll,SoA,outer,simd,symmLB,quicker";
  print_time_stats(prefix,is_cll,N,h,seed,runs,lsph,box,times);
  print_sph_particles_density(prefix,is_cll,N,h,seed,runs,lsph,box);

  SPHparticleSOA_safe_free(N,&lsph);
  safe_free_box(box);
  free(swap_arr);
  free(temp_hash);

  return 0;
}

/*
 *  Function main_loop:
 *    Runs the main loop of the program, including the particle array generation,
 *    density calculation and the timings annotations.
 *
 *    Arguments:
 *       run <int>            : index (or value) or the present iteration
 *       run_seed <bool>      : boolean defining whether to use run index for seed or not
 *       N <int>              : Number of SPH particles to be used in the run
 *       h <double>           : Smoothing Length for the Smoothing Kernel w_bspline
 *       seed <long int>      : seed for GSL PRNG generator to generate particle positions
 *       box  <linkedListBox> : Box of linked list cells, encapsulating the 3d domain
 *       lsph <SPHparticle>   : Array (pointer) of SPH particles to be updated
 *       times <double>       : Array to store the computation timings to be updated
 *    Returns:
 *       0                    : error code returned
 *       lsph <SPHparticle>   : SPH particle array is updated in the rho field by reference
 *       times <double>       : Times is updated by reference
 */

int main_loop(int run, bool run_seed, int64_t N, double h, long int seed,
              void *swap_arr, int64_t *temp_hash, linkedListBox *box, SPHparticle *lsph, double *times)
{
  int err;

  if(run_seed)
    err = gen_unif_rdn_pos_box(N,seed+run,box,lsph);
  else
    err = gen_unif_rdn_pos_box(N,seed,box,lsph);

  if(err)
    fprintf(stderr,"error in gen_unif_rdn_pos\n");

  // ------------------------------------------------------ //

  double t0,t1,t2,t3,t4,t5;

  t0 = omp_get_wtime();

  err = compute_hash_MC3D(N,lsph,box);                               // Compute Morton Z 3D hash based on the
  if(err)                                                            // cell index for each of the X, Y and Z
    fprintf(stderr,"error in compute_hash_MC3D\n");                  // directions, in which a given particle reside

  t1 = omp_get_wtime();

  counting_sort(N,box,lsph,swap_arr,temp_hash);

  t2 = omp_get_wtime();

  t3 = omp_get_wtime();

  t4 = omp_get_wtime();

  err = compute_density_3d_symmetrical_load_ballance(N,h,lsph,box);  // Compute the density of the particles based
  if(err)                                                            // on the cell linked list method for fast
    fprintf(stderr,"error in compute_density_3d_load_ballanced\n");  // neighbor search

  // --------------------------------------------------------------- //

  t5 = omp_get_wtime();

  kh_clear(0, box->hbegin);
  kh_clear(1, box->hend);

  times[COMPUTE_BLOCKS*run+0] = t1-t0;                               // Time for compute morton Z 3d hash
  times[COMPUTE_BLOCKS*run+1] = t2-t1;                               // Time for sorting the particles' hashes
  times[COMPUTE_BLOCKS*run+2] = t3-t2;                               // Time for reordering all other arrays accordingly
  times[COMPUTE_BLOCKS*run+3] = t4-t3;                               // Time for setting up the interval hash tables
  times[COMPUTE_BLOCKS*run+4] = t5-t4;                               // Time for computing the SPH particle densities

  return 0;
}

/*
 *  Function compute_density_3d_symmetrical_load_ballance:
 *    Computes the SPH density from the particles using cell linked list with
 *    vectorization at the compute_density_3d_chunk level, but the parallelization
 *    done at the level of the outer-most loop of the compute_density_3d_cll_outerOmp
 *    function, not at the chunk level.
 *
 *    The parallelization is done at the level of unique cell pair instead of cells,
 *    with the indexes for the cell pairs pre-computed before parallelization.
 *
 *    Arguments:
 *       N <int>              : Number of SPH particles to be used in the run
 *       h <double>           : Smoothing Length for the Smoothing Kernel w_bspline
 *       lsph <SPHparticle>   : Array (pointer) of SPH particles to be updated
 *    Returns:
 *       0                    : error code returned
 *       lsph <SPHparticle>   : SPH particle array is updated in the rho field by reference
 */
int compute_density_3d_symmetrical_load_ballance(int N, double h, SPHparticle *lsph, linkedListBox *box){
  int64_t *node_begin,*node_end,*nb_begin,*nb_end;                        // Define the arrays for cell boundaries
  int64_t max_cell_pair_count = 0;                                        // and the number of cell pairs
  const double kernel_constant = w_bspline_3d_constant(h);

  max_cell_pair_count = count_box_pairs(box);                             // compute the maximum number of cell pairs

  node_begin = (int64_t*)malloc(max_cell_pair_count*sizeof(int64_t));     // allocate space for node_begin
  node_end   = (int64_t*)malloc(max_cell_pair_count*sizeof(int64_t));     // allocate space for node_end
  nb_begin   = (int64_t*)malloc(max_cell_pair_count*sizeof(int64_t));     // allocate space for nb_begin
  nb_end     = (int64_t*)malloc(max_cell_pair_count*sizeof(int64_t));     // allocate space for nb_end

  max_cell_pair_count = setup_unique_box_pairs(box,                       // set the values for cell pairs
                                               node_begin,node_end,
                                               nb_begin,nb_end);

  memset(lsph->rho,(int)0,N*sizeof(double));                              // Pre-initialize the density to zero

                                                                          // Parallelism was moved
                                                                          // to the level of unique pairs
  #pragma omp parallel for schedule(dynamic,5) proc_bind(master)          // Execute in parallel
  for(size_t i=0;i<max_cell_pair_count;i+=1){                             // over the unique pairs' array
    double local_rhoi[node_end[i] - node_begin[i]];                       // partial density array for node indexs
    double local_rhoj[  nb_end[i] -   nb_begin[i]];                       // partial density array for nb   indexs

    memset(local_rhoi,(int)0,(node_end[i]-node_begin[i])*sizeof(double)); // initialize node partial density to zero
    memset(local_rhoj,(int)0,    (nb_end[i]-nb_begin[i])*sizeof(double)); // initialize nb partial density to zero

    compute_density_3d_chunk_symmetrical(node_begin[i],node_end[i],       // Compute the density contribution
                                         nb_begin[i],nb_end[i],h,         // from this particular cell pair
                                         lsph->x,lsph->y,lsph->z,         // for both node and nb partial density
                                         lsph->nu,local_rhoi,
                                         local_rhoj);

    // merging the results can result in race conditions, therefore needs to be serialized
    #pragma omp critical                                                  // this serializes this code section
    {

      for(size_t ii=node_begin[i];ii<node_end[i];ii+=1){                  // iterate over the node_ cell
        lsph->rho[ii] += kernel_constant*local_rhoi[ii-node_begin[i]];    // add the partial density contribution
      }

      if(node_begin[i] != nb_begin[i])                                    // if sender and receiver are different
        for(size_t ii=nb_begin[i];ii<nb_end[i];ii+=1){                    // iterate over the nb_ cell
          lsph->rho[ii] += kernel_constant*local_rhoj[ii-nb_begin[i]];    // add the partial density contribution
        }
    }
  }

  free(node_begin);
  free(node_end);
  free(nb_begin);
  free(nb_end);

  return 0;
}

/*
 *  Function compute_density_3d_chunk_symmetrical:
 *    Computes the SPH density contribution to both the node_ cell and the nb_ cell.
 *    Vectorization in the inner-most loop, but no parallelization.
 *    The density contribution is symmetrical.
 *
 *    Arguments:
 *       N <int>              : Number of SPH particles to be used in the run
 *       h <double>           : Smoothing Length for the Smoothing Kernel w_bspline
 *       x       <double*>    : Array of particles' X positions
 *       y       <double*>    : Array of particles' Y positions
 *       z       <double*>    : Array of particles' Z positions
 *       nu      <double*>    : Array of particles' density weights (i.e. masses)
 *    Returns:
 *       0                    : error code returned
 *       rho       <double*>  : Array of particles' densities
 */
int compute_density_3d_chunk_symmetrical(int64_t node_begin, int64_t node_end,
                                         int64_t nb_begin, int64_t nb_end,double h,
                                         double* restrict x, double* restrict y,
                                         double* restrict z, double* restrict nu,
                                         double* restrict rhoi, double* restrict rhoj){
  const double inv_h = 1./h;

  for(int64_t ii=node_begin;ii<node_end;ii+=1){ // Iterate over the ii index of the chunk
    double xii = x[ii];                         // Load the X component of the ii particle position
    double yii = y[ii];                         // Load the Y component of the ii particle position
    double zii = z[ii];                         // Load the Z component of the ii particle position

    #pragma omp simd                            // Hint at the compiler to vectorize the inner most loop
    for(int64_t jj=nb_begin;jj<nb_end;jj+=1){   // Iterate over the each other particle in jj loop
      double q = 0.;                            // Initialize the distance

      double xij = xii-x[jj];                   // Load and subtract jj particle's X position component
      double yij = yii-y[jj];                   // Load and subtract jj particle's Y position component
      double zij = zii-z[jj];                   // Load and subtract jj particle's Z position component

      q += xij*xij;                             // Add the jj contribution to the ii distance in X
      q += yij*yij;                             // Add the jj contribution to the ii distance in Y
      q += zij*zij;                             // Add the jj contribution to the ii distance in Z

      q = sqrt(q)*inv_h;                        // Sqrt to compute the normalized distance, measured in h

      double wij = w_bspline_3d_simd(q);        // compute the smoothing kernel separately for re-use

      rhoi[ii-node_begin] += nu[jj]*wij;        // add the jj contribution to ii density
      rhoj[jj-nb_begin]   += nu[ii]*wij;        // add the ii contribution to jj density
    }
  }

  return 0;
}

/*
 *  Function w_bspline_3d_constant:
 *    Returns the 3d normalization constant for the cubic b-spline SPH smoothing kernel
 *
 *    Arguments:
 *       h <double>           : Smoothing Length for the Smoothing Kernel w_bspline
 *    Returns:
 *       3d bspline normalization density <double>
 */
double w_bspline_3d_constant(double h){
  return 3./(2.*M_PI*h*h*h);  // 3d normalization value for the b-spline kernel
}

/*
 *  Function w_bspline_3d_simd:
 *    Returns the un-normalized value of the cubic b-spline SPH smoothing kernel
 *
 *    Arguments:
 *       q <double>           : Distance between particles normalized by the smoothing length h
 *    Returns:
 *       wq <double>          : Unnormalized value of the kernel
 *
 *    Observation:
 *       Why not else if(q<2.)?
 *       Because if you use "else if", the compiler refuses to vectorize,
 *       This results in a large slowdown, as of 2.5x slower for example_04
 */
#pragma omp declare simd
double w_bspline_3d_simd(double q){
  double wq=0;
  double wq1 = (0.6666666666666666 - q*q + 0.5*q*q*q);             // The first polynomial of the spline
  double wq2 = 0.16666666666666666*(2.-q)*(2.-q)*(2.-q);           // The second polynomial of the spline

  if(q<2.)                                                         // If the distance is below 2
    wq = wq2;                                                      // Use the 2nd polynomial for the spline

  if(q<1.)                                                         // If the distance is below 1
    wq = wq1;                                                      // Use the 1st polynomial for the spline

  return wq;                                                       // return which ever value corresponds to the distance
}

int cmp_int64_t(const void *p,const void *q){
  int64_t *data1,*data2;
  data1 = (int64_t*)p;
  data2 = (int64_t*)q;

  if(*data1 < *data2)         // data[0] is the hash value,
    return -1;
  else if(*data1 == *data2)   // in the unsorted array
    return 0;
  else
    return 1;
}

#define swap_loop(N,lsph,temp_swap,member,type) for(int64_t i=0;i<(N);i+=1)                            \
                                                  (temp_swap)[i] = (lsph)->member[(lsph)->hash[2*i+1]];\
                                                memcpy((lsph)->member,temp_swap,(N)*sizeof(type))

/*
 *  Function counting_sort:
 *
 *
 *    Arguments:
 *     N            <int64_t>    :
 *     box      <linkedListBox*> :
 *     lsph       <SPHparticle*> :
 *     swap_arr     <double*>    :
 *     temp_hash   <int64_t*>    :
 *    Returns:
 *     box      <linkedListBox*> :
 *
 *    Observation:
 *       Why not else if(q<2.)?
 *       Because if you use "else if", the compiler refuses to vectorize,
 *       This results in a large slowdown, as of 2.5x slower for example_04
 */
void counting_sort(int64_t N, linkedListBox *box, SPHparticle *lsph,void *swap_arr,int64_t *temp_hash){

  double t0,t1,t2,t3,t4,t5,t6;
  double t31,t32,t33,t34;

  t0 = omp_get_wtime();

  for(int64_t i=0;i<N;i+=1){
    int ret;
    khiter_t k = kh_put(0, box->hbegin, lsph->hash[2*i+0], &ret);
    if(ret==1)
      kh_value(box->hbegin, k) = 1;
    else if(ret==0){
      int64_t val = kh_value(box->hbegin, k);
      kh_value(box->hbegin, k) = val + 1;
    }
    else{
      printf("error counting_sort init");
      return;
    }
  }

  t1 = omp_get_wtime();

  unsigned int dict_size = kh_size(box->hbegin);
  int64_t hash[dict_size];
  int64_t prefix[dict_size];
  int idx = 0;

  for (khiter_t k = kh_begin(box->hbegin); k != kh_end(box->hbegin); ++k){
    if (kh_exist(box->hbegin, k)){
      hash[idx] = kh_key(box->hbegin,k);
      idx+=1;
    }
  }

  t2 = omp_get_wtime();

  qsort(hash,dict_size,sizeof(int64_t),cmp_int64_t);

  t3 = omp_get_wtime();

  prefix[0] = 0;
  for(int i=1;i<dict_size;i+=1){
    prefix[i] = prefix[i-1];

    khiter_t k = kh_get(0, box->hbegin, hash[i-1]);
    prefix[i] += kh_value(box->hbegin, k);
  }

  t31 = omp_get_wtime();

  for(int i = 0; i< dict_size;i+=1){
    khiter_t kp = kh_get(0, box->hbegin, hash[i]);
    kh_value(box->hbegin, kp) = prefix[i];
  }

  t32 = omp_get_wtime();

  for (khiter_t k = kh_begin(box->hbegin); k != kh_end(box->hbegin); ++k){
    if (kh_exist(box->hbegin, k)){
      int ret;
      int64_t hash = kh_key(box->hbegin,k);
      khiter_t ke = kh_put(1, box->hend, hash, &ret);
      kh_value(box->hend, ke) = kh_value(box->hbegin, k);
    }
  }

  t33 = omp_get_wtime();

  for(int64_t i=0;i<N;i+=1){
    khiter_t kp = kh_get(1, box->hend, lsph->hash[2*i]);
    if (kh_exist(box->hend, kp)){
      lsph->hash[2*i+1] = kh_value(box->hend, kp);
      kh_value(box->hend, kp) += 1;
    }
    else{
      printf("there is an issue\n");
    }
  }

  t34 = omp_get_wtime();

  t4 = omp_get_wtime();

  for(int64_t i=0;i<N;i+=1){
    int64_t idx = lsph->hash[2*i+1];
    temp_hash[2*idx+0] = lsph->hash[2*i+0];
    temp_hash[2*idx+1] = i;
  }

  for(int64_t i=0;i<N;i+=1){
    lsph->hash[2*i+0] = temp_hash[2*i+0];
    lsph->hash[2*i+1] = temp_hash[2*i+1];
  }

  t4 = omp_get_wtime();

  int64_t *int64_temp_swap = (int64_t *)swap_arr;
  swap_loop(N,lsph,int64_temp_swap,id ,int64_t);
  double *double_temp_swap = (double *)swap_arr;
  swap_loop(N,lsph,double_temp_swap,nu ,double);
  swap_loop(N,lsph,double_temp_swap,rho,double);
  swap_loop(N,lsph,double_temp_swap,x  ,double);
  swap_loop(N,lsph,double_temp_swap,y  ,double);
  swap_loop(N,lsph,double_temp_swap,z  ,double);
  swap_loop(N,lsph,double_temp_swap,ux ,double);
  swap_loop(N,lsph,double_temp_swap,uy ,double);
  swap_loop(N,lsph,double_temp_swap,uz ,double);
  swap_loop(N,lsph,double_temp_swap,Fx ,double);
  swap_loop(N,lsph,double_temp_swap,Fy ,double);
  swap_loop(N,lsph,double_temp_swap,Fz ,double);

  t5 = omp_get_wtime();

  for(int64_t i=0;i<N;i+=1)
    lsph->hash[i] = lsph->hash[2*i];

  t6 = omp_get_wtime();

  if(dbg){
    printf("1: %lf \n",t1-t0);
    printf("2: %lf \n",t2-t1);
    printf("3: %lf \n",t3-t2);
    printf("4: %lf \n",t4-t3);
    printf("  31: %lf\n",t31-t3);
    printf("  32: %lf\n",t32-t31);
    printf("  33: %lf\n",t33-t32);
    printf("  34: %lf\n",t34-t33);
    printf("  35: %lf\n",t4-t34);
    printf("5: %lf \n",t5-t4);
    printf("6: %lf \n",t6-t5);
  }

  return;
}


/*
 * Copyright (c) 2016-2021 lymastee, All rights reserved.
 * Contact: lymastee@hotmail.com
 *
 * This file is part of the gslib project.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#pragma once

#ifndef math_870b373b_c3fb_4c7a_abc7_bc4bd4095aef_h
#define math_870b373b_c3fb_4c7a_abc7_bc4bd4095aef_h

#include <gslib/config.h>
#include <gslib/basetype.h>

#ifndef PI
#define PI  (3.14159265358979323846f)
#endif

__gslib_begin__

struct _vec2 { float x, y; };
struct _vec3 { float x, y, z; };
struct _vec4 { float x, y, z, w; };
struct _mat2
{
    union
    {
        struct
        {
            float _11, _12,
                _21, _22;
        };
        float  m[2][2];
    };
};
struct _mat3
{
    union
    {
        struct
        {
            float _11, _12, _13,
                _21, _22, _23,
                _31, _32, _33;
        };
        float   m[3][3];
    };
};
struct _mat4
{
    union
    {
        struct
        {
            float _11, _12, _13, _14,
                _21, _22, _23, _24,
                _31, _32, _33, _34,
                _41, _42, _43, _44;
        };
        float   m[4][4];
    };
};
struct _quat
{
    union
    {
        struct  { float x, y, z, w; };
        float   q[4];
    };
};
struct _plane { float a, b, c, d; };

struct viewport
{
    int         left;
    int         top;
    uint        width;
    uint        height;
    float       min_depth;
    float       max_depth;
};

class vec2;
class vec3;
class vec4;
class mat3;
class mat4;
class quat;
class plane;

typedef vec2 point2;
typedef vec3 point3;
typedef mat4 matrix;
typedef mat3 matrix3;
typedef _mat2 matrix2;

/* inline */
float vec2length(const vec2* p);
float vec2lengthsq(const vec2* p);
float vec2dot(const vec2* p1, const vec2* p2);
float vec2ccw(const vec2* p1, const vec2* p2);
vec2* vec2add(vec2* out, const vec2* p1, const vec2* p2);
vec2* vec2sub(vec2* out, const vec2* p1, const vec2* p2);
vec2* vec2min(vec2* out, const vec2* p1, const vec2* p2);
vec2* vec2max(vec2* out, const vec2* p1, const vec2* p2);
vec2* vec2scale(vec2* out, const vec2* p, float s);
vec2* vec2lerp(vec2* out, const vec2* p1, const vec2* p2, float s);
float vec2lerp_x(const vec2* p1, const vec2* p2, float y);
float vec2lerp_y(const vec2* p1, const vec2* p2, float x);
vec2* vec2multiply(vec2* out, const vec2* p, const matrix2* m);
vec2* vec2transformcoord(vec2* out, const vec2* p, const mat3* m);
vec2* vec2transformnormal(vec2* out, const vec2* p, const mat3* m);
float vec3length(const vec3* v);
float vec3lengthsq(const vec3* v);
float vec3dot(const vec3* v1, const vec3* v2);
vec3* vec3cross(vec3* out, const vec3* v1, const vec3* v2);
vec3* vec3add(vec3* out, const vec3* v1, const vec3* v2);
vec3* vec3sub(vec3* out, const vec3* v1, const vec3* v2);
vec3* vec3min(vec3* out, const vec3* v1, const vec3* v2);
vec3* vec3max(vec3* out, const vec3* v1, const vec3* v2);
vec3* vec3scale(vec3* out, const vec3* v, float s);
vec3* vec3lerp(vec3* out, const vec3* v1, const vec3* v2, float s);
vec3* vec3multiply(vec3* out, const vec3* v, const mat3* m);
vec3* vec3project(vec3* out, const vec3* v, const viewport* vp, const matrix* prj, const matrix* view, const matrix* world);
vec3* vec3unproject(vec3* out, const vec3* v, const viewport* vp, const matrix* prj, const matrix* view, const matrix* world);
vec3* vec3projectarray(vec3* out, uint ostride, const vec3* v, uint vstride, const viewport* vp, const matrix* prj, const matrix* view, const matrix* world, uint n);
vec3* vec3unprojectarray(vec3* out, uint ostride, const vec3* v, uint vstride, const viewport* vp, const matrix* prj, const matrix* view, const matrix* world, uint n);
float vec4length(const vec4* v);
float vec4lengthsq(const vec4* v);
float vec4dot(const vec4* v1, const vec4* v2);
vec4* vec4add(vec4* out, const vec4* v1, const vec4* v2);
vec4* vec4sub(vec4* out, const vec4* v1, const vec4* v2);
vec4* vec4min(vec4* out, const vec4* v1, const vec4* v2);
vec4* vec4max(vec4* out, const vec4* v1, const vec4* v2);
vec4* vec4scale(vec4* out, const vec4* v, float s);
vec4* vec4lerp(vec4* out, const vec4* v1, const vec4* v2, float s);
vec4* vec4multiply(vec4* out, const vec4* v, const matrix* m);
matrix3* mat3identity(matrix3* out);
bool mat3isidentity(const matrix3* m);
float mat3determinant(const matrix3* m);
matrix3* mat3inverse(matrix3* out, float* det, const matrix3* m);
matrix3* mat3multiply(matrix3* out, const matrix3* m1, const matrix3* m2);
matrix3* mat3transpose(matrix3* out, const matrix3* m);
matrix3* mat3scaling(matrix3* out, float sx, float sy);
matrix3* mat3translation(matrix3* out, float x, float y);
matrix3* mat3rotation(matrix3* out, float angle);
matrix3* mat3rotationpos(matrix3* out, const vec2* p, float angle);
matrix* matidentity(matrix* out);
bool matisidentity(const matrix* m);
bool matdecompose(vec3* scale, quat* rot, vec3* trans, const matrix* m);
matrix* mattranspose(matrix* out, const matrix* m);
matrix* matscaling(matrix* out, float sx, float sy, float sz);
matrix* mattranslation(matrix* out, float x, float y, float z);
matrix* matrotation_x(matrix* out, float angle);
matrix* matrotation_y(matrix* out, float angle);
matrix* matrotation_z(matrix* out, float angle);
matrix* matrotationaxis(matrix* out, const vec3* v, float angle);
matrix* matrotationquatern(matrix* out, const quat* q);
matrix* matrotationeuler(matrix* out, float yaw, float pitch, float roll);
matrix* matlookatlh(matrix* out, const vec3* eye, const vec3* at, const vec3* up);
matrix* matlookatrh(matrix* out, const vec3* eye, const vec3* at, const vec3* up);
matrix* matperspectivelh(matrix* out, float w, float h, float zn, float zf);
matrix* matperspectiverh(matrix* out, float w, float h, float zn, float zf);
matrix* matperspectivefovlh(matrix* out, float fovy, float aspect, float zn, float zf);
matrix* matperspectivefovrh(matrix* out, float fovy, float aspect, float zn, float zf);
matrix* matperspectiveoffcenterlh(matrix* out, float l, float r, float b, float t, float zn, float zf);
matrix* matperspectiveoffcenterrh(matrix* out, float l, float r, float b, float t, float zn, float zf);
matrix* matortholh(matrix* out, float w, float h, float zn, float zf);
matrix* matorthorh(matrix* out, float w, float h, float zn, float zf);
matrix* matorthooffcenterlh(matrix* out, float l, float r, float b, float t, float zn, float zf);
matrix* matorthooffcenterrh(matrix* out, float l, float r, float b, float t, float zn, float zf);
matrix* matviewportproject(matrix* out, const viewport* vp);
matrix* matviewportunproject(matrix* out, const viewport* vp);
matrix* mattransform(matrix* out, const vec3* scalecenter, const quat* scalerot, const vec3* scale, const vec3* rotcenter, const quat* rot, const vec3* trans);
matrix* mattransform2d(matrix* out, const vec2* scalecenter, float scalerot, const vec2* scale, const vec2* rotcenter, float rot, const vec2* trans);
matrix* mataffinetransform(matrix* out, float scale, const vec3* rotcenter, const quat* rot, const vec3* trans);
matrix* mataffinetransform2d(matrix* out, float scale, const vec2* rotcenter, float rot, const vec2* trans);
float quatlength(const quat* q);
float quatlengthsq(const quat* q);
float quatdot(const quat* q1, const quat* q2);
quat* quatidentity(quat* out);
bool quatisidenetity(const quat* q);
quat* quatconjugate(quat* out, const quat* q);
void quattoaxisangle(const quat* q, vec3* axis, float* angle);
quat* quatrotatematrix(quat* out, const matrix* m);
quat* quatrotateaxis(quat* out, const vec3* v, float angle);
quat* quatln(quat* out, const quat* q);
quat* quatexp(quat* out, const quat* q);
quat* quatslerp(quat* out, const quat* q1, const quat* q2, float t);
quat* quatsquad(quat* out, const quat* q, const quat* a, const quat* b, const quat* c, float t);
quat* quatbarycentric(quat* out, const quat* q1, const quat* q2, const quat* q3, float f, float g);
void quatsquadsetup(quat* a, quat* b, quat* c, const quat* q1, const quat* q2, const quat* q3, const quat* q4);
float planedot(const plane* p, const vec4* v);
float planedotcoord(const plane* p, const vec3* v);
float planedotnormal(const plane* p, const vec3* v);
plane* planescale(plane* out, const plane* p, float s);
plane* planefrompointnormal(plane* out, const vec3* p, const vec3* normal);

/* non-inline */
typedef vec2* (__stdcall *fnvec2normalize)(vec2* out, const vec2* v);
typedef vec2* (__stdcall *fnvec2hermite)(vec2* out, const vec2* p1, const vec2* t1, const vec2* p2, const vec2* t2, float s);
typedef vec2* (__stdcall *fnvec2catmullrom)(vec2* out, const vec2* p1, const vec2* p2, const vec2* p3, const vec2* p4, float s);
typedef vec2* (__stdcall *fnvec2barycentric)(vec2* out, const vec2* p1, const vec2* p2, const vec2* p3, float f, float g);
typedef vec4* (__stdcall *fnvec2transform)(vec4* out, const vec2* p, const matrix* m);
typedef vec2* (__stdcall *fnvec2transformcoord)(vec2* out, const vec2* p, const matrix* m);
typedef vec2* (__stdcall *fnvec2transformnormal)(vec2* out, const vec2* p, const matrix* m);
typedef vec4* (__stdcall *fnvec2transformarray)(vec4* out, uint ostride, const vec2* v, uint vstride, const matrix* m, uint n);
typedef vec2* (__stdcall *fnvec2transformcoordarray)(vec2* out, uint ostride, const vec2* v, uint vstride, const matrix* m, uint n);
typedef vec2* (__stdcall *fnvec2transformnormalarray)(vec2* out, uint ostride, const vec2* v, uint vstride, const matrix* m, uint n);
typedef vec3* (__stdcall *fnvec3normalize)(vec3* out, const vec3* v);
typedef vec3* (__stdcall *fnvec3hermite)(vec3* out, const vec3* v1, const vec3* t1, const vec3* v2, const vec3* t2, float s);
typedef vec3* (__stdcall *fnvec3catmullrom)(vec3* out, const vec3* v1, const vec3* v2, const vec3* v3, const vec3* v4, float s);
typedef vec3* (__stdcall *fnvec3barycentric)(vec3* out, const vec3* v1, const vec3* v2, const vec3* v3, float f, float g);
typedef vec4* (__stdcall *fnvec3transform)(vec4* out, const vec3* v, const matrix* m);
typedef vec3* (__stdcall *fnvec3transformcoord)(vec3* out, const vec3* v, const matrix* m);
typedef vec3* (__stdcall *fnvec3transformnormal)(vec3* out, const vec3* v, const matrix* m);
typedef vec4* (__stdcall *fnvec3transformarray)(vec4* out, uint ostride, const vec3* v, uint vstride, const matrix* m, uint n);
typedef vec3* (__stdcall *fnvec3transformcoordarray)(vec3* out, uint ostride, const vec3* v, uint vstride, const matrix* m, uint n);
typedef vec3* (__stdcall *fnvec3transformnormalarray)(vec3* out, uint ostride, const vec3* v, uint vstride, const matrix* m, uint n);
typedef vec4* (__stdcall *fnvec4cross)(vec4* out, const vec4* v1, const vec4* v2, const vec4* v3);
typedef vec4* (__stdcall *fnvec4normalize)(vec4* out, const vec4* v);
typedef vec4* (__stdcall *fnvec4hermite)(vec4* out, const vec4* v1, const vec4* t1, const vec4* v2, const vec4* t2, float s);
typedef vec4* (__stdcall *fnvec4catmullrom)(vec4* out, const vec4* v1, const vec4* v2, const vec4* v3, const vec4* v4, float s);
typedef vec4* (__stdcall *fnvec4barycentric)(vec4* out, const vec4* v1, const vec4* v2, const vec4* v3, float f, float g);
typedef vec4* (__stdcall *fnvec4transform)(vec4* out, const vec4* v, const matrix* m);
typedef vec4* (__stdcall *fnvec4transformarray)(vec4* out, uint ostride, const vec4* v, uint vstride, const matrix* m, uint n);
typedef float (__stdcall *fnmatdeterminant)(const matrix* m);
typedef matrix* (__stdcall *fnmatmultiply)(matrix* out, const matrix* m1, const matrix* m2);
typedef matrix* (__stdcall *fnmatmultiplytranspose)(matrix* out, const matrix* m1, const matrix* m2);
typedef matrix* (__stdcall *fnmatinverse)(matrix* out, float* determinant, const matrix* m);
typedef matrix* (__stdcall *fnmatshadow)(matrix* out, const vec4* plight, const plane* pln);
typedef matrix* (__stdcall *fnmatreflect)(matrix* out ,const plane* pln);
typedef quat* (__stdcall *fnquatrotateeuler)(quat* out, float yaw, float pitch, float roll);
typedef quat* (__stdcall *fnquatmultiply)(quat* out, const quat* q1, const quat* q2);
typedef quat* (__stdcall *fnquatnormalize)(quat* out, const quat* q);
typedef quat* (__stdcall *fnquatinverse)(quat* out, const quat* q);
typedef plane* (__stdcall *fnplanenormalize)(plane* out, const plane* p);
typedef vec3* (__stdcall *fnplaneintersectline)(vec3* out, const plane* p, const vec3* v1, const vec3* v2);
typedef plane* (__stdcall *fnplanefrompoints)(plane* out, const vec3* v1, const vec3* v2, const vec3* v3);
typedef plane* (__stdcall *fnplanetransform)(plane* out, const plane* p, const matrix* m);
typedef plane* (__stdcall *fnplanetransformarray)(plane* out, uint ostride, const plane* p, uint pstride, const matrix* m, uint n);

gs_export extern fnvec2normalize vec2normalize;
gs_export extern fnvec2hermite vec2hermite;
gs_export extern fnvec2catmullrom vec2catmullrom;
gs_export extern fnvec2barycentric vec2barycentric;
gs_export extern fnvec2transform vec2transform;
gs_export extern fnvec2transformarray vec2transformarray;
gs_export extern fnvec2transformcoordarray vec2transformcoordarray;
gs_export extern fnvec2transformnormalarray vec2transformnormalarray;
gs_export extern fnvec3normalize vec3normalize;
gs_export extern fnvec3hermite vec3hermite;
gs_export extern fnvec3catmullrom vec3catmullrom;
gs_export extern fnvec3barycentric vec3barycentric;
gs_export extern fnvec3transform vec3transform;
gs_export extern fnvec3transformcoord vec3transformcoord;
gs_export extern fnvec3transformnormal vec3transformnormal;
gs_export extern fnvec3transformarray vec3transformarray;
gs_export extern fnvec3transformcoordarray vec3transformcoordarray;
gs_export extern fnvec3transformnormalarray vec3transformnormalarray;
gs_export extern fnvec4cross vec4cross;
gs_export extern fnvec4normalize vec4normalize;
gs_export extern fnvec4hermite vec4hermite;
gs_export extern fnvec4catmullrom vec4catmullrom;
gs_export extern fnvec4barycentric vec4barycentric;
gs_export extern fnvec4transform vec4transform;
gs_export extern fnvec4transformarray vec4transformarray;
gs_export extern fnmatdeterminant matdeterminant;
gs_export extern fnmatmultiply matmultiply;
gs_export extern fnmatmultiplytranspose matmultiplytranspose;
gs_export extern fnmatinverse matinverse;
gs_export extern fnmatshadow matshadow;
gs_export extern fnmatreflect matreflect;
gs_export extern fnquatrotateeuler quatrotateeuler;
gs_export extern fnquatmultiply quatmultiply;
gs_export extern fnquatnormalize quatnormalize;
gs_export extern fnquatinverse quatinverse;
gs_export extern fnplanenormalize planenormalize;
gs_export extern fnplaneintersectline planeintersectline;
gs_export extern fnplanefrompoints planefrompoints;
gs_export extern fnplanetransform planetransform;
gs_export extern fnplanetransformarray planetransformarray;

class gs_export vec2:
    public _vec2
{
public:
    vec2() {}
    vec2(const float*);
    vec2(float x, float y);

public:
    operator float*();
    operator const float*() const;

public:
    vec2& operator+=(const vec2&);
    vec2& operator-=(const vec2&);
    vec2& operator*=(float);
    vec2& operator/=(float);

public:
    vec2 operator+() const;
    vec2 operator-() const;

public:
    vec2 operator+(const vec2&) const;
    vec2 operator-(const vec2&) const;
    vec2 operator*(float) const;
    vec2 operator/(float) const;
    friend vec2 operator*(float, const vec2&);

public:
    bool operator==(const vec2&) const;
    bool operator!=(const vec2&) const;

public:
    float length() const { return vec2length(this); }
    float lengthsq() const { return vec2lengthsq(this); }
    float dot(const vec2& v) const { return vec2dot(this, &v); }
    float ccw(const vec2& v) const { return vec2ccw(this, &v); }
    vec2& add(const vec2& p1, const vec2& p2) { return *vec2add(this, &p1, &p2); }
    vec2& sub(const vec2& p1, const vec2& p2) { return *vec2sub(this, &p1, &p2); }
    vec2& scale(float s) { return *vec2scale(this, this, s); }
    vec2& scale(const vec2& v, float s) { return *vec2scale(this, &v, s); }
    vec2& lerp(const vec2& p1, const vec2& p2, float s) { return *vec2lerp(this, &p1, &p2, s); }
    vec2& multiply(const matrix2& m) { return *vec2multiply(this, this, &m); }
    vec2& multiply(const vec2& p, const matrix2& m) { return *vec2multiply(this, &p, &m); }
    vec2& normalize() { return *vec2normalize(this, this); }
    vec2& normalize(const vec2& v) { return *vec2normalize(this, &v); }
    vec2& hermite(const vec2& p1, const vec2& t1, const vec2& p2, const vec2& t2, float s) { return *vec2hermite(this, &p1, &t1, &p2, &t2, s); }
    vec2& catmullrom(const vec2& p1, const vec2& p2, const vec2& p3, const vec2& p4, float s) { return *vec2catmullrom(this, &p1, &p2, &p3, &p4, s); }
    vec2& barycentric(const vec2& p1, const vec2& p2, const vec2& p3, float f, float g) { return *vec2barycentric(this, &p1, &p2, &p3, f, g); }
    vec4& transform(vec4& v, const matrix& m) const { return *vec2transform(&v, this, &m); }
    vec2& transformcoord(const mat3& m) { return *vec2transformcoord(this, this, &m); }
    vec2& transformcoord(const vec2& p, const mat3& m) { return *vec2transformcoord(this, &p, &m); }
    vec2& transformnormal(const mat3& m) { return *vec2transformnormal(this, this, &m); }
    vec2& transformnormal(const vec2& p, const mat3& m) { return *vec2transformnormal(this, &p, &m); }
};

class gs_export vec3:
    public _vec3
{
public:
    vec3() {}
    vec3(const float*);
    vec3(float x, float y, float z);

public:
    operator float*();
    operator const float*() const;

public:
    vec3& operator+=(const vec3&);
    vec3& operator-=(const vec3&);
    vec3& operator*=(float);
    vec3& operator/=(float);

public:
    vec3 operator+() const;
    vec3 operator-() const;

public:
    vec3 operator+(const vec3&) const;
    vec3 operator-(const vec3&) const;
    vec3 operator*(float) const;
    vec3 operator/(float) const;
    friend vec3 operator*(float, const vec3&);

public:
    bool operator==(const vec3&) const;
    bool operator!=(const vec3&) const;

public:
    float length() const { return vec3length(this); }
    float lengthsq() const { return vec3lengthsq(this); }
    float dot(const vec3& v) const { return vec3dot(this, &v); }
    vec3& add(const vec3& v1, const vec3& v2) { return *vec3add(this, &v1, &v2); }
    vec3& sub(const vec3& v1, const vec3& v2) { return *vec3sub(this, &v1, &v2); }
    vec3& cross(const vec3& v) { return *vec3cross(this, &vec3(*this), &v); }
    vec3& cross(const vec3& v1, const vec3& v2) { return *vec3cross(this, &v1, &v2); }
    vec3& scale(float s) { return *vec3scale(this, this, s); }
    vec3& scale(const vec3& v, float s) { return *vec3scale(this, &v, s); }
    vec3& lerp(const vec3& p1, const vec3& p2, float s) { return *vec3lerp(this, &p1, &p2, s); }
    vec3& multiply(const vec3& p, const mat3& m) { return *vec3multiply(this, &p, &m); }
    vec3& project(const viewport& vp, const matrix& prj, const matrix& view, const matrix& world) { return *vec3project(this, this, &vp, &prj, &view, &world); }
    vec3& unproject(const viewport& vp, const matrix& prj, const matrix& view, const matrix& world) { return *vec3unproject(this, this, &vp, &prj, &view, &world); }
    vec3& normalize() { return *vec3normalize(this, this); }
    vec3& normalize(const vec3& v) { return *vec3normalize(this, &v); }
    vec3& hermite(const vec3& v1, const vec3& t1, const vec3& v2, const vec3& t2, float s) { return *vec3hermite(this, &v1, &t1, &v2, &t2, s); }
    vec3& catmullrom(const vec3& v1, const vec3& v2, const vec3& v3, const vec3& v4, float s) { return *vec3catmullrom(this, &v1, &v2, &v3, &v4, s); }
    vec3& barycentric(const vec3& v1, const vec3& v2, const vec3& v3, float f, float g) { return *vec3barycentric(this, &v1, &v2, &v3, f, g); }
    vec4& transform(vec4& v, const matrix& m) const { return *vec3transform(&v, this, &m); }
    vec3& transformcoord(const matrix& m) { return *vec3transformcoord(this, this, &m); }
    vec3& transformcoord(const vec3& v, const matrix& m) { return *vec3transformcoord(this, &v, &m); }
    vec3& transformnormal(const matrix& m) { return *vec3transformnormal(this, this, &m); }
    vec3& transformnormal(const vec3& v, const matrix& m) { return *vec3transformnormal(this, &v, &m); }
};

class gs_export vec4:
    public _vec4
{
public:
    vec4() {}
    vec4(const float*);
    vec4(const _vec3& xyz, float w);
    vec4(float x, float y, float z, float w);

public:
    operator float*();
    operator const float*() const;

public:
    vec4& operator+=(const vec4&);
    vec4& operator-=(const vec4&);
    vec4& operator*=(float);
    vec4& operator/=(float);

public:
    vec4 operator+() const;
    vec4 operator-() const;

public:
    vec4 operator+(const vec4&) const;
    vec4 operator-(const vec4&) const;
    vec4 operator*(float) const;
    vec4 operator/(float) const;
    friend vec4 operator*(float, const vec4&);

public:
    bool operator==(const vec4&) const;
    bool operator!=(const vec4&) const;

public:
    float length() const { return vec4length(this); }
    float lengthsq() const { return vec4lengthsq(this); }
    float dot(const vec4& v) const { return vec4dot(this, &v); }
    vec4& add(const vec4& v1, const vec4& v2) { return *vec4add(this, &v1, &v2); }
    vec4& sub(const vec4& v1, const vec4& v2) { return *vec4sub(this, &v1, &v2); }
    vec4& scale(float s) { return *vec4scale(this, this, s); }
    vec4& scale(const vec4& v, float s) { return *vec4scale(this, &v, s); }
    vec4& lerp(const vec4& v1, const vec4& v2, float s) { return *vec4lerp(this, &v1, &v2, s); }
    vec4& multiply(const matrix& m) { return *vec4multiply(this, this, &m); }
    vec4& multiply(const vec4& v, const matrix& m) { return *vec4multiply(this, &v, &m); }
    vec4& cross(const vec4& v1, const vec4& v2, const vec4& v3) { return *vec4cross(this, &v1, &v2, &v3); }
    vec4& hermite(const vec4& v1, const vec4& t1, const vec4& v2, const vec4& t2, float s) { return *vec4hermite(this, &v1, &t1, &v2, &t2, s); }
    vec4& catmullrom(const vec4& v1, const vec4& v2, const vec4& v3, const vec4& v4, float s) { return *vec4catmullrom(this, &v1, &v2, &v3, &v4, s); }
    vec4& barycentric(const vec4& v1, const vec4& v2, const vec4& v3, float f, float g) { return *vec4barycentric(this, &v1, &v2, &v3, f, g); }
    vec4& transform(const matrix& m) { return *vec4transform(this, this, &m); }
    vec4& transform(const vec4& v, const matrix& m) { return *vec4transform(this, &v, &m); }
};

class gs_export mat3:
    public _mat3
{
public:
    mat3() {}
    mat3(const float*);
    mat3(const mat3&);
    mat3(float f11, float f12, float f13,
         float f21, float f22, float f23,
         float f31, float f32, float f33
         );

public:
    float& operator()(uint row, uint col);
    float operator()(uint row, uint col) const;
    operator float*();
    operator const float*() const;

public:
    mat3& operator*=(const mat3&);
    mat3& operator+=(const mat3&);
    mat3& operator-=(const mat3&);
    mat3& operator*=(float);
    mat3& operator/=(float);

public:
    mat3 operator+() const;
    mat3 operator-() const;

public:
    mat3 operator*(const mat3&) const;
    mat3 operator+(const mat3&) const;
    mat3 operator-(const mat3&) const;
    mat3 operator*(float) const;
    mat3 operator/(float) const;
    friend mat3 operator*(float, const mat3&);

public:
    bool operator==(const mat3&) const;
    bool operator!=(const mat3&) const;

public:
    mat3& identity() { return *mat3identity(this); }
    bool isidentity() const { return mat3isidentity(this); }
    mat3& transpose() { return *mat3transpose(this, this); }
    mat3& transpose(const mat3& m) { return *mat3transpose(this, &m); }
    mat3& scaling(float sx, float sy) { return *mat3scaling(this, sx, sy); }
    mat3& translation(float x, float y) { return *mat3translation(this, x, y); }
    mat3& rotation(float angle) { return *mat3rotation(this, angle); }
    mat3& rotationpos(const vec2& p, float angle) { return *mat3rotationpos(this, &p, angle); }
    mat3& multiply(const mat3& m) { return *mat3multiply(this, this, &m); }
    mat3& multiply(const mat3& m1, const mat3& m2) { return *mat3multiply(this, &m1, &m2); }
    float determinant() const { return mat3determinant(this); }
    mat3& inverse(float* det, const mat3& m) { return *mat3inverse(this, det, &m); }
};

class gs_export mat4:
    public _mat4
{
public:
    mat4() {}
    mat4(const float*);
    mat4(const mat4&);
    mat4(float f11, float f12, float f13, float f14,
         float f21, float f22, float f23, float f24,
         float f31, float f32, float f33, float f34,
         float f41, float f42, float f43, float f44);

public:
    float& operator()(uint row, uint col);
    float operator()(uint row, uint col) const;
    operator float*();
    operator const float*() const;

public:
    mat4& operator*=(const mat4&);
    mat4& operator+=(const mat4&);
    mat4& operator-=(const mat4&);
    mat4& operator*=(float);
    mat4& operator/=(float);

public:
    mat4 operator+() const;
    mat4 operator-() const;

public:
    mat4 operator*(const mat4&) const;
    mat4 operator+(const mat4&) const;
    mat4 operator-(const mat4&) const;
    mat4 operator*(float) const;
    mat4 operator/(float) const;
    friend mat4 operator*(float, const mat4&);

public:
    bool operator==(const mat4&) const;
    bool operator!=(const mat4&) const;

public:
    mat4& identity() { return *matidentity(this); }
    bool isidentity() const { return matisidentity(this); }
    bool decompose(vec3& scale, quat& rot, vec3& trans) { return matdecompose(&scale, &rot, &trans, this); }
    mat4& transpose() { return *mattranspose(this, this); }
    mat4& transpose(const mat4& m) { return *mattranspose(this, &m); }
    mat4& scaling(float sx, float sy, float sz) { return *matscaling(this, sx, sy, sz); }
    mat4& translation(float x, float y, float z) { return *mattranslation(this, x, y, z); }
    mat4& rotation_x(float angle) { return *matrotation_x(this, angle); }
    mat4& rotation_y(float angle) { return *matrotation_y(this, angle); }
    mat4& rotation_z(float angle) { return *matrotation_z(this, angle); }
    mat4& rotation(const vec3& v, float angle) { return *matrotationaxis(this, &v, angle); }
    mat4& rotation(const quat& q) { return *matrotationquatern(this, &q); }
    mat4& rotation(float yaw, float pitch, float roll) { return *matrotationeuler(this, yaw, pitch, roll); }
    mat4& lookatlh(const vec3& eye, const vec3& at, const vec3& up) { return *matlookatlh(this, &eye, &at, &up); }
    mat4& lookatrh(const vec3& eye, const vec3& at, const vec3& up) { return *matlookatrh(this, &eye, &at, &up); }
    mat4& perspectivelh(float w, float h, float zn, float zf) { return *matperspectivelh(this, w, h, zn, zf); }
    mat4& perspectiverh(float w, float h, float zn, float zf) { return *matperspectiverh(this, w, h, zn, zf); }
    mat4& perspectivefovlh(float fovy, float aspect, float zn, float zf) { return *matperspectivefovlh(this, fovy, aspect, zn, zf); }
    mat4& perspectivefovrh(float fovy, float aspect, float zn, float zf) { return *matperspectivefovrh(this, fovy, aspect, zn, zf); }
    mat4& perspectiveoffcenterlh(float l, float r, float b, float t, float zn, float zf) { return *matperspectiveoffcenterlh(this, l ,r, b, t, zn, zf); }
    mat4& perspectiveoffcenterrh(float l, float r, float b, float t, float zn, float zf) { return *matperspectiveoffcenterlh(this, l ,r, b, t, zn, zf); }
    mat4& ortholh(float w, float h, float zn, float zf) { return *matortholh(this, w, h, zn, zf); }
    mat4& orthorh(float w, float h, float zn, float zf) { return *matorthorh(this, w, h, zn, zf); }
    mat4& orthooffcenterlh(float l, float r, float b, float t, float zn, float zf) { return *matorthooffcenterlh(this, l, r, b, t, zn, zf); }
    mat4& orthooffcenterrh(float l, float r, float b, float t, float zn, float zf) { return *matorthooffcenterrh(this, l, r, b, t, zn, zf); }
    mat4& viewportproject(const viewport& vp) { return *matviewportproject(this, &vp); }
    mat4& viewportunproject(const viewport& vp) { return *matviewportunproject(this, &vp); }
    mat4& transform(const vec3& scalecenter, const quat& scalerot, const vec3& scale, const vec3& rotcenter, const quat& rot, const vec3& trans) { return *mattransform(this, &scalecenter, &scalerot, &scale, &rotcenter, &rot, &trans); }
    mat4& transform2d(const vec2& scalecenter, float scalerot, const vec2& scale, const vec2& rotcenter, float rot, const vec2& trans) { return *mattransform2d(this, &scalecenter, scalerot, &scale, &rotcenter, rot, &trans); }
    mat4& affinetransform(float scale, const vec3& rotcenter, const quat& rot, const vec3& trans) { return *mataffinetransform(this, scale, &rotcenter, &rot, &trans); }
    mat4& affinetransform2d(float scale, const vec2& rotcenter, float rot, const vec2& trans) { return *mataffinetransform2d(this, scale, &rotcenter, rot, &trans); }
    float determinant() const { return matdeterminant(this); }
    mat4& multiply(const mat4& m) { return *matmultiply(this, this, &m); }
    mat4& multiply(const mat4& m1, const mat4& m2) { return *matmultiply(this, &m1, &m2); }
    mat4& multiplytranspose(const mat4& m) { return *matmultiplytranspose(this, this, &m); }
    mat4& multiplytranspose(const mat4& m1, const mat4& m2) { return *matmultiplytranspose(this, &m1, &m2); }
    mat4& inverse(float* determinant, const mat4& m) { return *matinverse(this, determinant, &m); }
    mat4& shadow(const vec4& plight, const plane& pln) { return *matshadow(this, &plight, &pln); }
    mat4& reflect(const plane& pln) { return *matreflect(this, &pln); }
};

class gs_export quat:
    public _quat
{
public:
    quat() {}
    quat(const float*);
    quat(float x, float y, float z, float w);

public:
    operator float*();
    operator const float*() const;

public:
    quat& operator+=(const quat&);
    quat& operator-=(const quat&);
    quat& operator*=(const quat&);
    quat& operator*=(float);
    quat& operator/=(float);

public:
    quat operator+() const;
    quat operator-() const;

public:
    quat operator+(const quat&) const;
    quat operator-(const quat&) const;
    quat operator*(const quat&) const;
    quat operator*(float) const;
    quat operator/(float) const;
    friend quat operator*(float, const quat&);

public:
    bool operator==(const quat&) const;
    bool operator!=(const quat&) const;

public:
    float length() const { return quatlength(this); }
    float lengthsq() const { return quatlengthsq(this); }
    float dot(const quat& q) const { return quatdot(this, &q); }
    quat& identity() { return *quatidentity(this); }
    bool isidentity() const { return quatisidenetity(this); }
    quat& conjugate() { return *quatconjugate(this, this); }
    quat& conjugate(const quat& q) { return *quatconjugate(this, &q); }
    float toaxisangle(vec3& axis) const
    {
        float f;
        quattoaxisangle(this, &axis, &f);
        return f;
    }
    quat& rotate(const matrix& m) { return *quatrotatematrix(this, &m); }
    quat& rotate(const vec3& v, float angle) { return *quatrotateaxis(this, &v, angle); }
    quat& ln() { return *quatln(this, this); }
    quat& ln(const quat& q) { return *quatln(this, &q); }
    quat& exp() { return *quatexp(this, this); }
    quat& exp(const quat& q) { return *quatexp(this, &q); }
    quat& slerp(const quat& q1, const quat& q2, float t) { return *quatslerp(this, &q1, &q2, t); }
    quat& squad(const quat& q, const quat& a, const quat& b, const quat& c, float f) { return *quatsquad(this, &q, &a, &b, &c, f); }
    quat& squad(const quat& a, const quat& b, const quat& c, float f) { return *quatsquad(this, this, &a, &b, &c, f); }
    quat& barycentric(const quat& q1, const quat& q2, const quat& q3, float f, float g) { return *quatbarycentric(this, &q1, &q2, &q3, f, g); }
    quat& rotate(float yaw, float pitch, float roll) { return *quatrotateeuler(this, yaw, pitch, roll); }
    quat& multiply(const quat& q1, const quat& q2) { return *quatmultiply(this, &q1, &q2); }
    quat& normalize() { return *quatnormalize(this, this); }
    quat& normalize(const quat& q) { return *quatnormalize(this, &q); }
    quat& inverse() { return *quatinverse(this, this); }
    quat& inverse(const quat& q) { return *quatinverse(this, &q); }
};

class gs_export plane:
    public _plane
{
public:
    plane() {}
    plane(const float*);
    plane(float a, float b, float c, float d);

public:
    operator float*();
    operator const float*() const;

public:
    plane& operator*=(float);
    plane& operator/=(float);
    plane operator+() const;
    plane operator-() const;
    plane operator*(float) const;
    plane operator/(float) const;
    friend plane operator*(float, const plane&);

public:
    bool operator==(const plane&) const;
    bool operator!=(const plane&) const;

public:
    float dot(const vec4& v) const { return planedot(this, &v); }
    float dotcoord(const vec3& v) const { return planedotcoord(this, &v); }
    float dotnormal(const vec3& v) const { return planedotnormal(this, &v); }
    plane& scale(float s) { return *planescale(this, this, s); }
    plane& scale(const plane& p, float s) { return *planescale(this, &p, s); }
    plane& frompointnormal(const vec3& p, const vec3& normal) { return *planefrompointnormal(this, &p, &normal); }
    plane& normalize() { return *planenormalize(this, this); }
    plane& normalize(const plane& p) { return *planenormalize(this, &p); }
    vec3& intersectline(vec3& v, const vec3& v1, const vec3& v2) const { return *planeintersectline(&v, this, &v1, &v2); }
    plane& frompoints(const vec3& v1, const vec3& v2, const vec3& v3) { return *planefrompoints(this, &v1, &v2, &v3); }
    plane& transform(const matrix& m) { return *planetransform(this, this, &m); }
    plane& transform(const plane& p, const matrix& m) { return *planetransform(this, &p, &m); }
};

__gslib_end__

#include <gslib/math.inl>

#endif


// @file ker2col_conv.c
//
//  \date Created on: Sep 24, 2017
//  \author Gopalakrishna Hegde
//
//   Description:
//
//
//

#include <stdbool.h>
#include <cblas.h>
#include <assert.h>
#include <stdlib.h>
#include <string.h>
#include "common_types.h"
#include "data_reshape.h"
#include "utils.h"
#include "conv_layers.h"

bool Kn2ColConvLayer(const float *in_data, const float *filters,
                         const float *bias, TensorDim in_dim,
                         TensorDim filt_dim, int stride, int pad, int group,
                         float *output) {
  // Currently we have limited support.
  assert(group == 1);
  assert((pad == 0) || (pad == filt_dim.w / 2));
  assert(in_dim.n == 1);
  assert(filt_dim.h == filt_dim.w);
  assert(stride == 1);

  // Output dimensions.
  TensorDim out_dim;
  out_dim.w = (in_dim.w + (pad + pad) - filt_dim.w) / stride + 1;
  out_dim.h = (in_dim.h + (pad + pad) - filt_dim.h) / stride + 1;
  out_dim.c = filt_dim.n;
  out_dim.n = in_dim.n;

  // Reshape filters in CHWN (ker_size x ker_size x no_in_ch x no_out_ch) format
  float *kkcm_filters = malloc(filt_dim.n * filt_dim.c * filt_dim.h *
                               filt_dim.w * sizeof(float));
  NCHW2HWCN(filters, filt_dim.n, filt_dim.c, filt_dim.h, filt_dim.w,
            kkcm_filters);

  // Just for convenience
  int H = in_dim.h;
  int W = in_dim.w;
  float alpha = 1.0;
  float beta = 0.0;

  // We need separate buffer because GEMM output will have width = H*W even
  // if there is no padding (pad = 0).
  float *gemm_output = malloc(out_dim.c * H * W * sizeof(float));
  float *nchw_gemm_output = malloc(out_dim.c * H * W * sizeof(float));
  // Prefill output buffer with bias if present else set to zero.
  if (bias) {
    for (int m = 0; m < out_dim.c; ++m) {
      for (int a = 0; a < out_dim.h * out_dim.w; ++a) {
        output[m * out_dim.h * out_dim.w + a] = bias[m];
      }
      // For batch size > 1
      for (int b = 1; b < out_dim.n; ++b) {
        memcpy(output + b * out_dim.c * out_dim.h * out_dim.w,
               output, out_dim.c * out_dim.h * out_dim.w * sizeof(float));
      }
    }
  } else {
    memset(output, 0, out_dim.n * out_dim.c * out_dim.h * out_dim.w *
           sizeof(float));
  }

  for (int kr = 0; kr < filt_dim.h; kr++) {
    int row_shift = pad - kr;
    for (int kc = 0; kc < filt_dim.w; kc++) {
      int group_no = kr * filt_dim.w + kc;
      int col_shift = pad - kc;
      // Matrix dimensions - A -> mxk B -> kxn  C --> mxn
      int m = in_dim.h * in_dim.w;
      int k = filt_dim.c;
      int n = filt_dim.n;

      // output is in H x W x C format.
      cblas_sgemm(CblasRowMajor, CblasTrans, CblasNoTrans,
                  m, n, k, alpha, in_data, m,
                  kkcm_filters + group_no * filt_dim.c * filt_dim.n, n, beta,
                  gemm_output, n);

      // convert to CxHxW format.
      // FIXME: this will be slow. Need to find other ways :(
      NHWC2NCHW(gemm_output, 1, filt_dim.n, H, W, nchw_gemm_output);

      for (int omap = 0; omap < filt_dim.n; omap++) {
        MatrixShiftAdd(output + omap * out_dim.h * out_dim.w,
                        out_dim.h, out_dim.w,
                        nchw_gemm_output + omap * H * W,
                        H, W, row_shift, col_shift);
      }
    }
  }
  free(kkcm_filters);
  free(gemm_output);
  free(nchw_gemm_output);
  return true;
}