/*
 *   GRacer
 *
 *   Copyright (C) 1999 Takashi Matsuda <matsu@users.sourceforge.net>
 *
 * This program is free software; you can redistribute it and/or 
 * modify it under the terms of the GNU General Public License as 
 * published by the Free Software Foundation; either version 2 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
 * USA
 */

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <GL/gl.h>
#include <GL/glu.h>
#include <common/gr_physics.h>
#include <common/gr_debug.h>
#include <common/gr_plane.h>
#include <common/gr_angle.h>
#include "vehicle.h"
#include "glbind.h"

#define ADJUST_INDEX(i,l,h)	{ \
  if ((i) < (l)) { \
    (i) = (l); \
  } else if ((i) > (h)) { \
    (i) = (h); \
  } \
}

#define MASS			800.0
#define MOMENT			10000.0

#define FRONT_KS		(GRAVITY * MASS / 4.0 / 0.1)
#define FRONT_KD		(4000.0 * FPS)

#define REAR_KS			(GRAVITY * MASS / 4.0 / 0.1)
#define REAR_KD			(4000.0 * FPS)

#define ENGINE_FORCE		3000.0

#define FOOT_BREAK_FORCE	6000.0
#define SIDE_BREAK_FORCE	5000.0

#define FPS			20

#define SIGN(d)  ((d)>=0.0? 1.0:-1.0)

static void
next_word (char *str, int *index)
{
  char c;

  while (c = str[*index], !(c == ' ' || c == '\t' || c == '\n' || c=='\0')) {
    (*index) ++;
  }

  while (c = str[*index], c == ' ' || c == '\t' || c == '\n') {
    (*index) ++;
  }
}

static int
token_match (char *token, char *str, int *index)
{
  int len;
  char c;

  while (c = str[*index], c == ' ' || c == '\t' || c == '\n') {
    (*index) ++;
  }

  len = strlen (token);
  if (strncmp (token, str + *index, len))
    return 0;

  c = str[*index + len];
  if (c == ' ' || c == '\t' || c == '\n' || c == '\0') {
    (*index) += len;
    while (c = str[*index], c == ' ' || c == '\t' || c == '\n') {
      (*index) ++;
    }
    return 1;
  }

  return 0;
}

static int
token_peek (char *token, char *str, int *index)
{
  int len;
  char c;

  while (c = str[*index], c == ' ' || c == '\t' || c == '\n') {
    (*index) ++;
  }

  len = strlen (token);
  if (strncmp (token, str + *index, len))
    return 0;

  c = str[*index + len];
  if (c == ' ' || c == '\t' || c == '\n' || c == '\0') {
    return 1;
  }

  return 0;
}

static char *
get_string (char *str, int *index)
{
  char *name = NULL;
  char *tmp;
  int i = *index;
  int len;
  int c;

  if (str[i] == '"') {
    i++;
    tmp = strchr (str+i, '"');
    while (tmp && tmp[-1] == '\\') {
      tmp = strchr (tmp+1, '"');
    }
    if (!tmp) {
      len = strlen (str + i);
    } else {
      len = tmp - (str + i);
    }
    if (len > 0) {
      name = gr_new (char, len+1);
      strncpy (name, str+i, len);
      name[len] = '\0';
    }
    i += len + 1;
  } else {
    tmp = strpbrk (str+i, " \t\n");
    if (!tmp) {
      len = strlen (str + i);
    } else {
      len = tmp - (str + i);
    }
    name = gr_new (char, len+1);
    strncpy (name, str+i, len);
    name[len] = '\0';
    i += len;
  }

  while (c = str[i], c == ' ' || c=='\t' || c == '\n') {
    i++;
  }

  *index = i;
  return name;
}

GrBreakData *
gr_break_data_new_from_file (FILE *file)
{
  int i;
  GrBreakData *data;

  data = gr_new0 (GrBreakData, 1);

  for (i=0; i<128 && !feof(file);  i++) {
    fscanf (file, "%f", &data->mu[i]);
  }
  if (i != 128) {
    fputs ("failed to read break data file.\n", stderr);
    free (data);
    return NULL;
  }

  return data;
}

GrTireData *
gr_tire_data_new_from_file (FILE *file)
{
  int i, j;
  char *buf;
  size_t top;
  size_t size;
  GrTireData *data;

  data = gr_new0 (GrTireData, 1);
  if (!data) {
    return NULL;
  }

  top = ftell (file);
  fseek (file, 0, SEEK_END);
  size = ftell (file) - top;
  fseek (file, top, SEEK_SET);

  buf = gr_new (char, size + 1);
  fread (buf, 1, size, file);
  buf[size] = '\0';

  for (i=0; i<size; ) {
    if (token_match ("RADIUS", buf, &i)) {
      sscanf (buf+i, "%f", &data->radius);
      next_word (buf, &i);
    } else if (token_match ("MU", buf, &i)) {
      for (j=0; j<128 && i<size; j++) {
	sscanf (buf+i, "%f", &data->mu[j]);
	next_word (buf, &i);
      }
    } else {
      goto ERROR;
    }
  }

  free (buf);
  return data;

ERROR:
  fputs ("failed to read tire data file.\n", stderr);
  free (buf);
  free (data);
  return NULL;
}

GrEngineData *
gr_engine_data_new_from_file (FILE *file)
{
  int i, j;
  char *buf;
  size_t top;
  size_t size;
  GrEngineData *data;

  data = gr_new0 (GrEngineData, 1);
  if (!data) {
    return NULL;
  }

  top = ftell (file);
  fseek (file, 0, SEEK_END);
  size = ftell (file) - top;
  fseek (file, top, SEEK_SET);

  buf = gr_new (char, size + 1);
  fread (buf, 1, size, file);
  buf[size] = '\0';

  for (i=0; i<size; ) {
    if (token_match ("MAXGEAR", buf, &i)) {
      sscanf (buf+i, "%d", &data->max_gear);
      data->gear_ratio = gr_new (float, data->max_gear);
      next_word (buf, &i);
      for (j=0; j<data->max_gear && i<size; j++) {
	sscanf (buf+i, "%f", &data->gear_ratio[j]);
	next_word (buf, &i);
      }
    } else if (token_match ("FRICTION", buf, &i)) {
      sscanf (buf+i, "%f", &data->friction);
      next_word (buf, &i);
    } else if (token_match ("MOMENT", buf, &i)) {
      sscanf (buf+i, "%f", &data->moment);
      next_word (buf, &i);
    } else if (token_match ("LIMIT", buf, &i)) {
      sscanf (buf+i, "%f", &data->limit);
      next_word (buf, &i);
    } else if (token_match ("TORQUE", buf, &i)) {
      for (j=0; j<128 && i<size; j++) {
	sscanf (buf+i, "%f", &data->torque[j]);
	next_word (buf, &i);
      }
    } else {
      goto ERROR;
    }
  }

  free (buf);
  return data;

ERROR:
  fputs ("failed to read engine data file.\n", stderr);
  free (buf);
  free (data);
  return NULL;
}

static void
gr_vehicle_data_free (GrVehicleData *data)
{
  int i;

  free (data->title);
  free (data->model_url);
  free (data->engine_url);

  for (i=0; i<data->num_wheel; i++) {
    free (data->wheel[i].object_name);
    free (data->wheel[i].tire_url);
    free (data->wheel[i].fb_url);
    free (data->wheel[i].sb_url);

    if (data->wheel[i].tire)
      gr_DECREF (data->wheel[i].tire);
    if (data->wheel[i].fb)
      gr_DECREF (data->wheel[i].fb);
    if (data->wheel[i].sb)
      gr_DECREF (data->wheel[i].sb);
  }

  free (data);
}

GrVehicleData*
gr_vehicle_data_new_from_file (FILE *file)
{
  GrVehicleData *data;
  char *buf;
  size_t top;
  size_t size;
  int i, j, k;
  double length;

  data = gr_new0 (GrVehicleData, 1);
  if (!data) {
    return NULL;
  }
  gr_FREE_FUNC (data, gr_vehicle_data_free);

  top = ftell (file);
  fseek (file, 0, SEEK_END);
  size = ftell (file) - top;
  fseek (file, top, SEEK_SET);

  buf = gr_new (char, size + 1);
  if (!buf) {
    return NULL;
  }
  fread (buf, 1, size, file);
  buf[size] = '\0';

  for (i=0; i<size; ) {
    if (token_match ("TITLE", buf, &i)) {
      data->title = get_string (buf, &i);
    } else if (token_match ("TIMESTAMP", buf, &i)) {
      data->timestamp = get_string (buf, &i);
    } else if (token_match ("MODEL", buf, &i)) {
      data->model_url = get_string (buf, &i);
    } else if (token_match ("OBJECT", buf, &i)) {
      data->object_name = get_string (buf, &i);
    } else if (token_match ("TYPE", buf, &i)) {
      if (token_match ("motorcar", buf, &i)) {
	data->type = GR_VEHICLE_MOTORCAR;
      } else if (token_match ("motorcycle", buf, &i)) {
	data->type = GR_VEHICLE_MOTORCYCLE;
      } else if (token_match ("bicycle", buf, &i)) {
	data->type = GR_VEHICLE_BICYCLE;
      }
    } else if (token_match ("COCKPITHEIGHT", buf, &i)) {
      sscanf (buf+i, "%f", &data->cockpit_height);
      next_word (buf, &i);
    } else if (token_match ("COCKPITPOS", buf, &i)) {
      for (j=0; j<3; j++) {
	sscanf (buf+i, "%f", &data->cockpit.f[j]);
	next_word (buf, &i);
      }
    } else if (token_match ("MASS", buf, &i)) {
      sscanf (buf+i, "%f", &data->mass);
      next_word (buf, &i);
    } else if (token_match ("MOMENT", buf, &i)) {
      for (j=0; j<9; j++) {
	sscanf (buf+i, "%f", &data->moment[j]);
	next_word (buf, &i);
      }
    } else if (token_match ("GEARRATIO", buf, &i)) {
      sscanf (buf+i, "%f", &data->gear_ratio);
      next_word (buf, &i);
    } else if (token_match ("DOWNFORCE", buf, &i)) {
      sscanf (buf+i, "%f", &data->down_force_speed_ratio);
      next_word (buf, &i);
    } else if (token_match ("ENGINE", buf, &i)) {
      data->engine_url = get_string (buf, &i);
    } else if (token_match ("NUMWHEEL", buf, &i)) {
      sscanf (buf+i, "%d", &data->num_wheel);
      next_word (buf, &i);
      data->wheel = gr_new0 (GrWheelData, data->num_wheel);
      for (j=0; j<data->num_wheel && i<size; j++) {
	if (token_match ("WHEEL", buf, &i)) {
	  sscanf (buf+i, "%d", &data->wheel[j].flag);
	  next_word (buf, &i);
	  if (data->wheel[j].flag & GR_WHEEL_DRIVING) {
	    data->num_driving_wheel ++;
	  }
	  for (; i<size; ) {
	    if (token_peek ("WHEEL", buf, &i)) {
	      break;
	    } else if (token_match ("MODEL", buf, &i)) {
	      data->wheel[j].model_url = get_string (buf, &i);
	    } else if (token_match ("OBJECT", buf, &i)) {
	      data->wheel[j].object_name = get_string (buf, &i);
	    } else if (token_match ("TIRE", buf, &i)) {
	      data->wheel[j].tire_url = get_string (buf, &i);
	    } else if (token_match ("POS", buf, &i)) {
	      for (k=0; k<3; k++) {
		sscanf (buf+i, "%f", &data->wheel[j].pos.f[k]);
		next_word (buf, &i);
	      }
	    } else if (token_match ("PIVOT", buf, &i)) {
	      for (k=0; k<3; k++) {
		sscanf (buf+i, "%f", &data->wheel[j].pivot.f[k]);
		next_word (buf, &i);
	      }
	    } else if (token_match ("MOMENT", buf, &i)) {
	      sscanf (buf+i, "%f", &data->wheel[j].m);
	      next_word (buf, &i);
	    } else if (token_match ("SUSPLEN", buf, &i)) {
	      sscanf (buf+i, "%f", &data->wheel[j].slen);
	      next_word (buf, &i);
	    } else if (token_match ("SUSPDIR", buf, &i)) {
	      for (k=0; k<3; k++) {
		sscanf (buf+i, "%f", &data->wheel[j].susp.f[k]);
		next_word (buf, &i);
	      }
	    } else if (token_match ("SUSPKS", buf, &i)) {
	      sscanf (buf+i, "%f", &data->wheel[j].ks);
	      next_word (buf, &i);
	    } else if (token_match ("SUSPKD", buf, &i)) {
	      sscanf (buf+i, "%f", &data->wheel[j].kd);
	      next_word (buf, &i);
	    } else if (token_match ("FOOTBREAK", buf, &i)) {
	      data->wheel[j].fb_url = get_string (buf, &i);
	    } else if (token_match ("SIDEBREAK", buf, &i)) {
	      data->wheel[j].sb_url = get_string (buf, &i);
	    } else {
	      goto ERROR;
	    }
	  }
	} else {
	  goto ERROR;
	}
      }
    } else {
      goto ERROR;
    }
  }

  for (i=0; i<data->num_wheel; i++) {
    length = gr_vertex_length (&data->wheel[i].susp);
    if (length) {
      data->wheel[i].susp.c.x /= length;
      data->wheel[i].susp.c.y /= length;
      data->wheel[i].susp.c.z /= length;
    } else {
      data->wheel[i].susp.c.x = 0;
      data->wheel[i].susp.c.y = 0;
      data->wheel[i].susp.c.z = 1;
    }
  }

  free (buf);
  return data;

ERROR:
  fputs ("failed to read grv file.\n", stderr);
  free (buf);
  if (data && data->wheel) {
    free (data->wheel);
  }
  free (data);
  return NULL;
}

GrVehicleData *
gr_get_vehicle_data (char *url)
{
  GrVehicleData *data;
  char *str;
  FILE *file;
  int i;

  data = tcl_GetCache (url);
  if (!data) {
    file = gr_open_file (url, "r");
    if (!file) {
      return NULL;
    }

    data = gr_vehicle_data_new_from_file (file);
    fclose (file);
    if (!data) {
      return NULL;
    }

    if (data->engine_url) {
      str = gr_get_fullurl (data->engine_url, url);
      data->engine = tcl_GetCache (str);
      if (!data->engine) {
	file = gr_open_file (str, "r");
	if (file) {
	  data->engine = gr_engine_data_new_from_file (file);
	  fclose (file);
	  
	  if (!data) {
	    goto ERROR;
	  }
	  tcl_PutCache (str, data);
	} else {
	  goto ERROR;
	}
      }
      gr_INCREF (data->engine);
    }

    for (i=0; i<data->num_wheel; i++) {
      if (data->wheel[i].tire_url) {
	str = gr_get_fullurl (data->wheel[i].tire_url, url);
	data->wheel[i].tire = tcl_GetCache (str);
	if (!data->wheel[i].tire) {
	  file = gr_open_file (str, "r");
	  if (file) {
	    data->wheel[i].tire = gr_tire_data_new_from_file (file);
	    fclose (file);
	    
	    if (!data->wheel[i].tire) {
	      goto ERROR;
	    }
	    tcl_PutCache (str, data->wheel[i].tire);
	  } else {
	    goto ERROR;
	  }
	}
	gr_INCREF (data->wheel[i].tire);
      }

      if (data->wheel[i].fb_url) {
	str = gr_get_fullurl (data->wheel[i].fb_url, url);
	data->wheel[i].fb = tcl_GetCache (str);
	if (!data->wheel[i].fb) {
	  file = gr_open_file (str, "r");
	  if (file) {
	    data->wheel[i].fb = gr_break_data_new_from_file (file);
	    fclose (file);
	    
	    if (!data->wheel[i].fb) {
	      goto ERROR;
	    }
	    tcl_PutCache (str, data->wheel[i].fb);
	  } else {
	    goto ERROR;
	  }
	}
	gr_INCREF (data->wheel[i].fb);
      }

      if (data->wheel[i].sb_url) {
	str = gr_get_fullurl (data->wheel[i].sb_url, url);
	data->wheel[i].sb = tcl_GetCache (str);
	if (!data->wheel[i].sb) {
	  file = gr_open_file (str, "r");
	  if (file) {
	    data->wheel[i].sb = gr_break_data_new_from_file (file);
	    fclose (file);
	    
	    if (!data->wheel[i].sb) {
	      goto ERROR;
	    }
	    tcl_PutCache (str, data->wheel[i].sb);
	  } else {
	    goto ERROR;
	  }
	}
	gr_INCREF (data->wheel[i].sb);
      }
    }
    tcl_PutCache (url, data);
  }

  gr_INCREF (data);
  return data;

ERROR:
  if (data) {
    if (data->engine) {
      gr_DECREF (data->engine);
    }
    if (data->wheel) {
      for (i=0; i<data->num_wheel; i++) {
	if (data->wheel[i].tire) {
	  gr_DECREF (data->wheel[i].tire);
	}
	if (data->wheel[i].fb) {
	  gr_DECREF (data->wheel[i].fb);
	}
	if (data->wheel[i].sb) {
	  gr_DECREF (data->wheel[i].sb);
	}
      }
    }
    free (data);
  }
  return NULL;
}
  
static void
gr_vehicle_calculate_constants (GrVehicle *vehicle)
{
  float s, c;
  GrMatrix m;
  int i;

  GR_BEGIN_FUNCTION ();

  s = 1.0;	/* sin (90[deg]) */
  c = 0.0;	/* cos (90[deg]) */

  gr_matrix_rotate_z (gr_matrix_identity, &m, s, c);
  for (i=0; i<vehicle->data->num_wheel; i++) {
    gr_matrix_mult_vertex (&m, &vehicle->data->wheel[i].pos,
			       &vehicle->data->wheel[i].rpos);
  }

  vehicle->fg.c.x = 0;
  vehicle->fg.c.y = 0;
  vehicle->fg.c.z = - GRAVITY * vehicle->body.mass;

  gr_rigid_body_reset (&vehicle->body, 0);
  gr_rigid_body_reset (&vehicle->body, 1);
}

static void
gr_vehicle_free (GrVehicle *vehicle)
{
  int i;

  if (vehicle->scene) {
    gr_DECREF (vehicle->scene);
  }
  if (vehicle->wheel && vehicle->data) {
    for (i=0; i<vehicle->data->num_wheel; i++) {
      if (vehicle->wheel[i].scene) {
	gr_DECREF (vehicle->wheel[i].scene);
      }
    }
    free (vehicle->wheel);
  }

  if (vehicle->data)
    gr_DECREF (vehicle->data);

  free (vehicle);
}

GrVehicle *
gr_vehicle_new (GrVehicleData *vd)
{
  GrVehicle *v;
  int i, j;

  v = gr_new0 (GrVehicle, 1);
  gr_FREE_FUNC (v, gr_vehicle_free);

  v->wheel = gr_new0 (GrWheel, vd->num_wheel);
  v->data = vd;

  /* initialize rigid body's parameters */
  v->body.mass = v->data->mass;

  v->body.m_j = *gr_matrix_identity;

  for (i=0; i<3; i++) {
    for (j=0; j<3; j++) {
      v->body.m_j.f[i][j] = v->data->moment[i*3 + j];
    }
  }

  gr_vehicle_calculate_constants (v);

  return v;
}

static void
gr_vehicle_calc_geometry (GrVehicle *vehicle, GrCourse *course, int which)
{
  int i;
  GrWheel *w;
  GrWheelData *wd;

  GR_BEGIN_FUNCTION ();

  for (i=0; i<vehicle->data->num_wheel; i++) {
    w = &vehicle->wheel[i];
    wd = &vehicle->data->wheel[i];

#if 0
    /* RELATIVE WHEEL POS */
    gr_vertex_scale (&w->state[which].pos, &wd->susp, w->state[which].ds);
    gr_vertex_add_a (&w->state[which].pos, &wd->pos);
#endif

    /* ABSOLUTE WHEEL POS */
    gr_matrix_mult_vertex (&vehicle->body.state[which].m_zyx,
    			   &wd->pos,
			   &w->state[which].abs_pos);
    gr_vertex_add_a (&w->state[which].abs_pos,
		     &vehicle->body.state[which].pos);

#if 0
    if (wd->flag & GR_WHEEL_STEERING) {
      /* RELATIVE WHEEL PIVOT */
      gr_vertex_rotate (&w->pivot, &wd->pivot,
			sin (w->control.angle), cos (w->control.angle),
			wd->susp.c.x, wd->susp.c.y, wd->susp.c.z);
    }

    /* ABSOLUTE WHEEL PIVOT */
    gr_matrix_mult_vertex (&vehicle->body.state[which].m_zyx,
			   &pivot, &w->state[which].pivot);
#endif
  }
}

typedef struct {
  int num;
  GrVehicle *vehicle;
  int wheel_no;
  int which;
  float ds;
  GrVertex top;
  GrVertex dir;
  GrVertex susp;
  GrVertex pivot;
  GrVertex pos;
} GeometryCalcHint;

static int
surf_foreach_callback (GrCourse *course, int seg, int surf, void *data)
{
  GeometryCalcHint *hint = data;
  GrCSurface *surface;
  GrVehicle *vehicle;
  GrWheel *w;
  GrWheelData *wd;
  GrVertex top, tmp, pos, cp;
  GrVertex pivot;
  float ds;

  vehicle = hint->vehicle;
  w = &vehicle->wheel[hint->wheel_no];
  wd = &vehicle->data->wheel[hint->wheel_no];
  surface = course->segs[seg]->surfs + surf;

  gr_matrix_mult_vertex (&surface->plane.m, &hint->pos, &pos);
  if (pos.c.z > hint->ds) {
    /* already more closed plane was found. */
    return 0;
  }

  /*
   * top = pivot x normal x pivot
   */
  gr_matrix_mult_vertex (&surface->plane.m, &hint->pivot, &pivot);
  gr_vertex_sub_a (&pivot, &surface->plane.orig);
  //gr_vertex_cross (&tmp, &pivot, gr_vertex_unit_z);
  tmp.c.x = pivot.c.y;
  tmp.c.y = -pivot.c.x;
  tmp.c.z = 0;
  gr_vertex_cross (&top, &tmp, &pivot);
  gr_vertex_normalize (&top, &top);

  if (top.c.z == 0) {
    /*
     * FIXME
     * when top.c.z == 0, then disk and plane are parallel. This type of
     * collision must be handled by different algorism.
     */

    return 0;
  }

  ds = pos.c.z / top.c.z;
  if (ds >= hint->ds) {
    /* Already more closed plane was found. */
    /* By initializing hint->ds as wd->tire->radius,
     * non-contacting surface is ignored.
     */
    return 0;
  }

  /* cp vector is where disk and plane contact with each other */

  gr_vertex_scale_xy (&tmp, &top, ds);
  gr_vertex_sub_xy (&cp, &pos, &tmp);
  cp.c.z = 0.0;

  if (gr_course_surface_check_inner (surface, &cp)) {
    w->state[hint->which].surf = surface;
    w->state[hint->which].segment = seg;
    hint->ds = ds;
    hint->top = top;

    return 1;
  } else {
    return 0;
  }
}

#define MAX_SPEED (400.0 * 1000 / 3600)

void gr_vehicle_integral (GrVehicle *vehicle,
			  GrCourse *course,
			  int from, int to, double h)
{
  GrVehicleData *vdata = vehicle->data;
  int i;
  GrVertex speed;
  GrVertex a, b, c, grip, top, pivot, plane_n;
  GrVertex tmp;					/* temporary */
  float la, lb, lc;
  float slip;
  float grip_friction;
  float n;
  float mu;
  float f;					/* temorary */
  GrVertex force;
  GrWheel *w;
  GrWheelData *wd;
  GrTireData *td;
  int index;
  float engine_torque;
  GeometryCalcHint hint;

  GR_BEGIN_FUNCTION ();

  gr_vertex_copy (&tmp, &vehicle->body.state[from].spd);
  gr_matrix_mult_vertex (&vehicle->body.state[from].m_rzyx,
      			 &tmp, &speed);

  if (vehicle->control.gear >= vdata->engine->max_gear) {
    vehicle->control.gear = vdata->engine->max_gear - 1;
  } else if (vehicle->control.gear < 0) {
    vehicle->control.gear = 0;
  }

  f = vehicle->state[from].rpm / vdata->engine->limit;
  if (f > 1.0) {
    engine_torque = 0;
  } else {
    if (f < 0.0)
      f = 0.0;

    index = 127 * f;
    ADJUST_INDEX (index, 0, 127);
    engine_torque = vehicle->control.accel * vdata->engine->torque[index];
  }
  engine_torque += (vehicle->control.accel - 1.0) * f * vdata->engine->friction;
  engine_torque *= vdata->engine->gear_ratio[vehicle->control.gear]
    		    * vehicle->data->gear_ratio
		    / vehicle->data->num_driving_wheel;

  hint.vehicle = vehicle;
  hint.which = from;
  vehicle->state[to].rpm = 0;

  for (i=0; i<vehicle->data->num_wheel; i++) {
    w = vehicle->wheel + i;
    wd = vehicle->data->wheel + i;
    td = wd->tire;

    w->control.f = 0.0;
    if (wd->flag & GR_WHEEL_DRIVING) {
      w->control.f += engine_torque / td->radius;
    }

    if (wd->flag & GR_WHEEL_STEERING) {
      w->control.angle = - RAD(50.0) * vehicle->control.steering;
    }

    slip = SLIP_FACTOR * fabs(w->state[from].dr) * td->radius;
    if (slip > 1.0)
      slip = 1.0;
    index = slip * 127;
    ADJUST_INDEX (index, 0, 127);

    if (wd->sb) {
      w->control.f -= SIDE_BREAK_FORCE * SIGN(w->state[from].dr)
		* wd->sb->mu[index] * vehicle->control.side_break;
    }
    if (wd->fb) {
      w->control.f -= FOOT_BREAK_FORCE * SIGN(w->state[from].dr)
		* wd->fb->mu[index] * vehicle->control.foot_break;
    }

    w->state[from].surf = NULL;
    hint.wheel_no = i;
    hint.which = from;
    hint.num = 0;
    hint.ds = td->radius;

    /* ABSOLUTE WHEEL PIVOT DIRECTORY WITH CURRENT STEERING ANGLE
     *      pivot: pivot vector in vehicle's view.
     * hint.pivot: pivot vector in world view.
     */
    gr_vertex_rotate (&pivot, &wd->pivot,
		      sin (w->control.angle), cos (w->control.angle),
		      wd->susp.c.x, wd->susp.c.y, wd->susp.c.z);
    gr_matrix_mult_vertex (&vehicle->body.state[from].m_zyx,
			   &pivot, &hint.pivot);

    /* CURRENT ABSOLUTE COORDINATE OF WHEEL */
    hint.pos = w->state[from].abs_pos;

    gr_course_foreach_surface (course, &hint.pos, MAX_SPEED * h + 3,
			       surf_foreach_callback, &hint);

    grip_friction = 0.0;
    if (!w->state[from].surf) {
      w->state[from].ds = 0;
      w->state[from].slip = 0;
    } else {
      gr_vertex_add (&tmp, &hint.top, &w->state[from].surf->plane.orig);
      gr_matrix_mult_vertex (&w->state[from].surf->plane.im, &tmp, &tmp);
      gr_matrix_mult_vertex (&vehicle->body.state[from].m_rzyx, &tmp, &top);
      gr_vertex_cross (&grip, &pivot, &top);

		  /* NORMAL VECTOR OF SURFACE RELATIVE FROM VEHICLE'S VIEW */
      gr_matrix_mult_vertex (&vehicle->body.state[from].m_rzyx,
	  		     &w->state[from].surf->plane.n,
			     &plane_n);

      f = gr_vertex_product (&top, &wd->susp);
      w->state[from].ds = - f * hint.ds + td->radius;
      n = w->state[from].ds * wd->ks
	+ (w->state[from].ds - w->state[from].pds) / h * wd->kd;

      gr_vertex_scale (&tmp, &top, -f);
      gr_vertex_add_a (&tmp, &plane_n);
      f = gr_vertex_product (&plane_n, &tmp);
      n += f * 10000;

      force = *gr_vertex_origin;

      if (n > 0) {
		 /* WHEEL CIRCUMFERENCE VELOCITY RELATIVE FROM VEHICLE BODY */
	la = td->radius * w->state[from].dr;
	gr_vertex_scale (&a, &grip, la);
	la = fabs(la);

#if 0
	w->state[from].grip = grip;
	gr_vertex_scale_a (&w->state[from].grip,
			   1.0 / gr_vertex_length (&w->state[from].grip));
#endif

			       /* WHEEL VELOCITY RELATIVE FROM VEHICLE BODY */
	gr_vertex_scale (&b, &wd->rpos, vehicle->body.state[from].omg.c.z);
	gr_vertex_add_a (&b, &speed);
	f = gr_vertex_product (&plane_n, &b);
	gr_vertex_scale (&tmp, &plane_n, f);
	gr_vertex_sub_a (&b, &tmp);
	lb = gr_vertex_length (&b);

	gr_vertex_normalize (&w->state[from].speed, &b);

			     /* TRUE GRIP VECTOR RELATIVE FROM VEHICLE BODY */
	gr_vertex_sub (&c, &a, &b);
	lc = gr_vertex_length (&c);

	if (lc > 0.0001) {
	  if (la + lb > 0)
	    slip = lc / (la + lb);
	  else
	    slip = 0;

	  gr_message ("slip ratio[%d] %f", i, slip);

	  index = slip * 127;
	  ADJUST_INDEX (index, 0, 127);
	  mu = td->mu[index];

	  gr_vertex_normalize (&tmp, &c);
	  gr_vertex_scale (&force, &tmp, mu * n);
#if 0
	  gr_vertex_scale_a (&w->state[from].force, 1 / mu / n);
#endif

	  grip_friction = gr_vertex_product (&grip, &force);
	}

	w->state[to].slip = slip;
	lc /= 30.0 / 3.6;				// 30 [km/h]
	if (lc < 1.0) {
	  w->state[to].slip *= lc;
	}
      } else {
	w->state[to].slip = 0;
      }

      gr_vertex_scale (&tmp, &plane_n, n);
      gr_vertex_add_a (&force, &tmp);

#if 0
      //printf ("%f %f %f\n", force.c.x, force.c.y, force.c.z);
      w->state[from].force = force;
      gr_vertex_scale_a (&w->state[from].force,
			 1.0 / gr_vertex_length (&w->state[from].force));
#endif

      gr_vertex_scale_a (&top, wd->tire->radius);
      gr_vertex_scale (&tmp, &wd->susp, w->state[from].ds);
      gr_vertex_sub_a (&tmp, &top);
      gr_vertex_add_a (&tmp, &wd->pos);
      gr_rigid_body_add_force_relative (&vehicle->body, from, &tmp, &force);
    }
    w->state[to].pds = w->state[from].ds;
						     /* INTEGRAL WHEEL SPIN */
    w->state[to].dr = w->state[from].dr + 
		      h * td->radius * (w->control.f - grip_friction) / wd->m;

    if (wd->flag & GR_WHEEL_DRIVING) {
      vehicle->state[to].rpm += w->state[to].dr;
    }
  }

  /*
   * FIXME
   * Engine rotational velocity is calculated from average of driving wheels.
   */
  vehicle->state[to].rpm *= 60 / 2.0 / M_PI
			/ vehicle->data->num_driving_wheel
  			* vdata->engine->gear_ratio[vehicle->control.gear]
			* vdata->gear_ratio;

						      /* DOWN FORCE BY WING */
  if (speed.c.x > 0) {
    force = *gr_vertex_unit_z;
    force.c.z *= - vehicle->data->down_force_speed_ratio * speed.c.x;
    gr_rigid_body_add_force_relative (&vehicle->body, from,
				       gr_vertex_origin, &force);
  }
								 /* GRAVITY */
  gr_rigid_body_add_force (&vehicle->body, from,
			   gr_vertex_origin, &vehicle->fg);
  gr_rigid_body_integral (&vehicle->body, from, h);

  gr_vehicle_calc_geometry (vehicle, course, to);
}

static int
height_foreach_callback (GrCourse *course,
			 int seg,
			 int surf,
			 float z,
			 void *data)
{
  float *biggest = data;

  if (*biggest < z) {
    *biggest = z;
  }

  return 0;
}

/*
 * gr_vehicle_find_initial_height
 * 
 * x and y coordinate of vehicle must be set as initial position.
 * This routine find the biggest z coordinate in specified course and
 * initialize z coordinate of vehicle.
 *
 * vehicle: vehicle.
 *  course: course data.
 *   which: which state of vehicle must be initialized.
 */

void gr_vehicle_find_initial_height (GrVehicle *vehicle,
				     GrCourse *course,
				     int which)
{
  int i;
  GrWheel *w;
  GrWheelData *wd;
  float min_z;
  float max_z;
  float z;

  GR_BEGIN_FUNCTION ();

  /* find smallest z coordinate of the course */
  min_z = course->segs[0]->min.c.z;
  for (i=1; i<course->num_segs; i++) {
    if (min_z > course->segs[i]->min.c.z) {
      min_z = course->segs[i]->min.c.z;
    }
  }
  max_z = min_z;

  gr_vehicle_calc_geometry (vehicle, course, which);

  for (i=0; i<vehicle->data->num_wheel; i++) {
    w = vehicle->wheel + i;
    wd = vehicle->data->wheel + i;
    z = min_z;

    gr_course_foreach_surface_xy (course, &w->state[which].abs_pos,
				  height_foreach_callback, &z);
    z += wd->tire->radius - wd->pos.c.z;
    if (max_z < z) {
      max_z = z;
    }
  }

  vehicle->body.state[which].pos.c.z = max_z;
  gr_vehicle_calc_geometry (vehicle, course, which);
}
  
static void
gr_vehicle_init (GrVehicle *vehicle, int state)
{
  int i;

  gr_rigid_body_reset (&vehicle->body, state);

  for (i=0; i<vehicle->data->num_wheel; i++) {
    vehicle->wheel[i].state[state].dr = 0;
    vehicle->wheel[i].state[state].ds = 0;
    vehicle->wheel[i].state[state].pds = 0;
    vehicle->wheel[i].state[state].pds = 0;
  }
}

int
gr_vehicle_put (GrVehicle *vehicle,
		GrCourse *course,
		double threshold,
		double h,
		int limit)
{
  GrVertex diff;
  double len;
  int i, j;
  int from, to;
  int done = 0;

  GR_BEGIN_FUNCTION ();

  gr_vehicle_init (vehicle, 0);
  gr_vehicle_init (vehicle, 1);

  gr_vehicle_find_initial_height (vehicle, course, 0);

  for (i=0; i<vehicle->data->num_wheel; i++) {
    vehicle->wheel[i].state[0].pds = 0;
    vehicle->data->wheel[i].kd /= 100;

    if (!(vehicle->data->wheel[i].flag & GR_WHEEL_STEERING)) {
      vehicle->wheel[i].state[0].pivot = vehicle->data->wheel[i].pivot;
      vehicle->wheel[i].state[1].pivot = vehicle->data->wheel[i].pivot;
    }
  }

  for (i=0; i<limit; i++) {
    from = i % 2;
    to = (i + 1) % 2;

    gr_vehicle_integral (vehicle, course, from, to, h);
    gr_vertex_scale (&vehicle->body.state[to].spd,
		     &vehicle->body.state[to].spd, 0.1);
    gr_vertex_scale (&vehicle->body.state[to].omg,
		     &vehicle->body.state[to].omg, 0.1);

    gr_vertex_sub (&diff,
		   &vehicle->body.state[to].pos,
		   &vehicle->body.state[from].pos);

    len = gr_vertex_length (&diff);

    if (len <= threshold) {
      vehicle->body.state[from] = vehicle->body.state[to];
      for (j=0; j<vehicle->data->num_wheel; j++) {
	vehicle->wheel[j].state[from] = vehicle->wheel[j].state[to];
      }
      done = 1;
      break;
    }
  }

  return done;
}