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

Sensor::Sensor(const Point &delta, double angle1, double angle2, int stim,
	       PermString name)
  : _name(name), _stimulus(stim),
    _delta(delta), _angle1(angle1), _angle2(angle2), _sensitivity(0.1)
{
}

double
Sensor::measurement(Vehicle *vehicle)
{
  double angle = vehicle->rotation();
  Point sensor_pos = vehicle->position() + _delta.rotated(angle);
  
  double m = 0;
  World *world = vehicle->world();
  for (int i = 0; i < world->nbodies(); i++) {
    Body *body = world->body(i);
    if (body->has_stimulus(_stimulus)) {
      Point stim_pos;
      double cur_m = body->stimulus(_stimulus, sensor_pos, stim_pos);
      double cur_angle = (stim_pos - sensor_pos).angle() - angle;
      if (cur_angle > M_PI) cur_angle -= 2*M_PI;
      if (cur_angle < -M_PI) cur_angle += 2*M_PI;
      if (cur_angle >= _angle1 && cur_angle <= _angle2
	  && !body->attached_to(vehicle))
	m += cur_m * _sensitivity;
    }
  }
  
  //fprintf(stderr, "---------%g\n", m);
  if (m < 0) m = 0;
  return m;
}

void
Sensor::draw(Vehicle *vehicle, View *view)
{
  double angle = vehicle->rotation();
  Point sensor_pos = vehicle->position() + _delta.rotated(angle);
  Point corner[2];

  corner[0] = sensor_pos;
  corner[1] = sensor_pos + Point(5, 0).rotated(_angle1 + angle);
  view->draw_lines(vehicle->sensor_gc(), 2, corner, false);
  
  corner[1] = sensor_pos + Point(5, 0).rotated(_angle2 + angle);
  view->draw_lines(vehicle->sensor_gc(), 2, corner, false);
  
  view->draw_arc(vehicle->sensor_gc(), sensor_pos, 5, _angle1 + angle, _angle2 + angle);
}