/* 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 <iostream>
#include <string>
#include <vector>

using namespace std;

#include "../headers/CircBufPacket.h"
#include "../headers/FileSystem.h"
#include "../Behaviors/Behavior.h"
#include "../Main/RobotMain.h"
#include "../Motion/MotionInterface.h"
#include "Events.h"
#include "SystemEvent.h"

// ===== BehaviorManager implementation ===================================== //

std::string BehaviorManager::name("BehaviorManager");

BehaviorManager::BehaviorManager()
{
}

BehaviorManager::~BehaviorManager()
{
}

void BehaviorManager::addBehavior(const std::string &name)
{
  //cout << "adding behavior " << name  << endl;

  uint beh_id;
  IndependentBehavior *beh;
  EventManager *event_mgr;

  event_mgr = EventManager::getManager();
  beh_id = event_mgr->getEventProcessorId(name);
  if(beh_id==~0UL){
    pprintf(TextOutputStream,"unable to find behavior '%s', skipping\n",name.c_str());
    return;
  }
  beh = dynamic_cast<IndependentBehavior *>(event_mgr->getEventProcessor(beh_id));
  if(beh==NULL){
    pprintf(TextOutputStream,"'%s' is not a behavior, skipping\n",name.c_str());
    return;
  }

  event_mgr->listenEventProcessor(my_id,beh_id);

  BehaviorInfo info;
  info.beh = beh;
  behaviors.push_back(info);
}

bool BehaviorManager::initConnections()
{
  my_id = EventManager::getManager()->getEventProcessorId(name);

  HFS::FILE *beh_cfg_file;

  beh_cfg_file = HFS::fopen("/MS/config/run.cfg","rt");

  if(beh_cfg_file == NULL)
    return false;

  char buf[1024];
  while(HFS::fgets(buf,sizeof(buf),beh_cfg_file)!=NULL){
    bool comment=false; // comment and/or white space only
    bool done;
    char *cur_pos;

    done = false;
    for(cur_pos=buf; !done && *cur_pos!='\00'; cur_pos++){
      switch(*cur_pos){
        case ' ':
        case '\t': 
        case '\n':
          break;
        case '#':
          comment = true;
          done = true;
          break;
        default:
          comment = false;
          done = true;
          break;
      }
    }

    if(comment){
      cout << "no non-whitespace/comments found in line '" << buf << "'" << endl;
      //pprintf(TextOutputStream,"no non-whitespace/comments found in line '%s'\n",buf);
      continue;
    }

    char beh_name[1024];
    int temp_priority;
    if(sscanf(buf," %s ",beh_name)==1){
      addBehavior(std::string(beh_name));
    }else if(sscanf(buf," %s %d ",beh_name,&temp_priority)==2){
      addBehavior(std::string(beh_name));
    }else{
      cout << "error parsing run.cfg line '" << buf << "', ignoring" << endl;
      //pprintf(TextOutputStream,"error parsing run.cfg line '%s', ignoring\n",buf);
    }
  }

  HFS::fclose(beh_cfg_file);

  return true;
}

bool BehaviorManager::update(ulong time,const EventList *events)
{
  return true;
}

const Motion::MotionCommand *BehaviorManager::get(ulong time)
{
  int led_idx;
  int leg_pri,head_pri,tail_pri,led_pri;
  leg_pri = head_pri = tail_pri = led_pri = -1;

  //pprintf(TextOutputStream,"BehaviorManager has %d behaviors\n",behaviors.size());
  if(behaviors.size()==0)
    return NULL;


#define COPY_CMD(x) command.x = new_cmd->x;
  mzero(command);
  for(uint i=0; i<behaviors.size(); i++){
    const Motion::MotionCommand *new_cmd;
    int beh_pri;
    
    beh_pri = i;
    new_cmd = behaviors[i].beh->get(time);

    if(new_cmd->motion_cmd!=0 && beh_pri>leg_pri){
      leg_pri = beh_pri;

      COPY_CMD(motion_cmd);
      COPY_CMD(bound_mode);
      COPY_CMD(vx);
      COPY_CMD(vy);
      COPY_CMD(va);
      COPY_CMD(sound_cmd);
      COPY_CMD(sound_frequency);
      COPY_CMD(sound_duration);
      COPY_CMD(sound_file);
    }
    if(new_cmd->head_cmd!=0 && beh_pri>head_pri){
      head_pri = beh_pri;

      COPY_CMD(head_cmd);
      COPY_CMD(head_lookat);
      COPY_CMD(head_tilt_offset);
      COPY_CMD(head_tilt);
      COPY_CMD(head_pan);
      COPY_CMD(head_roll);
      COPY_CMD(head_tilt2);
      COPY_CMD(mouth);
    }
    if(new_cmd->tail_cmd!=0 && beh_pri>tail_pri){
      tail_pri = beh_pri;

      COPY_CMD(tail_cmd);
      COPY_CMD(tail_pan);
      COPY_CMD(tail_tilt);
    }
    if((new_cmd->led.set) && beh_pri>led_pri){
      led_pri = beh_pri;      
      command.led.cmd |= new_cmd->led.cmd;
      
      //if any of the face LEDs are set, get the mode
      //for ERS210 mode will be ignored 
      for(led_idx = 6; led_idx < 20; led_idx++){
	if((new_cmd->led.cmd >> led_idx) & 1){
	  command.led.mode = new_cmd->led.mode;
	  break;
	}
      }
      
      for(led_idx=0; led_idx<NUM_LEDS; led_idx++) {
	if((new_cmd->led.cmd >> led_idx) & 1){
	  command.led.intensity[led_idx] = new_cmd->led.intensity[led_idx];
	}
      }
     }
  }
#undef COPY_CMD

  return &command;
}

REGISTER_EVENT_PROCESSOR(BehaviorManager,BehaviorManager::name,BehaviorManager::create);

// ===== CameraFrameSource implementation =================================== //

std::string CameraFrameSource::name("CameraFrameSource");

void CameraFrameSource::setFrameInfo(uchar *bytes_y,uchar *bytes_u,uchar *bytes_v,int width,int height,int skip)
{
  info.bytes_y = bytes_y;
  info.bytes_u = bytes_u;
  info.bytes_v = bytes_v;
  info.width   = width;
  info.height  = height;
  info.skip    = skip;
}

bool CameraFrameSource::update(ulong time,const EventList *events)
{
  return true;
}

const CameraFrameSource::CameraFrameInfo *CameraFrameSource::get(ulong time) const
{
  return &info;
}

REGISTER_EVENT_PROCESSOR(CameraFrameSource,CameraFrameSource::name,CameraFrameSource::create);

// ===== MotionUpdateSource implementation =================================== //

std::string MotionUpdateSource::name("MotionUpdateSource");

MotionUpdateSource::MotionUpdateSource()
{
  queue = NULL;
  last_timestamp = 0UL;
  last_idx = 0;
}

MotionUpdateSource::~MotionUpdateSource()
{
}

bool MotionUpdateSource::update(ulong time,const EventList *events)
{
  if(queue==NULL)
    queue = LocUpdateQueueBuffer;
  if(queue==NULL)
    return false;

  bool done = false;
  bool new_info = false;
  while(!done){
    ulong buffer_timestamp=queue->buffer[last_idx].timestamp;
    if(last_timestamp < buffer_timestamp && buffer_timestamp <= time){
      new_info = true;

      last_timestamp=buffer_timestamp;      
      last_idx = (last_idx+1) % MotionLocalizationUpdateQueueBufferSize;
    } else {
      done=true;
    }
  }  

  return new_info;
}

Motion::MotionLocalizationUpdateQueue *MotionUpdateSource::get(ulong time)
{
  return queue;
}

REGISTER_EVENT_PROCESSOR(MotionUpdateSource,MotionUpdateSource::name,MotionUpdateSource::create);

// ===== MotionCommandSink implementation =================================== //

std::string MotionCommandSink::name("MotionCommandSink");

MotionCommandSink::MotionCommandSink()
{
}

MotionCommandSink::~MotionCommandSink()
{
}

bool MotionCommandSink::initConnections()
{
  EventManager *event_mgr;
  EventProcessor *beh_mgr_ep;
  uint beh_mgr_id;

  event_mgr = EventManager::getManager();
  beh_mgr_id = event_mgr->getEventProcessorId(BehaviorManager::name);
  beh_mgr_ep = event_mgr->listenEventProcessor(name,beh_mgr_id);
  beh_mgr = dynamic_cast<BehaviorManager *>(beh_mgr_ep);

  return true;
}

bool MotionCommandSink::update(ulong time,const EventList *events)
{
  RobotMain::GetObject().newMotionCommandAvailable(time);

  return true;
}

REGISTER_EVENT_PROCESSOR(MotionCommandSink,MotionCommandSink::name,MotionCommandSink::create);