#include <stdio.h>
#include <gsl/gsl_integration.h>
#include <math.h> /* For M_PI */

/* From Google maps. Probably not very accurate, but should be good enough for
 * this calculation. */
double latitude = 46.471857;
double longitude = -81.186755;

/* Radius of the earth in mm. */
double radius_earth = 6.371e9;

/* Depth of the SNO detector in mm. Don't be fooled by all the digits. I just
 * converted 6800 feet -> mm. */
double sno_depth = 2072640;

/* Fiducial volume in mm. */
double radius_fiducial = 5000;

/* Cartesian coordinates of SNO in earth frame. They need to be global since
 * they are used in some functions. */
double x_sno[3];

double deg2rad(double deg)
{
    return deg*M_PI/180.0;
}

double rad2deg(double rad)
{
    return rad*180.0/M_PI;
}

/* Convert spherical coordinates to cartesian coordinates.
 *
 * See https://en.wikipedia.org/wiki/Spherical_coordinate_system. */
void sphere2cartesian(double r, double theta, double phi, double *x, double *y, double *z)
{
    *x = r*sin(theta)*cos(phi);
    *y = r*sin(theta)*sin(phi);
    *z = r*cos(theta);
}

/* Convert cartesian coordinates to spherical coordinates.
 *
 * See https://en.wikipedia.org/wiki/Spherical_coordinate_system. */
void cartesian2sphere(double x, double y, double z, double *r, double *theta, double *phi)
{
    *r = sqrt(x*x + y*y + z*z);
    *theta = acos(z/(*r));
    *phi = atan2(y,x);
}

void cross(double *a, double *b, double *c)
{
    c[0] = a[1]*b[2] - a[2]*b[1];
    c[1] = a[2]*b[0] - a[0]*b[2];
    c[2] = a[0]*b[1] - a[1]*b[0];
}

double dot(double *a, double *b)
{
    return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
}

double norm(double *a)
{
    return sqrt(dot(a,a));
}

void normalize(double *a)
{
    double n = norm(a);
    a[0] /= n;
    a[1] /= n;
    a[2] /= n;
}

/* Rotate a vector x around the vector dir by an angle theta. */
void rotate(double *result, double *x, double *dir, double theta)
{
    double a = dot(dir,x);
    double b[3];

    double sin_theta = sin(theta);
    double cos_theta = cos(theta);

    /* Make sure the direction vector is normalized. */
    normalize(dir);

    cross(x,dir,b);

    result[0] = x[0]*cos_theta + dir[0]*a*(1-cos_theta) + b[0]*sin_theta;
    result[1] = x[1]*cos_theta + dir[1]*a*(1-cos_theta) + b[1]*sin_theta;
    result[2] = x[2]*cos_theta + dir[2]*a*(1-cos_theta) + b[2]*sin_theta;
}

/* Rotate a vector in earth centered coordinates to SNO coordinates (doesn't do
 * the translation). */
void rotate_earth_to_sno(double *x_earth, double *x_sno)
{
    double dir[3];
    double z[3] = {0,0,1};

    cross(x_sno, z, dir);

    /* Normalize. */
    normalize(dir);

    double theta = acos(dot(x_sno,z)/norm(x_sno));

    rotate(x_sno, x_earth, dir, theta);
}

int main(int argc, char **argv)
{
    /* Spherical angles for the SNO detector in the earth frame which has z
     * along the north and south poles and the x axis passing through Greenwich.
     * Should double check this. */
    double sno_theta = deg2rad(latitude + 90.0);
    double sno_phi = deg2rad(longitude);

    sphere2cartesian(radius_earth - sno_depth, sno_theta, sno_phi, x_sno, x_sno+1, x_sno+2);

    return 0;
}