/*
 * --- GSMP-COPYRIGHT-NOTE-BEGIN ---
 * 
 * This copyright note is auto-generated by ./scripts/Create-CopyPatch.
 * Please add additional copyright information _after_ the line containing
 * the GSMP-COPYRIGHT-NOTE-END tag. Otherwise it might get removed by
 * the ./scripts/Create-CopyPatch script. Do not edit this copyright text!
 * 
 * GSMP: pcm/tests/ResamplingAndStretching.cc
 * General Sound Manipulation Program is Copyright (C) 2000 - 2004
 *   Valentin Ziegler and René Rebe
 * 
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; version 2. A copy of the GNU General
 * Public License can be found in the file LICENSE.
 * 
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANT-
 * ABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
 * Public License for more details.
 * 
 * --- GSMP-COPYRIGHT-NOTE-END ---
 */

#include <iostream>

#include "OggVorbisStream.hh"
#include "ALSAHelper.hh"

#include <sys/time.h>

using namespace std;
using namespace GSMP;

class ResampleAndStretch
{
public:
  ResampleAndStretch (BasicFileStream& i_input, BasicStream& i_output, int output_period_size)
    : data (2, rd_size),
      up_data (2, up_size),
      window (1, window_size),
      back_buffer (2, window_size),
      new_data (2, new_size),
      input(i_input),
      output(i_output)
  {
    period = output_period_size;
    factor = 1.0;
    stretch = false;
    cur_offs = 1;
    cur_fraction = .0;
    Refill ();
    da[0]=(*data[0])[0];
    db[0]=(*data[0])[1];
    da[1]=(*data[1])[0];
    db[1]=(*data[1])[1];

    InitWindow ();
    back_buffer[0] -> WriteZero (window_size);
    back_buffer[1] -> WriteZero (window_size);
    up_data[0] -> WriteZero (up_size);
    up_data[1] -> WriteZero (up_size);

    k_max = 0;
    w_skip = 0;
  }

  void SetResamplingFactor (double n_factor)
  {
    factor = std::min(1.0, n_factor);
    if (factor == 1.0)
      cur_fraction = .0;
  }

  void SetStretch (bool n_stretch)
  {
    stretch = n_stretch;
  }

  void SetStretchSamples (local_index n_stretch)
  {
    local_index w_skip_max_ = w_skip_max;
    local_index k_max_max_ = k_max_max;
    w_skip = std::min (n_stretch, w_skip_max_);
    k_max = std::min (k_max, k_max_max_);
  }

  void CalculatePeriod ()
  {
      for (int i = 0; i < period; i++)
	{
	  for (int ch = 0; ch < 2; ch++)
	    (*output[ch])[i] = PcmArithmetic <int32_t>::Weighted (da[ch], db[ch], cur_fraction);
	  MoveOn ();
	}
  }

private:
  void InitWindow ()
  {
    for (local_index i = 0; i < window_size; i++)
      (*window[0])[i] = PcmConvert <double, int32_t, HostEndianessTraits, HostEndianessTraits>
	:: Convert ( 0.5 * (1.0 + cos ( (M_PI * (double) i ) / (double) window_size) ));
  }

  void OLA (channel_index channel, local_index up_data_offset)
  {
    for (local_index i = 0; i < window_size; i++)
      {
	(*data[channel])[i] = PcmArithmetic <int32_t>::Prod ((*back_buffer[channel])[i], (*window[0])[i]);
	PcmArithmetic <int32_t>::Macc ((*data[channel])[i], (*up_data[channel])[i+up_data_offset], (*window[0])[(window_size - 1) - i]);
      }
  }

  void ShiftData ()
  {
    input.Read (window_size + w_skip, &new_data);

    for (int ch = 0; ch < 2; ch++)
      {
	local_index j = 0;
	for (local_index i = 0; i < up_size; i++)
	  if (i + window_size + w_skip < up_size)
	    (*up_data[ch])[i] = (*up_data[ch])[i + window_size + w_skip];
	  else
	    (*up_data[ch])[i] = (*new_data[ch])[j++];
      }

  };

  double GetMatch (channel_index channel, local_index k)
  {
    double sum = .0;
    for (local_index i = 0; i < window_size; i += cor_inc_xy)
      sum += ((double) (*back_buffer[channel])[i]) * ((double) (*up_data[channel])[i+k]);
    return sum;
  }

  local_index OptimalMatch (channel_index channel)
  {
    double best = .0;
    local_index k_best = 0;
    for (local_index k = 0; k < k_max; k += cor_inc_k)
      {
	double cur = GetMatch (channel, k);
	if (cur > best)
	  {
	    best = cur;
	    k_best = k;
	  }
      }
    return k_best;
  }

  void Refill ()
  {
    if (!stretch)
      input.Read (rd_size, &data);
    else
      {
	ShiftData ();
	for (int ch = 0; ch < 2; ch++)
	  {
	    local_index k = OptimalMatch (ch);
	    OLA (ch, k);
	    up_data[ch] -> CopyData (window_size + k, window_size, (*back_buffer[ch]));
	  }
      }
  };

  inline void MoveOn ()
  {
    cur_fraction += factor;
    if (cur_fraction >= 1.0)
      {
	cur_fraction -= 1.0;
	da[0]=db[0];
	da[1]=db[1];
	cur_offs++;
	if (cur_offs >= rd_size)
	  {
	    Refill ();
	    cur_offs=0;
	  }
	db[0]=(*data[0])[cur_offs];
	db[1]=(*data[1])[cur_offs];
      }
  };




  static const local_index window_size = 2 * 512;
  static const local_index w_skip_max = window_size;
  static const local_index rd_size = window_size;
  static const local_index up_size = 4 * window_size;
  static const local_index new_size = window_size + w_skip_max;
  static const local_index k_max_max = window_size / 2;

  static const local_index cor_inc_xy = window_size / 32;
  static const local_index cor_inc_k = 2;

  BasicStream data;
  BasicStream up_data;
  BasicStream window;
  BasicStream back_buffer;
  BasicStream new_data;


  BasicFileStream& input;
  BasicStream& output;
  int period;
  double factor;
  local_index cur_offs;
  double cur_fraction;
  int32_t da[2];
  int32_t db[2];
  bool stretch;
  local_index w_skip;
  local_index k_max;
};

int main()
{
  const int period_size = 1024;
  BasicStream stream (2, period_size);
  int16_t raw_stream [period_size * 2];
  
  std::string cname = "hw:0";
  
  ALSAControl alsa_control;
  ALSAPCM alsa_pcm;
  
  if (alsa_pcm.Open (cname, SND_PCM_STREAM_PLAYBACK, SND_PCM_NONBLOCK) )
    {    
      if (!alsa_pcm.SetAccess (SND_PCM_ACCESS_RW_INTERLEAVED) )
	std::cout << "Setting RW_INTERLEAVED failed!" << std::endl;
  
      // try the formats we support ...
      if (alsa_pcm.SetFormat (SND_PCM_FORMAT_S16) ) {
	std::cout << "Format 16 bit" << std::endl;
      }
      else if (alsa_pcm.SetFormat (SND_PCM_FORMAT_S32) ) {
	std::cout << "Format 32 bit" << std::endl;
      }
      
      if (!alsa_pcm.SetChannels (2) )
	std::cout << "Setting the channels failed!" << std::endl;
      
      std::cout << "Rate min: " << alsa_pcm.GetRateMin () << std::endl;
      std::cout << "Rate max: " << alsa_pcm.GetRateMax () << std::endl;
      alsa_pcm.SetRateNear (44100);
      
      std::cout << "Periods min: " << alsa_pcm.GetPeriodsMin () << std::endl;
      std::cout << "Periods max: " << alsa_pcm.GetPeriodsMax () << std::endl;
      
      if (!alsa_pcm.SetPeriodsNear (4) )
    	std::cout << "Setting periods near failed!" << std::endl;
      
      if (!alsa_pcm.SetPeriodTimeNear (400000 / 4) )
	std::cout << "Setting period time near failed!" << std::endl;
      
      if (!alsa_pcm.InstallParams () )
	std::cout << "Setting params failed!" << std::endl;
      
      if (!alsa_pcm.InstallSwParams () )
	std::cout << "Setting software params failed!" << std::endl;
      
      std::cout << "Period Size: " << alsa_pcm.GetPeriodSize() << std::endl;
      std::cout << "Get periods: " << alsa_pcm.GetPeriods() << std::endl;
      
      OggVorbisFileStream input_stream;
      if (!input_stream.Open ("test.ogg"))
	{
	  std::cerr << "Could not open test.ogg" << std::endl;
	  exit (-1);
	}

      ResampleAndStretch ras (input_stream, stream, period_size);


      bool up = true;
      double cur_fact = 0.3;
      int ups = 0;

      const int max_ups = 1;

      cout << "Resampling Test" << endl;
      cout << "Up" << endl;
      
      while (true)
      {
	int written = 0;

	ras.SetResamplingFactor (cur_fact);
	ras.CalculatePeriod ();

	if (ups < max_ups)
	  {
	    if (up)
	      {
		cur_fact += (1.0 - cur_fact) / 100.0;
		if (cur_fact > 0.9)
		  {
		    if (++ups == max_ups)
		      {
			cur_fact = 1.0;
			cout << "Stretching" << endl;
			ras.SetStretch (true);
		      }
		    else {
		      cout << "Down" << endl;
		      up = false;
		    }
		  }

	      }
	    else
	      {
		cur_fact *= 0.99;
		if (cur_fact < 0.3)
		  {
		    cout << "Up" << endl;
		    up = true;
		  }
	      }
	  }
	else
	  {
	    ras.SetStretchSamples (ups);
	    if (++ups == 400)
	      cur_fact = 0.5;
	  }

	for (int ch = 0; ch < 2; ++ch) {
	  for (int i = 0; i < period_size; ++i)
	    raw_stream [i*2 + ch] =
	      PcmConvert<int32_t,int16_t>::Convert ((*stream[ch])[i]);
	}
	
	while (written < period_size) {
	  int t_written = alsa_pcm.WriteI (raw_stream, period_size - written);
	  if (t_written < 0)
	    {
	      if (t_written == -EAGAIN) {
		//std::cout << "try again ..." << std::endl;
		usleep (200);
	      }
	      else if (t_written == -EPIPE) { // xrun
		std::cout << "xrun" << std::endl;
		
		struct timeval now, diff, tstamp;
		gettimeofday(&now, 0);
		
		alsa_pcm.GetTriggerTStamp (&tstamp);
		timersub(&now, &tstamp, &diff);
		std::cout << "  !!!-->> XRUNx !!! (at least "
			  << diff.tv_sec * 1000 + diff.tv_usec / 1000.0
			  << " ms long)" << std::endl;
      
		if (!alsa_pcm.Prepare () )
		  std::cout << "  BUR: can't reprepare during XRun!! "
			    << std::endl;
	      }
	      else
		std::cout << "error: " << snd_strerror (t_written)
			  << std::endl;
	    }
	  else {
	    written += t_written;
	  }
	}
      }

      input_stream.Close ();
      
      alsa_pcm.Close ();
    }
  
  return 0;
}