/* 
   Project: coevolutionary GP moto driving
   Dana Vrajitoru
   agent_update.h
   
   An agent class determining when an update event should occur and
   sending a message to all the others.
  
**/

#include <iostream>
using namespace std;
#include <cmath>
#include "agent_update.h"
#include "manager.h"

// constructor
Agent_update::Agent_update() : Agent()
{
  set_nr_config();
}

// Agent-specific set data to be defined in the derivate class.
void Agent_update::set_data(float values[])
{
  int i=0;
  thr_lateral = values[i++];
  thr_front = values[i++];
  thr_speed_high = values[i++];
  thr_speed_low = values[i++];
  thr_safe_speed = values[i++];
}

// output the values
void Agent_update::output_data()
{
  cout << "nr config: " << nr_config << endl;  
  cout << "thr_lateral " << thr_lateral<< endl;
  cout << "thr_front " << thr_front << endl;
  cout << "thr_speed_high " << thr_speed_high << endl;
  cout << "thr_speed_low " << thr_speed_low << endl;
  cout << "thr_safe_speed " << thr_safe_speed << endl;
}

// Set the number of configuration values. To be defined by the
// derivate classes.
void Agent_update::set_nr_config()
{
  nr_config = 5;
  //cout << "seting the config to 5" << endl;
}

// This will be specific to each agent.
void Agent_update::update_action()
{
  get_status();

  //max_speed_rate * throttle_twist
  if (abs(left_dist-right_dist) < thr_lateral ||
      front_dist < thr_front ||
      speed > speed_limit * thr_speed_high ||
      speed < speed_limit * thr_speed_low ||
      speed > speed_limit * thr_safe_speed) {
    Event_id local_evt;
    do {
      parent->lock_evt();
      local_evt = parent->event;
      if (local_evt == null_evt)
	parent->event = update_evt;
      parent->unlock_evt();
    } while (local_evt != null_evt);
  }
}
