#include "tmcoct.h"
#include <gsl/gsl_rng.h>

const double G_COS_0_D = 1.0 - 1.0E-14;
const double G_COS_90_D = 1.0E-7;

#define STANDARDTEST 0
/* testing program using fixed rnd seed. */

#define PARTIALREFLECTION 1
/* 1=split photon, 0=statistical reflection. */

#define COSZERO (1.0-1.0E-14)
/* cosine of about 1e-6 rad. */

#define COS90D  1.0E-6
/* cosine of about 1.57 - 1e-6 rad. */

const gsl_rng_type *T_RNG_GSL;
gsl_rng *r_RNG_GSL;

/***********************************************************
 *	A random number generator from Numerical Recipes in C.
 ****/
#define MBIG 1000000000
#define MSEED 161803398
#define MZ 0
#define FAC 1.0E-9

#undef MBIG
#undef MSEED
#undef MZ
#undef FAC

/***********************************************************
 *	Generate a random number between 0 and 1.  Take a
 *	number as seed the first time entering the function.
 *	The seed is limited to 1<<15.
 *	We found that when idum is too large, ran3 may return
 *	numbers beyond 0 and 1.
 ****/
double RandomNum(void) {
    static Boolean first_time = 1;
    static unsigned long int idum;    /* seed for ran3. */

    if (first_time) {
        T_RNG_GSL = gsl_rng_default;
        r_RNG_GSL = gsl_rng_alloc(T_RNG_GSL);

#if STANDARDTEST /* Use fixed seed to test the program. */
        printf("*** Using seed to test. ***\n");
    printf("*** Remove the fixed seed after the test. ***\n");
    idum =  7923;

#else
        idum = (unsigned long int) time(NULL) % (1 << 15);
        /* use 16-bit integer as the seed. */
#endif
        printf("Random seed = %lu \n", idum);

        //ran3(&idum);
        gsl_rng_set(r_RNG_GSL, idum);

        first_time = 0;
        //idum = 1;
    }
    // TODO FIXME remove 1.0 -
    return (1.0 - gsl_rng_uniform(r_RNG_GSL));
}

/***********************************************************
 *	Compute the specular reflection.
 *
 *	If the first layer is a turbid medium, use the Fresnel
 *	reflection from the boundary of the first layer as the
 *	specular reflectance.
 *
 *	If the first layer is glass, multiple reflections in
 *	the first layer is considered to get the specular
 *	reflectance.
 *
 *	The subroutine assumes the Layerspecs array is correctly
 *	initialized.
 ****/
double Rspecular(double ni, double nt) {
    double r1;
    /* direct reflections from the 1st and 2nd layers. */
    double temp;
    temp = (ni - nt) / (ni + nt);
    r1 = temp * temp;
    return (r1);
}

/***********************************************************
 *	Initialize a photon packet.
 ****/
void LaunchPhoton(SimulationStruct *in_parm, double Rspecular, TetrahedronPhotonStruct *Photon_Ptr,
                  TetrahedronPhotonStruct *PhotonCont_Ptr, TetrahedronStruct *TetrahedronRoot) {
    if (Photon_Ptr->FstBackReflectionFlag) {
        //Most often case for biased simulations
        //The photon will be re-initialized from where the bias was applied
        long double LikelihoodRatioTmp = Photon_Ptr->LikelihoodRatioAfterFstBias;
        CopyPhotonStruct(PhotonCont_Ptr, Photon_Ptr);

        //Apply one unbiased random scatter to the split photon
        Spin(regionspecs[Photon_Ptr->tetrahedron->region].g, Photon_Ptr);

        if (LikelihoodRatioTmp < 1)
            // Adjust the likelihoodRation to make the simulation unbiased
            Photon_Ptr->LikelihoodRatio = 1 - LikelihoodRatioTmp;
        else
            Photon_Ptr->LikelihoodRatio = 1;

    } else {

        Photon_Ptr->w = 1.0 - Rspecular;

        Photon_Ptr->x = in_parm->probe_x;
        Photon_Ptr->y = in_parm->probe_y;
        Photon_Ptr->z = in_parm->probe_z;

        Photon_Ptr->s = 0.0;
        Photon_Ptr->sleft = 0.0;

        Photon_Ptr->ux = Photon_Ptr->uy = 0.0;
        Photon_Ptr->uz = 1.0;
        Photon_Ptr->dead = 0.0;

        Photon_Ptr->OpticalPath = 0.0;
        Photon_Ptr->MaxDepth = 0.0;
        Photon_Ptr->LikelihoodRatio = 1.0;
        Photon_Ptr->LikelihoodRatioAfterFstBias = 1.0;

        Photon_Ptr->FstBackReflectionFlag = 0;
        Photon_Ptr->LocationFstBias = -1.0;
        Photon_Ptr->NumBackwardsSpecularReflections = 0;

        Photon_Ptr->LikelihoodRatioIncreaseFstBias = 0;

        Photon_Ptr->LikelihoodRatioIncreaseFstBias = 0;

        Photon_Ptr->NextTetrahedron = -1;

        Photon_Ptr->tetrahedron = TetrahedronRoot;
    }
}

/***********************************************************
 *	Choose (sample) a new theta angle for photon propagation
 *	according to the anisotropy.
 *
 *	If anisotropy g is 0, then
 *		cos(theta) = 2*rand-1.
 *	otherwise
 *		sample according to the Henyey-Greenstein function.
 *
 *	Returns the cosine of the polar deflection angle theta.
 ****/
double SpinTheta(double g) {
    double cost;

    if (g == 0.0)
        cost = 2.0 * RandomNum() - 1.0;
    else {
        double temp = (1.0 - g * g) / (1.0 - g + 2.0 * g * RandomNum());
        cost = (1.0 + g * g - temp * temp) / (2.0 * g);
        if (cost < -1.0) cost = -1.0;
        else if (cost > 1.0) cost = 1.0;
    }
    return (cost);
}

double SpinThetaForwardFstBias(double g) {
    double cost;

    if (g == 0.0)
        cost = RandomNum();
    else {
        double RandomNumTmp = RandomNum();
        double temp = RandomNumTmp / (1 - g) + (1 - RandomNumTmp) / sqrt(g * g + 1);
        cost = (g * g + 1 - 1. / (temp * temp)) / (2 * g);
        if (cost < -1) cost = -1;
        else if (cost > 1) cost = 1;
    }
    return (cost);
}

/***********************************************************
 *	Choose a new direction for photon propagation by
 *	sampling the polar deflection angle theta and the
 *	azimuthal angle psi.
 *
 *	Note:
 *  	theta: 0 - pi so sin(theta) is always positive
 *  	feel free to use sqrt() for cos(theta).
 *
 *  	psi:   0 - 2pi
 *  	for 0-pi  sin(psi) is +
 *  	for pi-2pi sin(psi) is -
 ****/

void Spin(double g,
          TetrahedronPhotonStruct *Photon_Ptr) {
    double cost, sint;    /* cosine and sine of the */
    /* polar deflection angle theta. */
    double cosp, sinp;    /* cosine and sine of the */
    /* azimuthal angle psi. */
    double ux = Photon_Ptr->ux;
    double uy = Photon_Ptr->uy;
    double uz = Photon_Ptr->uz;
    double psi;

    cost = SpinTheta(g);
    sint = sqrt(1.0 - cost * cost);
    /* sqrt() is faster than sin(). */

    psi = 2.0 * PI * RandomNum(); /* spin psi 0-2pi. */
    cosp = cos(psi);
    if (psi < PI)
        sinp = sqrt(1.0 - cosp * cosp);
        /* sqrt() is faster than sin(). */
    else
        sinp = -sqrt(1.0 - cosp * cosp);

    if (fabs(uz) > COSZERO) {    /* normal incident. */
        Photon_Ptr->ux = sint * cosp;
        Photon_Ptr->uy = sint * sinp;
        Photon_Ptr->uz = cost * SIGN(uz);
        /* SIGN() is faster than division. */
    } else {
        /* regular incident. */
        double temp = sqrt(1.0 - uz * uz);
        Photon_Ptr->ux = sint * (ux * uz * cosp - uy * sinp)
                         / temp + ux * cost;
        Photon_Ptr->uy = sint * (uy * uz * cosp + ux * sinp)
                         / temp + uy * cost;
        Photon_Ptr->uz = -sint * cosp * temp + uz * cost;
    }

}

void SpinBias37(double g,
                TetrahedronPhotonStruct *Photon_Ptr,
                TetrahedronPhotonStruct *PhotonCont_Ptr,
                SimulationStruct *In_Ptr)
{
    double g_squared = sq(g);

    double cost, sint;    /* cosine and sine of the */
    /* polar deflection angle theta. */
    double cosp, sinp;    /* cosine and sine of the */
    /* azimuthal angle psi. */
    double ux = Photon_Ptr->ux;
    double uy = Photon_Ptr->uy;
    double uz = Photon_Ptr->uz;
    double ux_Orig = ux;
    double uy_Orig = uy;
    double uz_Orig = uz;
    double psi;

    double BackwardBiasCoefficient = In_Ptr->BackwardBiasCoefficient;
    double costg;
    double costg1, costg2;

    double BiasCoefficientTmp = 0;

    short int ReachedTargetOpticalDepthFlag = 0;
    short int ThisIsFirstBackwardBiasFlag = 0;

    if (Photon_Ptr->z > In_Ptr->TargetDepthMin
        && Photon_Ptr->z < In_Ptr->TargetDepthMax
        && Photon_Ptr->uz > 0
        && !Photon_Ptr->FstBackReflectionFlag) {
        // The bias backwards will be applied only if the photon is going forward
        // The status of the photon prior to the bias will be saved

        CopyPhotonStruct(Photon_Ptr, PhotonCont_Ptr);
        ReachedTargetOpticalDepthFlag = 1;
        Photon_Ptr->FstBackReflectionFlag = 1; // Bias backwards only once
        Photon_Ptr->LocationFstBias = Photon_Ptr->z;
        BiasCoefficientTmp = BackwardBiasCoefficient;
        ThisIsFirstBackwardBiasFlag = 1;
    }

    /**********************************
	 ** Biased Direction towards probe
	 **********************************/
    double vx = In_Ptr->probe_x-Photon_Ptr->x;
    double vy = In_Ptr->probe_y-Photon_Ptr->y;
    double vz = In_Ptr->probe_z-Photon_Ptr->z;
    double LengthVector = sqrt(sq(vx) + sq(vy) + sq(vz));
    vx /= LengthVector;
    vy /= LengthVector;
    vz /= LengthVector;
    /*********************************/

    if ((Photon_Ptr->FstBackReflectionFlag
         || Photon_Ptr->NumBackwardsSpecularReflections > 0)
        && !ReachedTargetOpticalDepthFlag)
    {
        // It was biased at least once before, and is moving backwards
        ReachedTargetOpticalDepthFlag = 2;

        double NextStepSize = -log(In_Ptr->rndStepSizeInTissue) / regionspecs[Photon_Ptr->tetrahedron->region].muas;
        double CurrentDistanceToOrigin = sqrt(sq(Photon_Ptr->x-In_Ptr->probe_x) + sq(Photon_Ptr->y-In_Ptr->probe_y) + sq(Photon_Ptr->z-In_Ptr->probe_z));

        if (NextStepSize >= CurrentDistanceToOrigin
            && acos(-vz) <= In_Ptr->MaxCollectingAngleDeg * PI / 180) {
            ReachedTargetOpticalDepthFlag = 1;
        }

        BiasCoefficientTmp = BackwardBiasCoefficient;

    }

    int BiasFunctionRandomlySelected = 0;
    if (ReachedTargetOpticalDepthFlag) {
        // Photon reached target optical layer or may undergo an additional biased
        // scattering or unbiased scattering

        // BiasFunctionRandomlySelected=1 means use biased scattering and 2 means unbiased scattering
        if (RandomNum() <= In_Ptr->ProbabilityAdditionalBias)
            BiasFunctionRandomlySelected = 1;
        else
            BiasFunctionRandomlySelected = 2;

        if (ReachedTargetOpticalDepthFlag == 1
            || BiasFunctionRandomlySelected == 1) {
            /*************************************************************************
			** The photon is within the target depth and going forward
			** The additional biased scattering is randomly chosen
			** So the scattering is biased Henyey-Greenstein scattering
			*************************************************************************/
            cost = SpinThetaForwardFstBias(BiasCoefficientTmp);
            ux = vx;
            uy = vy;
            uz = vz;
        } else {
            /**************************************************************************************
			** The photon is within the target depth but the scattering is randomly selected is
			** unbiased scattering
			** or the photon is already going backward or it is out of target depth
			**************************************************************************************/
            cost = SpinTheta(g);
        }
    } else {
        /**************************************************************************
		**  The photon is not within the target depth or it is not going forward
		**  so do unbiased scattering
		**************************************************************************/
        cost = SpinTheta(g);
    }

    if (cost < -1)
        cost = -1;
    else if (cost > 1)
        cost = 1;

    sint = sqrt(1.0 - cost * cost);
    /* sqrt() is faster than sin(). */

    psi = 2.0 * PI * RandomNum(); /* spin psi 0-2pi. */
    cosp = cos(psi);
    if (psi < PI)
        sinp = sqrt(1.0 - cosp * cosp);
        /* sqrt() is faster than sin(). */
    else
        sinp = -sqrt(1.0 - cosp * cosp);

    if (fabs(uz) > COSZERO) {    /* normal incident. */
        Photon_Ptr->ux = sint * cosp;
        Photon_Ptr->uy = sint * sinp;
        Photon_Ptr->uz = cost * SIGN(uz);
        /* SIGN() is faster than division. */
    } else {        /* regular incident. */
        double temp = sqrt(1.0 - uz * uz);
        Photon_Ptr->ux = sint * (ux * uz * cosp - uy * sinp)
                         / temp + ux * cost;
        Photon_Ptr->uy = sint * (uy * uz * cosp + ux * sinp)
                         / temp + uy * cost;
        Photon_Ptr->uz = -sint * cosp * temp + uz * cost;
    }

    costg = ux_Orig * Photon_Ptr->ux
            + uy_Orig * Photon_Ptr->uy
            + uz_Orig * Photon_Ptr->uz;
    costg2 = costg;
    costg1 = vx * Photon_Ptr->ux
             + vy * Photon_Ptr->uy
             + vz * Photon_Ptr->uz;

    if (BiasCoefficientTmp) {
        double one_plus_a_squared = 1 + sq(BiasCoefficientTmp);
        double sqrt_one_plus_a_squared = sqrt(one_plus_a_squared);
        double LikelihoodRatioIncreaseFactor;
        if (ReachedTargetOpticalDepthFlag == 1)
            /****************************************************************************************
			 ** Likelihood for the first Bias scattering. Equation (8) of the paper:
			 ** Malektaji, Siavash, Ivan T. Lima, and Sherif S. Sherif. "Monte Carlo simulation of
			 ** optical coherence tomography for turbid media with arbitrary spatial distributions."
			 ** Journal of biomedical optics 19.4 (2014): 046001-046001.
			 ****************************************************************************************/
            LikelihoodRatioIncreaseFactor = ((1.0 - g_squared) * (sqrt_one_plus_a_squared - 1.0 + BiasCoefficientTmp) *
                                             sqrt(cube(one_plus_a_squared - 2.0 * BiasCoefficientTmp * cost))) /
                                            (2.0 * BiasCoefficientTmp * (1.0 - BiasCoefficientTmp) *
                                             sqrt_one_plus_a_squared * sqrt(cube(1.0 + g_squared - 2.0 * g * costg)));
        else {
            double cost1, cost2;
            if (BiasFunctionRandomlySelected == 1) {
                cost1 = cost;
                cost2 = costg2;
            } else {
                cost1 = costg1;
                cost2 = cost;
            }
            /*******************************************************************************************************
			 **  The likelihood ratio of additional biased scatterings, whether the biased or the unbiased
			 **  probability density function is randomly selected, is calculated according to the equation (9)
			 **  of the paper:
			 **  Malektaji, Siavash, Ivan T. Lima, and Sherif S. Sherif. "Monte Carlo simulation of
			 **  optical coherence tomography for turbid media with arbitrary spatial distributions."
			 **  Journal of biomedical optics 19.4 (2014): 046001-046001.
			 *******************************************************************************************************/
            double pdf1 = (sqrt_one_plus_a_squared * BiasCoefficientTmp * (1.0 - BiasCoefficientTmp)) /
                          ((sqrt_one_plus_a_squared - 1.0 + BiasCoefficientTmp) *
                           sqrt(cube(one_plus_a_squared - 2.0 * BiasCoefficientTmp * cost1)));

            double pdf2 = (1.0 - g_squared) / (2.0 * cube(sqrt(1.0 + g_squared - 2.0 * g * cost2)));

            LikelihoodRatioIncreaseFactor = pdf2 /
                                            (In_Ptr->ProbabilityAdditionalBias * pdf1 +
                                             (1.0 - In_Ptr->ProbabilityAdditionalBias) * pdf2);

        }

        Photon_Ptr->LikelihoodRatio *= LikelihoodRatioIncreaseFactor;
        if (ThisIsFirstBackwardBiasFlag == 1) {
            // In case there was a sure backward bias and that was the very first one
            Photon_Ptr->LikelihoodRatioAfterFstBias = Photon_Ptr->LikelihoodRatio;
            Photon_Ptr->LikelihoodRatioIncreaseFstBias = LikelihoodRatioIncreaseFactor;
        }
    }
}

/***********************************************************
 *	Move the photon s away in the current layer of medium.
 ****/
void Hop(TetrahedronPhotonStruct *Photon_Ptr) {
    double s = Photon_Ptr->s;
    Photon_Ptr->x += s * Photon_Ptr->ux;
    Photon_Ptr->y += s * Photon_Ptr->uy;
    Photon_Ptr->z += s * Photon_Ptr->uz;

    Photon_Ptr->OpticalPath += s;
    if (Photon_Ptr->MaxDepth < Photon_Ptr->z)
        Photon_Ptr->MaxDepth = Photon_Ptr->z;
}
/***********************************************************
 *	If uz != 0, return the photon step size in glass,
 *	Otherwise, return 0.
 *
 *	The step size is the distance between the current
 *	position and the boundary in the photon direction.
 *
 *	Make sure uz !=0 before calling this function.
 ****/

/***********************************************************
 *	Pick a step size for a photon packet when it is in
 *	tissue.
 *	If the member sleft is zero, make a new step size
 *	with: -log(rnd)/(mua+mus).
 *	Otherwise, pick up the leftover in sleft.
 *
 *	Layer is the index to layer.
 *	In_Ptr is the input parameters.
 ****/
void StepSizeInTissue(TetrahedronPhotonStruct *Photon_Ptr) {

    if (Photon_Ptr->sleft == 0.0) {  /* make a new step. */
        double rnd;

        do rnd = RandomNum();
        while (rnd <= 0.0);    /* avoid zero. */

        Photon_Ptr->s = -log(rnd) * regionspecs[Photon_Ptr->tetrahedron->region].rmuas;
    } else {    /* take the leftover. */
        Photon_Ptr->s = Photon_Ptr->sleft * regionspecs[Photon_Ptr->tetrahedron->region].rmuas;
        Photon_Ptr->sleft = 0.0;
    }
}

/***********************************************************
 *	Check if the step will hit the boundary.
 *	Return 1 if hit boundary.
 *	Return 0 otherwise.
 *
 * 	If the projected step hits the boundary, the members
 *	s and sleft of Photon_Ptr are updated.
 ****/
Boolean HitBoundary(TetrahedronPhotonStruct *Photon_Ptr) {
    TetrahedronStruct *tetrahedron;
    tetrahedron = Photon_Ptr->tetrahedron;

    double min_distance = 1e10;
    int index_of_tetrahedron_with_min_distance = -1;

    double cos_normals_and_photon_direction[4];
    double distance_from_face_in_photon_direction;
    double perpendicular_distance;

    for (int i = 0; i < 4; i++) {
        cos_normals_and_photon_direction[i] = (Photon_Ptr->tetrahedron->Faces[i].Nx) * Photon_Ptr->ux +
                      (Photon_Ptr->tetrahedron->Faces[i].Ny) * Photon_Ptr->uy +
                      (Photon_Ptr->tetrahedron->Faces[i].Nz) * Photon_Ptr->uz;
    }

    for (int i = 0; i < 4; i++) {
        if (cos_normals_and_photon_direction[i] < 0) {
            perpendicular_distance = ((Photon_Ptr->tetrahedron->Faces[i].Nx) * Photon_Ptr->x +
                      (Photon_Ptr->tetrahedron->Faces[i].Ny) * Photon_Ptr->y +
                      (Photon_Ptr->tetrahedron->Faces[i].Nz) * Photon_Ptr->z + Photon_Ptr->tetrahedron->Faces[i].d);
            distance_from_face_in_photon_direction = -perpendicular_distance / cos_normals_and_photon_direction[i];

            if (distance_from_face_in_photon_direction < min_distance) {
                min_distance = distance_from_face_in_photon_direction;
                index_of_tetrahedron_with_min_distance = i;
                Photon_Ptr->MinCos = cos_normals_and_photon_direction[i];
            }
        }
    }

    Boolean hit_boundary;
    if (Photon_Ptr->s > min_distance) {
        /* not horizontal & crossing. */

        Photon_Ptr->sleft = (Photon_Ptr->s - min_distance) * regionspecs[tetrahedron->region].muas;
        Photon_Ptr->s = min_distance;
        hit_boundary = 1;
        Photon_Ptr->NextTetrahedron = index_of_tetrahedron_with_min_distance;

    } else {

        hit_boundary = 0;
        Photon_Ptr->NextTetrahedron = -1;

    }
    return hit_boundary;
}

/***********************************************************
 *	Drop photon weight inside the tissue (not glass).
 *
 *  The photon is assumed not dead.
 *
 *	The weight drop is dw = w*mua/(mua+mus).
 *
 *	The dropped weight is assigned to the absorption array
 *	elements.
 ****/
void Drop(TetrahedronPhotonStruct *Photon_Ptr) {
    double dwa;    /* absorbed weight.*/
    TetrahedronStruct *tetrahedron = Photon_Ptr->tetrahedron;

    dwa = Photon_Ptr->w * regionspecs[tetrahedron->region].mua_muas;
    Photon_Ptr->w -= dwa;
}

/***********************************************************
 *	The photon weight is small, and the photon packet tries
 *	to survive a roulette.
 ****/
void Roulette(TetrahedronPhotonStruct *Photon_Ptr) {
    if (Photon_Ptr->w == 0.0)
        Photon_Ptr->dead = 1;
    else if (RandomNum() < CHANCE) /* survived the roulette.*/
        Photon_Ptr->w /= CHANCE;
    else
        Photon_Ptr->dead = 1;
}

/***********************************************************
 *	Compute the Fresnel reflectance.
 *
 *	Make sure that the cosine of the incident angle a1
 *	is positive, and the case when the angle is greater
 *	than the critical angle is ruled out.
 *
 * 	Avoid trigonometric function operations as much as
 *	possible, because they are computation-intensive.
 ****/
double
RFresnel(SimulationStruct *In_Ptr, TetrahedronPhotonStruct *Photon_Ptr, double *ux_reflected, double *uy_reflected,
         double *uz_reflected, double *ux_refracted, double *uy_refracted, double *uz_refracted) {

    double r;

    double cosa, sina, sinb, cosb;

    cosa = -(Photon_Ptr->MinCos);

    double ni = regionspecs[Photon_Ptr->tetrahedron->region].n;
    double nt;
    if (Photon_Ptr->tetrahedron->adjTetrahedrons[Photon_Ptr->NextTetrahedron] == NULL)
        nt = regionspecs[0].n;
    else {

        int next_region = Photon_Ptr->tetrahedron->adjTetrahedrons[Photon_Ptr->NextTetrahedron]->region;
        nt = regionspecs[next_region].n;
    }

    sina = sqrt(1.0 - cosa * cosa);
    sinb = ni * sina / nt;
    cosb = sqrt(1.0 - sinb * sinb);

    if (cosa > G_COS_0_D) {
        r = (ni - nt)
            / (ni + nt);
        r *= r;
        cosb = 1;
    } else if (cosa < G_COS_90_D) {
        r = 1.0;
    } else {
        sina = sqrt(1 - cosa * cosa);
        sinb = ni * sina / nt;
        if (sinb >= 1) {
            r = 1.0;
        } else {
            cosb = sqrt(1.0 - sinb * sinb);
            double cosacosb = cosa * cosb;
            double sinasinb = sina * sinb;
            double sinacosb = sina * cosb;
            double cosasinb = cosa * sinb;
            double cap = cosacosb - sinasinb;
            double cam = cosacosb + sinasinb;
            double sap = sinacosb + cosasinb;
            double sam = sinacosb - cosasinb;
            r = 0.5 * sam * sam * (cam * cam + cap * cap) / (sap * sap * cam * cam);
        }
    }

    double sbdivsa = ni / nt;
    *ux_refracted = -cosb * Photon_Ptr->tetrahedron->Faces[Photon_Ptr->NextTetrahedron].Nx +
                    sbdivsa * (cosa * Photon_Ptr->tetrahedron->Faces[Photon_Ptr->NextTetrahedron].Nx + Photon_Ptr->ux);
    *uy_refracted = -cosb * Photon_Ptr->tetrahedron->Faces[Photon_Ptr->NextTetrahedron].Ny +
                    sbdivsa * (cosa * Photon_Ptr->tetrahedron->Faces[Photon_Ptr->NextTetrahedron].Ny + Photon_Ptr->uy);
    *uz_refracted = -cosb * Photon_Ptr->tetrahedron->Faces[Photon_Ptr->NextTetrahedron].Nz +
                    sbdivsa * (cosa * Photon_Ptr->tetrahedron->Faces[Photon_Ptr->NextTetrahedron].Nz + Photon_Ptr->uz);

    *ux_reflected = 2 * cosa * Photon_Ptr->tetrahedron->Faces[Photon_Ptr->NextTetrahedron].Nx + Photon_Ptr->ux;
    *uy_reflected = 2 * cosa * Photon_Ptr->tetrahedron->Faces[Photon_Ptr->NextTetrahedron].Ny + Photon_Ptr->uy;
    *uz_reflected = 2 * cosa * Photon_Ptr->tetrahedron->Faces[Photon_Ptr->NextTetrahedron].Nz + Photon_Ptr->uz;

    return r;

}

/***********************************************************
 *	Record the photon weight exiting the first layer(uz<0),
 *	no matter whether the layer is glass or not, to the
 *	reflection array.
 *
 *	Update the photon weight as well.
 ***********************************************************/
void RecordValidationR(double Refl, SimulationStruct *In_Ptr, TetrahedronPhotonStruct *Photon_Ptr, OutStruct *Out_Ptr) {
    double x = Photon_Ptr->x;
    double y = Photon_Ptr->y;

    short SpatialFilterFlag = 0;
    if (sqrt(sq(x-In_Ptr->probe_x) + sq(y-In_Ptr->probe_y)) <
        In_Ptr->MaxCollectingRadius        /* The position of photon should be within the radius of the probe */
        && acos(-Photon_Ptr->uz) < In_Ptr->MaxCollectingAngleDeg * PI / 180) {
        if ((In_Ptr->TypeBias == 0 || In_Ptr->TypeBias == 3))
            SpatialFilterFlag = 1;

        if ((Photon_Ptr->FstBackReflectionFlag && In_Ptr->TypeBias == 37 ||
             Photon_Ptr->LocationFstBias == Photon_Ptr->MaxDepth
             && In_Ptr->TypeBias != 37 ||
             Photon_Ptr->NumBackwardsSpecularReflections > 0))
            SpatialFilterFlag = 1;
    }

    if (SpatialFilterFlag) {

        In_Ptr->NumFilteredPhotons++;
        double FilterOpticalPath = Photon_Ptr->OpticalPath;
        double FilterOpticalDepth = FilterOpticalPath / 2.0;

        unsigned int VectorPosition = (unsigned int) (
                (FilterOpticalPath / 2.0 - In_Ptr->OpticalDepthShift)
                / (In_Ptr->CoherenceLengthSource / NUM_SUBSTEPS_RESOLUTION));

        int ii;
        for (ii = VectorPosition - NUM_SUBSTEPS_RESOLUTION / 2;
             ii < VectorPosition + NUM_SUBSTEPS_RESOLUTION / 2;
             ii++) {
            if (ii >= 0 && ii < In_Ptr->NumOpticalDepthLengthSteps) {
                short FilterClassI_Flag =
                        Photon_Ptr->MaxDepth > (FilterOpticalDepth
                                                - In_Ptr->CoherenceLengthSource / 2.0);

                double tmpPhotonContribution = Photon_Ptr->w * (1.0 - Refl) * Photon_Ptr->LikelihoodRatio;
                if (FilterClassI_Flag) {
                    if (ii == VectorPosition)
                        In_Ptr->NumFilteredPhotonsClassI++;
                    Out_Ptr->ReflectanceClassI_Sum[ii] += tmpPhotonContribution;
                    if (Out_Ptr->ReflectanceClassI_Max[ii] < tmpPhotonContribution)
                        Out_Ptr->ReflectanceClassI_Max[ii] = tmpPhotonContribution;
                    Out_Ptr->ReflectanceClassI_SumSq[ii] += sq(tmpPhotonContribution);
                    Out_Ptr->NumClassI_PhotonsFilteredInRange[ii]++;

                } else {
                    if (ii == VectorPosition)
                        In_Ptr->NumFilteredPhotonsClassII++;
                    if (Out_Ptr->ReflectanceClassII_Max[ii] < tmpPhotonContribution)
                        Out_Ptr->ReflectanceClassII_Max[ii] = tmpPhotonContribution;
                    Out_Ptr->ReflectanceClassII_Sum[ii] += tmpPhotonContribution;
                    Out_Ptr->ReflectanceClassII_SumSq[ii] += sq(tmpPhotonContribution);
                    Out_Ptr->NumClassII_PhotonsFilteredInRange[ii]++;

                }
            }
        }
    }

    Photon_Ptr->w *= Refl;

}

/***********************************************************
 *	Decide whether the photon will be transmitted  or be
 *	reflected on the bottom boundary (uz>0) of the current
 *	layer.
 *
 *	If the photon is transmitted, move the photon to
 *	"layer+1". If "layer" is the last layer, record the
 *	transmitted weight as transmittance. See comments for
 *	CrossUpOrNot.
 *
 *	Update the photon parmameters.
 ****/
void CrossOrNot(SimulationStruct *In_Ptr, TetrahedronPhotonStruct *Photon_Ptr, OutStruct *Out_Ptr) {

    double uz_reflected = 0, ux_reflected = 0, uy_reflected = 0;    /* cosines of transmission alpha. */
    double uz_refracted = 0, ux_refracted = 0, uy_refracted = 0;

    double r = 0.0;                        /* reflectance */

    r = RFresnel(In_Ptr, Photon_Ptr, &ux_reflected, &uy_reflected, &uz_reflected, &ux_refracted, &uy_refracted,
                 &uz_refracted);

#if PARTIALREFLECTION
    if (Photon_Ptr->tetrahedron->adjTetrahedrons[Photon_Ptr->NextTetrahedron] == NULL && r < 1.0) { /* We use actual photon splitting if the photon is exiting the media
																							 ** We store the part the is exiting and the rest will be traced */
        Photon_Ptr->ux = ux_refracted;
        Photon_Ptr->uy = uy_refracted;
        Photon_Ptr->uz = uz_refracted;

        if (Photon_Ptr->uz < 0)
            RecordValidationR(r, In_Ptr, Photon_Ptr, Out_Ptr);
        else {
            Photon_Ptr->NumBackwardsSpecularReflections++;
            Photon_Ptr->w *= r;
        }
        Photon_Ptr->ux = ux_reflected;
        Photon_Ptr->uy = uy_reflected;
        Photon_Ptr->uz = uz_reflected;

    }
        /* If photon is not exiting the medium we use the statistical splitting */
    else if (RandomNum() > r) {/* transmitted */

        Photon_Ptr->tetrahedron = Photon_Ptr->tetrahedron->adjTetrahedrons[Photon_Ptr->NextTetrahedron];

        Photon_Ptr->ux = ux_refracted;
        Photon_Ptr->uy = uy_refracted;
        Photon_Ptr->uz = uz_refracted;

        Photon_Ptr->NextTetrahedron = -1;

    } else                        /* reflected. */
    {
        Photon_Ptr->NextTetrahedron = -1;

        if (Photon_Ptr->uz > 0)
            Photon_Ptr->NumBackwardsSpecularReflections++;

        Photon_Ptr->ux = ux_reflected;
        Photon_Ptr->uy = uy_reflected;
        Photon_Ptr->uz = uz_reflected;

    }
#else	/* use just statistical splitting */
    if(RandomNum() > r) {		/* transmitted. */

    if(Photon_Ptr->tetrahedron->adjTetrahedrons[Photon_Ptr->NextTetrahedron]==NULL) {

      Photon_Ptr->ux = ux_refracted;
      Photon_Ptr->uy = uy_refracted;
      Photon_Ptr->uz = uz_refracted;

      if (Photon_Ptr->uz<0)
            RecordValidationR(0.0, In_Ptr, Photon_Ptr, Out_Ptr);

      Photon_Ptr->dead = 1;
    }
    else {

      /* Siavash Refraction Direction Cosines */
      Photon_Ptr->ux = ux_refracted;
      Photon_Ptr->uy = uy_refracted;
      Photon_Ptr->uz = uz_refracted;
      Photon_Ptr->tetrahedron=Photon_Ptr->tetrahedron->adjTetrahedrons[Photon_Ptr->NextTetrahedron];
      Photon_Ptr->NextTetrahedron=-1;
    }
  }
  else {
    /* reflected. */

    Photon_Ptr->ux = ux_reflected;
    Photon_Ptr->uy = uy_reflected;
    Photon_Ptr->uz = uz_reflected;
    Photon_Ptr->NextTetrahedron=-1;
  }
#endif

}

/***********************************************************
 ****/
/***********************************************************
 *	Move the photon packet in glass layer.
 *	Horizontal photons are killed because they will
 *	never interact with tissue again.
 ****/
/***********************************************************
 *	Set a step size, move the photon, drop some weight,
 *	choose a new photon direction for propagation.
 *
 *	When a step size is long enough for the photon to
 *	hit an interface, this step is divided into two steps.
 *	First, move the photon to the boundary free of
 *	absorption or scattering, then decide whether the
 *	photon is reflected or transmitted.
 *	Then move the photon in the current or transmission
 *	medium with the unfinished stepsize to interaction
 *	site.  If the unfinished stepsize is still too long,
 *	repeat the above process.
 ****/
void HopDropSpinInTissue(SimulationStruct *In_Ptr,
                         TetrahedronPhotonStruct *Photon_Ptr,
                         TetrahedronPhotonStruct *PhotonCont_Ptr,
                         OutStruct *Out_Ptr) {
    StepSizeInTissue(Photon_Ptr);
    if (HitBoundary(Photon_Ptr)) {
        Hop(Photon_Ptr);    /* move to boundary plane. */
        CrossOrNot(In_Ptr, Photon_Ptr, Out_Ptr);
    } else {
        Hop(Photon_Ptr);
        Drop(Photon_Ptr);

        switch (In_Ptr->TypeBias) {
            case 0:                        /* No importance sampling */
                Spin(regionspecs[Photon_Ptr->tetrahedron->region].g, Photon_Ptr);
                break;
            case 37:                    /* BOE2 Biasing by Lima et al. */
                SpinBias37(regionspecs[Photon_Ptr->tetrahedron->region].g, Photon_Ptr, PhotonCont_Ptr, In_Ptr);
        }
    }
}

void HopDropSpin(SimulationStruct *In_Ptr, TetrahedronPhotonStruct *Photon_Ptr, TetrahedronPhotonStruct *PhotonCont_Ptr,
                 OutStruct *Out_Ptr) {
    HopDropSpinInTissue(In_Ptr, Photon_Ptr, PhotonCont_Ptr, Out_Ptr);

    if (Photon_Ptr->w < WEIGHT && !Photon_Ptr->dead)
        Roulette(Photon_Ptr);

}

void CopyPhotonStruct(TetrahedronPhotonStruct *OrigPhoton_Ptr, TetrahedronPhotonStruct *DestPhoton_Ptr) {

    DestPhoton_Ptr->x = OrigPhoton_Ptr->x;
    DestPhoton_Ptr->y = OrigPhoton_Ptr->y;
    DestPhoton_Ptr->z = OrigPhoton_Ptr->z;    /* Cartesian coordinates.[cm] */
    DestPhoton_Ptr->ux = OrigPhoton_Ptr->ux;
    DestPhoton_Ptr->uy = OrigPhoton_Ptr->uy;
    DestPhoton_Ptr->uz = OrigPhoton_Ptr->uz;/* directional cosines of a photon. */
    DestPhoton_Ptr->w = OrigPhoton_Ptr->w;            /* weight. */
    DestPhoton_Ptr->dead = OrigPhoton_Ptr->dead;        /* 1 if photon is terminated. */

    DestPhoton_Ptr->MinCos = OrigPhoton_Ptr->MinCos;
    DestPhoton_Ptr->tetrahedron = OrigPhoton_Ptr->tetrahedron;
    DestPhoton_Ptr->NextTetrahedron = OrigPhoton_Ptr->NextTetrahedron;

    DestPhoton_Ptr->s = OrigPhoton_Ptr->s;            /* current step size. [cm]. */
    DestPhoton_Ptr->sleft = OrigPhoton_Ptr->sleft;        /* step size left. dimensionless [-]. */

    DestPhoton_Ptr->OpticalPath = OrigPhoton_Ptr->OpticalPath;
    DestPhoton_Ptr->MaxDepth = OrigPhoton_Ptr->MaxDepth;
    DestPhoton_Ptr->LikelihoodRatio = OrigPhoton_Ptr->LikelihoodRatio;
    DestPhoton_Ptr->FstBackReflectionFlag = OrigPhoton_Ptr->FstBackReflectionFlag;
    DestPhoton_Ptr->LocationFstBias = OrigPhoton_Ptr->LocationFstBias;
    DestPhoton_Ptr->NumBackwardsSpecularReflections = OrigPhoton_Ptr->NumBackwardsSpecularReflections;

}


#pragma once

#include <utility>
#include <array>
#include <vector>
#include <unordered_map>
#include <DirectXMathExtend.h>
#include <gsl.h>
#include <VertexTraits.h>
#include <minmax>
#include "bvh.h"
#include <hlslm\hlsl.hpp>
#include <iostream>

namespace Geometrics
{
	template <typename IndexType>
	struct Triangle
	{
		static constexpr size_t VertexCount = 3;
		typedef std::array<IndexType, VertexCount> container_type;
		container_type v;

		typedef IndexType value_type;
		typedef size_t size_type;
		typedef ptrdiff_t difference_type;
		typedef IndexType *pointer;
		typedef const IndexType *const_pointer;
		typedef IndexType& reference;
		typedef const IndexType& const_reference;

		typedef typename container_type::iterator iterator;
		typedef typename container_type::const_iterator const_iterator;

		typedef typename container_type::reverse_iterator reverse_iterator;
		typedef typename container_type::const_reverse_iterator const_reverse_iterator;

		Triangle() {}
		Triangle(const IndexType &v0, const IndexType &v1, const IndexType &v2)
		{
			v[0] = v0; v[1] = v1; v[2] = v2;
		}
		Triangle(const IndexType *v_)
		{
			v[0] = v_[0]; v[1] = v_[1]; v[2] = v_[2];
		}
		template <class S> explicit Triangle(const S &x)
		{
			v[0] = x[0];  v[1] = x[1];  v[2] = x[2];
		}
		IndexType &operator[] (int i) { return v[i]; }
		const IndexType &operator[] (int i) const { return v[i]; }
		operator const IndexType * () const { return &(v[0]); }
		operator IndexType * () { return &(v[0]); }

		void rewind() {
			std::swap(v[0], v[2]);
		}

		int indexof(IndexType v_) const
		{
			return (v[0] == v_) ? 0 :
				(v[1] == v_) ? 1 :
				(v[2] == v_) ? 2 : -1;
		}

		// contrainer access
		auto begin() { return v.begin(); }
		auto end() { return v.end(); }
		auto begin() const { return v.begin(); }
		auto end() const { return v.end(); }

		auto rbegin() { return v.rbegin(); }
		auto rend() { return v.rend(); }
		auto rbegin() const { return v.rbegin(); }
		auto rend() const { return v.rend(); }

		size_t size() const { return VertexCount; }
	};

	struct submesh_data
	{
		uint32_t vertex_offset; // start vertices index of this geometry
		uint32_t vertex_count;
		uint32_t index_offset; // start index position of this geometry
		uint32_t index_count;
	};

	// generate per vertex normal for given triangle mesh
	template <typename VertexType, typename IndexType>
	bool generate_normal(gsl::span<VertexType> vertices, gsl::span<const Triangle<IndexType>> facets)
	{
		using namespace DirectX::VertexTraits;
		using namespace DirectX;

		if (!has_normal<VertexType>::value)
			return false;

		//static_assert(has_normal<VertexType>::value, "The vertex type dose not contains normal field");

		std::vector<XMVECTOR, XMAllocator> normals(vertices.size());

		// set zero
		std::memset(normals.data(), 0, normals.size() * sizeof(XMVECTOR));

		XMVECTOR v0, v1, v2, n;
		for (const auto& face : facets)
		{
			v0 = get_position(vertices[face[0]]);
			v1 = get_position(vertices[face[1]]);
			v2 = get_position(vertices[face[2]]);
			v1 -= v0;
			v2 -= v0;

			v1 = XMVector3Normalize(v1);
			v2 = XMVector3Normalize(v2);
			n = XMVector3Cross(v1, v2); // weighted normal

			normals[face[0]] += n;
			normals[face[1]] += n;
			normals[face[2]] += n;
		}

		for (size_t i = 0; i < vertices.size(); i++)
		{
			n = XMVector3Normalize(normals[i]);
			set_normal(vertices[i], n);
		}

		return true;
	}

	// generate per vertex tangent for given triangle mesh
	template <typename VertexType, typename IndexType>
	bool generate_tangent(gsl::span<VertexType> vertices, gsl::span<const Triangle<IndexType>> facets)
	{
		using namespace DirectX::VertexTraits;
		using namespace DirectX;

		if (!has_tangent<VertexType>::value)
			return false;
		//static_assert(has_tangent<VertexType>::value, "The vertex type dose not contains tangent field");

		std::vector<XMVECTOR, XMAllocator> tan1(vertices.size() * 2);
		XMVECTOR* tan2 = &tan1[vertices.size()];
		std::memset(tan1.data(), 0, tan1.size() * sizeof(XMVECTOR));

		for (const auto& face : facets)
		{
			{
				XMVECTOR v0, v1, v2, w0, w1, w2;
				v0 = get_position(vertices[face[0]]);
				w0 = get_uv(vertices[face[0]]);

				v1 = get_position(vertices[face[1]]);
				w1 = get_uv(vertices[face[1]]);

				v2 = get_position(vertices[face[2]]);
				w2 = get_uv(vertices[face[2]]);
			}
			XMFLOAT4A v_1, v_2, w_1, w_2;

			XMStoreA(v_1, v1 - v0);
			XMStoreA(v_2, v2 - v0);
			XMStoreA(w_1, w1 - w0);
			XMStoreA(w_2, w2 - w0);

			float x1 = v_1.x; float x2 = v_2.x;
			float y1 = v_1.y; float y2 = v_2.y;
			float z1 = v_1.z; float z2 = v_2.z;

			float s1 = w_1.x; float s2 = w_2.x;
			float t1 = w_1.y; float t2 = w_2.y;

			float r = 1.0F / (s1 * t2 - s2 * t1);
			XMVECTOR sdir = XMVectorSet(
				(t2 * x1 - t1 * x2) * r,
				(t2 * y1 - t1 * y2) * r,
				(t2 * z1 - t1 * z2) * r, .0f);
			XMVECTOR tdir = XMVectorSet(
				(s1 * x2 - s2 * x1) * r,
				(s1 * y2 - s2 * y1) * r,
				(s1 * z2 - s2 * z1) * r, .0f);

			tan1[face[0]] += sdir;
			tan1[face[1]] += sdir;
			tan1[face[2]] += sdir;

			tan2[face[0]] += tdir;
			tan2[face[1]] += tdir;
			tan2[face[2]] += tdir;
		}

		for (size_t i = 0; i < vertices.size(); i++)
		{
			XMVECTOR n = get_normal(vertices[i], n);
			XMVECTOR t = tan1[i];

			// Gram-Schmidt orthogonalize
			XMVECTOR nt = XMVector3Normalize(t - n * XMVector3Dot(n, t));

			XMVECTOR w = XMVectorLess(XMVector3Dot(XMVector3Cross(n, t), tan2[i]), XMVectorZero());
			w = XMVectorSelect(g_XMNegativeOne.v, g_XMOne.v, w);

			nt = _DXMEXT XMVectorSelect<0, 0, 0, 1>(nt, w);
			set_tangent(vertices[i], nt);
		}

		return true;
	}

	namespace Detail
	{
		XM_HAS_MEMBER(vertices, has_vertices);
		XM_HAS_MEMBER(indices, has_indices);
	}

	template <typename MeshType>
	struct concept_mesh_type : public std::integral_constant<bool, Detail::has_vertices<MeshType>::value && Detail::has_indices<MeshType>::value> {};

	template <typename _MeshType>
	class SubMesh : public submesh_data
	{
	public:
		typedef _MeshType MeshType; // Parent MeshTypw
		typedef typename MeshType::VertexType	VertexType;
		typedef typename MeshType::IndexType	IndexType;
		typedef typename MeshType::FaceType		FaceType;

		static constexpr size_t PolygonVertex = FaceType::VertexCount;

		SubMesh(MeshType& mesh, const submesh_data& metric)
			: parent(mesh), submesh_data(metric)
		{
			vertices = gsl::span<VertexType>(&mesh.vertices[metric.vertex_offset], metric.vertex_count);
			indices = gsl::span<IndexType>(&mesh.indices[metric.index_offset], metric.index_count);
		}

		MeshType&						parent;
		gsl::span<VertexType>			vertices;
		gsl::span<IndexType>			indices;

		gsl::span<Triangle<IndexType>>	facets()
		{
			return gsl::span<const FaceType>(
				reinterpret_cast<const FaceType*>(indices.data()),
				indices.size() / FaceType::VertexCount);
		}

		bool empty() const {
			return vertices.empty() || indices.empty();
		}

	};

	template <typename _VertexType, typename _IndexType = uint16_t, typename _FaceType = Triangle<_IndexType>>
	class PolygonSoup
	{
	public:
		typedef _VertexType VertexType;
		typedef _IndexType	IndexType;
		typedef _FaceType	FaceType;

		static constexpr size_t PolygonVertex = FaceType::VertexCount;

		PolygonSoup() {}

		bool empty() const {
			return vertices.empty() || indices.empty();
		}

		void clear()
		{
			vertices.clear();
			indices.clear();
		}

		inline const FaceType& facet(int idx) const
		{
			return reinterpret_cast<const FaceType&>(indices[idx * FaceType::VertexCount]);
		}

		inline FaceType& facet(int idx)
		{
			return reinterpret_cast<FaceType&>(indices[idx * FaceType::VertexCount]);
		}

		void add_facet(const FaceType& new_facet)
		{
			indices.insert(indices.end(), new_facet.begin(), new_facet.end());
		}

		template <class... _TIndecies>
		std::enable_if_t<sizeof...(_TIndecies) == FaceType::VertexCount> add_facet(_TIndecies... _indices)
		{
			indices.insert(indices.end(), { static_cast<IndexType>(_indices)... });
		}

		gsl::span<FaceType> facets() {
			return gsl::span<FaceType>(
				reinterpret_cast<FaceType*>(indices.data()),
				indices.size() / FaceType::VertexCount);
		}

		gsl::span<const FaceType> facets() const {
			return gsl::span<const FaceType>(
				reinterpret_cast<const FaceType*>(indices.data()),
				indices.size() / FaceType::VertexCount);
		}

		const VertexType& vertex(int facet, int vidx) const
		{
			return this->vertices[this->indices[facet * FaceType::VertexCount + vidx]];
		}

		VertexType& vertex(int facet, int vidx)
		{
			return this->vertices[this->indices[facet * FaceType::VertexCount + vidx]];
		}

		// flip all the polygons' winding and vertices' normal
		void flip()
		{
			using namespace DirectX::VertexTraits;

			if (has_normal<VertexType>::value)
			{
				for (auto& v : vertices)
					set_normal(v, -get_normal(v));
			}

			static constexpr int vc = FaceType::VertexCount;
			for (int i = 0; i < indices.size() / vc; i++)
				std::reverse(indices.begin() + i * vc, indices.begin() + (i + 1)* vc);
		}

		void generate_normal()
		{
			static_assert(PolygonVertex == 3, "This mesh is not trigle mesh, please trianglize it first");
			generate_normal<VertexType, IndexType>(this->vertices, this->facets());
		}

		void generate_tangent()
		{
			static_assert(PolygonVertex == 3, "This mesh is not trigle mesh, please trianglize it first");
			generate_tangent<VertexType, IndexType>(this->vertices, this->facets());
		}

		// applies an uniform transform to all vertices in the mesh
		void XM_CALLCONV transform(DirectX::FXMMATRIX M)
		{
			using namespace DirectX::VertexTraits;
			for (auto& v : vertices)
			{
				XMVECTOR p = get_position(v);
				p = _DXMEXT XMVector3TransformCoord(p, M);
				set_position(v, p);

				if (has_normal<VertexType>::value)
				{
					p = get_normal(v);
					p = _DXMEXT XMVector3TransformNormal(p, M);
					set_normal(v, p);

					if (has_tangent<VertexType>::value)
					{
						p = get_tangent(v);
						p = _DXMEXT XMVector3TransformNormal(p, M);
						set_tangent(v, p);
					}
				}
			}
		}

		std::vector<VertexType, DirectX::AlignedAllocator<VertexType>> vertices;
		std::vector<IndexType>	indices;
	};

	template <class _VertexType, class _IndexType>
	int XM_CALLCONV intersect(const PolygonSoup<_VertexType, _IndexType, Triangle<_IndexType>> &Mesh, DirectX::FXMVECTOR Origin, DirectX::FXMVECTOR Direction, std::vector<float>* distances = nullptr)
	{
		using namespace DirectX;
		using namespace DirectX::VertexTraits;
		int count = 0;
		XMVECTOR vDir = XMVector3Normalize(Direction);
		for (const auto& tri : Mesh.facets())
		{
			float distance;

			XMVECTOR v0 = get_position(&Mesh.vertices[tri[0]]);
			XMVECTOR v1 = get_position(&Mesh.vertices[tri[1]]);
			XMVECTOR v2 = get_position(&Mesh.vertices[tri[2]]);

			bool hr = DirectX::TriangleTests::Intersects(Origin, vDir, v0, v1, v2, distance);
			if (hr) {
				++count;
				if (distances) {
					distances->push_back(distances);
				}
			}
		}
		return count;
	}

	template <typename _TVertex>
	struct MeshRayIntersectedVertex : public _TVertex
	{
		using Vector3 = DirectX::Vector3;

		float	distance;
		Vector3 barycentric;
		int		facet; // facet id

		inline bool operator < (const MeshRayIntersectedVertex& rhs)
		{
			return this->distance < rhs.distance;
		}

		inline bool is_valiad() const {return  facet >= 0; }
		inline operator bool() const { return is_valiad(); }
	};

	template <size_t _Dim>
	struct PolygonContainmentInfo
	{
		static constexpr size_t Dim = _Dim;
		DirectX::ContainmentType containment; // overall polygon containment type
		DirectX::ContainmentType vertex_containments[Dim];
		operator DirectX::ContainmentType() const { return containment; }
	};

	struct XM_ALIGNATTR aabb_t {
		::hlsl::xmvector3f min;
		::hlsl::xmvector3f max;
		mutable int	 intersected;

		template <size_t dim>
		static bool less(const aabb_t& lhs, const aabb_t & rhs)
		{
			using namespace hlsl;
			return hlsl::less(lhs.min, rhs.min).get<dim>();
		}

		aabb_t operator+(const aabb_t& rhs) const
		{
			const aabb_t& lhs = *this;
			using namespace ::hlsl;
			aabb_t ret{ ::hlsl::min(lhs.min,rhs.min),::hlsl::max(lhs.max,rhs.max) };
			return ret;
		}

		// Convert min-max representation of this box to center-extent representation
		DirectX::BoundingBox get_dxbox() const
		{
			const float epsilon = 1e-3;
			using namespace ::hlsl;
			auto& aabb = *this;
			DirectX::BoundingBox box;
			xmvector3f extent = (aabb.max - aabb.min) * 0.5f;
			extent = ::hlsl::max(extent, xmfloat(epsilon));
			box.Center << (aabb.max + aabb.min) * 0.5f;
			box.Extents << extent;
			return box;
		}

		operator DirectX::BoundingBox() const { return get_dxbox(); }
	};

	// A struct stores fixed size polygon (position only)
	template <size_t _Size>
	struct PlainPolygon {
		hlsl::xmvector3f v[_Size];
	};

	/// <summary>
	/// Basic triangle mesh, each index represent an edge, which is the edge oppsite to the vertex in it's owner triangle
	/// </summary>
	template <typename _VertexType, typename _IndexType = uint16_t>
	class TriangleMesh : public PolygonSoup<_VertexType, _IndexType, Triangle<_IndexType>>
	{
	public:
		static const size_t VertexCount = FaceType::VertexCount;
		using triangle_handle = IndexType;
		using PlainTriangle = PlainPolygon<3>;
		using ContainmentType = DirectX::ContainmentType;
		using BaseType = PolygonSoup<_VertexType, _IndexType, Triangle<_IndexType>>;
		using BaseType::PolygonVertex;
		using typename BaseType::VertexType;
		using typename BaseType::IndexType;
		using IntersectionVertex = MeshRayIntersectedVertex<_VertexType>;

	public:
		// edge's reverse edge
		// stores the adjacent edges of a edge in a triangle
		std::vector<IndexType>	revedges;
		DirectX::BoundingBox	aabb;
		DirectX::BoundingOrientedBox obb;

		PlainTriangle XM_CALLCONV get_triangle_position(IndexType fid) const
		{
			using namespace DirectX::VertexTraits;
			using namespace hlsl;
			auto f = this->indices.begin() + fid * 3;
			PlainTriangle tri = {
			xmvector3f(get_position(this->vertices[f[0]]))
			,xmvector3f(get_position(this->vertices[f[1]]))
			,xmvector3f(get_position(this->vertices[f[2]])) };
			return tri;
		}

#define _EXPANDTRI(tri) tri.v[0].v, tri.v[1].v, tri.v[2].v

		aabb_t XM_CALLCONV get_triangle_aabb(triangle_handle t) const
		{
			using namespace hlsl;
			PlainTriangle tri = get_triangle_position(t);
			xmvector3f vmax = hlsl::max(tri.v[0], tri.v[1]);
			vmax = hlsl::max(vmax, tri.v[2]);
			xmvector3f vmin = hlsl::min(tri.v[0], tri.v[1]);
			vmin = hlsl::min(vmin, tri.v[2]);
			aabb_t ret;
			ret.min = vmin;
			ret.max = vmax;
			ret.intersected = true;
			return ret;
		}

		using triangle_bvh_t = kd_bvh<triangle_handle, float, 3, aabb_t>;
		triangle_bvh_t				triangle_bvh;

		using vertex_bvh_t = kd_bvh<::hlsl::xmvector3f, float, 3, aabb_t>;
		vertex_bvh_t				vertex_bvh;
		// An Lookup table to find the 'Ienditification' of a vertex
		// Make sense when multiple vertex shares the same spatial location but have different index
		std::vector<IndexType>		vertex_id;

		TriangleMesh() : triangle_bvh([this](const triangle_handle& t) {
			return this->get_triangle_aabb(t);
		}) {}

		// vertex's first adjacant edge
		// std::vector<IndexType> vedges;

		union EdgeType
		{
			_IndexType v[2];
			struct {
				_IndexType v0, v1;
			};
		};

		inline EdgeType edge(int facet, int edge) const
		{
			EdgeType e;
			auto& tri = this->facet(facet);
			switch (edge)
			{
			case 0:
				e.v0 = tri[1];
				e.v1 = tri[2];
				break;
			case 1:
				e.v0 = tri[2];
				e.v1 = tri[0];
				break;
			case 2:
				e.v0 = tri[0];
				e.v1 = tri[1];
				break;
			default:
				e.v0 = -1;
				e.v1 = -1;
				break;
			}
			return e;
		}

		inline EdgeType edge(int eid) const
		{
			return edge(eid / VertexCount, eid % VertexCount);
		}

		inline int adjacentFacet(int facet, int edge) const
		{
			return revedges[facet * VertexCount + edge] / VertexCount;
		}

		inline int adjacentFacet(int eid) const
		{
			return revedges[eid] / VertexCount;
		}

		void build_reverse_edges()
		{
			// intialize all adjacant edges to -1
			revedges.resize(this->indices.size(), -1);
			_IndexType vsize = this->vertices.size();
			int esize = this->indices.size();
			int fsize = this->indices.size() / VertexCount;

			using hash_type = uint64_t;
			// make sure int for hash is enough
			assert(vsize*vsize < std::numeric_limits<hash_type>::max());
			std::unordered_map<hash_type, _IndexType> edges(esize * 2);
			// max items inside this table should be less than (esize / 2)

			for (_IndexType fid = 0; fid < fsize; fid++)
			{
				for (_IndexType i = 0; i < VertexCount; i++)
				{
					_IndexType eid = i + fid * VertexCount;
					auto e = this->edge(fid, i);
					e.v0 = vertex_id[e.v0];
					e.v1 = vertex_id[e.v1];

					hash_type ehash = e.v0 * vsize + e.v1;
					hash_type revehash = e.v0 + e.v1 * vsize;

					auto revItr = edges.find(revehash);
					if (revItr == edges.end())
						edges[ehash] = eid;
					else // find reversed edge, remove from edges map
					{
						auto revEid = revItr->second;
						revedges[eid] = revEid;
						revedges[revEid] = eid;
						auto reve = this->edge(revEid);
						reve.v0 = vertex_id[reve.v0];
						reve.v1 = vertex_id[reve.v1];
						assert(reve.v0 == e.v1 && reve.v1 == e.v0);
						edges.erase(revItr);
					}
				}
			}
		}

		// build the adjacent map so we can access all the 1 rings
		void build()
		{
			int vsize = this->vertices.size();
			int esize = this->indices.size();
			int fsize = this->indices.size() / VertexCount;

			// rebuild the triangle-bvh
			triangle_bvh.resize(fsize);
			std::iota(triangle_bvh.begin(), triangle_bvh.end(), 0);
			triangle_bvh.rebuild();

			// build the vertex-bvh
			const aabb_t& bb = this->triangle_bvh.get_volumn(this->triangle_bvh.root());
			::hlsl::xmvector3f voxel_ext(1e-4f);

			vertex_bvh.set_volumn_getter([voxel_ext](const ::hlsl::xmvector3f v) {
				return aabb_t{ v - voxel_ext , v + voxel_ext, 0 };
			});
			vertex_bvh.resize(vsize);
			for (int i = 0; i < vsize; i++)
				vertex_bvh[i] = ::hlsl::xmvector3f(::DirectX::VertexTraits::get_position(this->vertices[i]));
			vertex_bvh.rebuild();

			vertex_id.resize(vsize);
			std::iota(vertex_id.begin(), vertex_id.end(), 0);
			std::unordered_map<uint64_t, IndexType> vids(vsize);

			// Use 20-bits to hash a float component, plus 1 bit signbit
			// Generate a total 63-bits
			auto hash_position = [](const ::hlsl::xmvector3f vp) -> uint64_t{
				auto vf = vp * 3e4f; // 30 * 3e4 = 9e5 < 1e6 = 2^20
				auto vi = vf.cast<uint32_t>();
				uint32_t signbits32 = ::hlsl::detail::move_mask(vi);
				uint64_t signbits = signbits32;
				vi &= ::hlsl::xmuint((1ULL << 20ULL) - 1ULL);
				uint32_t vis32[3];
				vis32 << vi;
				uint64_t vis[3] = { vis32[0], vis32[1], vis32[2]};
				uint64_t vhash = ((signbits&7) << 60ULL) | (vis[0] << 40ULL) | (vis[1] << 20ULL) | (vis[2]);
				return vhash;
			};

			for (int i = 0; i < vsize; i++)
			{
				auto vp = *vertex_bvh.get_object(i);
				std::pair<uint64_t, IndexType> p = { hash_position(vp), i };
				auto rib = vids.insert(p);
				IndexType vid = rib.first->second;
				if (!rib.second && vid != i)
				{
					// query-merge-set merge algorithm
					int root = vid;
					// find the ultimate parent (root)
					while (vertex_id[root] != root) root = vertex_id[root];

					while (vid != root)
					{
						int cvid = vid;
						vid = vertex_id[vid];
						vertex_id[cvid] = root;
					}
					vertex_id[i] = root;
					//if (vid != i)
					//	std::cout << "Glued vertex pair (" << i << ',' << vid << ')' << std::endl;
				}
			}

			build_reverse_edges();

			//for (int fid = 0; fid < this->indices.size() / 3; ++fid)
			//{
			//	auto tri = this->facet(fid);
			//	using namespace DirectX::VertexTraits;
			//	DirectX::XMVECTOR v0 = get_position(this->vertices[tri[0]]);
			//	DirectX::XMVECTOR v1 = get_position(this->vertices[tri[1]]);
			//	DirectX::XMVECTOR v2 = get_position(this->vertices[tri[2]]);
			//	auto&& vol = triangle_bvh.get_volumn(fid);
			//	bool cond = true;
			//	assert(DirectX::XMVector3LessOrEqual(v0, vol.max.v));
			//	assert(DirectX::XMVector3LessOrEqual(v1, vol.max.v));
			//	assert(DirectX::XMVector3LessOrEqual(v2, vol.max.v));
			//	assert(DirectX::XMVector3LessOrEqual(vol.min.v, v0));
			//	assert(DirectX::XMVector3LessOrEqual(vol.min.v, v1));
			//	assert(DirectX::XMVector3LessOrEqual(vol.min.v, v2));
			//}
		}

		// generate a persoude vertex from the interpolation of the trianlge
		VertexType persudo_vertex(int fid, DirectX::FXMVECTOR baycentric) const
		{
			const auto& tri = this->facet(fid);

			const VertexType& v0 = this->vertices[tri[0]];
			const VertexType& v1 = this->vertices[tri[1]];
			const VertexType& v2 = this->vertices[tri[2]];

			using namespace DirectX::VertexTraits;
			return interpolate_vertex(baycentric, v0, v1, v2);
		}

		protected:
			IntersectionVertex XM_CALLCONV get_ray_intersected_vertex(DirectX::FXMVECTOR vOri, DirectX::FXMVECTOR vDir, IndexType fid) const
			{
				using DirectX::XMVECTOR;
				IntersectionVertex info;
				float distance = .0f;
				info.facet = -1;
				PlainTriangle tri = this->get_triangle_position(fid);
				bool hr = DirectX::TriangleTests::Intersects(vOri, vDir, _EXPANDTRI(tri), distance);
				if (!hr) return info; // No intersection, return with info.facet == -1
				// Fill the persudo-vertex
				XMVECTOR pos = distance * vDir + vOri;
				XMVECTOR baryc = DirectX::TriangleTests::BarycentricCoordinate(pos, _EXPANDTRI(tri));
				info.barycentric = baryc;
				info.facet = fid;
				info.distance = distance;
				(VertexType&)info = this->persudo_vertex(fid, baryc);
				return info;
			}

			template <typename _Ty>
			inline static void push_back_output_itr(nullptr_t itr, _Ty&& element)
			{
			}

			template <typename _OutItr, typename _Ty>
			inline static std::enable_if_t<!std::is_same<_OutItr,nullptr_t>::value>
				push_back_output_itr(_OutItr& itr, _Ty&& element)
			{
				static_assert(std::is_same<std::decay_t<decltype(*itr)>,std::decay_t<_Ty>>::value,"Output Iterator's Type is not expected");
				*(itr++) = element;
			}

		public:

		IntersectionVertex XM_CALLCONV first_intersect(DirectX::FXMVECTOR Origin, DirectX::FXMVECTOR Direction) const
		{
			using namespace DirectX;
			assert(this->triangle_bvh.valiad() && "This triangle mesh is modified but not rebuild");

			XMVECTOR vOri = Origin;
			XMVECTOR vDir = XMVector3Normalize(Direction);
			auto ray_aabb_inter = [vOri, vDir](const aabb_t& aabb) -> float
			{
				float f;
				auto box = aabb.get_dxbox();
				bool succ = box.Intersects(vOri, vDir, f);
				aabb.intersected = succ;
				return succ ? f : -1.0f;
			};
			auto ray_triangle_inter = [this, vOri, vDir](const int fid) -> float
			{
				PlainTriangle tri = this->get_triangle_position(fid);
				float f;
				bool succ = DirectX::TriangleTests::Intersects(vOri, vDir, _EXPANDTRI(tri), f);
				return succ ? f : -1.0f;
			};

			//std::cout << counter << std::endl;
			auto pfid = find_first_of(this->triangle_bvh, ray_triangle_inter, ray_aabb_inter);
			IntersectionVertex info;
			info.facet = -1;
			if (pfid)
				info = get_ray_intersected_vertex(vOri, vDir, *pfid);
			return info;
		}

		// Ray intersection test with advanced infomation
		template <typename _TOutItr>
		int XM_CALLCONV all_intersections(DirectX::FXMVECTOR Origin, DirectX::FXMVECTOR Direction, _TOutItr outItr) const
		{
			using namespace DirectX;
			assert(this->triangle_bvh.valiad() && "This triangle mesh is modified but not rebuild");

			size_t count = 0;
			XMVECTOR vOri = Origin;
			XMVECTOR vDir = XMVector3Normalize(Direction);

			struct XM_ALIGNATTR ray_aabb_intersector {
				DirectX::XMVECTOR origin;
				DirectX::XMVECTOR direction;
				bool XM_CALLCONV operator ()(const aabb_t& aabb) const
				{
					float f;
					auto box = aabb.get_dxbox();
					bool succ = box.Intersects(origin, direction, f);
					aabb.intersected = succ;
					return succ;
				}
			} intersector{ vOri , vDir };

			for (auto triangles = find_all_of(triangle_bvh, intersector);
				triangles;
				++triangles)
			{
				int fid = *triangles;
				IntersectionVertex info = get_ray_intersected_vertex(vOri, vDir, fid);
				if (info.is_valiad()) {
					++count;
					push_back_output_itr(outItr, std::move(info));
				}
			}
			return count;
		}

		template <typename _TPred, typename _TContainer>
		void find_adjacant_facets_of(IndexType facet_id, _TPred&& pred, _TContainer& result_map) const
		{
			auto containment = pred(facet_id);
			result_map.insert(std::make_pair(facet_id, containment));
			if (!containment) return;
			for (int e = 0; e < PolygonVertex; e++)
			{
				int eid = facet_id * PolygonVertex + e;
				auto rev_edge = this->revedges[eid];
				auto adj_face = rev_edge / PolygonVertex;
				// if not visited yet
				if (rev_edge != -1 && result_map.find(adj_face) == result_map.end())
					this->find_adjacant_facets_of(adj_face, pred, result_map);
			}
		}

	};

	namespace Internal
	{
#ifndef min
		template <typename T>
		inline T clamp(T value, T minV, T maxV)
		{
			return std::max(std::min(value, maxV), minV);
		}
#else
		template <typename T>
		inline T clamp(T value, T minV, T maxV)
		{
			return max(min(value, maxV), minV);
		}
#endif
	}

	/// <summary>
	/// Compute the closest projection point from P0 to triangle(V0,V1,V2).
	/// </summary>
	/// <param name="P0">The p0.</param>
	/// <param name="V0">The v0.</param>
	/// <param name="V1">The v1.</param>
	/// <param name="V2">The v2.</param>
	/// <returns></returns>
	inline DirectX::XMVECTOR XM_CALLCONV Projection(DirectX::FXMVECTOR P0, DirectX::FXMVECTOR V0, DirectX::FXMVECTOR V1, DirectX::GXMVECTOR V2)
	{
		using namespace DirectX;
		using namespace Geometrics::Internal;
		XMVECTOR edge0 = V1 - V0;
		XMVECTOR edge1 = V2 - V0;
		XMVECTOR p0 = V0 - P0;

		//XMVectorBaryCentric();

		float a = XMVectorGetX(_DXMEXT XMVector3LengthSq(edge0));
		float b = XMVectorGetX(_DXMEXT XMVector3Dot(edge0, edge1));
		float c = XMVectorGetX(_DXMEXT XMVector3LengthSq(edge1));
		float d = XMVectorGetX(_DXMEXT XMVector3Dot(edge0, p0));
		float e = XMVectorGetX(_DXMEXT XMVector3Dot(edge1, p0));

		float det = a*c - b*b;
		float s = b*e - c*d;
		float t = b*d - a*e;

		if (s + t < det)
		{
			if (s < 0.f)
			{
				if (t < 0.f)
				{
					if (d < 0.f)
					{
						s = clamp(-d / a, 0.f, 1.f);
						t = 0.f;
					}
					else
					{
						s = 0.f;
						t = clamp(-e / c, 0.f, 1.f);
					}
				}
				else
				{
					s = 0.f;
					t = clamp(-e / c, 0.f, 1.f);
				}
			}
			else if (t < 0.f)
			{
				s = clamp(-d / a, 0.f, 1.f);
				t = 0.f;
			}
			else
			{
				float invDet = 1.f / det;
				s *= invDet;
				t *= invDet;
			}
		}
		else
		{
			if (s < 0.f)
			{
				float tmp0 = b + d;
				float tmp1 = c + e;
				if (tmp1 > tmp0)
				{
					float numer = tmp1 - tmp0;
					float denom = a - 2 * b + c;
					s = clamp(numer / denom, 0.f, 1.f);
					t = 1 - s;
				}
				else
				{
					t = clamp(-e / c, 0.f, 1.f);
					s = 0.f;
				}
			}
			else if (t < 0.f)
			{
				if (a + d > b + e)
				{
					float numer = c + e - b - d;
					float denom = a - 2 * b + c;
					s = clamp(numer / denom, 0.f, 1.f);
					t = 1 - s;
				}
				else
				{
					s = clamp(-e / c, 0.f, 1.f);
					t = 0.f;
				}
			}
			else
			{
				float numer = c + e - b - d;
				float denom = a - 2 * b + c;
				s = clamp(numer / denom, 0.f, 1.f);
				t = 1.f - s;
			}
		}

		return V0 + s * edge0 + t * edge1;
	}

	inline float XM_CALLCONV Distance(DirectX::FXMVECTOR P0, DirectX::FXMVECTOR V0, DirectX::FXMVECTOR V1, DirectX::GXMVECTOR V2)
	{
		using namespace DirectX;
		XMVECTOR vProj = Projection(P0, V0, V1, V2);
		vProj -= P0;
		return XMVectorGetX(_DXMEXT XMVector3Length(vProj));
	}

	template <typename _VertexType, typename _IndexType>
	inline float XM_CALLCONV Distance(const TriangleMesh<_VertexType, _IndexType> &Mesh, DirectX::FXMVECTOR Point)
	{
		using namespace DirectX;
		float minDis = numeric_limits<float>::max();
		for (const auto& tri : Mesh.facets())
		{
			float dis;

			XMVECTOR v0 = XMLoadFloat3(&Mesh.vertices[tri[0]].position);
			XMVECTOR v1 = XMLoadFloat3(&Mesh.vertices[tri[1]].position);
			XMVECTOR v2 = XMLoadFloat3(&Mesh.vertices[tri[2]].position);

			dis = Distance(Point, v0, v1, v2);
			if (dis < minDis) minDis = dis;
			//minDis = std::min(dis,minDis);
		}

		//XMVECTOR vDis = XMVectorReplicate(numeric_limits<float>::max());
		//for (const auto& vertex : Mesh.vertices)
		//{
		//	XMVECTOR vPos = XMLoadFloat3(&vertex.position);
		//	vPos -= Point;
		//	XMVECTOR dis = XMVector3Length(vPos);
		//	vDis = XMVectorMin(vDis,dis);
		//}

		//float minDisV = XMVectorGetX(vDis);
		//assert(minDisV >= minDis);
		return minDis;
		//return XMVectorGetX(minDis);
	}

	template <typename _VertexType, typename _IndexType>
	bool XM_CALLCONV Inside(const TriangleMesh<_VertexType, _IndexType> &Mesh, DirectX::FXMVECTOR Point)
	{
		XMFLOAT3A Direction;
		Direction.x = (float)std::rand() / (RAND_MAX + 1);
		Direction.y = (float)std::rand() / (RAND_MAX + 1);
		Direction.z = (float)std::rand() / (RAND_MAX + 1);
		XMVECTOR vDir = XMLoadFloat3A(&Direction);
		auto count = intersect(Mesh, Point, vDir, nullptr);
		return count & 1; //count % 2
	}

}


/*
 * Copyright (c) 1997-1999 Massachusetts Institute of Technology
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/* This file was automatically generated --- DO NOT EDIT */
/* Generated on Sun Nov  7 20:43:56 EST 1999 */

#include <fftw-int.h>
#include <fftw.h>

/* Generated by: ./genfft -magic-alignment-check -magic-twiddle-load-all -magic-variables 4 -magic-loopi -real2hc 13 */

/*
 * This function contains 76 FP additions, 34 FP multiplications,
 * (or, 57 additions, 15 multiplications, 19 fused multiply/add),
 * 34 stack variables, and 26 memory accesses
 */
static const fftw_real K083333333 = FFTW_KONST(+0.083333333333333333333333333333333333333333333);
static const fftw_real K075902986 = FFTW_KONST(+0.075902986037193865983102897245103540356428373);
static const fftw_real K251768516 = FFTW_KONST(+0.251768516431883313623436926934233488546674281);
static const fftw_real K503537032 = FFTW_KONST(+0.503537032863766627246873853868466977093348562);
static const fftw_real K113854479 = FFTW_KONST(+0.113854479055790798974654345867655310534642560);
static const fftw_real K265966249 = FFTW_KONST(+0.265966249214837287587521063842185948798330267);
static const fftw_real K387390585 = FFTW_KONST(+0.387390585467617292130675966426762851778775217);
static const fftw_real K300462606 = FFTW_KONST(+0.300462606288665774426601772289207995520941381);
static const fftw_real K258260390 = FFTW_KONST(+0.258260390311744861420450644284508567852516811);
static const fftw_real K132983124 = FFTW_KONST(+0.132983124607418643793760531921092974399165133);
static const fftw_real K2_000000000 = FFTW_KONST(+2.000000000000000000000000000000000000000000000);
static const fftw_real K1_732050807 = FFTW_KONST(+1.732050807568877293527446341505872366942805254);
static const fftw_real K156891391 = FFTW_KONST(+0.156891391051584611046832726756003269660212636);
static const fftw_real K256247671 = FFTW_KONST(+0.256247671582936600958684654061725059144125175);
static const fftw_real K011599105 = FFTW_KONST(+0.011599105605768290721655456654083252189827041);
static const fftw_real K300238635 = FFTW_KONST(+0.300238635966332641462884626667381504676006424);
static const fftw_real K174138601 = FFTW_KONST(+0.174138601152135905005660794929264742616964676);
static const fftw_real K575140729 = FFTW_KONST(+0.575140729474003121368385547455453388461001608);
static const fftw_real K866025403 = FFTW_KONST(+0.866025403784438646763723170752936183471402627);
static const fftw_real K500000000 = FFTW_KONST(+0.500000000000000000000000000000000000000000000);

/*
 * Generator Id's :
 * $Id: exprdag.ml,v 1.41 1999/05/26 15:44:14 fftw Exp $
 * $Id: fft.ml,v 1.43 1999/05/17 19:44:18 fftw Exp $
 * $Id: to_c.ml,v 1.25 1999/10/26 21:41:32 stevenj Exp $
 */

void fftw_real2hc_13(const fftw_real *input, fftw_real *real_output, fftw_real *imag_output, int istride, int real_ostride, int imag_ostride)
{
     fftw_real tmp65;
     fftw_real tmp11;
     fftw_real tmp28;
     fftw_real tmp37;
     fftw_real tmp51;
     fftw_real tmp62;
     fftw_real tmp22;
     fftw_real tmp58;
     fftw_real tmp59;
     fftw_real tmp66;
     fftw_real tmp56;
     fftw_real tmp63;
     fftw_real tmp35;
     fftw_real tmp38;
     ASSERT_ALIGNED_DOUBLE;
     tmp65 = input[0];
     {
	  fftw_real tmp3;
	  fftw_real tmp53;
	  fftw_real tmp21;
	  fftw_real tmp30;
	  fftw_real tmp26;
	  fftw_real tmp16;
	  fftw_real tmp29;
	  fftw_real tmp25;
	  fftw_real tmp6;
	  fftw_real tmp32;
	  fftw_real tmp9;
	  fftw_real tmp33;
	  fftw_real tmp10;
	  fftw_real tmp54;
	  fftw_real tmp1;
	  fftw_real tmp2;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp1 = input[8 * istride];
	  tmp2 = input[5 * istride];
	  tmp3 = tmp1 - tmp2;
	  tmp53 = tmp1 + tmp2;
	  {
	       fftw_real tmp17;
	       fftw_real tmp18;
	       fftw_real tmp19;
	       fftw_real tmp20;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp17 = input[12 * istride];
	       tmp18 = input[4 * istride];
	       tmp19 = input[10 * istride];
	       tmp20 = tmp18 + tmp19;
	       tmp21 = tmp17 + tmp20;
	       tmp30 = tmp17 - (K500000000 * tmp20);
	       tmp26 = tmp18 - tmp19;
	  }
	  {
	       fftw_real tmp12;
	       fftw_real tmp13;
	       fftw_real tmp14;
	       fftw_real tmp15;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp12 = input[istride];
	       tmp13 = input[3 * istride];
	       tmp14 = input[9 * istride];
	       tmp15 = tmp13 + tmp14;
	       tmp16 = tmp12 + tmp15;
	       tmp29 = tmp12 - (K500000000 * tmp15);
	       tmp25 = tmp13 - tmp14;
	  }
	  {
	       fftw_real tmp4;
	       fftw_real tmp5;
	       fftw_real tmp7;
	       fftw_real tmp8;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp4 = input[6 * istride];
	       tmp5 = input[11 * istride];
	       tmp6 = tmp4 - tmp5;
	       tmp32 = tmp4 + tmp5;
	       tmp7 = input[2 * istride];
	       tmp8 = input[7 * istride];
	       tmp9 = tmp7 - tmp8;
	       tmp33 = tmp7 + tmp8;
	  }
	  tmp10 = tmp6 + tmp9;
	  tmp54 = tmp32 + tmp33;
	  tmp11 = tmp3 - tmp10;
	  {
	       fftw_real tmp24;
	       fftw_real tmp27;
	       fftw_real tmp49;
	       fftw_real tmp50;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp24 = tmp3 + (K500000000 * tmp10);
	       tmp27 = K866025403 * (tmp25 + tmp26);
	       tmp28 = tmp24 - tmp27;
	       tmp37 = tmp27 + tmp24;
	       tmp49 = tmp9 - tmp6;
	       tmp50 = tmp25 - tmp26;
	       tmp51 = tmp49 - tmp50;
	       tmp62 = tmp50 + tmp49;
	  }
	  tmp22 = tmp16 - tmp21;
	  tmp58 = tmp16 + tmp21;
	  tmp59 = tmp53 + tmp54;
	  tmp66 = tmp58 + tmp59;
	  {
	       fftw_real tmp52;
	       fftw_real tmp55;
	       fftw_real tmp31;
	       fftw_real tmp34;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp52 = tmp29 + tmp30;
	       tmp55 = tmp53 - (K500000000 * tmp54);
	       tmp56 = tmp52 - tmp55;
	       tmp63 = tmp52 + tmp55;
	       tmp31 = tmp29 - tmp30;
	       tmp34 = K866025403 * (tmp32 - tmp33);
	       tmp35 = tmp31 + tmp34;
	       tmp38 = tmp31 - tmp34;
	  }
     }
     real_output[0] = tmp65 + tmp66;
     {
	  fftw_real tmp23;
	  fftw_real tmp45;
	  fftw_real tmp40;
	  fftw_real tmp48;
	  fftw_real tmp44;
	  fftw_real tmp46;
	  fftw_real tmp41;
	  fftw_real tmp47;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp23 = (K575140729 * tmp11) - (K174138601 * tmp22);
	  tmp45 = (K575140729 * tmp22) + (K174138601 * tmp11);
	  {
	       fftw_real tmp36;
	       fftw_real tmp39;
	       fftw_real tmp42;
	       fftw_real tmp43;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp36 = (K300238635 * tmp28) + (K011599105 * tmp35);
	       tmp39 = (K256247671 * tmp37) + (K156891391 * tmp38);
	       tmp40 = tmp36 - tmp39;
	       tmp48 = K1_732050807 * (tmp39 + tmp36);
	       tmp42 = (K300238635 * tmp35) - (K011599105 * tmp28);
	       tmp43 = (K156891391 * tmp37) - (K256247671 * tmp38);
	       tmp44 = K1_732050807 * (tmp42 - tmp43);
	       tmp46 = tmp43 + tmp42;
	  }
	  imag_output[imag_ostride] = tmp23 + (K2_000000000 * tmp40);
	  tmp41 = tmp23 - tmp40;
	  imag_output[3 * imag_ostride] = tmp41 - tmp44;
	  imag_output[4 * imag_ostride] = -(tmp41 + tmp44);
	  imag_output[5 * imag_ostride] = -(tmp45 + (K2_000000000 * tmp46));
	  tmp47 = tmp46 - tmp45;
	  imag_output[2 * imag_ostride] = tmp47 - tmp48;
	  imag_output[6 * imag_ostride] = tmp48 + tmp47;
     }
     {
	  fftw_real tmp61;
	  fftw_real tmp70;
	  fftw_real tmp74;
	  fftw_real tmp76;
	  fftw_real tmp68;
	  fftw_real tmp69;
	  fftw_real tmp57;
	  fftw_real tmp60;
	  fftw_real tmp71;
	  fftw_real tmp75;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp57 = (K132983124 * tmp51) + (K258260390 * tmp56);
	  tmp60 = K300462606 * (tmp58 - tmp59);
	  tmp61 = (K2_000000000 * tmp57) + tmp60;
	  tmp70 = tmp60 - tmp57;
	  {
	       fftw_real tmp72;
	       fftw_real tmp73;
	       fftw_real tmp64;
	       fftw_real tmp67;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp72 = (K387390585 * tmp51) - (K265966249 * tmp56);
	       tmp73 = (K113854479 * tmp62) - (K503537032 * tmp63);
	       tmp74 = tmp72 + tmp73;
	       tmp76 = tmp73 - tmp72;
	       tmp64 = (K251768516 * tmp62) + (K075902986 * tmp63);
	       tmp67 = tmp65 - (K083333333 * tmp66);
	       tmp68 = (K2_000000000 * tmp64) + tmp67;
	       tmp69 = tmp67 - tmp64;
	  }
	  real_output[real_ostride] = tmp61 + tmp68;
	  real_output[5 * real_ostride] = tmp68 - tmp61;
	  tmp71 = tmp69 - tmp70;
	  real_output[2 * real_ostride] = tmp71 - tmp74;
	  real_output[6 * real_ostride] = tmp74 + tmp71;
	  tmp75 = tmp70 + tmp69;
	  real_output[3 * real_ostride] = tmp75 - tmp76;
	  real_output[4 * real_ostride] = tmp76 + tmp75;
     }
}

fftw_codelet_desc fftw_real2hc_13_desc =
{
     "fftw_real2hc_13",
     (void (*)()) fftw_real2hc_13,
     13,
     FFTW_FORWARD,
     FFTW_REAL2HC,
     288,
     0,
     (const int *) 0,
};


/* Copyright (C) 2021 Atsushi Togo */
/* All rights reserved. */

/* This file is part of phonopy. */

/* Redistribution and use in source and binary forms, with or without */
/* modification, are permitted provided that the following conditions */
/* are met: */

/* * Redistributions of source code must retain the above copyright */
/*   notice, this list of conditions and the following disclaimer. */

/* * Redistributions in binary form must reproduce the above copyright */
/*   notice, this list of conditions and the following disclaimer in */
/*   the documentation and/or other materials provided with the */
/*   distribution. */

/* * Neither the name of the phonopy project nor the names of its */
/*   contributors may be used to endorse or promote products derived */
/*   from this software without specific prior written permission. */

/* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS */
/* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT */
/* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS */
/* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE */
/* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, */
/* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, */
/* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; */
/* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER */
/* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT */
/* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN */
/* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE */
/* POSSIBILITY OF SUCH DAMAGE. */

#ifndef __phono3py_H__
#define __phono3py_H__

#ifdef MKL_LAPACKE
#include <mkl.h>
#else
#include <lapacke.h>
#endif
#include "phonoc_array.h"

long ph3py_get_interaction(Darray *fc3_normal_squared,
                           const char *g_zero,
                           const Darray *frequencies,
                           const lapack_complex_double *eigenvectors,
                           const long (*triplets)[3],
                           const long num_triplets,
                           const long (*bz_grid_addresses)[3],
                           const long D_diag[3],
                           const long Q[3][3],
                           const double *fc3,
                           const long is_compact_fc3,
                           const double (*svecs)[3],
                           const long multi_dims[2],
                           const long (*multi)[2],
                           const double *masses,
                           const long *p2s_map,
                           const long *s2p_map,
                           const long *band_indices,
                           const long symmetrize_fc3_q,
                           const double cutoff_frequency);
long ph3py_get_pp_collision(double *imag_self_energy,
                            const long relative_grid_address[24][4][3], /* thm */
                            const double *frequencies,
                            const lapack_complex_double *eigenvectors,
                            const long (*triplets)[3],
                            const long num_triplets,
                            const long *triplet_weights,
                            const long (*bz_grid_addresses)[3], /* thm */
                            const long *bz_map,                 /* thm */
                            const long bz_grid_type,
                            const long D_diag[3],
                            const long Q[3][3],
                            const double *fc3,
                            const long is_compact_fc3,
                            const double (*svecs)[3],
                            const long multi_dims[2],
                            const long (*multi)[2],
                            const double *masses,
                            const long *p2s_map,
                            const long *s2p_map,
                            const Larray *band_indices,
                            const Darray *temperatures,
                            const long is_NU,
                            const long symmetrize_fc3_q,
                            const double cutoff_frequency);
long ph3py_get_pp_collision_with_sigma(
    double *imag_self_energy,
    const double sigma,
    const double sigma_cutoff,
    const double *frequencies,
    const lapack_complex_double *eigenvectors,
    const long (*triplets)[3],
    const long num_triplets,
    const long *triplet_weights,
    const long (*bz_grid_addresses)[3],
    const long D_diag[3],
    const long Q[3][3],
    const double *fc3,
    const long is_compact_fc3,
    const double (*svecs)[3],
    const long multi_dims[2],
    const long (*multi)[2],
    const double *masses,
    const long *p2s_map,
    const long *s2p_map,
    const Larray *band_indices,
    const Darray *temperatures,
    const long is_NU,
    const long symmetrize_fc3_q,
    const double cutoff_frequency);
void ph3py_get_imag_self_energy_at_bands_with_g(
    double *imag_self_energy,
    const Darray *fc3_normal_squared,
    const double *frequencies,
    const long (*triplets)[3],
    const long *triplet_weights,
    const double *g,
    const char *g_zero,
    const double temperature,
    const double cutoff_frequency,
    const long num_frequency_points,
    const long frequency_point_index);
void ph3py_get_detailed_imag_self_energy_at_bands_with_g(
    double *detailed_imag_self_energy,
    double *imag_self_energy_N,
    double *imag_self_energy_U,
    const Darray *fc3_normal_squared,
    const double *frequencies,
    const long (*triplets)[3],
    const long *triplet_weights,
    const long (*bz_grid_addresses)[3],
    const double *g,
    const char *g_zero,
    const double temperature,
    const double cutoff_frequency);
void ph3py_get_real_self_energy_at_bands(double *real_self_energy,
                                         const Darray *fc3_normal_squared,
                                         const long *band_indices,
                                         const double *frequencies,
                                         const long (*triplets)[3],
                                         const long *triplet_weights,
                                         const double epsilon,
                                         const double temperature,
                                         const double unit_conversion_factor,
                                         const double cutoff_frequency);
void ph3py_get_real_self_energy_at_frequency_point(
    double *real_self_energy,
    const double frequency_point,
    const Darray *fc3_normal_squared,
    const long *band_indices,
    const double *frequencies,
    const long (*triplets)[3],
    const long *triplet_weights,
    const double epsilon,
    const double temperature,
    const double unit_conversion_factor,
    const double cutoff_frequency);
void ph3py_get_collision_matrix(double *collision_matrix,
                                const Darray *fc3_normal_squared,
                                const double *frequencies,
                                const long (*triplets)[3],
                                const long *triplets_map,
                                const long *map_q,
                                const long *rotated_grid_points,
                                const double *rotations_cartesian,
                                const double *g,
                                const long num_ir_gp,
                                const long num_gp,
                                const long num_rot,
                                const double temperature,
                                const double unit_conversion_factor,
                                const double cutoff_frequency);
void ph3py_get_reducible_collision_matrix(double *collision_matrix,
                                          const Darray *fc3_normal_squared,
                                          const double *frequencies,
                                          const long (*triplets)[3],
                                          const long *triplets_map,
                                          const long *map_q,
                                          const double *g,
                                          const long num_gp,
                                          const double temperature,
                                          const double unit_conversion_factor,
                                          const double cutoff_frequency);
void ph3py_get_isotope_scattering_strength(
    double *gamma,
    const long grid_point,
    const double *mass_variances,
    const double *frequencies,
    const lapack_complex_double *eigenvectors,
    const long num_grid_points,
    const long *band_indices,
    const long num_band,
    const long num_band0,
    const double sigma,
    const double cutoff_frequency);
void ph3py_get_thm_isotope_scattering_strength(
    double *gamma,
    const long grid_point,
    const long *ir_grid_points,
    const long *weights,
    const double *mass_variances,
    const double *frequencies,
    const lapack_complex_double *eigenvectors,
    const long num_ir_grid_points,
    const long *band_indices,
    const long num_band,
    const long num_band0,
    const double *integration_weights,
    const double cutoff_frequency);
void ph3py_distribute_fc3(double *fc3,
                          const long target,
                          const long source,
                          const long *atom_mapping,
                          const long num_atom,
                          const double *rot_cart);
void ph3py_rotate_delta_fc2(double (*fc3)[3][3][3],
                            const double (*delta_fc2s)[3][3],
                            const double *inv_U,
                            const double (*site_sym_cart)[3][3],
                            const long *rot_map_syms,
                            const long num_atom,
                            const long num_site_sym,
                            const long num_disp);
void ph3py_get_permutation_symmetry_fc3(double *fc3, const long num_atom);
void ph3py_get_permutation_symmetry_compact_fc3(double *fc3,
                                                const long p2s[],
                                                const long s2pp[],
                                                const long nsym_list[],
                                                const long perms[],
                                                const long n_satom,
                                                const long n_patom);
void ph3py_transpose_compact_fc3(double *fc3,
                                 const long p2s[],
                                 const long s2pp[],
                                 const long nsym_list[],
                                 const long perms[],
                                 const long n_satom,
                                 const long n_patom,
                                 const long t_type);
long ph3py_get_triplets_reciprocal_mesh_at_q(long *map_triplets,
                                             long *map_q,
                                             const long grid_point,
                                             const long mesh[3],
                                             const long is_time_reversal,
                                             const long num_rot,
                                             const long (*rec_rotations)[3][3],
                                             const long swappable);
long ph3py_get_BZ_triplets_at_q(long (*triplets)[3],
                                const long grid_point,
                                const long (*bz_grid_addresses)[3],
                                const long *bz_map,
                                const long *map_triplets,
                                const long num_map_triplets,
                                const long D_diag[3],
                                const long Q[3][3],
                                const long bz_grid_type);
long ph3py_get_integration_weight(double *iw,
                                  char *iw_zero,
                                  const double *frequency_points,
                                  const long num_band0,
                                  const long relative_grid_address[24][4][3],
                                  const long mesh[3],
                                  const long (*triplets)[3],
                                  const long num_triplets,
                                  const long (*bz_grid_addresses)[3],
                                  const long *bz_map,
                                  const long bz_grid_type,
                                  const double *frequencies1,
                                  const long num_band1,
                                  const double *frequencies2,
                                  const long num_band2,
                                  const long tp_type,
                                  const long openmp_per_triplets,
                                  const long openmp_per_bands);
void ph3py_get_integration_weight_with_sigma(double *iw,
                                             char *iw_zero,
                                             const double sigma,
                                             const double sigma_cutoff,
                                             const double *frequency_points,
                                             const long num_band0,
                                             const long (*triplets)[3],
                                             const long num_triplets,
                                             const double *frequencies,
                                             const long num_band,
                                             const long tp_type);
long ph3py_get_grid_index_from_address(const long address[3],
                                       const long mesh[3]);
void ph3py_get_gr_grid_addresses(long gr_grid_addresses[][3],
                                 const long D_diag[3]);
long ph3py_get_reciprocal_rotations(long rec_rotations[48][3][3],
                                    const long (*rotations)[3][3],
                                    const long num_rot,
                                    const long is_time_reversal);
long ph3py_transform_rotations(long (*transformed_rots)[3][3],
                               const long (*rotations)[3][3],
                               const long num_rot,
                               const long D_diag[3],
                               const long Q[3][3]);
long ph3py_get_snf3x3(long D_diag[3],
                      long P[3][3],
                      long Q[3][3],
                      const long A[3][3]);
long ph3py_transform_rotations(long (*transformed_rots)[3][3],
                               const long (*rotations)[3][3],
                               const long num_rot,
                               const long D_diag[3],
                               const long Q[3][3]);
long ph3py_get_ir_grid_map(long *ir_grid_map,
                           const long D_diag[3],
                           const long PS[3],
                           const long (*grg_rotations)[3][3],
                           const long num_rot);
long ph3py_get_bz_grid_addresses(long (*bz_grid_addresses)[3],
                                 long *bz_map,
                                 long *bzg2grg,
                                 const long D_diag[3],
                                 const long Q[3][3],
                                 const long PS[3],
                                 const double rec_lattice[3][3],
                                 const long type);
long ph3py_rotate_bz_grid_index(const long bz_grid_index,
                                const long rotation[3][3],
                                const long (*bz_grid_addresses)[3],
                                const long *bz_map,
                                const long D_diag[3],
                                const long PS[3],
                                const long bz_grid_type);
void ph3py_symmetrize_collision_matrix(double *collision_matrix,
                                       const long num_column,
                                       const long num_temp,
                                       const long num_sigma);
void ph3py_expand_collision_matrix(double *collision_matrix,
                                   const long *rot_grid_points,
                                   const long *ir_grid_points,
                                   const long num_ir_gp,
                                   const long num_grid_points,
                                   const long num_rot,
                                   const long num_sigma,
                                   const long num_temp,
                                   const long num_band);
long ph3py_get_neighboring_gird_points(long *relative_grid_points,
                                       const long *grid_points,
                                       const long (*relative_grid_address)[3],
                                       const long mesh[3],
                                       const long (*bz_grid_addresses)[3],
                                       const long *bz_map,
                                       const long bz_grid_type,
                                       const long num_grid_points,
                                       const long num_relative_grid_address);
long ph3py_get_thm_integration_weights_at_grid_points(
    double *iw,
    const double *frequency_points,
    const long num_band0,
    const long num_band,
    const long num_gp,
    const long (*relative_grid_address)[4][3],
    const long D_diag[3],
    const long *grid_points,
    const long (*bz_grid_addresses)[3],
    const long *bz_map,
    const long bz_grid_type,
    const double *frequencies,
    const long *gp2irgp_map,
    const char function);

#endif