/* 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 "../Main/globals.h"

#include "LogWrite.h"

char const * const LogWrite::beh_name = "LogWrite";

LogWrite::LogWrite()
{
  wrote_log = false;
  fs_id = ~0;
  fs = NULL;
}

LogWrite::~LogWrite()
{
}

void
LogWrite::reset(ulong time) {
}

bool LogWrite::initConnections()
{
  EventManager *event_mgr;
  EventProcessor *fs_ep;

  event_mgr = EventManager::getManager();
  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->listenEventProcessor(beh_name,fs_id);
  fs = (FeatureSet *)(fs_ep);
  
  return true;
}

bool LogWrite::setupEventMgr(){
  return true;
};

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

const MotionCommand *LogWrite::get(ulong time)
{
  FeatureSet *lfs=NULL; // local feature set
  bool old_wrote_log=wrote_log;

  static int button_down_count = 0;

  mzero(out_command);
  
  if(fs!=NULL){
    lfs = fs->get(time);

    if(lfs->sensors->getFrame(0)->button & LogButton)
      button_down_count++;
    else
      button_down_count = 0;
      
    if(button_down_count > 25 && !wrote_log) {
      
      if(LogControlStream!=NULL) {
	pprintf(TextOutputStream,"writing log\n");
	LogControl control;
	control.command = LogControl::WriteLog;
	LogControlStream->writeBinary((const uchar *)&control,(int)sizeof(control));
	wrote_log = true;
      } else {
	pprintf(TextOutputStream,"LogControlStream is null\n");
      }
    }
  }

  if(wrote_log) {
    out_command.led.cmd = LED_MIDDLE_LEFT | LED_LOWER_RIGHT | LED_UPPER_RIGHT;
    
    if(!old_wrote_log){
      out_command.sound_cmd = SOUND_NOTE;
      out_command.sound_frequency = 2000;
      out_command.sound_duration = 1000;
    }

    out_command.head_cmd = HEAD_SCAN_BALL;
    out_command.motion_cmd = MOTION_STAND_NEUTRAL;
  }
  
  return &out_command;
}

REGISTER_EVENT_PROCESSOR(LogWrite,LogWrite::beh_name,LogWrite::create);