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

// Module specification
// <rtc-template block="module_spec">
static const char* portaudiooutput_spec[] =
  {
    "implementation_id", "PortAudioOutput",
    "type_name",         "PortAudioOutput",
    "description",       "Audio output component using portaudio (C++ implementation)",
    "version",           "0.0.1",
    "vendor",            "AIST",
    "category",          "communication",
    "activity_type",     "DataFlowComponent",
    "max_instance",      "1",
    "language",          "C++",
    "lang_type",         "script",
    "conf.default.OutputSampleRate", "16000",
    "conf.default.OutputSampleByte", "int16",
    "conf.default.OutputChannelNumbers", "1",
    "conf.default.DelayCount", "0",
    "conf.__widget__.OutputSampleByte", "radio",
    "conf.__constraints__.OutputSampleByte", "(int8, int16, int24)",
    "conf.__widget__.OutputChannelNumbers", "spin",
    "conf.__constraints__.OutputChannelNumbers", "x >= 1",
    "conf.__widget__.OutputSampleRate", "spin",
    "conf.__constraints__.OutputSampleRate", "x >= 1",
    "conf.__widget__.DelayCount", "spin",
    "conf.__constraints__.DelayCount", "x >= 0",
    ""
  };
// </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)
{
  if(m_name == "ON_BUFFER_WRITE")
  {
    PortAudioOutput *p = (PortAudioOutput *)m_obj;
    p->RcvBuffer(data);
  }
}

int StreamCB( const void *inputBuffer,
			  void *outputBuffer,
			  unsigned long framesPerBuffer,
			  const PaStreamCallbackTimeInfo *timeInfo,
			  PaStreamCallbackFlags statusFlags,
			  void *userData )
{
  PortAudioOutput *p = (PortAudioOutput *)userData;
  unsigned long nbytes = p->m_totalframes;
  if(FRAMES_PER_BUFFER != framesPerBuffer){
    nbytes = framesPerBuffer * p->m_totalframes / FRAMES_PER_BUFFER;
  }
  p->WriteBufferCB(outputBuffer, nbytes);
  return 0;
}

/*!
 * @brief constructor
 */
PortAudioOutput::PortAudioOutput(RTC::Manager* manager)
    // <rtc-template block="initializer">
  : RTC::DataFlowComponentBase(manager),
    m_in_dataIn("SampleInPort0", m_in_data),
    m_out_dataOut("OutputData", m_out_data)

    // </rtc-template>
{
}

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

RTC::ReturnCode_t PortAudioOutput::onInitialize()
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  addInPort("SampleInPort0", m_in_dataIn);
//  registerInPort("SampleInPort0", m_in_dataIn);

  /* setiing datalistener event */
  m_in_dataIn.addConnectorDataListener(ON_BUFFER_WRITE, new DataListener("ON_BUFFER_WRITE", this), false);
  // Set OutPort buffer
  registerOutPort("OutputData", m_out_dataOut);

  // Set service provider to Ports

  // Set service consumers to Ports

  // Set CORBA Service Ports

  // </rtc-template>
  bindParameter("OutputSampleRate", m_samplerate, "16000");
  bindParameter("OutputSampleByte", m_formatstr, "int16");
  bindParameter("OutputChannelNumbers", m_channels, "1");
  bindParameter("DelayCount", m_delay, "0");

  return RTC::RTC_OK;
}

PaSampleFormat PortAudioOutput::getFormat(std::string str)
{
  if(str == "int8")
  {
    return paInt8;
  }
  else if (str == "int16")
  {
    return paInt16;
  }
  else if (str == "int24")
  {
    return paInt24;
  }
  else
  {
    return paInt16;
  }
}

RTC::ReturnCode_t PortAudioOutput::onActivated(RTC::UniqueId ec_id)
{
  PaStreamParameters outputParameters;

  try {
    m_err = Pa_Initialize();
    if( m_err != paNoError )
    {
      throw m_err;
    }

    m_format = getFormat(m_formatstr);
    m_totalframes = FRAMES_PER_BUFFER * m_channels;
    m_err = Pa_GetSampleSize(m_format);
    if( m_err > 0 )
    {
      m_totalframes *= m_err;
    }
    m_data.clear();  //!< queue buffer clear

    /* stream setiing */
    outputParameters.device = Pa_GetDefaultOutputDevice(); /* default output device */
    if( outputParameters.device < 0 )
    {
      throw (paNotInitialized);
    }
    outputParameters.channelCount = m_channels;
    outputParameters.sampleFormat = m_format;
    outputParameters.suggestedLatency = Pa_GetDeviceInfo( outputParameters.device )->defaultLowInputLatency;
    outputParameters.hostApiSpecificStreamInfo = NULL;
#if 0
    m_err = Pa_OpenStream(
          &m_stream,         //!< PortAudioStream
          NULL,              //!< InputParameters
          &outputParameters, //!< outputParameters
          m_samplerate,      //!< sampleRate
          FRAMES_PER_BUFFER, //!< framesPerBuffer
          paClipOff,         //!< streamFlags:we won't output out of range samples so don't bother clipping
          NULL,              //!< streamCallback:no callback, use blocking API
          NULL );            //!< no callback, so no callback userData
#endif
	m_err = Pa_OpenStream(
          &m_stream,         //!< PortAudioStream
          NULL,              //!< InputParameters
          &outputParameters, //!< outputParameters
          m_samplerate,      //!< sampleRate
          FRAMES_PER_BUFFER, //!< framesPerBuffer
          paClipOff,         //!< streamFlags:we won't output out of range samples so don't bother clipping
          StreamCB,          //!< streamCallback
          this );            //!< callback userData

    if( m_err != paNoError )
    {
      throw m_err;
    }

    m_err = Pa_StartStream( m_stream );
    if( m_err != paNoError )
    {
      throw m_err;
    }

  } catch (...) {
    std::cout << Pa_GetErrorText(m_err) << std::endl;
  }

  if (!m_data.empty()) {
    m_data.clear();  //!< queue buffer clear
  }
  is_active = true;
  m_waitflg = true;

  return RTC::RTC_OK;
}

RTC::ReturnCode_t PortAudioOutput::onExecute(RTC::UniqueId ec_id)
{
//  WriteBuffer();

  return RTC::RTC_OK;
}

void PortAudioOutput::RcvBuffer(TimedOctetSeq data)
{
  if (is_active == true) {
    //! The queuing does input data.
    m_mutex.lock();
    unsigned long i = 0;
    if(m_waitflg == true)
    {
      m_waitflg = false;
      if(m_delay > 0){
        for(i = 0; i < (m_delay * m_totalframes); i++)
        {
          m_data.push_back(0);  //!< enqueue
        }
      }
    }
    unsigned long len = data.data.length();  //!< inport data length
    for (i = 0; i < len; i++)
    {
      m_data.push_back(data.data[i]);  //!< enqueue
    }
    m_mutex.unlock();
  }
  return;
}

void PortAudioOutput::WriteBufferCB(void *data, unsigned long fream_len)
{
  m_mutex.lock();
  char *buff = (char *)data;
  for (unsigned long i = 0; i < fream_len; i++)
  {
    if ( !m_data.empty() )
    {
      *buff++ = m_data.front();
      m_data.pop_front();  //!< dequeue
    }
    else
    {
      *buff++ = 0;
      m_waitflg = true;
    }
  }
  m_mutex.unlock();

  m_out_data.data.length(fream_len);  //!< set outport data length
  memcpy((void *)&(m_out_data.data[0]), data, fream_len);
  m_out_dataOut.write();

  return;
}

int PortAudioOutput::WriteBuffer(void)
{
  //! Input data(queue buffer) is output to the device
  m_mutex.lock();
  if ( Pa_IsStreamActive(m_stream) )
  {
    if( FRAMES_PER_BUFFER <= (unsigned long)(Pa_GetStreamWriteAvailable(m_stream)) )
    {
      unsigned char *buffer;  //!< Temporary buffer
      buffer = new unsigned char[m_totalframes];  //!< Temporary buffer

      for (unsigned long i = 0; i < m_totalframes; i++)
      {
        if ( !m_data.empty() ){
          buffer[i] = m_data.front();
          m_data.pop_front();  //!< dequeue
        }
		else{
          buffer[i] = 0;
          m_waitflg = true;
        }
      }

      m_out_data.data.length(m_totalframes);  //!< set outport data length
      memcpy((void *)&(m_out_data.data[0]), buffer, m_totalframes);
      m_out_dataOut.write();

      //! write stream
      m_err = Pa_WriteStream( m_stream, buffer, FRAMES_PER_BUFFER );
      if( m_err != paNoError )
      {
        std::cout << Pa_GetErrorText(m_err) << std::endl;
      }
      delete [] buffer;
    }
  }
  else
  {
    if ( !m_data.empty() )
    {
      m_data.clear();  //!< queue buffer clear
    }
  }
  m_mutex.unlock();
  return 0;
}

RTC::ReturnCode_t PortAudioOutput::onDeactivated(RTC::UniqueId ec_id)
{
  is_active = false;
  m_mutex.lock();
  try {
    if ( !m_data.empty() )
    {
      m_data.clear();  //!< queue buffer clear
    }

    if ( m_stream )
    {
      if(!Pa_IsStreamStopped( m_stream ))
      {
        m_err = Pa_AbortStream( m_stream );
        if( m_err != paNoError )
        {
          throw m_err;
        }
      }

      m_err = Pa_CloseStream( m_stream );
      if( m_err != paNoError )
      {
          throw m_err;
      }
      m_stream = NULL;
    }
  } catch (...) {
    std::cout << Pa_GetErrorText(m_err) << std::endl;
  }
  m_mutex.unlock();
  return RTC::RTC_OK;
}

RTC::ReturnCode_t PortAudioOutput::onFinalize()
{
  is_active = false;
  m_mutex.lock();
  try {
    if ( m_stream )
    {
      if(!Pa_IsStreamStopped( m_stream ))
      {
        m_err = Pa_AbortStream( m_stream );
        if( m_err != paNoError )
        {
          throw m_err;
        }
      }

	  m_err = Pa_CloseStream( m_stream );
      if( m_err != paNoError )
      {
        throw m_err;
      }
      m_stream = NULL;
    }
  } catch (...) {
    std::cout << Pa_GetErrorText(m_err) << std::endl;
  }
  Pa_Terminate();
  m_mutex.unlock();
  return RTC::RTC_OK;
}

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


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

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

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

  PortAudioOutputInit(manager);
  comp = manager->createComponent("PortAudioOutput");

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


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

int main(int argc, char** argv)
{
  PortAudioOutputManager output_manager;

  output_manager.start();
  return 0;
}

