// -*- C++ -*-
/*!
 * @file  EchoSuppressor.cpp
 * @author Yosuke Matsusaka <yosuke.matsusaka@aist.go.jp> and OpenHRI development team
 *
 * Copyright (C) 2010
 *     Intelligent Systems Research Institute,
 *     National Institute of
 *         Advanced Industrial Science and Technology (AIST), Japan
 *     All rights reserved.
 *
 * @date $Date$
 *
 * $Id$
 */

#include "EchoSuppressor.h"

// Module specification
// <rtc-template block="module_spec">
static const char* echosuppressor_spec[] =
  {
    "implementation_id", "EchoSuppressor",
    "type_name",         "EchoSuppressor",
    "description",       "acoustic echo cancellation component",
    "version",           "0.0.1",
    "vendor",            "AIST",
    "category",          "communication",
    "activity_type",     "DataFlowComponent",
    "max_instance",      "1",
    "language",          "C++",
    "lang_type",         "script",
    ""
  };
// </rtc-template>

/*!
 * @brief constructor
 */
DataListener::DataListener(const char *name, void* data)
{
  m_obj = data;
  m_name = name;
}

/*!
 * @brief destructor
 */
DataListener::~DataListener()
{
}

void DataListener::operator ()(const ConnectorInfo& info,const TimedOctetSeq& data)
{
  EchoSuppressor *p = (EchoSuppressor *)m_obj;
  if(m_name == "ON_BUFFER_WRITE_N")
  {
    p->RcvInBuffer(data);
  }
  else if(m_name == "ON_BUFFER_WRITE_F")
  {
    p->RcvOutBuffer(data);
  }
}

/*!
 * @brief constructor
 * @param manager Maneger Object
 */
EchoSuppressor::EchoSuppressor(RTC::Manager* manager)
    // <rtc-template block="initializer">
  : RTC::DataFlowComponentBase(manager),
    m_ninIn("from_input", m_nin),
    m_finIn("from_output", m_fin),
    m_foutOut("to_output", m_fout)

    // </rtc-template>
{
}

/*!
 * @brief destructor
 */
EchoSuppressor::~EchoSuppressor()
{
}

RTC::ReturnCode_t EchoSuppressor::onInitialize()
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  addInPort("from_input", m_ninIn);
  addInPort("from_output", m_finIn);

  /* setiing datalistener event */
  m_ninIn.addConnectorDataListener(ON_BUFFER_WRITE,
                        new DataListener("ON_BUFFER_WRITE_N", this));
  m_finIn.addConnectorDataListener(ON_BUFFER_WRITE,
                        new DataListener("ON_BUFFER_WRITE_F", this));

  // Set OutPort buffer
//  registerOutPort("foutput", m_foutOut);
  addOutPort("to_output", m_foutOut);

  // Set service provider to Ports

  // Set service consumers to Ports

  // Set CORBA Service Ports

  // </rtc-template>

  return RTC::RTC_OK;
}

RTC::ReturnCode_t EchoSuppressor::onActivated(RTC::UniqueId ec_id)
{
  is_active = true;
  BufferClr();
  return RTC::RTC_OK;
}

void EchoSuppressor::BufferClr(void)
{
  m_mutex.lock();
  if (!m_indata.empty()) {
	m_indata.clear();  //!< queue buffer clear
  }
  if (!m_outdata.empty()) {
	m_outdata.clear();  //!< queue buffer clear
  }
  m_mutex.unlock();
}

void EchoSuppressor::RcvInBuffer(TimedOctetSeq data)
{
  if (is_active == true)
  {
    m_mutex.lock();
    //! The queuing does input data.
    unsigned long len = data.data.length();  //!< inport data length
    int num = (int)len / 2;
    short *buffer = new short[num];
    memcpy((void *)&(buffer[0]), (void *)&(data.data[0]), len);

    for (int i = 0; i < num; i++)
    {
      m_indata.push_back(buffer[i]);  //!< enqueue
    }
    delete [] buffer;
    m_mutex.unlock();
  }
  return;
}

void EchoSuppressor::RcvOutBuffer(TimedOctetSeq data)
{
  if (is_active == true)
  {
    //! The queuing does input data.
    m_mutex.lock();
    unsigned long len = data.data.length();  //!< inport data length
    int num = (int)len / 2;
    short *buffer = new short[num];
    memcpy((void *)&(buffer[0]), (void *)&(data.data[0]), len);
    for (int i = 0; i < num; i++)
    {
      m_outdata.push_back(buffer[i]);  //!< enqueue
    }
    delete [] buffer;
    m_mutex.unlock();
  }
  return;
}

RTC::ReturnCode_t EchoSuppressor::onExecute(RTC::UniqueId ec_id)
{
  if(!m_indata.empty())
  {
    m_mutex.lock();
    int num = (int)m_indata.size();
    short *buffer = new short[num];
    short wk;

    for(int i=0;i<num;i++)
    {
      buffer[i] = m_indata.front();
      m_indata.pop_front();
      if(!m_outdata.empty())
      {
        wk = m_outdata.front();
        m_outdata.pop_front();
        if(wk != 0)
        {
          buffer[i] = 0;
        }
      }
    }
    m_fout.data.length(num*2);  //!< set outport data length
    memcpy((void *)&(m_fout.data[0]), (void *)&(buffer[0]), num*2);
    m_foutOut.write();
    delete [] buffer;
    m_mutex.unlock();
  }
  else
  {
    if(m_outdata.size() > BUFFER_MAX)
      BufferClr();
  }
  return RTC::RTC_OK;
}

RTC::ReturnCode_t EchoSuppressor::onDeactivated(RTC::UniqueId ec_id)
{
  is_active = false;
  BufferClr();
  return RTC::RTC_OK;
}

RTC::ReturnCode_t EchoSuppressor::onFinalize()
{
  is_active = false;
  BufferClr();
  return RTC::RTC_OK;
}

/*!
 * @brief constructor
 */
EchoSuppressorManager::EchoSuppressorManager()
{
  m_manager = RTC::Manager::init(0,NULL);
  m_manager->init(0,NULL);
  m_manager->setModuleInitProc(ModuleInit);
  m_manager->activateManager();
}

/*!
 * @brief destructor
 */
EchoSuppressorManager::~EchoSuppressorManager()
{
}

void EchoSuppressorManager::start()
{
  m_manager->runManager();
}

void EchoSuppressorManager::ModuleInit(RTC::Manager* manager)
{
  RTC::RtcBase* comp;

  EchoSuppressorInit(manager);
  comp = manager->createComponent("EchoSuppressor");

  if (comp == NULL)
  {
  }
  return;
}


extern "C"
{
  void EchoSuppressorInit(RTC::Manager* manager)
  {
    coil::Properties profile(echosuppressor_spec);
    manager->registerFactory(profile,
                           RTC::Create<EchoSuppressor>,
                           RTC::Delete<EchoSuppressor>);
  }
};

int main(int argc, char** argv)
{
  EchoSuppressorManager input_manager;

  input_manager.start();
  return 0;
}
