/*
Vector math, rays, and planes
*/

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

#include "vector.h"



/* debug: prints a 3d vector */
void vector_dump_3d(vector a)
{
 fprintf(stderr, "x: %f, y: %f, z: %f\n", a[0], a[1], a[2]);
}
void vector_dump_3d_nnl(vector a)
{
 fprintf(stderr, "x: %f, y: %f, z: %f", a[0], a[1], a[2]);
}


/* adds two 3d vectors and stores the result in the first argument */
inline void vector_add_3d(vector r, vector a, vector b)
{
 r[0] = a[0] + b[0];
 r[1] = a[1] + b[1];
 r[2] = a[2] + b[2];
}
inline void vector_add(vector r, vector a, vector b)
{
 r[0] = a[0] + b[0];
 r[1] = a[1] + b[1];
 r[2] = a[2] + b[2];
 r[3] = a[3] + b[3];
}


/* subtracts two 3d vectors */
inline void vector_sub_3d(vector r, vector a, vector b)
{
 r[0] = a[0] - b[0];
 r[1] = a[1] - b[1];
 r[2] = a[2] - b[2];
}


/* dot product of two vectors */
inline vectype vector_dotprod_3d(vector a, vector b)
{
 return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
}
/* dot product of two vectors, including the fourth value */
inline vectype vector_dotprod_4d(vector a, vector b)
{
 return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3];
}

/* length of a vector */
inline vectype vector_length_3d(vector a)
{
 return sqrt(vector_dotprod_3d(a, a));
}


/* multiply a vector by a scalar */
inline void vector_scale_3d(vector r, vector a, vectype b)
{
 r[0] = a[0] * b;
 r[1] = a[1] * b;
 r[2] = a[2] * b;
}
inline void vector_scale(vector r, vector a, vectype b)
{
 r[0] = a[0] * b;
 r[1] = a[1] * b;
 r[2] = a[2] * b;
 r[3] = a[3] * b;
}


/* normalize a vector.  as a bonus, returns the original length */
inline vectype vector_norm_3d(vector r, vector a)
{
 vectype s = vector_length_3d(a);
 vector_scale_3d(r, a, 1 / s);
 return s;
}
inline void vector_norm_w(vector a, vector b)
{
 a[0] = b[0] / b[3];
 a[1] = b[1] / b[3];
 a[2] = b[2] / b[3];
 a[3] = 1.0;
}


/* cross product */
inline void vector_crossprod_3d(vector r, vector a, vector b)
{
 r[0] = a[1]*b[2] - a[2]*b[1];
 r[1] = a[2]*b[0] - a[0]*b[2];
 r[2] = a[0]*b[1] - a[1]*b[0];
}

/* copy.  this likely could be improved with a memcpy()... */
inline void vector_copy(vector r, vector a)
{
 r[0] = a[0];
 r[1] = a[1];
 r[2] = a[2];
 r[3] = a[3];
}

/* copy limited to the first three */
inline void vector_copy_3d(vector r, vector a)
{
 r[0] = a[0];
 r[1] = a[1];
 r[2] = a[2];
/* r[3] = 1; */
}

/* returns |b-a| */
inline vectype vector_dist_3d(vector a, vector b)
{
 vector t;
 vector_sub_3d(t, b, a);
 return vector_length_3d(t);
}

inline void vector_invert_3d(vector r, vector a)
{
 r[0] = -a[0];
 r[1] = -a[1];
 r[2] = -a[2];
}

inline void vector_swap(vector a, vector b)
{
 vector t;
 vector_copy(t, a);
 vector_copy(a, b);
 vector_copy(b, t);
}


void vector_zerow(vector v)
{
 v[0] = v[1] = v[2] = 0.0;
 v[3] = 1.0;
}






/* debug display a ray */
inline void ray_dump_3d(ray_p a)
{
 fprintf(stderr, "Point: ");
 vector_dump_3d_nnl(a->point);
 fprintf(stderr, "   Direction: ");
 vector_dump_3d(a->dir);
}

/* assuming the direction is normalized, extend a ray to a given length */
inline void ray_extend_3d(vector p, ray_p r, vectype l)
{
 vector_scale_3d(p, r->dir, l);
 vector_add_3d(p, p, r->point);
}




/* debug display a plane */
inline void plane_dump(plane_p a)
{
 fprintf(stderr, "Point: ");
 vector_dump_3d(a->point);
 fprintf(stderr, "Normal: ");
 vector_dump_3d(a->normal);
}

inline void plane_eq_compute(vector r, vector p, vector n)
{
 vector_norm_3d(r, n);
 r[3] = - vector_dotprod_3d(r, p);
}

/* set the values of a plane, assuming a denormalized vector */
inline void plane_set(plane_p r, vector p, vector n)
{
 vector_copy_3d(r->point, p);
 plane_eq_compute(r->normal, p, n);
/*
 vector_norm_3d(r->normal, n);
 r->normal[3] = - vector_dotprod_3d(r->normal, r->point);
*/
}

inline vectype plane_eq_eval(vector n, vector p)
{
 return vector_dotprod_3d(n, p) + n[3];
}

inline vectype plane_eval(plane_p a, vector p)
{
 return plane_eq_eval(a->normal, p);
 /* return vector_dotprod_3d(a->normal, p) + a->normal[3]; */
}

/* intersect a ray with this plane.  optimize me! */
inline vectype plane_ints_ray_sided(vector a, vectype *b, plane_p p, ray_p r)
{
 vectype e = plane_eval(p, r->point);
 vectype t = -e / vector_dotprod_3d(p->normal, r->dir);
 ray_extend_3d(a, r, t);
 /* fprintf(stderr, "DEBUG: plane_ints_ray: Ray intersects plane on the %s at a distance of %f.\n", e>0?"front":"back", t); */
 *b = e;
 return t;
}

inline vectype plane_ints_ray(vector a, plane_p p, ray_p r)
{
 vectype e;
 return plane_ints_ray_sided(a, &e, p, r);
}




/* there has got to be a better way to do this! */
double drand()
{
 return ((double)rand()) / (((double)RAND_MAX) + ((double)1.0));
/* return ((double)rand()) / (((double)RAND_MAX) + ((double)1.0)) + ((double)rand()) / (((double)RAND_MAX) + ((double)1.0)) / (((double)RAND_MAX) + ((double)1.0)); */
}

double drandn()
{
 return drand() * 2.0 - 1.0;
}


inline vectype vtmax(vectype a, vectype b)
{
 return a>b?a:b;
}

inline vectype vtmin(vectype a, vectype b)
{
 return a<b?a:b;
}


inline vectype vtmax3(vectype a, vectype b, vectype c)
{
 if(a > b)
    if(a > c)
        return a;
       else
        return c;
   else
    if(b > c)
        return b;
       else
        return c;
}


inline vectype vtmin3(vectype a, vectype b, vectype c)
{
 if(a < b)
    if(a < c)
        return a;
       else
        return c;
   else
    if(b < c)
        return b;
       else
        return c;
}