#define _ISOC99_SOURCE			/* to get isfinite() */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <grass/gis.h>
#include "list.h"
#include "mapcalc.h"
#include "vector.h"

typedef struct Vecfunc
{
  char *name;
  void *func;
  char *proto;
}VECFUNC;

static VECFUNC vf[] =
{
  { "v_copy", v_copy, "p=rp" },
  { "v_add", v_add, "p=rpp" },
  { "pnt_op_func_+", v_add, "p=rpp" },
  { "v_sub", v_sub, "p=rpp" },
  { "pnt_op_func_-", v_sub, "p=rpp" },
  { "v_abs", v_abs, "p=rp" },
  { "v_neg", v_neg, "p=rp" },
  { "pnt_op_func__", v_neg, "p=rp" },
  { "v_mul", v_mul, "p=rpd" },
  { "pnt_op_func_*", v_mul, "p=rpd" },
  { "v_div", v_div, "p=rpd" },
  { "pnt_op_func_/", v_div, "p=rpd" },
  { "v_unit", v_unit, "p=rp" },
  { "v_cross", v_cross, "p=rpp" },
  { "pnt_op_func_^", v_cross, "p=rpp" },
  { "v_val", v_val, "d=p" },
  { "v_dot", v_dot, "d=pp" },
  { "pnt_op_func_%", v_dot, "d=pp" },
  { "v_area", v_area, "d=pp" },
  { "v_eq", v_eq, "d=pp" },
  { "v_eq_epsilon", v_eq_epsilon, "d=ppp" },
  { "v_isortho", v_isortho, "d=pp" },
  { "v_ispara", v_ispara, "d=pp" },
  { "v_isacute", v_isacute, "d=pp" },
  { NULL, NULL, NULL }
};

typedef VECTOR *(*p_func)(void);
typedef VECTOR *(*p_func_p)(void *p0);
typedef VECTOR *(*p_func_pp)(void *p0, void *p1);
typedef VECTOR *(*p_func_ppp)(void *p0, void *p1, void *p2);
typedef VECTOR *(*p_func_ppd)(void *p0, void *p1, double d);

double nanval;
static VECTOR pnt_o = { NULL, 0, 0, 0, 1};
static VECTOR pnt_i = { NULL, 1, 0, 0, 1};
static VECTOR pnt_j = { NULL, 0, 1, 0, 1};
static VECTOR pnt_k = { NULL, 0, 0, 1, 1};

void init_vec (void);
void printvec (SYMBOL *sym);
void showvec (SYMBOL *sym);
void setpnt (SYMBOL *var, SYMBOL *pnt);
SYMBOL *mkpnt (double x, double y, double z);
SYMBOL *mkpntvar (SYMBOL *var, SYMBOL *pnt);
SYMBOL *pntfunc (SYMBOL *func, SYMBOL *arglist);
SYMBOL *pntop (int op, SYMBOL *pnt1, SYMBOL *pnt2);
SYMBOL *pntapp (SYMBOL *head, SYMBOL *elt);
VECTOR *v_copy (VECTOR *p, VECTOR *p1);
VECTOR *v_add (VECTOR *p, VECTOR *p1, VECTOR *p2);
VECTOR *v_sub (VECTOR *p, VECTOR *p1, VECTOR *p2);
VECTOR *v_abs (VECTOR *p, VECTOR *p1);
VECTOR *v_neg (VECTOR *p, VECTOR *p1);
static inline int _is_zero (double r);
double v_eq (VECTOR *p1, VECTOR *p2);
double v_eq_epsilon (VECTOR *p1, VECTOR *p2, VECTOR *e);
VECTOR *v_mul (VECTOR *p, VECTOR *p1, double d);
VECTOR *v_div (VECTOR *p, VECTOR *p1, double d);
double v_val (VECTOR *p);
VECTOR *v_unit (VECTOR *p, VECTOR *p1);
double v_dot (VECTOR *p1, VECTOR *p2);
VECTOR *v_cross (VECTOR *p, VECTOR *p1, VECTOR *p2);
double v_isortho (VECTOR *p1, VECTOR *p2);
double v_ispara (VECTOR *p1, VECTOR *p2);
double v_isacute (VECTOR *p1, VECTOR *p2);
double v_area (VECTOR *p1, VECTOR *p2);


void
init_vec (void)
{
  SYMBOL *sym;
  int i;

  for (i = 0; vf[i].name; i++)
  {
    sym = putsym (vf[i].name);
    switch (vf[i].proto[0])
    {
      case 'p':
	sym->type = st_pfunc;
	sym->rettype = st_pnt;
	break;
      case 'd':
	sym->type = st_nfunc;
	sym->rettype = st_num;
	break;
    }
    sym->itype = sym->type;
    sym->v.p = vf[i].func;
    sym->proto = vf[i].proto + 2;
  }

  /* add some handy constants */
  sym = putsym ("pnt_o");
  sym->type = sym->itype = st_pnt;
  sym->v.p = &pnt_o;

  sym = putsym ("pnt_i");
  sym->type = sym->itype = st_pnt;
  sym->v.p = &pnt_i;

  sym = putsym ("pnt_j");
  sym->type = sym->itype = st_pnt;
  sym->v.p = &pnt_j;

  sym = putsym ("pnt_k");
  sym->type = sym->itype = st_pnt;
  sym->v.p = &pnt_k;

  /* initialize NaN */
  nanval = sqrt (-1);
}

void
printvec (SYMBOL *sym)
{
  VECTOR *v;

  v = (VECTOR *)sym->v.p;

  printf ("\t(");
  if (!isfinite (v->x))
    printf ("??.??");
  else if (v->x == (int)v->x)
    printf ("%d", (int)v->x);
  else
    printf ("%g", v->x);
  printf (", ");
  if (!isfinite (v->y))
    printf ("??.??");
  else if (v->y == (int)v->y)
    printf ("%d", (int)v->y);
  else
    printf ("%g", v->y);
  if (isfinite (v->z))
  {
    printf (", ");
    if (v->z == (int)v->z)
      printf ("%d", (int)v->z);
    else
      printf ("%g", v->z);
  }
  printf (")\n");
}

void
showvec (SYMBOL *sym)
{

  VECTOR *v;

  v = (VECTOR *)sym->v.p;
  printvec (sym);

  if (v && --v->refcnt > 0)
    sym->v.p = NULL;
  freesym (sym);
}

void
setpnt (SYMBOL *var, SYMBOL *pnt)
{
  SYMBOL *sym;

  if (var->name)
  {
    sym = getsym (var->name);
    if (sym)
    {
      if (--((VECTOR *)sym->v.p)->refcnt < 1)
	G_free (sym->v.p);
      /*
       * If refcnt(pnt) == 1, this was anonymous, else it's used
       * somewhere else. Must we dup then?
       */
      sym->v.p = pnt->v.p;
    }
  }

  if (--((VECTOR *)var->v.p)->refcnt < 1)
    G_free (var->v.p);
  var->v.p = NULL;
  freesym (var);

  printvec (pnt);
  pnt->v.p = NULL;
  freesym (pnt);
}

SYMBOL *
mkpnt (double x, double y, double z)
{
  SYMBOL *pnt;
  VECTOR *vec;

  vec = (VECTOR *)listitem (sizeof (VECTOR));
  vec->x = x;
  vec->y = y;
  vec->z = z;
  vec->refcnt = 1;

  pnt = (SYMBOL *)listitem (sizeof (SYMBOL));
  pnt->type = pnt->itype = st_pnt;
  pnt->v.p = vec;

  return pnt;
}

SYMBOL *
mkpntvar (SYMBOL *var, SYMBOL *pnt)
{
  var->type = var->itype = st_pnt;
  var->name = var->v.p;
  var->v.p = pnt->v.p;
  pnt->v.p = NULL;
  freesym (pnt);

  symtab = (SYMBOL *)listadd ((LIST *)symtab, (LIST *)var, cmpsymsym);

  printvec (var);

  return var;
}

SYMBOL *
pntfunc (SYMBOL *func, SYMBOL *arglist)
{
  SYMBOL *sym, *sptr;
  VECTOR *res = NULL;
  int argc = -1, i;

  sym = (SYMBOL *)listitem (sizeof (SYMBOL));
  sym->type = sym->itype = st_pnt;

  if (!func || !func->v.p || func->type != st_pfunc)
  {
    parseerror = 1;
    printf ("Can't call bad function\n");
  }
  else
    argc = listcnt ((LIST *)arglist);

  for (i = 0, sptr = arglist; sptr; sptr = sptr->next, i++)
  {
    if (func->proto[i] == 'r')
      i++;
    if (func->proto[i] == 'p')
    {
      if (sptr->itype != st_pnt || !sptr->v.p)
	argc = -1;
    }
    else if (func->proto[i] == 'd' && sptr->itype != st_num)
      argc = -1;
  }

  res = (VECTOR *)listitem (sizeof (VECTOR));

  if (argc == 0 && (!func->proto || !*func->proto))
    res = (*(p_func)func->v.p)();
  else if (argc == 1 && !strcmp (func->proto, "p"))
    res = (*(p_func_p)func->v.p)(arglist->v.p);
  else if (argc == 1 && !strcmp (func->proto, "rp"))
    res = (*(p_func_pp)func->v.p)(res, arglist->v.p);
  else if (argc == 2 && !strcmp (func->proto, "rpd"))
    res = (*(p_func_ppd)func->v.p)(res, arglist->v.p,
				  arglist->next->v.d);
  else if (argc == 2 && !strcmp (func->proto, "pp"))
    res = (*(p_func_pp)func->v.p)(arglist->v.p,
				  arglist->next->v.p);
  else if (argc == 2 && !strcmp (func->proto, "rpp"))
    res = (*(p_func_ppp)func->v.p)(res, arglist->v.p,
				  arglist->next->v.p);
  else if (argc == 3 && !strcmp (func->proto, "ppp"))
    res = (*(p_func_ppp)func->v.p)(arglist->v.p,
				   arglist->next->v.p,
				   arglist->next->next->v.p);
  else
  {
    printf ("Bad arguments to pointfunc %s\n", func->name);
    parseerror = 1;
    sym = (SYMBOL *)listdelall ((LIST *)sym, freesym);
    if (res)
      G_free (res);
    return NULL;
  }
  sym->v.p = res;

  listdelall ((LIST *)arglist, freesym);

  return sym;
}

SYMBOL *
pntop (int op, SYMBOL *pnt1, SYMBOL *pnt2)
{
  SYMBOL *func, *arglist, *res = NULL;
  char buf[32];

  sprintf (buf, "pnt_op_func_%c", op);

  func = getsym (buf);
  if (!func)
  {
    printf ("No function defined to perform ``point %c point''\n", op);
    parseerror = 1;
  }
  else if (func->rettype == st_pnt)
  {
    res = (SYMBOL *)listitem (sizeof (SYMBOL));
    symcpy (res, func);
    res->next = NULL;
    func = res;
    arglist = (SYMBOL *)listapp (NULL, (LIST *)pnt1);
    arglist = (SYMBOL *)listapp ((LIST *)arglist, (LIST *)pnt2);

    res = pntfunc (func, arglist);
  }

  return res;
}

SYMBOL *
pntapp (SYMBOL *head, SYMBOL *elt)
{
  return (SYMBOL *)listapp ((LIST *)head, (LIST *)elt);
}

/*
 * Utility function to copy a point: p = p1;
 * The dimension (2D/3D) depends on p1. Note, that copying a constant
 * will always yield 3D.
 */
VECTOR *
v_copy (VECTOR *p, VECTOR *p1)
{
  p->x = p1->x;
  p->y = p1->y;
  p->z = p1->z;

  return p;
}

/*
 * Vector addition
 * Result is 2D if at least one of p1 or p2 is 2D.
 */
VECTOR *
v_add (VECTOR *p, VECTOR *p1, VECTOR *p2)
{
  p->x = p1->x + p2->x;
  p->y = p1->y + p2->y;
  /*
   * resist the tentation setting p->z to nanval and then testing for
   * dimension, as p might be the same as p1 or p2
   */
  if (!isnan (p1->z) && !isnan (p2->z))
    p->z = p1->z + p2->z;
  else
    p->z = nanval;

  return p;
}

/*
 * Vector substraction
 * Result is 2D if at least one of p1 or p2 is 2D.
 */
VECTOR *
v_sub (VECTOR *p, VECTOR *p1, VECTOR *p2)
{
  p->x = p1->x - p2->x;
  p->y = p1->y - p2->y;
  if (!isnan (p1->z) && !isnan (p2->z))
    p->z = p1->z + p2->z;
  else
    p->z = nanval;

  return p;
}

/*
 * Utility function to make all coordinates positive
 */
VECTOR *
v_abs (VECTOR *p, VECTOR *p1)
{
  p->x = fabs (p1->x);
  p->y = fabs (p1->y);
  if (!isnan (p1->z))
    p->z = fabs (p1->z);
  else
    p->z = nanval;

  return p;
}

/*
 * Utility function to negate all coordinates
 */
VECTOR *
v_neg (VECTOR *p, VECTOR *p1)
{
  p->x = -p1->x;
  p->y = -p1->y;
  if (!isnan (p1->z))
    p->z = -p1->z;
  else
    p->z = nanval;

  return p;
}

/*
 * Utility function to compare two doubles for equality without epsilon
 * This is not really true, as we consider NaN to be zero.
 */
static inline int
_is_zero (double r)
{
  if ((isfinite (r) && r == 0.0) || isnan (r))
    return 1;
  return 0;
}

/*
 * Test for equality of two points. No epsion applied
 */
double
v_eq (VECTOR *p1, VECTOR *p2)
{
  VECTOR p;
  int dim = 2;

  if (!isnan (p1->z) && !isnan (p2->z))
    dim = 3;

  v_sub (&p, p1, p2);
  v_abs (&p, &p);
  if (_is_zero (p.x) && _is_zero (p.y) && (dim == 2 || _is_zero (p.z)))
    return 1;
  return 0;
}

/*
 * Test for equality of two points by a given epsilon
 * epsilon is supposed to have positive values only.
 */
double
v_eq_epsilon (VECTOR *p1, VECTOR *p2, VECTOR *e)
{
  VECTOR p;
  int dim = 2;

  if (!isnan (p1->z) && !isnan (p2->z))
    dim = 3;

  v_sub (&p, p1, p2);
  v_abs (&p, &p);
  if (p.x < e->x && p.y < e->y && (dim == 2 || (p.z < e->z)))
    return 1;
  return 0;
}

/*
 * Multiply a vector by a scalar
 */
VECTOR *
v_mul (VECTOR *p, VECTOR *p1, double d)
{
  p->x = d * p1->x;
  p->y = d * p1->y;
  if (!isnan (p1->z))
    p->z = d * p1->z;
  else
    p->z = nanval;

  return p;
}

/*
 * Divide a vector by a scalar
 */
VECTOR *
v_div (VECTOR *p, VECTOR *p1, double d)
{
  if (!isfinite (d) || d == 0.0)
  {
    parseerror = 1;
    return p;
  }
  p->x = p1->x / d;
  p->y = p1->y / d;
  if (!isnan (p1->z))
    p->z = p1->z / d;
  else
    p->z = nanval;

  return p;
}

/*
 * Compute the value of a vector
 */
double
v_val (VECTOR *p)
{
  return sqrt (p->x * p->x + p->y * p->y + ((isnan (p->z)) ? 0 : p->z * p->z));
}

/*
 * The only way to get a value of zero is that P1 is the origin.
 * The unit vector of the origin doesn't exist, but we return the
 * origin.
 */
VECTOR *
v_unit (VECTOR *p, VECTOR *p1)
{
  double val = v_val (p1);

  if (_is_zero (val))
    return v_copy (p, &pnt_o);

  p->x = p1->x / val;
  p->y = p1->y / val;
  if (!isnan (p1->z))
    p->z = p1->z / val;
  else
    p->z = nanval;

  return p;
}

/*
 * Compute the dot product of P1 and P2
 */
double
v_dot (VECTOR *p1, VECTOR *p2)
{
  int dim = 2;

  if (!isnan (p1->z) && !isnan (p2->z))
    dim = 3;
  return p1->x * p2->x + p1->y * p2->y + ((dim == 2) ? 0 : p1->z * p2->z);
}

/*
 * Compute the cross product of P1 and P2
 * Return (0,0) for 2D
 */

VECTOR *
v_cross (VECTOR *p, VECTOR *p1, VECTOR *p2)
{
  VECTOR p0;

  if (!isnan (p1->z) && !isnan (p2->z))
  {
    p0.x = p1->y * p2->z + p1->z * p2->y;
    p0.y = p1->z * p2->x + p1->x * p2->z;
    p0.z = p1->x * p2->y + p1->y * p2->x;
    v_copy (p, &p0);
  }
  else
  {
    p->x = p->y = 0;
    p->z = nanval;
  }

  return p;
}

/*
 * Decide if vector P1 is ortogonal to vector P2
 * Should test if either P1 or P2 are (0,0,0);
 */
double
v_isortho (VECTOR *p1, VECTOR *p2)
{
  return v_dot (p1, p2) == 0;
}

/*
 * Decide if P1 and P2 are parallel. If they are but have a diferent
 * direction, -1 is returned.
 */
double
v_ispara (VECTOR *p1, VECTOR *p2)
{
  double dot, val, dif;

  dot = v_dot (p1, p2);
  val = v_val (p1);
  val *= v_val (p2);

  dif = fabs (dot - val);
  if (_is_zero (dif))
    return 1;

  dif = fabs (dot + val);
  if (_is_zero (dif))
    return -1;

  return 0;
}

/*
 * Decide if P1 and P2 have and angle alpha which: 0 < alpha < 90.0
 */
double
v_isacute (VECTOR *p1, VECTOR *p2)
{
  return v_dot (p1, p2) > 0;
}

/*
 * Return the area spanned by the two vectors P1 and P2
 * Works only in 3D.
 */
double
v_area (VECTOR *p1, VECTOR *p2)
{
  VECTOR p;

  return 0.5 * v_val (v_cross (&p, p1, p2));
}