#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#define	_M_PI	3.14159265358979323846

#define	_S_SPEED	343.0 /* sound speed */
#define	_DISC_FR	44100 /* sampling frequency */

double	_headradius = 0.087;
double	_trsoradius = 0.169;
double	_neckheight = 0.053;
double	_alpha_min  = 0.1;
double	_theta_min  = (5.0 * _M_PI / 6.0);
double	_trso_rflct = 0.3;

struct	_vecpnt {
	double _x, _y, _z;
	};

double	sphereH(double sfreq, double theta, double sphereRadius) {

	double alpha, tau;

	alpha = (1.0 + _alpha_min / 2.0) + (1 - _alpha_min / 2.0) * cos(theta / _theta_min * _M_PI);

	tau   = 2 * sphereRadius / _S_SPEED;

	return((alpha * tau * sfreq + 1) / (tau * sfreq + 1)); }

double	sphereT(double theta, double sphereRadius) {

	if (theta < - _M_PI || theta > _M_PI) puts("theta in sphereT out of bound");

	if (fabs(theta) < _M_PI / 2)
		return(-1.0 * sphereRadius * cos(theta) / _S_SPEED + 45.0 / (double)_DISC_FR);
	else
		return(sphereRadius * (fabs(theta) - _M_PI / 2) / _S_SPEED + 45.0 / (double)_DISC_FR); }

double	_scalar_product(struct _vecpnt _x1, struct _vecpnt _x2) {
	return(_x1._x * _x2._x + _x1._y * _x2._y + _x1._z * _x2._z); }

double	_vector_len(struct _vecpnt _x1) { return(sqrt(_scalar_product(_x1, _x1))); }

double	_solve_for_alpha(double A, double eps) {

	double alpha0, alphaM;

	alpha0 = (A - 1) / (2 * A - 1) * _M_PI / 2;
	alphaM = acos(1 / A);

	if (-1.0 * alphaM <= eps && eps <= 0)
		return(alpha0 - (1 - alpha0 / alphaM) * eps);
	else if (0 <= eps && eps <= _M_PI / 2)
		return(alpha0 * (1 - eps * 2 / _M_PI));
	else
		puts("Out of bound arg in solve_for_alpha");

	return(0.00); }

struct	_vecpnt	_vector_sum(struct _vecpnt _x1, struct _vecpnt _x2) {

	struct	_vecpnt _rlt;

	_rlt._x = _x1._x + _x2._x;
	_rlt._y = _x1._y + _x2._y;
	_rlt._z = _x1._z + _x2._z;

	return(_rlt); }

struct	_vecpnt	_vector_dif(struct _vecpnt _x1, struct _vecpnt _x2) {

	struct	_vecpnt _rlt;

	_rlt._x = _x1._x - _x2._x;
	_rlt._y = _x1._y - _x2._y;
	_rlt._z = _x1._z - _x2._z;

	return(_rlt); }

struct	_vecpnt	_vector_byc(double _alpha, struct _vecpnt _x1) {

	struct	_vecpnt	_rlt;

	_rlt._x = _alpha * _x1._x;
	_rlt._y = _alpha * _x1._y;
	_rlt._z = _alpha * _x1._z;

	return(_rlt); }

void	snowmanHRTF(double sfreq, struct _vecpnt _s, int _ear_number, double *_rltr, double *_rlti) {

	/* this function computes snowman's HRTF for the frequency sfreq, */
	/* for the source direction _s, and for the ear _ear_number */

	/* sfreq in the frequency in Hz */

	/* _s is the direction to the source */

	/* _s MUST be normalized (so that its length is equal to 1) */

	/* _ear_number should be 0 to compute left ear HRTF and 1 for right ear HRTF */

	/* the computed HRTF is a complex number */
	/* real part is returned in _rltr */
	/* imaginary part in _rlti */

	/* the coordinate system is as follows */
	/* the origin is located in the center of the head */
	/* X axis goes to the RIGHT */
	/* Y axis goes UP */
	/* Z axis goes BACK */
	/* so for example for the source over the user's head _s would be */
	/* X = 0 Y = 1 Z = 0 */
	/* and for the source directly to the left of user _s would be X = -1 Y = 0 Z = 0 */

	/* do not forget to normalize _s before passing it to this function!!! */

	struct	_vecpnt _b, _bc, _d, _d2, _e, _th, _r; double A, eps, alpha, f, beta, phi, _w1, _w2, _phz;

	double	dTR, dTHD, dTHR, hHD, hHR, hHS, hT, dTHT, thetaD, thetaR, thetaHS, thetaFlat, thetaHT, thetaT, eta, etaMin;

	_e._x = (_ear_number) ? (_headradius) : (-1.0 * _headradius);
	_e._y = 0;
	_e._z = 0;

	_b._x = 0;
	_b._y = _trsoradius;
	_b._z = 0;

	_th._x = 0;
	_th._y = _trsoradius + _neckheight + _headradius;
	_th._z = 0;

	_d = _vector_sum(_th, _e);

	if (_scalar_product(_d, _s) < -1.0 * sqrt(_scalar_product(_d, _d) - _scalar_product(_b, _b))) {

		/* we're inside torso shadow cone */

		_d2 = _vector_dif(_vector_byc(_scalar_product(_d, _d), _s), _vector_byc(_scalar_product(_d, _s), _d));

		_w1 = _scalar_product(_b, _b) / _scalar_product(_d, _d);

		_w2 = sqrt(_w1 * (1.0 - _w1) / (_scalar_product(_d, _d) - _scalar_product(_d, _s) * _scalar_product(_d, _s)));

		_bc = _vector_sum(_vector_byc(_w1, _d), _vector_byc(_w2, _d2));

		_r = _vector_byc(1.0 / _vector_len(_vector_dif(_bc, _d)), _vector_dif(_bc, _d));

		thetaFlat = _theta_min * (0.5 + asin(_alpha_min / (2.0 - _alpha_min)) / _M_PI);

		eta = acos(_scalar_product(_s, _d) / _vector_len(_d));

		etaMin = _M_PI / 2 + acos(_vector_len(_b) / _vector_len(_d));



		thetaHS = acos(_scalar_product(_r, _e) / _vector_len(_e));

		thetaHT = acos(_scalar_product(_s, _e) / _vector_len(_e));

		thetaT = (_M_PI * (eta - etaMin) - thetaFlat * (eta - _M_PI)) / (_M_PI - etaMin);


		hT   = sphereH(sfreq, thetaT,  _trsoradius);

		hHS  = sphereH(sfreq, thetaHS, _headradius);

		dTHT = sphereT(thetaHT, _headradius);

		*_rltr = hT * hHS * cos(2 * _M_PI * sfreq * dTHT);
		*_rlti = hT * hHS * sin(2 * _M_PI * sfreq * dTHT);

		}
	else
		{

		/* we're outside torso shadow cone */

		/* find the reflection point by some long numerics */

		A = _vector_len(_d) / _vector_len(_b);

		eps = _M_PI / 2 - acos(_scalar_product(_d, _s) / _vector_len(_d));

		alpha = _solve_for_alpha(A, eps);

		f = sqrt(_scalar_product(_b, _b) + _scalar_product(_d, _d) - 2 * _vector_len(_b) * _vector_len(_d) * cos(alpha));

		beta = atan2(sin(alpha), A - cos(alpha));

		phi = alpha + beta;

		dTR = f / _S_SPEED * (1 + cos(2 * phi));

		_d2 = _vector_dif(_vector_byc(_scalar_product(_d, _d), _s), _vector_byc(_scalar_product(_d, _s), _d));

		_bc = _vector_sum(_vector_byc(_vector_len(_b) * cos(alpha) / _vector_len(_d), _d), _vector_byc(_vector_len(_b) * sin(alpha) / _vector_len(_d2), _d2));

		_r = _vector_byc(1.0 / _vector_len(_vector_dif(_bc, _d)), _vector_dif(_bc, _d));



		thetaD = acos(_scalar_product(_s, _e) / _vector_len(_e));

		thetaR = acos(_scalar_product(_r, _e) / _vector_len(_e));

		dTHD = sphereT(thetaD, _headradius);

		dTHR = sphereT(thetaR, _headradius);

		hHD = sphereH(sfreq, thetaD, _headradius);

		hHR = sphereH(sfreq, thetaR, _headradius);

		*_rltr = (_trso_rflct * hHR * cos(2 * _M_PI * sfreq * (dTR + dTHR)) + hHD * cos(2 * _M_PI * sfreq * dTHD)) / (1.0 + _trso_rflct);
		*_rlti = (_trso_rflct * hHR * sin(2 * _M_PI * sfreq * (dTR + dTHR)) + hHD * sin(2 * _M_PI * sfreq * dTHD)) / (1.0 + _trso_rflct);

		}

	*_rlti *= -1.0; // if this is not done, then HRIR after IFFT gets time-inverted

	return; }

void	main(void) { double _rr, _ri; struct _vecpnt _src; double _r; double _f;

	_src._x =  2.0; /* source direction */
	_src._y =  1.0;
	_src._z =  0.0;

	_r = _vector_len(_src);

	_src._x /= _r; /* normalizing */
	_src._y /= _r;
	_src._z /= _r;

	for (_f = 0; _f < 5000.0; _f += 10.0) {
        	snowmanHRTF(_f, _src, 0, &_rr, &_ri); printf("freq %lf HRTF %lf%+lfi\n", _f, _rr, _ri);
		}

	exit(0); }