#include <stdio.h>
#include <math.h>
#include <string.h>
#include "Math2.h"
#include "VMisc.h"
#include "Vec.h"


/*****************************************************************
 * latlonrhoToCartesianVec
 * CartesianTolatlonrhoVec
 * 
 * Latitude,  longitude, and  radius to Cartesian coordinates conversions.
 *         [90 to -90]  [0 to 360]      [1.0 at surface]
 * 
 * Inputs & Outputs:
 *  A 3 vector spherical coordinate.
 *  A 3 vector Cartesian coordinate.
 * Assumptions:
 *  That the right handed Cartesian system is such that 
 *      The X axis points thru the intersection of
 *      the prime meridian (lon=0) and
 *      the equator        (lat=0); 
 *  
 *      The Z axis points thru the north pole.
 *  
 *      The Y axis finishes the right handed triad (Z cross X).
 *  
 *  Note that in lat, lon, rho coordinates the latitude is zero at
 *  the equator.
 *  In regular spherical coordinates latitude is zero at the north pole.
 * Algorithm:
 *  p 300 CRC Standard Math Tables 25th Edition
 */

Vec
latlonrhoToCartesianVec(Vec a)
{
    return
    (
        sphericalToCartesianVec
        (
            latlonrhoTosphericalVec
            (
                a
            )
        )
    );
}

Vec
CartesianTolatlonrhoVec(Vec a)
{
	Vec4 b;
	
	b.CartesianTospherical(a);
	b.sphericalTolatlonrho(
	
    return
    (
        sphericalTolatlonrhoVec
        (
            CartesianTosphericalVec
            (
                a
            )
        )
    );
}

void Vec4::sphericalToCartesian(Vec4 a)
{

    vX = a.vRHO * cos(a.vTHETA) * sin(a.vPHI);
    vY = a.vRHO * sin(a.vTHETA) * sin(a.vPHI);
    vZ = a.vRHO                 * cos(a.vPHI);
    vW = 0;
}

void Vec4::CartesianTosphericalVec(Vec4 a)
{
    vRHO   = sqrt (dotVec(a, a));
    vPHI   = acos ( a.vZ/b.vRHO);
    vTHETA = atan2( a.vY, a.vX);
    vW     = 0;
}

void Vec4::sphericalTolatlonrho(Vec4 a)
{
    vLAT = MATH2_PI_2 - a.vPHI;
    vLON = a.vTHETA;
    vRHO = a.vRHO;
    vW   = 0;
}

void Vec4::latlonrhoTospherical(Vec4 a)
{
	vPHI   = MATH2_PI_2 - a.vLAT;
    vTHETA = a.vLON;
    vRHO   = a.vRHO;
    vW     = 0;
}

void Vec4::cylindricalToCartesian(Vec4 a)
{
    vX = a.vRHO*cos(a.vTHETA);
    vY = a.vRHO*sin(a.vTHETA);
    vZ = a.vZED;
    vW = 0;
}

void Vec4::CartesianTocylindrical(Vec4 a)
{
	vRHO   = sqrt(a.vX*a.vX + a.vY*a.vY);
    vTHETA = atan2(a.vY, a.vX);
    vZED   = a.vZ;
    vW     = 0;
}

Vec
mixThreeVec(Vec A, Vec B, Vec C, Vec Gains)
{
	return
	(
	 	addVec
	 	(
		 	 sclVec(Gains.vX, A),
			 addVec
		 	(
			 	 sclVec(Gains.vY, B),
				 sclVec(Gains.vZ, C)
			)
		)
	);

}


Vec
avgFourVec(Vec A, Vec B, Vec C, Vec D)
{
	
	return(sclVec(0.25, addVec(addVec(A, B), addVec(C, D))));
}


Vec
threePntsToPlaneEqn(a, b, c) Vec a, b, c;
{
    Vec plane;

    double x0, y0, z0, x1, y1, z1, x2, y2, z2;

    x0 = a.x[0];
    y0 = a.x[1];
    z0 = a.x[2];
    x1 = b.x[0];
    y1 = b.x[1];
    z1 = b.x[2];
    x2 = c.x[0];
    y2 = c.x[1];
    z2 = c.x[2];

    plane.x[0] = y0 * (z1 - z2) + y1 * (z2 - z0) + y2 * (z0 - z1);

    plane.x[1] = x0 * (z1 - z2) + x1 * (z2 - z0) + x2 * (z0 - z1);
    plane.x[1] = -(plane.x[1]);

    plane.x[2] = x0 * (y1 - y2) + x1 * (y2 - y0) + x2 * (y0 - y1);

    plane.x[3] = x0 * (y1 * z2 - y2 * z1) + x1 * (y2 * z0 - y0 * z2)
    + x2 * (y0 * z1 - y1 * z0);
    plane.x[3] = -(plane.x[3]);

    return(plane);
}

Vec
HSVToRGB(Vec hsv)
{
	double h, s, v, p, q, t, f;
	int i;
	Vec rgb;
	
	h = hsv.vX * 360.0;
	s = hsv.vY;
	v = hsv.vZ;
	
	if(hsv.vX == 360.0)
	{
		hsv.vX = 0.0;
	}
	
	h = h/60.0;
	i = floor(h);
	f = h - i;
	p = v *(1-s);
	q = v*(1 - (s*f));
	t = v*(1-(s*(1-f)));
	
	switch(i)
	{
		case 0: rgb = asnVec(v, t, p, 1.0); break;
		case 1: rgb = asnVec(q, v, p, 1.0); break;
		case 2: rgb = asnVec(p, v, t, 1.0); break;
		case 3: rgb = asnVec(p, q, v, 1.0); break;
		case 4: rgb = asnVec(t, p, v, 1.0); break;
		case 5: rgb = asnVec(v, p, q, 1.0); break;
		default: rgb = asnVec(1, 1, 1, 1.0); break;
	}
	rgb.vW = hsv.vW;
	
	return(rgb);
}

Vec fscanVec(FILE * f)
{
    Vec ret ;

	fscanf (f, "%*s %lf %lf %lf %lf\n", &ret.x[0], &ret.x[1], &ret.x[2], &ret.x[3]);

    return ret ;
}

void fprintVec (FILE * f, Vec inst)
{
	fprintf (f, "Vec: %9.4f %9.4f %9.4f %lg\n", inst.x[0], inst.x[1], inst.x[2], inst.x[3]);
}

Vec freadVec(FILE * f)
{
    Vec ret ;

    fread (&ret, sizeof(ret), 1, f) ;
    return ret ;
}

void fwriteVec (FILE * f, Vec inst)
{
    fwrite (&inst, sizeof(inst), 1, f) ;
}