#include "solid_angle.h"
#include <gsl/gsl_sf_ellint.h>
#include <math.h>

double get_solid_angle(double *pos, double *pmt, double *n, double r)
{
    /* Returns the solid angle subtended by a circular disk of radius r at a
     * position `pmt` with a normal vector `n` from a position `pos`.
     *
     * See http://www.umich.edu/~ners312/CourseLibrary/SolidAngleOfADiskOffAxis.pdf. */
    double dir[3];
    double L, r0, R, a2, k, Rmax, K, P;

    dir[0] = pos[0] - pmt[0];
    dir[1] = pos[1] - pmt[1];
    dir[2] = pos[2] - pmt[2];

    L = fabs(dir[0]*n[0] + dir[1]*n[1] + dir[2]*n[2]);
    R = sqrt(dir[0]*dir[0] + dir[1]*dir[1] + dir[2]*dir[2]);
    r0 = sqrt(R*R - L*L);

    a2 = 4*r0*r/pow(r0+r,2);
    Rmax = sqrt(L*L + (r0+r)*(r0+r));
    k = sqrt(4*r0*r/(L*L + pow(r0+r,2)));

    if (fabs(r0-r) < 1e-9) {
        /* If r0 is very close to r, the GSL routines below will occasionally
         * throw a domain error. */
        K = gsl_sf_ellint_Kcomp(k, GSL_PREC_DOUBLE);
        return M_PI - 2*L*K/Rmax;
    } else if (r0 <= r) {
        K = gsl_sf_ellint_Kcomp(k, GSL_PREC_DOUBLE);
        P = gsl_sf_ellint_Pcomp(k, -a2, GSL_PREC_DOUBLE);
        return 2*M_PI - 2*L*K/Rmax + (2*L/Rmax)*(r0-r)*P/(r0+r);
    } else {
        K = gsl_sf_ellint_Kcomp(k, GSL_PREC_DOUBLE);
        P = gsl_sf_ellint_Pcomp(k, -a2, GSL_PREC_DOUBLE);
        return -2*L*K/Rmax + (2*L/Rmax)*(r0-r)*P/(r0+r);
    }
}