/* 
   Project: coevolutionary GP moto driving
   Dana Vrajitoru
   agent.cc
   
   A base class for an agent controling one unit of the automatic
   pilot.
  
**/

#include "agent.h"
#include "pilot_manager.h"
#include "manager.h"
#include <ctime>
#include <iostream>
#include <fstream>
using namespace std;
#include <cmath>

// Default constructor. Not much to do.
Agent::Agent(float timing)
  :boss(NULL), is_on(false), tic(0), parent(NULL)
{
  set_nr_config();
}

// Constructor for which we know the manager.
Agent::Agent(Manager *the_boss, float timing)
  :boss(the_boss), is_on(false), tic(timing), parent(NULL)
{
  set_nr_config();
}

// Destructor: nothing to do for now.
Agent::~Agent()
{
  boss=NULL;
  parent = NULL;
  set_nr_config();
}

// output the values
void Agent::output_data()
{
  cout << "nr config: " << nr_config << endl;
}

// Set the number of configuration values. To be defined by the
// derivate classes.
void Agent::set_nr_config()
{
  nr_config = 0;
}

// To be called if we find out who the boss is later.
void Agent::set_manager(Manager *the_boss)
{
  boss = the_boss;
  moto_length = boss->moto->length();
}

// To reset the timing lapse
void Agent::set_timing(float timing)
{
  tic = timing;
}

// Set the id of this agent.
void Agent::set_id(int id)
{
  my_id = id;
}

// Set the pilot manager.
void Agent::set_parent(Pilot_manager *pilot)
{
  parent = pilot;
}

// Set the speed limit.
void Agent::set_speed_limit(float speed)
{
  speed_limit = speed;
}

// Agent-specific set data to be defined in the derivate class.
void Agent::set_data(float values[])
{;}

// Very important. This function creates a new thread for this agent
// and sends it into the main loop.
void Agent::launch()
{
  cout << "launching " << my_id << endl;
  if (pthread_create(&the_thread, NULL, launch_agent, 
		     (void *)this) != 0)
    cerr << "Pthread_create fails";
}

// Wrapper function to be able to pass the agent in as a parameter.
void *launch_agent(void *agnt_ptr)
{
  Agent_ptr agnt = (Agent_ptr) agnt_ptr;
  agnt->main_loop();
  agnt = NULL;
  return NULL;
}

// Filling in the status values from the manager.
void Agent::get_status()
{
  float leftd, rightd, forward_leftd, forward_rightd;
  parent->lock();
  float sidew = boss->side_watch;
  float frontw = boss->front_watch;
  leftd = boss->position.distance(boss->left_pos);
  if (leftd > sidew)
    left_dist = sqrt(leftd*leftd - sidew*sidew);
  rightd = boss->position.distance(boss->right_pos);
  if (rightd >sidew)
    right_dist = sqrt(rightd*rightd - sidew*sidew);
  forward_leftd = boss->position.distance(boss->forward_left_pos);
  if (forward_leftd > frontw)
    forward_left_dist = sqrt(forward_leftd*forward_leftd - frontw*frontw);
  forward_rightd = boss->position.distance(boss->forward_right_pos);
  if (forward_rightd > frontw)
    forward_right_dist = sqrt(forward_rightd*forward_rightd - frontw*frontw);
  front_left_dist = boss->position.distance(boss->left_horizon);
  front_right_dist = boss->position.distance(boss->right_horizon);
  front_dist = boss->position.distance(boss->horizon);
  slope = boss->discrete_slope();
  speed = boss->moto->get_speed();
  parent->unlock();
}

// Print out these values.
void Agent::print_status()
{
  cout << "left_dist " << left_dist;
  cout << " right_dist " << right_dist;
  cout << " front_dist " << front_dist;
  cout << " front_left_dist " << front_left_dist;
  cout << " front_right_dist " << front_right_dist;
  cout << " slope " << slope;
  cout << " speed " << speed << endl;
}

// This will be specific to each agent.
void Agent::update_action()
{
  return;
  // Not intended to be implemented at this level.
}

// This may be reimplemented by the agent if anything special must be
// done when it is turned on.
void Agent::turn_on_action()
{
  is_on = true;
}

// This may be reimplemented by the agent if anything special must be
// done when it is turned off.
void Agent::turn_off_action()
{
  is_on = false;
}

// This probably doesn't need reimplementation.
void Agent::main_loop()
{
  Event_id local_event = null_evt;
  time_t old_timer = 0, new_timer = 0; 
  double difference;
  time (&old_timer);
  while (local_event != terminate_evt) {
    parent->lock_evt();
    local_event = parent->event;
    if ((local_event != turn_on_evt || !is_on) &&
	(local_event != turn_off_evt && local_event != terminate_evt
	 || is_on))
      parent->event = null_evt;
    parent->unlock_evt();
    
    switch (local_event) {
    case update_evt:
      if (is_on) {
	update_action();
	parent->lock_evt();
	parent->count[update_evt]++;
	parent->unlock_evt();
	time (&old_timer); // start parent->counting the time again after the
			   // update action.
      }
      break;
    case turn_on_evt:
      if (!is_on) {
	turn_on_action();
	parent->lock_evt();
	parent->count[turn_on_evt]++;
	parent->unlock_evt();
	time (&old_timer); // start parent->counting the time again
	                   // after we turn the agent on.
      }
      break;
    case terminate_evt:
    case turn_off_evt:
      if (is_on) {
	turn_off_action();
	parent->lock_evt();
	parent->count[turn_off_evt]++;
	parent->unlock_evt();
      }
      break;
    }
    if (is_on) {
      time (&new_timer);
      difference = difftime(new_timer, old_timer);
      if (difference >= tic) {
	update_action();
	parent->lock_evt();
	parent->count[update_evt]++;
	parent->unlock_evt();
	//cerr << "updating " << endl;
	time (&old_timer);  // start parent->counting the time again after the
			    // update action.
      }
    }	
  }
  //fout << endl;
  //fout.close();
}
  
// Synchronises the termination of all the threads.
void Agent::join()
{
  cout << "joining " << my_id << endl;
  if (pthread_join(the_thread, NULL) != 0)
    cerr << "pthread join failed" << endl;
}
