/*
   File:        cones.c
   Author:      Andrew W. Moore
   Created:     Fri May 14 16:33:01 EDT 1993
   Description: Operations on 2d cones and lists thereof

   Copyright (C) 1992, Andrew W. Moore
*/

#include <stdio.h>
#include <math.h>
#include "ambs.h"      /* Very basic operations */
#include "amgr.h"      /* Basic (0,512)x(0,512) Graphics window */
#include "amma.h"      /* Fast, non-fragmenting, memory management */
#include "amar.h"      /* Obvious operations on 1-d arrays */
#include "geom.h"      /* Simple 2-d geometry structures and operations */
#include "contours.h"  /* Drawing contours of functions and 2d arrays */
#include "hilldyn.h"   /* Dynamics constrained to surface of (N-1)d hill */
#include "cones.h"     /* Operations on 2d cones and lists thereof */

#ifndef FLOAT_FUNCTION_DEFINED
typedef float (*float_function)();
#define FLOAT_FUNCTION_DEFINED
#endif

#define FOUR 4

float Cone_radius = -77.77;
float Cone_height = -77.77;

float eval_line(ln,p)
line *ln;
point *p;
{
  float d = distance_lp(ln,p);
  float result;

  if ( d > Cone_radius )
    result = 0.0;
  else
    result = Cone_height * (1.0 - d / Cone_radius);

  return(result);
}

float eval_lines(ls,p)
lines *ls;
point *p;
{
  return(eval_line(nearest_line_to_point(ls,p),p));
}

float line_derivative(ln,p,rdx,rdy)
line *ln;
point *p;
float *rdx,*rdy;
{
  float d = distance_lp(ln,p);

  if ( d > Cone_radius )
  {
    *rdx = 0.0;
    *rdy = 0.0;
  }
  else
  {
    point np;
    nearest_point_on_line(ln,p,&np);
    d = real_max(d,1e-4);
    *rdx = (Cone_height / Cone_radius) * (np.x - p->x) / d;
    *rdy = (Cone_height / Cone_radius) * (np.y - p->y) / d;
/*
    printf("*rdy = %g\n",*rdy);
    printf("Cone_height = %g\n",Cone_height);
    printf("Cone_radius = %g\n",Cone_radius);
    printf("np.y = %g\n",np.y);
    printf("p->y = %g\n",p->y);
    printf("d = %g\n",d);
    wait_for_key();
*/
  }
}

void lines_derivative(ls,p,rdx,rdy)
lines *ls;
point *p;
float *rdx,*rdy;
{
  line_derivative(nearest_line_to_point(ls,p),p,rdx,rdy);
}

float line_height_function(x,y,data)
float x,y;
char *data;
{
  lines *ls = (lines *) data;
  point p;
  point_from_spec(x,y,&p);
  return(eval_lines(ls,&p));
}

lines *cone_contours(gb,ls,lo_height,hi_height,parts,draw_parts)
geom_base *gb;
lines *ls;
float lo_height,hi_height;
int parts,draw_parts;
/*
   Returns a list of lines. These are represent contours of the function
   invoked by the function of sum of lineians in line list. The input
   domain is denoted by geom base. There are (parts+1) levels of contours,
   equally spaced with the lowest at lo_height and the highest at hi_height.

   The output lines are pre-scaled to fit on the standard ag_on("")
   graphics window, and can be drawn with the function draw_lines(ls)

   Don't forget to free the lines when you're through with them!
*/
{
  lines *con_ls;
  conaxes cax;

  init_caxis(&cax.x, gb->bottom_left.x , gb->top_right.x , draw_parts);
  init_caxis(&cax.y, gb->bottom_left.y , gb->top_right.y , draw_parts);
  init_caxis(&cax.z, lo_height         , hi_height       , parts);

  con_ls = find_function_contours(&cax,line_height_function,(char *)ls);
  return(con_ls);
}

void draw_cone_contours(gb,ls,lo_height,hi_height,parts,draw_parts)
geom_base *gb;
lines *ls;
float lo_height,hi_height;
int parts,draw_parts;
{
  lines *con_ls = cone_contours(gb,ls,lo_height,hi_height,parts,draw_parts);
  draw_lines(con_ls);
  free_lines(con_ls);
}

float hilldyn_line_fn(x,dim,hfd)
float *x;
int dim;
char *hfd;
{
  if ( dim != 2 )
    my_error("iwuvb");
  else
  {
    point p;
    point_from_spec(x[0],x[1],&p);
    return(eval_lines((lines *) hfd,&p));
  }
}

void hilldyn_line_derivative_fn(x,dim,hfd,gradx)
float *x;
int dim;
char *hfd;
float *gradx;
{
  if ( dim != 2 )
    my_error("iwuvb");
  else
  {
    point p;
    float dx,dy;

    point_from_spec(x[0],x[1],&p);
    lines_derivative((lines *) hfd,&p,&dx,&dy);
    gradx[0] = dx;
    gradx[1] = dy;
  }
}

void make_cone_hilldyn(hd,ls,friction,mass,max_move_dist,g)
hilldyn *hd;
lines *ls;
float friction,mass,max_move_dist,g;
{
  hd -> dim = 2;
  hd -> height_fn = (float_function) hilldyn_line_fn;
  hd -> height_derivative_fn = hilldyn_line_derivative_fn;
  hd -> height_fn_data = (char *) ls;
  hd -> friction = friction;
  hd -> g = g;
  hd -> mass = mass;
  hd -> max_move_dist = max_move_dist;
  hd -> max_pos = 0.99;
  hd -> max_vel = 0.3;
}

void cone_hilldyn_next_state(hd,xstate,f,h,xnext)
hilldyn *hd;
float *xstate; /* (2 * dim) - dimensional */
float *f;      /* (dim + 1) - dimensional */
float h;
float *xnext;
{
  hilldyn_next_state(hd,xstate,f,h,xnext);

  if ( Verbosity > 10.0 )
    fprintf_floats(stdout,"xnext b4 = ",xnext,FOUR,"\n");

  if ( Verbosity > 10.0 )
    fprintf_floats(stdout,"xnext = ",xnext,FOUR,"\n");
}

void animate_cone_hilldyn(gb,hd,x,f,time_step,steps)
geom_base *gb;
hilldyn *hd;
float *x; /* 4-d */
float *f; /* 3-d */
float time_step;
int steps;
{
  float xnext[FOUR];
  point p;
  point_from_spec(x[0],x[1],&p);
  graphics_point(gb,&p);

  for ( ; steps > 0 ; steps-- )
  {
    cone_hilldyn_next_state(hd,x,f,time_step,xnext);

    if ( Verbosity > 100.0 )
    {
      fprintf_floats(stdout,"",x,FOUR,"");
      fprintf_floats(stdout," x ",f,3,"");
      fprintf_floats(stdout," ->\n            ",xnext,FOUR,"\n");
    }

    copy_floats(xnext,x,FOUR);

    point_from_spec(x[0],x[1],&p);
    graphics_point(gb,&p);
  }
}
    
lines *edit_lines(gb,ls)
geom_base *gb;
lines *ls;
{
  extern lines *edit_lines_with_mouse();
  ls = edit_lines_with_mouse(gb,ls);
  return(ls);
}

/* Simple association lists */

alist *alist_lookup_node(al,key)
alist *al;
char *key;
{
  alist *result = NULL;

  for ( ; result == NULL && al != NULL ; al = al -> next )
    if ( eq_string(key,al->key) )
      result = al;

  return(result);
}

bool alist_member(al,key)
alist *al;
char *key;
{
  return(alist_lookup_node(al,key) != NULL);
}

float alist_lookup(al,key)
alist *al;
char *key;
{
  alist *al2 = alist_lookup_node(al,key);
  if ( al2 == NULL ) my_error("Alist lookup failed\n");
  return(al2->value);
}

alist *alist_add(al,key,value)
alist *al;
char *key;
float value;
{
  alist *result;
  alist *al2 = alist_lookup_node(al,key);
  if ( al2 == NULL )
  {
    alist *new = AM_MALLOC(alist);
    new -> key = key;
    new -> value = value;
    new -> next = al;
    result = new;
  }
  else
  {
    al2 -> value = value;
    result = al;
  }
  return(result);
}

void fprintf_alist(s,m1,al,m2)
FILE *s;
char *m1;
alist *al;
char *m2;
{
  if ( al == NULL )
    fprintf(s,"%s(alist *)NULL%s",m1,m2);
  else
    for ( ; al != NULL ; al = al -> next )
      fprintf(s,"%s%20s %9g%s",m1,al->key,al->value,m2);
}

alist *line_cli_values()
{
  alist *al = NULL;

  al = alist_add(al,"cone_radius",0.3);
  al = alist_add(al,"cone_height",10.0);
  al = alist_add(al,"lo_height",0.0);
  al = alist_add(al,"parts",10.0);
  al = alist_add(al,"draw_parts",29.0);
  al = alist_add(al,"friction",0.01);
  al = alist_add(al,"mass",1.0);
  al = alist_add(al,"max_move_dist",0.005);
  al = alist_add(al,"g",15.0);
  al = alist_add(al,"time_step",0.03);
  al = alist_add(al,"steps",50.0);

  return(al);
}

lines *lines_cli(gb,ls)
geom_base *gb;
lines *ls;
{
  alist *al = line_cli_values();
  char s[100];

  sprintf(s,"");

  while ( !eq_string(s,"qu") )
  {
    Cone_radius = alist_lookup(al,"cone_radius");
    Cone_height = alist_lookup(al,"cone_height");
    (void) input_string("line> ",s,100);
    s[2] = '\0';
    if ( eq_string(s,"he") )
    {
      printf("he - help\n");
      printf("se <paramname> <value> - set a parameter\n");
      printf("ls - list parameters\n");
      printf("ed - edit line list\n");
      printf("sa - save line list\n");
      printf("dc - draw contours of line list\n");
      printf("hd - simulate hill dynamics of line list\n");
    }
    if ( eq_string(s,"se") )
    {
      char pname[100];
      (void) input_string("Param name> ",pname,100);
      al = alist_add(al,pname,input_float("Value> "));
      Cone_radius = alist_lookup(al,"cone_radius");
      Cone_height = alist_lookup(al,"cone_height");
    }
    if ( eq_string(s,"ls") )
    {
      fprintf_alist(stdout,"",al,"\n");
    }
    if ( eq_string(s,"ed") )
    {
      ls = edit_lines(gb,ls);
    }
    if ( eq_string(s,"sa") )
    {
      char fname[100];
      (void) input_string("Filename> ",fname,100);
      save_lines(ls,fname);
    }
    if ( eq_string(s,"dc") )
    {
      ag_on("");
      draw_cone_contours(gb,ls,alist_lookup(al,"lo_height"),
                                 alist_lookup(al,"cone_height"),
                                 (int) alist_lookup(al,"parts"),
                                 (int) alist_lookup(al,"draw_parts")
                         );
      ag_off();
    }
    if ( eq_string(s,"hd") )
    {
      hilldyn hd;
      point p;
      float x[4];
      int button = 3;
      float f[3];

      make_cone_hilldyn(&hd,ls,alist_lookup(al,"friction"),
                                 alist_lookup(al,"mass"),
                                 alist_lookup(al,"max_move_dist"),
                                 alist_lookup(al,"g")
                        );
      printf("Click start\n");
      (void) get_point_with_mouse(gb,&p);
      x[0] = p.x;
      x[1] = p.y;
      x[2] = 0.0;
      x[3] = 0.0;

      while ( button != 2 )
      {
        if ( button != 1 )
        {        
          f[0] = input_float("f[0]: x axis force> ");
          f[1] = input_float("f[1]: y axis force> ");
          f[2] = input_float("f[2]: z axis force> ");
        }
        animate_cone_hilldyn(gb,&hd,x,f,alist_lookup(al,"time_step"),
                                         (int) alist_lookup(al,"steps")
                             );

        fprintf_floats(stdout,"State = ",x,FOUR,"\n");

        printf("LEFT: Continue, MID Quit RIGHT New force\n");
        button = get_point_with_mouse(gb,&p);
      }
    }
  }
  return(ls);
}  



void test_line(argc,argv)
int argc;
char *argv[];
{
  if ( argc < 2 )
    printf("Usage: %s {new | <filename>} [<new-filename>]\n",argv[0]);
  else
  {
    geom_base gb;
    int i;

    lines *ls = (eq_string(argv[1],"new")) ? 
                      NULL :
                      load_lines(argv[1]);

    i = index_of_arg("-verbose",argc,argv);
  
    if ( i >= 0 && i+1 < argc )
      Verbosity = atof(argv[i+1]);

    point_from_spec(-1.0,-1.0,&gb.bottom_left);
    point_from_spec(1.0,1.0,&gb.top_right);
    ls = lines_cli(&gb,ls);
    if ( argc > 2 )
    {
      save_lines(ls,argv[2]);
      printf("Saved line list in file %s\n",argv[2]);
    }
  }
}