#ifdef HAVE_CONFIG_H
# include <config.h>
#endif
#include "vehicle.hh"
#include "actuator.hh"
#include "grafix.hh"
#include "logic.hh"
#include "sensor.hh"
#include "view.hh"
#include "world.hh"
#include <stdio.h>

Vehicle::Vehicle(double length, double width)
  : _world(0), _length(length), _width(width),
    _mass(length * width / 100),
    _rotation(0),
    _velocity(0, 0), _angular_velocity(0), _total_firing(0)
{
  double radius = (length > width ? length : width);
  _moment_inertia = 0.5 * _mass * radius * radius / 200;
  _gc = Grafix::line_gc(255, 0, 0);
  _z_plane = 1;
  _radius = (length > width ? length/2 : width/2);
}

Vehicle::Vehicle(const Vehicle &v)
  : Body(v), _world(0), _gc(v._gc),
    _length(v._length), _width(v._width),
    _mass(v._mass), _moment_inertia(v._moment_inertia),
    _sensors(v._sensors), _actuators(v._actuators),
    _rotation(v._rotation),
    _velocity(v._velocity), _angular_velocity(v._angular_velocity)
{
  for (int i = 0; i < v._logics.size(); i++)
    _logics.push_back(v._logics[i]->clone());
  for (int i = 0; i < v._attachments.size(); i++)
    _attachments.push_back(v._attachments[i]->clone_attachment(this));
}

Vehicle *
Vehicle::clone()
{
  return new Vehicle(*this);
}

void
Vehicle::embody(World *w)
{
  assert(!_world);
  _world = w;
  for (int i = 0; i < _attachments.size(); i++)
    w->add(_attachments[i]);
}

void
Vehicle::set_color(int r, int g, int b)
{
  _gc = Grafix::line_gc(r, g, b);
}

void
Vehicle::place(const Point &p, double r)
{
  _position = p;
  _rotation = r;
  while (_rotation > M_PI) _rotation -= 2*M_PI;
  while (_rotation < -M_PI) _rotation += 2*M_PI;
}

void
Vehicle::reset()
{
  _velocity = Point(0, 0);
  _angular_velocity = 0;
  _trail.clear();
  _rotation_trail.clear();
}

void
Vehicle::add_actuator(Actuator *actor, Logic *logic)
{
  _actuators.push_back(actor);
  if (logic == 0) logic = Logic::null_logic();
  _logics.push_back(logic);
}

int
Vehicle::sensor_id(PermString name) const
{
  for (int i = 0; i < _sensors.size(); i++)
    if (_sensors[i]->name() == name)
      return i;
  return -1;
}

Point
Vehicle::sensor_position(int n) const
{
  return _position + _sensors[n]->position_delta().rotated(_rotation);
}

Point
Vehicle::actuator_position(int n) const
{
  return _position + _actuators[n]->position_delta().rotated(_rotation);
}

void
Vehicle::make_attachment(Body *attach)
{
  Body *a = attach->clone_attachment(this);
  _attachments.push_back(a);
  if (_world)
    _world->add(a);
}

void
Vehicle::rotate(double d)
{
  _rotation += d;
  while (_rotation > M_PI) _rotation -= 2*M_PI;
  while (_rotation < -M_PI) _rotation += 2*M_PI;
}

void
Vehicle::drive(double len)
{
  _position += Point(len, 0).rotated(_rotation);
}

bool
Vehicle::has_stimulus(int stim)
{
  return stim == 2;
}

double
Vehicle::stimulus(int stim, Point sensor, Point &stimulus_out)
{
  /* A light gives stimulus inversely proportional to distance from sensor.
     Light cannot stimulate more than strength(). */
  if (stim != 2) return 0;
  
  Point pos = stimulus_out = position();
  double distance = (sensor - pos).squared_length();
  double s = _total_firing;
  if (distance > 1) s /= distance;
  return s;
}

#define GRAVITATIONAL_CONSTANT .05
#define ANG_FRICTION_CONSTANT .01

void
Vehicle::move(double time)
{
  Vector<double> sensations;
  for (int i = 0; i < _sensors.size(); i++)
    sensations.push_back(_sensors[i]->measurement(this));
  
  // for (int i = 0; i < _sensors.size(); i++) fprintf(stderr, "%g ", sensations[i]); fputs("\n", stderr);
  for (int i = 0; i < _attachments.size(); i++)
    _attachments[i]->sensation(sensations);
  
  Vector<double> firings;
  for (int i = 0; i < _actuators.size(); i++) {
    double f = _logics[i]->calculate(sensations, this);
    // 19.Feb.2002 - Thomer reported that negative 'firings' didn't work;
    // these lines were the problem. Why were they there in the first place?
    if (f > 1) f = 1;
    if (f < -1) f = -1;
    firings.push_back(f);
  }
  
  Point force(0, 0);
  double torque = 0;
  _total_firing = 0;
  for (int i = 0; i < _actuators.size(); i++)
    if (firings[i]) {
      double fire = firings[i] * _actuators[i]->strength();
      double actuator_rot = _actuators[i]->rotation_delta();
      force += Point(fire, 0).rotated(actuator_rot + _rotation);
      torque += fire * _actuators[i]->unit_torque();
      _total_firing += fire;
    }
  
  
  // Handle friction
  double coeff = _mass * GRAVITATIONAL_CONSTANT;
  if (!_velocity && force.magnitude() < _world->static_friction(this) * coeff)
    force = Point(0, 0);
  else
    force -= _velocity.normal() * _world->dynamic_friction(this) * coeff;
  
  _position += _velocity * time;
  _velocity += force * time / _mass;
  
  
  // Handle "angular friction", an arbitrary creation sort of like friction
  //fprintf(stderr, "%g\n", torque * 10);
  //torque *= 10;
  if (torque > .1) torque = .1;
  if (torque < -.1) torque = -.1;
  
  double ang_friction = _world->angular_friction(this);
  double ang_friction_2 = _world->angular_friction_2(this);
  if (ang_friction < 0)
    // wipe out current angular velocity
    torque += _angular_velocity / time * _moment_inertia;
  else {
    double ang_friction_mag =
      ang_friction * _moment_inertia * ANG_FRICTION_CONSTANT;
    if (!_angular_velocity && fabs(torque) < ang_friction_mag)
      torque = 0;
    else
      torque += (_angular_velocity < 0 ? -1 : 1) * ang_friction_mag
	+ _angular_velocity * ang_friction_2;
  }
  
  rotate(_angular_velocity);// * time);
  _angular_velocity -= torque * time / _moment_inertia;
}

void
Vehicle::collide(Body *o_body)
{
  Vehicle *o = o_body->cast_vehicle();
  if (!o) return;

  // check if really colliding
  Point delta_pos =  o->position() - _position;
  double distance = delta_pos.length();
  if (distance > _radius + o->radius())
    return;

  // measure relative to `this' frame
  Point rest_v1 = _velocity;
  Point v2 = o->_velocity - rest_v1;

  // assume masses are equal
  if (_mass != o->_mass) {
    static int warned = 0;
    if (!warned) {
      fprintf(stderr, "warning: colliding two bodies with different masses; bad physics\n");
      warned = 1;
    }
  }
  
  double V = v2.length();
  double ang_from_origin = v2.angle();
  double theta = delta_pos.angle() - ang_from_origin;

  double sin_theta = sin(theta);
  double cos_theta = cos(theta);
  double new_v1_mag = V * cos_theta;
  double new_v2_mag = V * sin_theta;

  Point new_v1 = Point(new_v1_mag, 0).rotated(ang_from_origin + theta);
  Point new_v2 = Point(new_v2_mag, 0).rotated(ang_from_origin + theta - M_PI/2);
  
  _velocity = new_v1 + rest_v1;
  o->_velocity = new_v2 + rest_v1;
}

void
Vehicle::trail_snapshot()
{
  _trail.push_back(_position);
  _rotation_trail.push_back(_rotation);
}

void
Vehicle::draw(View *view)
{
  if (view->trails() && _trail.size()) {
    for (int i = 0; i < _trail.size() - 1; i++)
      view->draw_line(_gc, _trail[i], _trail[i+1]);
    view->draw_line(_gc, _trail.back(), _position);
  }
  
  for (int i = 0; i < _sensors.size(); i++)
    _sensors[i]->draw(this, view);
  
  Point broad_edge = Point(0, _width).rotated(_rotation);
  Point long_edge = Point(_length, 0).rotated(_rotation);
  Point corner[4];
  
  corner[0] = _position;
  corner[0].x -= (broad_edge.x + long_edge.x) / 2;
  corner[0].y -= (broad_edge.y + long_edge.y) / 2;
  
  corner[1] = corner[0] + broad_edge;
  corner[2] = corner[0] + broad_edge + long_edge;
  corner[3] = corner[0] + long_edge;
  
  view->erase_poly(4, corner);
  view->draw_lines(_gc, 4, corner, true);
  
  for (int i = 0; i < _actuators.size(); i++)
    _logics[i]->draw(this, i, view);
  
  for (int i = 0; i < _actuators.size(); i++)
    _actuators[i]->draw(this, view);
}