/* 
** ---------------------------------------------------------------
** Algorithms.c - This module provides functions for 
** vectorial multi-resolution.
**
*/

/*
** ---------------------------------------------------------------
** Includes:
*/

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

#include "Algorithms.h"

#define veTrue 1;
#define veFalse 0;

/*
** ---------------------------------------------------------------
** Internal Functions and Structures:
*/

typedef struct
{
  float tangent_;
  char semiplan_; // 1 - Esquerdo; 2 - Direito  
} veAngle;

typedef struct
{
  unsigned char empty_;
  veAngle iang_;
  veAngle fang_;
} veCone;


veAngle veCreateAngle()
{
  veAngle angle;
  angle.tangent_ = 0;
  angle.semiplan_ = 1;
  return angle;
}

float veGetAngleTangent(veAngle* ang) 
{	
  return ang->tangent_; 
};

void veSetAngleTangent(veAngle* ang, float tangent) 
{ 
  ang->tangent_ = tangent;	
};

char veGetAngleSemiplan(veAngle* ang) 
{ 
  return ang->semiplan_; 
};

void veSetAngleSemiplan(veAngle* ang, char semiplan) 
{ 
  ang->semiplan_ = semiplan; 
};

unsigned char veAngleGreaterThan(veAngle ang, veAngle ang1)
{
  return (ang.semiplan_ > ang1.semiplan_) || 
		((ang.semiplan_ == ang1.semiplan_) && (ang.tangent_ > ang1.tangent_));
};

unsigned char veAngleBetween(veAngle ang, veAngle ang1, veAngle ang2)
{
  return ((!veAngleGreaterThan(ang1, ang2))) && ((!veAngleGreaterThan(ang1, ang))) && ((!veAngleGreaterThan(ang, ang2))) ||
		(veAngleGreaterThan(ang1, ang2) && (veAngleGreaterThan(ang, ang1) || veAngleGreaterThan(ang2, ang)));
};

void veAngleChangeSemiplan(veAngle* ang) 
{
  if(ang->semiplan_ == 1) ang->semiplan_ = 2;
  else if(ang->semiplan_ == 2) ang->semiplan_ = 1;
};

veCone veCreateEmptyCone()
{
  veCone cone;
  cone.empty_ = veTrue;
  return cone;
};

veCone veCreateCone(veAngle iang, veAngle fang)
{
  veCone cone;
  cone.iang_ = iang;
  cone.fang_ = fang;
  cone.empty_ = veFalse;
  return cone;
};

unsigned char veIsConeEmpty(veCone* cone)
{
  return cone->empty_;
};

void veSetConeEmpty(veCone* cone, unsigned char empty)
{
  cone->empty_ = empty;
};

veAngle veGetInitAngle(veCone* cone)
{
  return cone->iang_;
};

void veSetInitAngle(veCone* cone, veAngle angle)
{
  cone->iang_ = angle;
};

veAngle veGetFinAngle(veCone* cone)
{
  return cone->fang_;
};

void veSetFinAngle(veCone* cone, veAngle angle)
{
  cone->fang_ = angle;
};

veCone veConeIntersection(veCone cone1, veCone cone2)
{
  veCone result = cone1;
  veSetConeEmpty(&result, 1);

  if(veAngleBetween(veGetInitAngle(&cone2), veGetInitAngle(&result), veGetFinAngle(&result)))
  {
	veSetInitAngle(&result, veGetInitAngle(&cone2));
	veSetConeEmpty(&result, 0);
  }

  if(veAngleBetween(veGetFinAngle(&cone2), veGetInitAngle(&result), veGetFinAngle(&result)))
  {
	veSetFinAngle(&result, veGetFinAngle(&cone2));
	veSetConeEmpty(&result, 0);
  }

  if(veIsConeEmpty(&result))
  {
	if(veAngleBetween(veGetInitAngle(&cone1), veGetInitAngle(&cone2), veGetFinAngle(&cone2))) veSetConeEmpty(&result, 0);
	else if(veAngleBetween(veGetFinAngle(&cone1), veGetInitAngle(&cone2), veGetFinAngle(&cone2))) veSetConeEmpty(&result, 0);
  }

  return result;
};

veCone veCalculateCone(float x0, float y0, float x1, float y1, float error)
{
  float dx, dy, dist2, error2, tana, tanb, tanatanb, itan, ftan;
  veAngle iang, fang;
  char semiplan;

  dx = (x1-x0);
  dy = (y1-y0);
  dist2 = dx*dx + dy*dy;
  error2 = error*error;
  if(dist2 <= error2) return veCreateEmptyCone();

  if(dx != 0) tana = dy/dx;
  else  if(dy<0) tana = (float)999999999;
		else tana = (float)-999999999;

  tanb = error / ((float)sqrt(dist2 - error2));
  tanatanb = tana*tanb;

  if(dx < 0) semiplan = 2;
  else semiplan = 1;

  iang = veCreateAngle();
  fang = veCreateAngle();

  itan = (tana - tanb) / (1 + tanatanb);
  veSetAngleTangent(&iang, itan);
  veSetAngleSemiplan(&iang, semiplan);
  if((itan >= 0) && (tana < 0)) veAngleChangeSemiplan(&iang);

  ftan = (tana + tanb) / (1 - tanatanb);
  veSetAngleTangent(&fang, ftan);
  veSetAngleSemiplan(&fang, semiplan);
  if((ftan < 0) && (tana >= 0)) veAngleChangeSemiplan(&fang);

  return veCreateCone(iang, fang);
};

typedef struct
{
  unsigned char* data_;
  int buffer_size_;
  int blob_size_;
  int curr_pos_;
} veVertexArray;

void veInitializeVertexArray(veVertexArray* array)
{
  array->curr_pos_ = 0;
  array->buffer_size_ = 100;
  array->blob_size_ = 0;
  array->data_ = (unsigned char*)malloc(array->buffer_size_);
};

void veDestroyVertexArray(veVertexArray* array)
{
  free(array->data_);
};

void veClearVertexArray(veVertexArray* array)
{
  array->curr_pos_ = 0;
  array->blob_size_ = 0;
};

void veDoubleVertexArray(veVertexArray* array)
{
  array->buffer_size_ = array->buffer_size_ * 2;
  array->data_ = (unsigned char*)realloc(array->data_, array->buffer_size_);
};

void veWriteFloat(veVertexArray* array, float n)
{
  int s = sizeof(float);
  if(array->curr_pos_ + s > array->buffer_size_)
  {
	veDoubleVertexArray(array);
  }
  *((float*)(array->data_ + array->curr_pos_)) = n;
  array->curr_pos_ += s;
  if(array->curr_pos_ > array->blob_size_) array->blob_size_ = array->curr_pos_;
};

void veWriteVertex(veVertexArray* array, float x, float y)
{
  veWriteFloat(array, x);
  veWriteFloat(array, y);
};

/*
** ---------------------------------------------------------------
** Public Functions:
*/

veVertexArray vertexArray_;
float error_;
veCone currentCone_;
float x0_, y0_, lastX_, lastY_;
unsigned char first_;

void veSetError(float error)
{
  error_ = error;
};

void veInit()
{
  veInitializeVertexArray(&vertexArray_);
  currentCone_ = veCreateEmptyCone();
  first_ = 1;
};

void veAddPoint(float x, float y)
{
  veCone nextCone;

  if(first_)
  {
	x0_ = x;
	y0_ = y;
	veWriteVertex(&vertexArray_, x, y);
	first_ = 0;
  } else
  {
	nextCone = veCalculateCone(x0_, y0_, x, y, error_);
	if(veIsConeEmpty(&nextCone)) 
	{
	  lastX_ = x;
	  lastY_ = y;
	  return;
	}

	if(veIsConeEmpty(&currentCone_)) currentCone_ = nextCone;
	else currentCone_ = veConeIntersection(currentCone_, nextCone);

	if(veIsConeEmpty(&currentCone_))
	{
	  veWriteVertex(&vertexArray_, lastX_, lastY_);
	  x0_ = lastX_;
	  y0_ = lastY_;
	  currentCone_ = veCalculateCone(x0_, y0_, x, y, error_);
	}

	lastX_ = x;
	lastY_ = y;
  }
};

void veNoMorePoints()
{
  if(!first_) veWriteVertex(&vertexArray_, lastX_, lastY_);
};

void veGetVertices(float** vertices, int* size)
{
  *vertices = (float*)(vertexArray_.data_);
  *size = vertexArray_.blob_size_ / sizeof(float);
};

void veClear()
{
  veClearVertexArray(&vertexArray_);
  currentCone_ = veCreateEmptyCone();
  first_ = 1;
};

void veFinish()
{
  veDestroyVertexArray(&vertexArray_);
};