#include <math.h>
#include <string.h>
#include "dejitter.h"




#ifdef SYNCER_LOG_TIMING
#include <streams.h>
#endif

static const int format_mask_dejitter = FORMAT_CLASS_PCM | FORMAT_MASK_LINEAR | FORMAT_MASK_SPDIF;




Syncer::SyncerStat::SyncerStat()
{
  reset();
}

void   
Syncer::SyncerStat::reset()
{
  memset(stat, 0, sizeof(stat));
}

void   
Syncer::SyncerStat::add(vtime_t _val)
{
  memmove(stat + 1, stat, sizeof(stat) - sizeof(stat[0]));
  stat[0] = _val;
}

vtime_t 
Syncer::SyncerStat::stddev() const
{
  vtime_t sum = 0;
  vtime_t avg = mean();
  for (int i = 0; i < array_size(stat); i++)
    sum += (stat[i] - avg) * (stat[i] - avg);
  return sqrt(sum/array_size(stat));
}

vtime_t 
Syncer::SyncerStat::mean() const
{
  vtime_t sum = 0;
  for (int i = 0; i < array_size(stat); i++)
    sum += stat[i];
  return sum/array_size(stat);
}

int 
Syncer::SyncerStat::len() const
{
  return array_size(stat);
}




bool
Syncer::query_input(Speakers _spk) const
{
  if (!_spk.sample_rate)
    return false;

  return (FORMAT_MASK(_spk.format) & format_mask_dejitter) != 0;
}

bool
Syncer::set_input(Speakers _spk)
{
  reset();

  if (!_spk.sample_rate)
    return false;

  switch (_spk.format)
  {
    case FORMAT_LINEAR:
      size2time = 1.0 / _spk.sample_rate;
      break;

    case FORMAT_PCM16:
    case FORMAT_PCM16_BE:
      size2time = 1.0 / 2.0 / _spk.nch()  / _spk.sample_rate;
      break;

    case FORMAT_PCM24:
    case FORMAT_PCM24_BE:
      size2time = 1.0 / 3.0 / _spk.nch()  / _spk.sample_rate; 
      break;

    case FORMAT_PCM32:
    case FORMAT_PCM32_BE:
      size2time = 1.0 / 4.0 / _spk.nch()  / _spk.sample_rate;
      break;

    case FORMAT_PCMFLOAT:
      size2time = 1.0 / sizeof(float) / _spk.nch()  / _spk.sample_rate;
      break;

    case FORMAT_SPDIF:
      size2time = 1.0 / 4.0 / _spk.sample_rate;
      break;

    default:
      return false;
  }

  return NullFilter::set_input(_spk);
}


void 
Syncer::reset()
{
  #ifdef SYNCER_LOG_TIMING
    DbgLog((LOG_TRACE, 3, "sync drop"));
  #endif
  continuous_sync = false;
  continuous_time = 0.0;
  istat.reset();
  ostat.reset();
  NullFilter::reset();
}

bool 
Syncer::process(const Chunk *_chunk)
{
  
  if (_chunk->is_dummy())
    return true;

  
  FILTER_SAFE(receive_chunk(_chunk));

  
  if (!_chunk->sync)
    return true;

  
  if (!continuous_sync)
  {
    #ifdef SYNCER_LOG_TIMING
      DbgLog((LOG_TRACE, 3, "sync catch: %ims", int(time * 1000)));
    #endif
    continuous_sync = true;
    continuous_time = time;
    return true;
  }

  
  vtime_t delta = time - continuous_time;

  if (dejitter)
  {
    
    
    
    
    
    
    
    

    if (fabs(delta) > threshold)
    {
      #ifdef SYNCER_LOG_TIMING
        DbgLog((LOG_TRACE, 3, "sync lost; resync: %ims", int(time * 1000)));
      #endif
      continuous_sync = true;
      continuous_time = time;
      istat.reset();
      ostat.reset();
      return true;
    }

    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    

    vtime_t mean = istat.mean();
    vtime_t stddev = istat.stddev();
    vtime_t correction = 0;

    if (fabs(mean) > stddev)
    {
      correction = istat.mean() * 2 / istat.len();
      continuous_time += correction;
    }

    istat.add(delta);
    ostat.add(correction);

    #ifdef SYNCER_LOG_TIMING
      DbgLog((LOG_TRACE, 3, "input:  %-6.0f delta: %-6.0f stddev: %-6.0f mean: %-6.0f", time, delta, istat.stddev(), istat.mean()));
      DbgLog((LOG_TRACE, 3, "output: %-6.0f correction: %-6.0f", continuous_time, correction));
    #endif
  }
  else 
  {
    istat.add(delta);
    ostat.add(delta);

    #ifdef SYNCER_LOG_TIMING
      DbgLog((LOG_TRACE, 3, "input:  %-6.0f delta: %-6.0f stddev: %-6.0f mean: %-6.0f", time, delta, istat.stddev(), istat.mean()));
    #endif
  }

  return true;
}

bool 
Syncer::get_chunk(Chunk *_chunk)
{
  if (dejitter)
  {
    sync = continuous_sync;
    time = continuous_time * time_factor + time_shift;
  }
  else if (sync)
    time = time * time_factor + time_shift;

  continuous_time += size * size2time;
  send_chunk_inplace(_chunk, size);
  sync = false;
  return true;
}
