/**
 * \file geometry.c
 * \brief implementation of some geometrical functions
 *
 *
 * \ingroup corefiles
 * \date Apr 20, 2009
 * \author Alexander Pimenov
 * \author part of the code comes form wikipedia
 */
#include "global.h"

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

#include "geometry.h"


#define LEFT_CODE     (0x1)
#define RIGHT_CODE    (0x2)
#define TOP_CODE      (0x4)
#define BOTTOM_CODE   (0x8)


#define GET_CODE(p)                       \
	((((p)->x < x1) ? LEFT_CODE   : 0)  | \
	 (((p)->x > x2) ? RIGHT_CODE  : 0)  | \
	 (((p)->y < y1) ? BOTTOM_CODE : 0)  | \
	 (((p)->y > y2) ? TOP_CODE    : 0))


/**
 * \brief This function clips a line to a rectangular window.
 *
 * This function uses the popular Cohen-Sutherland algorithm.
 * See http://en.wikipedia.org/wiki/Cohen-Sutherland for details
 *
 * \author this code is based on code from wikipedia
 *
 *
 * \param x1 the X coordinate of the first border of the rectangular
 * \param y1 the Y coordinate of the first border of the rectangular
 * \param x2 the X coordinate of the second border of the rectangular
 * \param y2 the Y coordinate of the second border of the rectangular
 *
 * \param[in,out] a on enter - the start of the line, on exit - the start of the clipped line.
 * \param[in,out] b on enter - the end of the line, on exit - the end of the clipped line.
 *
 * \return CLIP_HAD_INTERSECTIONS if there was an intersection
 *   	  CLIP_NO_INTERSECTIONS otherwise
 */

int clipCohenSutherland ( int16_t x1, int16_t y1, int16_t x2, int16_t y2, vector2Ds16 *a, vector2Ds16 *b)
{
	uint8_t code_a, code_b, code; /* codes of the ends */
	vector2Ds16 *c;

	// (0,0)-(821,617) : (-3,-1) : (0,1)
#ifdef DEEP_TRACE
	printf("(%d,%d)-(%d,%d) : (%d,%d) : (%d,%d)\n", x1, y1, x2, y2, a->x, a->y, b->x, b->y);
	fflush(stdout);
#endif
	int16_t tmp;
	if (x1 > x2) { tmp = x1; x1 = x2; x2 = tmp; };
	if (y1 > y2) { tmp = y1; y1 = y2; y2 = tmp; };

	code_a = GET_CODE(a);
	code_b = GET_CODE(b);

	/* while there are outliers */
	while (code_a || code_b) {
		/* check for no intersection */
		if (code_a & code_b)
			return CLIP_NO_INTERSECTIONS;

		/* take one point */
		if (code_a) {
			code = code_a;
			c = a;
		} else {
			code = code_b;
			c = b;
		}

		if (code & LEFT_CODE) {
			c->y += (a->y - b->y) * (x1 - c->x) / (a->x - b->x);
			c->x = x1;
		} else if (code & RIGHT_CODE) {
			c->y += (a->y - b->y) * (x2 - c->x) / (a->x - b->x);
			c->x = x2;
		}
		if (code & BOTTOM_CODE) {
			c->x += (a->x - b->x) * (y1 - c->y) / (a->y - b->y);
			c->y = y1;
		} else if (code & TOP_CODE) {
			c->x += (a->x - b->x) * (y2 - c->y) / (a->y - b->y);
			c->y = y2;
		}

		if (code == code_a)
			code_a = GET_CODE(a);
		else
			code_b = GET_CODE(b);
	}

#ifdef ASSERT
	   if (a->x < x1 || a->x > x2 || a->y < y1 || a->y > y2)
		   DOASSERT("OOPS1");
	   if (b->x < x1 || b->x > x2 || b->y < y1 || b->y > y2)
		   DOASSERT("OOPS2");
#endif

	return CLIP_HAD_INTERSECTIONS;
}

/**
 * \brief This function implements the vector cross product.
 * It is computed as follows:
 *
 * \code
 *  [  i   j   k  ]
 *  [ a.x a.y a.z ]
 *  [ b.x b.y b.z ]
 * \endcode
 *
 * \param[in] a the first vector
 * \param[in] b the second vector
 *
 * \return vector a x b
 */
vector3Dd crossProduct(vector3Dd *a, vector3Dd *b)
{
	vector3Dd toReturn;
	toReturn.x = + a->y * b->z - a->z * b->y;
	toReturn.y = - a->x * b->z + a->z * b->x;
	toReturn.z = + a->x * b->y - a->y * b->x;
	return toReturn;
}

/**
 * \brief This function implements the vector dot product.
 * It is computed as follows:
 *
 * \f[
 *  a_x b_x + a_y b_y + a_z b_z
 * \f]
 *
 * \param[in] a the first vector
 * \param[in] b the second vector
 *
 * \return vector a . b
 */

double dotProduct(vector3Dd *a, vector3Dd *b)
{
	return a->x * b->x + a->y * b->y + a->z * b->z;
}

/**
 * \brief This function normalizes the 3d vector
 * It is computed as follows:
 *
 * \f[
 *  v_{result} = \frac v {|v|}
 * \f]
 *
 * \param[in, out] v the vector to normalize
 * \return normalized vector
 */
void vectorNormalize(vector3Dd *a)
{
	double normSquare = a->x * a->x + a->y * a->y  + a->z * a->z;
	if (normSquare == 0)
		return;
	double norm = sqrt(normSquare);
	a->x /= norm;
	a->y /= norm;
	a->z /= norm;
}

void vectorHomogeniousNormalize(vector3Dd *a)
{
	if (a->z == 0)
		return;
	a->x /= a->z;
	a->y /= a->z;
	a->z = 1;
}

/**
 * \brief This function prints the vector to stdout
 *
 * \param[in] v vector to print
 */
void vectorPrint(vector3Dd *v)
{
	printf("[%lg : %lg : %lg ]\n", v->x, v->y, v->z);
}

vector3Dd vectorCreate(double x, double y, double z)
{
	vector3Dd toReturn = {x,y,z};
	return toReturn;
}

