/* 
   Project: coevolutionary GP moto driving
   Dana Vrajitoru
      Modified by: Russel Mehler
      Modified by: Poornima Konnanur
   pilot_manager.cc
   
   The class that manages the automatic pilot.
  
**/

#include <iostream>
#include <fstream>
#include <iomanip>
#include <ctime>
using namespace std;
#include <stdlib.h>
#include "pilot_manager.h"
#include "agent.h"
#include "agent_gas.h"
#include "agent_breaks.h"
#include "agent_hbar.h"
#include "agent_lean.h"
#include "agent_steering.h"
#include "agent_update.h"

//Agent **components;
//int nr_agents;
float speed_val = 0.0;

// Default constructor.
Pilot_manager::Pilot_manager()
{
  int i;
  boss = NULL;
  pthread_mutex_init(&mutex, NULL);
  pthread_mutex_unlock(&mutex);
  pthread_mutex_init(&mutex_evt, NULL);
  pthread_mutex_unlock(&mutex_evt);
  new_agents();
  for (i=null_evt; i<=terminate_evt; i++)
    count[i]=0;
  event = null_evt;
  srand(time(NULL));
  sequential = false;
}

// Constructor with known manager. Only the individual components
// need to know what this is.
Pilot_manager::Pilot_manager(Manager *the_boss, float timing)
{
  int i;
  tic = timing;
  boss = the_boss;
  pthread_mutex_init(&mutex, NULL);
  pthread_mutex_unlock(&mutex);
  pthread_mutex_init(&mutex_evt, NULL);
  pthread_mutex_unlock(&mutex_evt);
  new_agents();
  for (i=0; i<nr_agents; i++) 
    components[i]->set_timing(timing);
  for (i=null_evt; i<=terminate_evt; i++) {
    //cout << i << endl;
    count[i]=0;
  }
  //cout << "count 0" << endl;
  event = null_evt;
  sequential = false;
}

// Creates the agents with the appropriate classes.
void Pilot_manager::new_agents()
{
  nr_agents = 4;
  components = new (Agent *)[nr_agents];
  components[update_agt] =  new Agent_update();
  //cout << "new agent update" << endl;
  components[gas_agt] = new Agent_gas();
  //cout << "new agent gas" << endl;
  components[breaks_agt] = new Agent_breaks();
  //cout << "new agent breaks" << endl;
  //components[steer_agt] = new Agent_lean();//Agent_hbar(); // lean
  components[steer_agt] = new Agent_hbar(); // lean
  //cout << "new agent steering" << endl;
  for (int i=0; i<nr_agents; i++) {
    components[i]->set_id(i);
    components[i]->set_parent(this);
    if (boss)
      components[i]->set_manager(boss);
  }
}

// Desctructor. 
Pilot_manager::~Pilot_manager()
{
  int i;
  //terminate();
  //cout << "count of events: " << endl; 
  //cout << 0 << ' ' << endl;
  //for (i=null_evt; i<=terminate_evt; i++)
  //  cout << i << ' ' << count[i] << endl;
  //cout << endl;
  // for (i=0; i<nr_agents; i++) 
  //  delete components[i];
  delete components;
  pthread_mutex_destroy(&mutex);
  pthread_mutex_destroy(&mutex_evt);
}

// Set the manager is we find out about it afterwards.
void Pilot_manager::set_manager(Manager *the_boss)
{
  boss = the_boss;
  for (int i=0; i<nr_agents; i++) 
    components[i]->set_manager(the_boss);
}

// To reset the timing lapse
void Pilot_manager::set_timing(float timing)
{
  for (int i=0; i<nr_agents; i++) 
    components[i]->set_timing(timing);
}

// Set the speed limitation.
void Pilot_manager::set_speed_limit(float speed)
{
  speed_limit = speed;
  for (int i=0; i<nr_agents; i++) 
    components[i]->set_speed_limit(speed);
}

// Read the constants and thresholds from the configuration file.
void Pilot_manager::read(char *filename)
{
  ifstream fin(filename);
  char buffer[80], ch;
  float values[15];
  cout << "in read config file: " << filename << endl;
  if (fin) {
    cout << "fin good" << endl;
    for (int i=0; i<nr_agents; i++) {
      cout << components[i]->nr_config << endl;
      fin.getline(buffer, 80);   // the agent name
      //cout << buffer << endl;
      for (int j=0; j<components[i]->nr_config; j++) {
	fin.getline(buffer, 80); // the value name
	fin >> values[j];    // the value itself
	do
	  fin.get(ch);
	while (ch != '\n');
      }
      if (i<nr_agents-1)
	fin.getline(buffer, 80);   // a blanc line
      //cout << buffer << endl;
      components[i]->set_data(values);
      //components[i]->output_data();
    }
    //---------------------------------------------------------------
    // added by pk

    fin.getline(buffer, 80); // skip an empty line
    fin.getline(buffer,80);  // skip the "Speed limit" line
    
    fin >> speed_val;
    // setting speed limit by reading
    // from pilot_config.txt
    set_speed_limit(speed_val);
    //cout << "speed value from pilot_config.txt " << speed_val << endl;
    //---------------------------------------------------------------
    fin.close();
  }
}

void Pilot_manager::read_param(double *param_arr, int param_size)
{
  int param_inc = 0;

  //store the values for each agent
  // and then set_data func is called
  float values[15];
  
  for (int i=0; i<nr_agents; i++) {
    for (int j=0; j<components[i]->nr_config; j++) {
      //cout << components[i]->nr_config << endl;
      values[j] = param_arr[param_inc];
      //cout << "param array : " << param_inc << " values  " 
      //<< param_arr[param_inc] << endl;
      param_inc++;
    }
    components[i]->set_data(values);
    //components[i]->output_data();
  }
  
  set_speed_limit(5);
  //setting the last value sent in param array for speed
  // set_speed_limit(param_arr[param_size - 1]);
  // cout << "param array : " << param_inc << " values " << 
  // param_arr[param_size - 1] <<endl; 

}

// Launch all the agents.
void Pilot_manager::launch()
{
  for (int i=0; i<nr_agents; i++) 
    components[i]->launch();
}

// Sends the same event to all the agents
void Pilot_manager::broadcast_event(Event_id the_event)
{
  Event_id local_event = null_evt;
  int i;
  for (i=0; i<nr_agents; i++) {
    cout << "agent " << i << endl;
    do {
      lock_evt();
      local_event = event;
      cout << "event " << local_event << endl;
      if (event == null_evt) 
	event=the_event;
      unlock_evt();
    } while (local_event != null_evt);
    cout << "in broadcast event " << the_event << endl;
  }
  lock_evt();
  for (i=null_evt; i<=terminate_evt; i++)
    cout << i << ' ' << count[i] << endl;
  cout << endl;
  unlock_evt();
}

// Turns all the agents on.
void Pilot_manager::turn_on()
{
  broadcast_event(turn_on_evt);
}

// Turns all the agents off.
void Pilot_manager::turn_off()
{
  broadcast_event(turn_off_evt);
}
  
// Terminates all the agents.
void Pilot_manager::terminate()
{
  broadcast_event(terminate_evt);
  for (int i=0; i<nr_agents; i++)
    components[i]->join();
}

// Locks the event mutex.
void Pilot_manager::lock_evt()
{
  if (!sequential)
    pthread_mutex_lock(&mutex_evt);
}

// Unlock the event mutex when we're done.
void Pilot_manager::unlock_evt()
{
  if (!sequential)
    pthread_mutex_unlock(&mutex_evt);
}

// Locks the mutex while we move the motorcycle from the exterior.
void Pilot_manager::lock()
{
  if (!sequential)
    pthread_mutex_lock(&mutex);
}

// Unlock the mutex when we're done.
void Pilot_manager::unlock()
{
  if (!sequential)
    pthread_mutex_unlock(&mutex);
}

// Turn the sequential flag on and off.
void Pilot_manager::make_seq()
{
  sequential = true;
}

void Pilot_manager::make_multi_thr()
{
  sequential = false;
}

// A sequential probabilistic update.
void Pilot_manager::sequential_update()
{
  if (sequential) {
    float on=(rand() % 1000)/1000.0;
    event = null_evt;
    components[0]->update_action();
    if (on <= tic || event == update_evt) {
      int agent = 1+ rand() % (nr_agents+1);
      if (agent == 4)
	agent = 3;
      if (agent == 5)
	agent = 1;
      //if (agent == 1 || agent == 3)
      components[agent]->update_action();
    }
  }
}
