/* Functions implementing some helpers for computation.
 *
 * All angles in here are in rad.
 */

#ifndef _GNU_SOURCE
#  define _GNU_SOURCE         /* for M_PI */
#endif

#include <math.h>

#include "libxpa.h"


double xpa_norm3(vec3 *v)
{
	return sqrt(v->x*v->x+v->y*v->y+v->z*v->z);
}

double xpa_scalProd3(vec3 *v1, vec3 *v2)
/* computes the scalar product for *unit vectors* v1, v2
 */
{
	return v1->x*v2->x+v1->y*v2->y+v1->z*v2->z;
}

void xpa_mul3I(vec3 *v, double factor)
{
	v->x *= factor;
	v->y *= factor;
	v->z *= factor;
}

int xpa_norm3I(vec3 *v)
{
	double norm=xpa_norm3(v);

	if (norm<1e-12) {
		return 1;
	}
	xpa_mul3I(v, norm);
	return 0;
}

static int floatEqual(double a, double b)
{
	double sum=a+b, difference=fabs(a-b);
	if (sum) {
		return difference/sum<1e-12;
	} else {
		return difference<1e-12;
	}
}

int xpa_usPosEqual(usPos p1, usPos p2)
{
	return floatEqual(p1.alpha, p2.alpha) && floatEqual(p1.delta, p2.delta);
}

usPos xpa_v3ToUsPos(vec3 direction)
/* returns a unit sphere position for a direction vector.
 *
 * If direction is a zero vector, the position becomes NaN.
 */
{
	usPos pos;

	if (xpa_norm3I(&direction)) {
		pos.alpha = FP_NAN;
		pos.delta = FP_NAN;
	}
	pos.delta = asin(direction.z);
	pos.alpha = atan2(direction.y, direction.x);
	if (pos.alpha<0) {
		pos.alpha += 2*M_PI;
	}
	return pos;
}

vec3 xpa_usPosTov3(usPos p)
/* returns a direction vector for a unit sphere position
 */
{
	vec3 res;

	res.x = cos(p.alpha)*cos(p.delta);
	res.y = sin(p.alpha)*cos(p.delta);
	res.z = sin(p.delta);
	return res;
}

double xpa_gcDistanceC(usPos p1, usPos p2)
/* returns the distance on a great circle between p1 and p2
 *
 * The return value is in radians.
 */
{
	vec3 v1=xpa_usPosTov3(p1), v2=xpa_usPosTov3(p2);
	double cosDist = xpa_scalProd3(&v1, &v2);
	double res=acos(cosDist);

	while (res<0) {
		res += 2*M_PI;
	}
	while (res>2*M_PI) {
		res -= 2*M_PI;
	}
	return res;
}

vec3 xpa_diffAlpha(usPos p)
/* returns the direction vector of the alpha-derivative at p. 
 */
{
	vec3 result;

	result.x = -cos(p.delta)*sin(p.alpha);
	result.y =  cos(p.delta)*cos(p.alpha);
	result.z = 0;
	return result;
}

vec3 xpa_diffDelta(usPos p)
/* returns the direction vector of the delta-derivative at p. 
 */
{
	vec3 result;

	result.x = -sin(p.delta)*cos(p.alpha);
	result.y = -sin(p.delta)*sin(p.alpha);
	result.z = cos(p.delta);
	return result;
}