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

// Module specification
// <rtc-template block="module_spec">
static const char* echocanceler_spec[] =
  {
    "implementation_id", "EchoCanceler",
    "type_name",         "EchoCanceler",
    "description",       "acoustic echo cancellation component",
    "version",           "0.0.1",
    "vendor",            "AIST",
    "category",          "communication",
    "activity_type",     "DataFlowComponent",
    "max_instance",      "1",
    "language",          "C++",
    "lang_type",         "script",
    "conf.default.Gain", "100",
    "conf.__widget__.Gain", "spin",
    "conf.__constraints__.Gain", "x >= 1",
    ""
  };
// </rtc-template>


EC_TYPE* ec_init ( void )
{
  int i;
	EC_TYPE *ec = new EC_TYPE;

	ec->xidx = 0;
	ec->px = 0.0;
	ec->lx = 0.0;

	ec->nidx = 0;
	ec->pn = 0.0;
	ec->ln = 0.0;

	ec->gain = 1.0;

	for (i = 0; i < EC_TAP_SIZE * 2; i++)
	{
		ec->xtbl[i] = 0;
	}
	for (i = 0; i < EC_TAP_SIZE; i++)
	{
		ec->atbl[i] = 0.0;
		ec->ntbl[i] = 0;
	}

	return ec;
}

void ec_deinit ( EC_TYPE *ec )
{
  if (ec) {
		delete ec;
		ec = NULL;
	}
}

int eccan_run ( EC_TYPE *ec,
                short fin,
                short nin,
                short *fout,
                short *nout,
                double pow)
{
	int i;
	double mac, edat, xave, nave;
	short *xtbl;

	//! calc power of the sound input
	ec->px = ec->px + fabs((double)fin) - fabs((double)ec->xtbl[ec->xidx]);
	ec->lx = ec->lx - (ec->lx * EC_ENV_COEF) + (ec->px * EC_ENV_COEF);
	xave = ec->lx / EC_TAP_SIZE;

	ec->pn = ec->pn + fabs((double)nin) - fabs((double)ec->ntbl[ec->nidx]);
	ec->ln = ec->ln - (ec->ln * EC_ENV_COEF) + (ec->pn * EC_ENV_COEF);
	nave = ec->ln / EC_TAP_SIZE;

	//! insert reference data to ring buffer
	ec->xtbl[ec->xidx] = ec->xtbl[ec->xidx + EC_TAP_SIZE] = fin;
	ec->xidx++;
	xtbl = ec->xtbl + ec->xidx;
	if (ec->xidx == EC_TAP_SIZE) {
		ec->xidx = 0;
	}
	ec->ntbl[ec->nidx] = nin;
	ec->nidx++;
	if (ec->nidx == EC_TAP_SIZE) {
		ec->nidx = 0;
	}

	if(xave != 0){
		//! apply convolution filter to the reference signal
		mac = 0.0;
		for (i = 0; i < EC_TAP_SIZE; i++) {
			mac = mac + xtbl[EC_TAP_SIZE - 1 - i] * ec->atbl[i];
		}

		if(fabs(mac) > fabs(nave) + EC_DIV){

			ec->gain = ec->gain * EC_STEP_GAIN;
//			std::cout << " filter clr. gain = " << ec->gain << std::endl;
			for (i = 0; i < EC_TAP_SIZE; i++) {
				ec->atbl[i] = ec->atbl[i] * ec->gain;
			}
			mac = 0;
		}

		//! subtract convolved reference signal from the main signal
		edat = nin - mac;
		//! update filter parameters
		if ((xave > EC_UPDATE_SLICE) && (nave < EC_UPDATE_STOP) ){
			double nml = edat * pow * ec->gain / xave;
			for (i = 0; i < EC_TAP_SIZE; i++) {
				ec->atbl[i] = ec->atbl[i] + xtbl[EC_TAP_SIZE - 1 - i] * nml;
			}
		}
	}
	else{
		edat = nin;
	}
	*nout = fin;
	*fout = (short)edat;
	return 0;
}

/*!
 * @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)
{
  EchoCanceler *p = (EchoCanceler *) 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
 */
EchoCanceler::EchoCanceler(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),
    m_paraOut("parameter", m_para)

    // </rtc-template>
{
}

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

RTC::ReturnCode_t EchoCanceler::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("to_output", m_foutOut);
  registerOutPort("parameter", m_paraOut);
//  addOutPort("to_output", m_foutOut);

  // Set service provider to Ports

  // Set service consumers to Ports

  // Set CORBA Service Ports

  // </rtc-template>
  bindParameter("Gain", m_gain, "100");

  return RTC::RTC_OK;
}

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

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

void EchoCanceler::RcvInBuffer(TimedOctetSeq data)
{
	if (is_active == true) {
		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_indata.push_back(buffer[i]); //!< enqueue
		}
		delete[] buffer;
		m_mutex.unlock();
	}
	return;
}

void EchoCanceler::RcvOutBuffer(TimedOctetSeq data)
{
	if (is_active == true) {
		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 EchoCanceler::onExecute(RTC::UniqueId ec_id)
{
	if (!m_indata.empty() && !m_outdata.empty()) {
		m_mutex.lock();
		int num = (m_indata.size() < m_outdata.size()) ? m_indata.size() : m_outdata.size();
		short *inbuffer = new short[num];
		short *outbuffer = new short[num];
		short *result = new short[num];

		for (int i = 0; i < num; i++) {
			inbuffer[i] = m_indata.front();
			m_indata.pop_front();
			outbuffer[i] = m_outdata.front();
			m_outdata.pop_front();
			result[i] = 0;
		}

		m_mutex.unlock();
		if(abs((int)(m_indata.size() - m_outdata.size())) > 10000) BufferClr();

		for (int i = 0; i < num; i++) {
			if(m_gain == 0.0) result[i] = inbuffer[i];
			else{
				short fout, nout;
				eccan_run(mp_ec, outbuffer[i], inbuffer[i], &fout, &nout,1/m_gain);
				result[i] = fout;
			}
		}
		m_fout.data.length(num * 2); //!< set outport data length
		memcpy((void *) &(m_fout.data[0]), (void *) &(result[0]), num * 2);
		m_foutOut.write();

		m_para.data.length(EC_TAP_SIZE); //!< set outport data length
		memcpy((void *) &(m_para.data[0]), (void *) &(mp_ec->atbl[0]), EC_TAP_SIZE);
		m_paraOut.write();

		delete[] inbuffer;
		delete[] outbuffer;
		delete[] result;
	}
	else
	{
		if( ( m_indata.size() > BUFFER_MAX ) || ( m_outdata.size() > BUFFER_MAX ) ){
		    BufferClr();
		}
	}

	return RTC::RTC_OK;
}

RTC::ReturnCode_t EchoCanceler::onDeactivated(RTC::UniqueId ec_id)
{
  try {
    is_active = false;
    BufferClr();
    ec_deinit(mp_ec);
  } catch (...) {
    std::cout << "onDeactivated error" << std::endl;
  }
  return RTC::RTC_OK;
}

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

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

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

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

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

  EchoCancelerInit(manager);
  comp = manager->createComponent("EchoCanceler");

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


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

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

  input_manager.start();
  return 0;
}
