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

// Module specification
// <rtc-template block="module_spec">
static const char* noisereduction_spec[] =
  {
    "implementation_id", "NoiseReduction",
    "type_name",         "NoiseReduction",
    "description",       "Audio Noise Reduction component using fftw3 (C++ implementation)",
    "version",           "0.0.1",
    "vendor",            "AIST",
    "category",          "communication",
    "activity_type",     "DataFlowComponent",
    "max_instance",      "1",
    "language",          "C++",
    "lang_type",         "script",
    "conf.default.SampleRate", "16000",
    "conf.default.CenterFrequency", "500",
    "conf.default.FrequencyMax", "700",
    "conf.default.FrequencyMin", "300",
    "conf.default.Power", "1",
    "conf.default.ModeSelect", "premp",
    "conf.__widget__.ModeSelect", "radio",
    "conf.__constraints__.ModeSelect", "(premp, deemp, bpf)",
    "conf.__widget__.SampleRate", "spin",
    "conf.__constraints__.SampleRate", "x >= 1",
    "conf.__widget__.CenterFrequency", "spin",
    "conf.__constraints__.CenterFrequency", "x >= 1",
    "conf.__widget__.FrequencyMax", "spin",
    "conf.__constraints__.FrequencyMax", "x >= 1",
    "conf.__widget__.FrequencyMin", "spin",
    "conf.__constraints__.FrequencyMin", "x >= 1",
    "conf.__widget__.Power", "spin",
    "conf.__constraints__.Power", "x >= 1",
    ""
  };
// </rtc-template>

//! enumerate window function
enum WindowFunc
{
  Hamming,
  Hanning,
  Blackman
};

//! dft
void fftw_dft( double *reImg, double *imImg, int num, int flag)
{
  fftw_complex *in, *out;
  fftw_plan p;
  int i;

  in  = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * num);
  out = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * num);
  if( flag == 0 )
  {
    p = fftw_plan_dft_1d(num, in, out, FFTW_FORWARD, FFTW_ESTIMATE);
  }
  else
  {
    p = fftw_plan_dft_1d(num, in, out, FFTW_BACKWARD, FFTW_ESTIMATE);
  }
  for( i=0; i<num; i++ ){
    in[i][0]=reImg[i];
    in[i][1]=imImg[i];
  }
  fftw_execute(p);

  for( i=0; i<num; i++ ){
    reImg[i] = out[i][0];
    imImg[i] = out[i][1];
  }

  fftw_destroy_plan(p);

  fftw_free(in);
  fftw_free(out);
}

//! window function
void Windowing( double* data, int size, enum WindowFunc type)
{
  double winValue = 0;

  for (int i = 0; i < size; i++)
  {
    //! hamming window
    if (type == Hamming)
      winValue = 0.54 - 0.46 * cos(2 * PI * i / (size - 1));
    //! hanning window
    else if (type == Hanning)
      winValue = 0.5 - 0.5 * cos(2 * PI * i / (size - 1));
    //! Blackman window
    else if (type == Blackman)
      winValue = 0.42 - 0.5 * cos(2 * PI * i / (size - 1)) + 0.08 * cos(4 * PI * i / (size - 1));
    //! rectangular window
    else
      winValue = 1.0;
    data[i] *= winValue;
  }
}

//! Band-pass filter
void bpf( double* rdata, double* idata, int size, int samplef, int fmax, int fmin, double gain)
{
  double winValue = 0;
  double df = (double)(samplef / size);

  if (fmax < fmin) return;
  for (int i = 0; i < size; i++)
  {
    if( ( (i * df) > (double)fmin ) && ( (i * df) < (double)fmax ) )
    {
//      winValue = gain * sin(2 * PI * i * df / centerf);
      winValue = gain;
      rdata[i] *= winValue;
      idata[i] *= winValue;
    }
    else
    {
      rdata[i] = 0;
      idata[i] = 0;
    }
  }
}

//! pre-emphasis
void pre_emp( double* rdata, double* idata, int size, int samplef, int centerf, double gain)
{
  double value = 0;
  double df = (double)(samplef / size);

  if(centerf > samplef) return;
  for (int i = 0; i < size; i++)
  {
    //! Blackman window
//      value = 0.42 - 0.5 * cos((2 * PI * i * df + start) / freq) + 0.08 * cos((4 * PI * i * df + start) / freq);
    //! Gauss window
    double x = pow((double)( i * df - centerf), 2.0);
    value = exp(-1 * x / (samplef*df) );
    rdata[i] = rdata[i] * value * gain;
    idata[i] = idata[i] * value * gain;
  }
}

//! de-emphasis
void de_emp( double* rdata, double* idata, int size, int samplef, int centerf, double gain)
{
  double value = 0;
  double df = (double)(samplef / size);

  if(centerf > samplef) return;
  for (int i = 0; i < size; i++)
  {
    /* Gauss window */
    double x = pow((double)( i * df - centerf), 2.0);
    value = 1 - exp(-1 * x / (samplef*df) );

    rdata[i] = rdata[i] * value / gain;
    idata[i] = idata[i] * value / gain;
  }
}

/*!
 * @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")
  {
    NoiseReduction *p = (NoiseReduction *)m_obj;
    p->RcvInBuffer(data);
  }
}

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

    // </rtc-template>
{
}

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

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

  /* setiing datalistener event */
  m_ninIn.addConnectorDataListener(ON_BUFFER_WRITE,
                        new DataListener("ON_BUFFER_WRITE", 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>
  bindParameter("SampleRate", m_rate, "16000");
  bindParameter("CenterFrequency", m_freqcenter, "500");
  bindParameter("FrequencyMax", m_freqmax, "700");
  bindParameter("FrequencyMin", m_freqmin, "300");
  bindParameter("Power", m_power, "1");
  bindParameter("ModeSelect", m_mode, "premp");
  return RTC::RTC_OK;
}

RTC::ReturnCode_t NoiseReduction::onActivated(RTC::UniqueId ec_id)
{
  for(int i = 0; i < HALFSIZE; i++ )
  {
    m_outbuffer[i] = 0;
  }
  for(int i = 0; i < MAXSIZE; i++ )
  {
    m_noise[i][0] = 0.0;
    m_noise[i][1] = 0.0;
  }
  if(!m_queue.empty())
  {
	  m_queue.clear();
  }

  is_active = true;
  m_noise_end = NOISESAMPLE;
  return RTC::RTC_OK;
}

void NoiseReduction::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_queue.push_back(buffer[i]);  //!< enqueue
    }
    m_mutex.unlock();
    delete [] buffer;
  }
  return;
}

//! order : dft->pre-emphasis->idft->repetition addition
RTC::ReturnCode_t NoiseReduction::onExecute(RTC::UniqueId ec_id)
{
  m_mutex.lock();
  if(m_queue.size() >= MAXSIZE)
  {
    int i = 0;
    double reImg[MAXSIZE],imImg[MAXSIZE];
    for(i = 0; i < MAXSIZE; i++ )
    {
      //! takes buffer
      reImg[i] = (double)m_queue.front();
      m_queue.pop_front();  //!< dequeue
      imImg[i] = 0.0;
    }

    for(i = MAXSIZE-1; i > HALFSIZE-1; i--)
    {
      //! latter half part is returned to the buffer
      m_queue.push_front((short)reImg[i]);  //!< enqueue
    }
    //! window function
    Windowing( reImg, MAXSIZE, Hamming );
    //! dft
    fftw_dft( reImg, imImg, MAXSIZE, 0 );
    //! sampling frequency 16000Hz, number 1024, resolution 16000/1024=15.625
    //! voice frequency male：300～550Hz,female：400～700Hz
    if(m_mode == "premp"){
      //! pre-emphasis
      pre_emp( reImg, imImg, MAXSIZE, m_rate, m_freqcenter, (double)m_power);
    }
    else if (m_mode == "deemp"){
      //! de-emphasis
      de_emp( reImg, imImg, MAXSIZE, m_rate, m_freqcenter, (double)m_power);
    }
    else if (m_mode == "bpf"){
      //! bpf
      bpf( reImg, imImg, MAXSIZE, m_rate, m_freqmax, m_freqmin, (double)m_power);
    }
    //! idft
    fftw_dft( reImg, imImg, MAXSIZE, 1 );

    short buffer[MAXSIZE];
    for(i = 0; i < MAXSIZE; i++)
    {
      //! Regularization: Each data is divided by the data number */
      buffer[i] = (short)(reImg[i] / MAXSIZE);
      if(i < HALFSIZE)
      {
        //! The part in the latter half of the last value is added to the forequarter of the present value.
        buffer[i] += m_outbuffer[i];
      }
      else
      {
        //! The part in the latter half of the present value is preserved.
        m_outbuffer[i-HALFSIZE] = buffer[i];
      }
    }
    //! output the added forequarter.
    m_fout.data.length(MAXSIZE);  //!< set outport data length
    memcpy((void *)&(m_fout.data[0]), (void *)&(buffer[0]), MAXSIZE);
    m_foutOut.write();
  }
  m_mutex.unlock();
  return RTC::RTC_OK;
}

RTC::ReturnCode_t NoiseReduction::onDeactivated(RTC::UniqueId ec_id)
{
  is_active = false;
  m_mutex.lock();
  if(!m_queue.empty())
  {
	  m_queue.clear();
  }
  m_mutex.unlock();
  return RTC::RTC_OK;
}

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

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

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

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

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

  NoiseReductionInit(manager);
  comp = manager->createComponent("NoiseReduction");

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


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

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

  input_manager.start();
  return 0;
}
