// -*- C++ -*-
/*!
 * @file  SamplingRateConverter.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 "SamplingRateConverter.h"

// Module specification
// <rtc-template block="module_spec">
static const char* samplingrateconverter_spec[] =
  {
    "implementation_id", "SamplingRateConverter",
    "type_name",         "SamplingRateConverter",
    "description",       "Sampling Rate Converter",
    "version",           "0.0.1",
    "vendor",            "AIST",
    "category",          "communication",
    "activity_type",     "DataFlowComponent",
    "max_instance",      "1",
    "language",          "C++",
    "lang_type",         "script",
    "conf.default.InputSampleRate", "16000",
    "conf.default.OutputSampleRate", "16000",
    "conf.__widget__.InputSampleRate", "spin",
    "conf.__constraints__.InputSampleRate", "x >= 1",
    "conf.__widget__.OutputSampleRate", "spin",
    "conf.__constraints__.OutputSampleRate", "x >= 1",
    ""
  };
// </rtc-template>
typedef double TYPE;
/*!
 * @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)
{
  if(m_name == "ON_BUFFER_WRITE")
  {
    SamplingRateConverter *p = (SamplingRateConverter *)m_obj;
    p->RcvInBuffer(data);
  }
}

/*!
 * @brief constructor
 * @param manager Maneger Object
 */
SamplingRateConverter::SamplingRateConverter(RTC::Manager* manager)
    // <rtc-template block="initializer">
  : RTC::DataFlowComponentBase(manager),
    m_inIn("from_output", m_in),
    m_outOut("to_output", m_out)

    // </rtc-template>
{
}

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

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

  /* setiing datalistener event */
  m_handle = NULL;
  m_inIn.addConnectorDataListener(ON_BUFFER_WRITE, new DataListener("ON_BUFFER_WRITE", this), false);

  // Set OutPort buffer
  addOutPort("to_output", m_outOut);

  // Set service provider to Ports

  // Set service consumers to Ports

  // Set CORBA Service Ports

  // </rtc-template>
  bindParameter("InputSampleRate", m_in_rate, "16000");
  bindParameter("OutputSampleRate", m_out_rate, "16000");

  return RTC::RTC_OK;
}

RTC::ReturnCode_t SamplingRateConverter::onActivated(RTC::UniqueId ec_id)
{
  m_ratio = (double)m_out_rate / (double)m_in_rate;
  if(!m_handle)
  {
    m_handle = resample_open(1, m_ratio, m_ratio);
  }
  is_active = true;

  if(!m_queue.empty())
  {
    m_queue.clear();
  }
  return RTC::RTC_OK;
}

void SamplingRateConverter::RcvInBuffer(TimedOctetSeq data)
{
  if (is_active == true)
  {
    //! The queuing does input data.
    unsigned long len = data.data.length();  //!< inport data length
    if( m_ratio == 1 )
    {
      m_out.data.length(len);  //!< set outport data length
      memcpy((void *)&(m_out.data[0]), (void *)&(data.data[0]), len);
      m_outOut.write();
    }
    else
    {
      m_mutex.lock();
      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_queue.push_back(buffer[i]);  //!< enqueue
      }
      delete [] buffer;
      m_mutex.unlock();
    }
  }
  return;
}

RTC::ReturnCode_t SamplingRateConverter::onExecute(RTC::UniqueId ec_id)
{
  if((!m_queue.empty()) && ( m_ratio != 1 ))
  {
    m_mutex.lock();
    int num = m_queue.size();
    int dnum = (int)(num * m_ratio);
    int i = 0;
    float *src = new float[num];
    float *dst = new float[dnum];
    for(i=0;i<num;i++)
    {
      src[i] = (float)m_queue.front();
      m_queue.pop_front();
    }

    int o = 0, srcused = 0, srcpos = 0, out = 0;

    for(;;)
    {
      int srcBlock =num - srcpos;
      o = resample_process(  m_handle,
                             m_ratio,
                             &src[srcpos],
                             srcBlock,
                                 0,
                             &srcused,
                             &dst[out],
                             dnum - out);
      srcpos += srcused;
      if (o >= 0)
      {
        out += o;
      }
      if (o < 0 || (o == 0 && srcpos == num))
      {
        break;
      }
    }
    delete [] src;

    short *dbuffer = new short[dnum];
    for(i=0;i<dnum;i++)
    {
      dbuffer[i] = (short)dst[i];
    }
    delete [] dst;

    int dlen = dnum * 2;
    m_out.data.length(dlen);  //!< set outport data length
    memcpy((void *)&(m_out.data[0]), (void *)&(dbuffer[0]), dlen);
    m_outOut.write();
    delete [] dbuffer;
    m_mutex.unlock();
  }
  return RTC::RTC_OK;
}

RTC::ReturnCode_t SamplingRateConverter::onDeactivated(RTC::UniqueId ec_id)
{
  is_active = false;
  m_mutex.lock();
  if(!m_queue.empty())
  {
    m_queue.clear();
  }
  if(m_handle)
  {
    resample_close(m_handle);
    m_handle = NULL;
  }
  m_mutex.unlock();

  return RTC::RTC_OK;
}

RTC::ReturnCode_t SamplingRateConverter::onFinalize()
{
  is_active = false;
  m_mutex.lock();
  if(!m_queue.empty())
  {
    m_queue.clear();
  }
  if(m_handle)
  {
    resample_close(m_handle);
    m_handle = NULL;
  }
  m_mutex.unlock();
  return RTC::RTC_OK;
}

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

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

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

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

  SamplingRateConverterInit(manager);
  comp = manager->createComponent("SamplingRateConverter");

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


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

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

  input_manager.start();
  return 0;
}
