/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#include <ERA201D1.h>
#include "../headers/CircBufPacket.h"
#include "../headers/Reporting.h"
#include "../headers/Sensors.h"
#include "../headers/Dictionary.h"
#include "../Main/Events.h"
#include "../Main/SystemEvent.h"
#include "RobotMain.h"
#include "../WorldModel/WorldModel.h"

extern Motion::MotionLocalizationUpdateQueue *LocUpdateQueueBuffer;

static const bool dance = true;

static const bool new_event_system = true;

double MaxDX=1;
double MaxDY=1;
double MaxDA=1;

char* model="";

RobotMain RMain;

RobotMain::RobotMain()
{
  memset(&command,0,sizeof(command));

  sensorData = NULL;

  available_command_time = 0UL;
  last_sent_command_time = 0UL;

  back_pressed_count = 0;

  robot_is_paused = true;
  we_scored = false;
  opp_scored = false;
  our_old_score = 0;
  opp_old_score = 0;

  robot_state = ROBOCUP_STATE_INITIAL;
  state_changed = false;
  trans_to_play = false;
  transition_pressed_count = 0;
  team_color_pressed_count = 0;
  kickoff_pressed_count    = 0;
  robotID = -1;
  robotName = -1;
}

RobotMain &
RobotMain::GetObject()
{
  return RMain;
}

void
RobotMain::init()
{

  behave.readConfig("/MS/config/behave.cfg");

  stuck.init();

  readRobotInfo(); //load ID and name from macaddr.cfg
  teamMsgMgr.init();
  
  uint ep_id;
  EventProcessor *ep;
  
  // get connection to BehaviorManager
  event_mgr = EventManager::getManager();
  ep_id  = event_mgr->getEventProcessorId(std::string("BehaviorManager"));
  ep = event_mgr->getEventProcessor(ep_id);
  beh_mgr = dynamic_cast<BehaviorManager *>(ep);
  
  // get ids for places to inject events
  camera_frame_source_id  = event_mgr->getEventProcessorId(std::string("CameraFrameSource" ));
  camera_frame_source = dynamic_cast<CameraFrameSource *>(event_mgr->getEventProcessor(camera_frame_source_id));
  motion_update_source_id = event_mgr->getEventProcessorId(std::string("MotionUpdateSource"));
  sensor_data_id  = event_mgr->getEventProcessorId(std::string("SensorData"));
  
  // instantiate EventProcessors which need instantiated
  event_mgr->getEventProcessorId(std::string("MotionCommandSink"));
  
  ep = event_mgr->getEventProcessor(sensor_data_id);
  sensorData = dynamic_cast<SensorData *>(ep);
}

void
RobotMain::start(PacketStreamCollection *out_psc)
{
  getVision()->initializeStreams(out_psc);
  world_model.initializeStreams(out_psc);
  behave.initializeStreams(out_psc);
  //sharedWorldModel.initializeStreams(out_psc);
}

void
RobotMain::destroy()
{
  // Die(0xDEAD);
  // getVision()->close();
}

MotionCommand *RobotMain::processImage(ulong time,uchar *bytes_y,
				       uchar *bytes_u,uchar *bytes_v,
				       int width,int height,int skip, 
				       ulong sensor_frame)
{
  static EventTimeReporter reporter("RobotMain::processImage",SecToTime(5.0),
				    SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);

  updateHeadVelocities(sensorData->getByFrameNumber(sensor_frame)->headAngles);
  updateHeadPosition(sensorData->getByFrameNumber(sensor_frame)->headAngles);

  // these must be called before behave.run
  camera_frame_source->setFrameInfo(bytes_y,bytes_u,bytes_v,width,height,skip);
  getVision()->forceUpdate(time); // FIXME: this is a compatibility hack

  // FIXME: this function call is a compatibility hack until more stuff can be moved out of BehaviorSystem
  behave.prerun(&command, sensorData, &teamMsgMgr, time);
  event_mgr->injectEvent(camera_frame_source_id);
  event_mgr->injectEvent(motion_update_source_id);
  {
#ifdef PLATFORM_APERIOS
    static EventTimeReporter reporter("RobotMain::processImage::event_mgr->propagateEvents",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
    EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);
#endif
    event_mgr->propagateEvents(time);
  }

  if(new_event_system){
    const MotionCommand *cmd;

    //available_command_time = time; // for debugging use only
    if(available_command_time > last_sent_command_time){
#ifdef PLATFORM_APERIOS
      static EventTimeReporter reporter("RobotMain::processImage::beh_mgr->get",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
      EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);
#endif
      cmd = beh_mgr->get(time);
      last_sent_command_time = time;
      if(cmd!=NULL)
        command = *cmd;
    }
  }else{
    behave.run(&command, sensorData, &teamMsgMgr, time);
  }

  if(behave.setupStates==0) {
    if(robot_is_paused) {
      if(dance && we_scored){
	command.motion_cmd = MOTION_GOAL_HAPPY1 + (our_old_score%3);
	if(command.motion_cmd == MOTION_GOAL_HAPPY3){
	  //wag tail here
	  command.tail_cmd = TAIL_AIM;
	  command.tail_pan  = (ushort)(100 * sin(GetTime() * 2 * M_PI/ 0.2e6));
	  command.tail_tilt = (ushort)(100 * cos(GetTime() * 2 * M_PI/ 0.2e6));
	}
	we_scored = false;
      }else if(dance && opp_scored){
	command.motion_cmd = MOTION_GOAL_SAD;
	command.tail_cmd = TAIL_AIM;
	command.tail_pan = 0;
	command.tail_tilt = -100;
	opp_scored = false;
      }else{
	//stand neutral unless motion is getup motion
	int top_state = MOTION_STATE(command.motion_cmd);
	if(top_state != STATE_GETUP){
	  command.motion_cmd = MOTION_STAND_NEUTRAL;
	}
	/* Didn't this indicate something about our jersey
	   color? I'll add that later - for now a visible indication
	   that we're paused would be nice.
	*/
	command.tail_cmd = TAIL_AIM;
	command.tail_pan = 100;
	command.tail_tilt = 100;
      }
    }
  }

  return &command;
}

void
RobotMain::updateHeadVelocities(const double *head_angles)
{
  getVision()->setHeadVelocities(head_angles);
}

void
RobotMain::updateHeadPosition(const double *head_angles)
{
  getVision()->setHeadAngles(head_angles);
}

void RobotMain::processSensorFrame(ulong time)
{
  if(LocUpdateQueueBuffer!=NULL){
    MotionLocalizationUpdate *update = LocUpdateQueueBuffer->getLatest();

    if(update->timestamp > 0){
      posture.update(*update);
      posture.update(*sensorData);
      behave.setPosture(posture.getPosture());
      for(int i=-3; i<=0; i++)
	stuck.update(sensorData->getFrame(i)->accel);
      behave.setStuckInfo(stuck.getStuckInfo());
      teamMsgMgr.setStuckInfo(stuck.getStuckInfo());
    }
  }

  /**
   * It is specified in behave.cfg not to run the setup
   * states that were made to comply with RoboCup 2004 rules.
   */
  if(behave.setupStates==0){
    if(sensorData->button & BackButton){
      back_pressed_count++;
      //pprintf(TextOutputStream,"NEW Back Pressed=%d\n",back_pressed_count);
      
      if(back_pressed_count > 
	 (!robot_is_paused ? 500/32 : 64/32)){
	robot_is_paused = !robot_is_paused;
	
	//pprintf(TextOutputStream,"NEW Toggle Enable Motion\n");
	back_pressed_count = -500/32;
      }
    }else{
      back_pressed_count = 0;
      // motion.setLEDState(led,0);
    }
  }
  
  event_mgr->injectEvent(sensor_data_id);
  {
#ifdef PLATFORM_APERIOS
    static EventTimeReporter reporter("RobotMain::processSensorFrame::event_mgr->propagateEvents",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
    EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);
#endif
    event_mgr->propagateEvents(time);
  }
}

Vision *RobotMain::getVision()
{
  EventManager *event_mgr;
  uint vis_id;

  event_mgr = EventManager::getManager();
  vis_id = event_mgr->getEventProcessorId("Vision");

  return dynamic_cast<Vision *>(event_mgr->getEventProcessor(vis_id));
}

void RobotMain::newMotionCommandAvailable(ulong time)
{
  available_command_time = time;
}


void RobotMain::readRobotInfo(){

  Dictionary addr_info;
  char field_name[64];
  const char *field_value;
  addr_info.read("/MS/config/macaddr.cfg");

  int mac[6];
  int ip[4];
  int id;
  char name[32];
  
  // Load my MAC address and look up my ID based on it.
  EtherDriverGetMACAddressMsg msg; 
  ERA201D1_GetMACAddress(&msg);
  
  // Read all of our ers210 fields.
  for(int i=0; i<MAX_BOTS; i++) {
    sprintf(field_name, "ers210_%02d", i);

    // see if there is an entry for this robot name
    if(addr_info.getValueString(field_name, field_value)) {
      // okay, this robot exists in the file. Make sure we
      // can read all of the info.

      if(sscanf(field_value, "MAC=%X:%X:%X:%X:%X:%X IP=%d.%d.%d.%d ID=%d NAME=%s",
		&mac[0], &mac[1], &mac[2], 
		&mac[3], &mac[4], &mac[5],
		&ip[0],&ip[1],&ip[2],&ip[3],
		&id, name)!=12) {
	cout << "died reading: " << field_name << endl;
      } else {
	//check if that mac address matches our own
	bool match = true;
	for(int b=0; b<6; b++) {
	  if(msg.address.octet[b]!=mac[b]) {
	    match = false;
	    break;
	  }
	}
     }
    }
  }

  // Read all of our ers7 fields
  for(int i=0; i<MAX_BOTS; i++) {
    sprintf(field_name, "ers7_%02d", i);
    
    // see if there is an entry for this robot name
    if(addr_info.getValueString(field_name, field_value)) {
      // okay, this robot exists in the file. Make sure we
      // can read all of the info.
      
      if(sscanf(field_value, "MAC=%X:%X:%X:%X:%X:%X IP=%d.%d.%d.%d ID=%d NAME=%s",
		&mac[0], &mac[1], &mac[2], 
		&mac[3], &mac[4], &mac[5],
		&ip[0],&ip[1],&ip[2],&ip[3],
		&id, name)!=12) {
	cout << "died reading: " << field_name << endl;
      } else {
	//check if that mac address matches our own
	bool match = true;
	for(int b=0; b<6; b++) {
	  if(msg.address.octet[b]!=mac[b]) {
	    match = false;
	    break;
	  }
	}
	if(match) {
	  robotID = id;
	  if(strcmp(name,"Moon")==0)
	    robotName = MOON;
	  if(strcmp(name,"Cloud")==0)
	    robotName = CLOUD;
	  if(strcmp(name,"Silver")==0)
	    robotName = SILVER;
	  if(strcmp(name,"Mist")==0)
	    robotName = MIST;
	  if(strcmp(name,"Ice")==0)
	    robotName = ICE;
	  if(strcmp(name,"Snow")==0)
	    robotName = SNOW;
	  if(strcmp(name,"Frost")==0)
	    robotName = FROST;
	  if(strcmp(name,"Cotton")==0)
	    robotName = COTTON;
	 
	  break;
	}
	
      }
    }
  }
  
}

int
RobotMain::getRobotName(){
  return robotName;
}

int
RobotMain::getRobotID(){
  return robotID;
}