/* -*- c++ -*- */
/*
 * Copyright 2005 Free Software Foundation, Inc.
 * 
 * This file is part of GNU Radio
 * 
 * GNU Radio 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; either version 3, or (at your option)
 * any later version.
 * 
 * GNU Radio is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <gr_align_on_samplenumbers_ss.h>
#include <gr_io_signature.h>
#include <assert.h>
#include <stdexcept>
#include <string.h>
#include <cstdio>

//define ALIGN_ADVANCED_IMPLEMENTATION to have an alternative implementation of the align algoritm which exactly follows the align_interval spec.
//It is more resource intensive, less tested and probably not needed
//define ALIGN_ADVANCED_IMPLEMENTATION

//define DEBUG_TOCONSUME to see debug messages about the synchronisation part of this block
//define DEBUG_TOCONSUME
#ifdef DEBUG_TOCONSUME
#define tcPrintf if(dprint) printf
#else
#define tcPrintf //printf
#endif

#define ePrintf printf

gr_align_on_samplenumbers_ss_sptr
gr_make_align_on_samplenumbers_ss (int nchan, int align_interval)
{
  return gr_align_on_samplenumbers_ss_sptr (new gr_align_on_samplenumbers_ss (nchan,align_interval));
}

gr_align_on_samplenumbers_ss::gr_align_on_samplenumbers_ss (int nchan,int align_interval)
  : gr_block ("align_on_samplenumbers_ss",
        gr_make_io_signature (2, -1, sizeof (short)), //2, -1
        gr_make_io_signature (2, -1, sizeof (short))), //2,-1
  d_align_interval (align_interval),
  d_nchan(nchan),
  d_ninputs(0)
{
  if (d_align_interval<0)
    set_output_multiple (d_nchan*2);
  else
  {
    set_output_multiple (d_align_interval*d_nchan*2);
  }
    
}

gr_align_on_samplenumbers_ss::~gr_align_on_samplenumbers_ss()
{
  
}
void
gr_align_on_samplenumbers_ss::forecast (int noutput_items, gr_vector_int &ninput_items_required)
{
  //assert (0 == noutput_items % d_align_interval);
  unsigned ninputs = ninput_items_required.size();
  for (unsigned int i = 0; i < ninputs; i++)
    ninput_items_required[i] = std::max(noutput_items*d_nchan*2+ history() - 1,1024*d_nchan*2+ history() - 1);//TODO include the diffs found in determine input_items_required
}

bool
gr_align_on_samplenumbers_ss::check_topology (int ninputs, int noutputs)
{
  bool result=true;
  if(noutputs!=ninputs)
  {
    result=false;
    ePrintf("gr_align_on_samplenumbers_ss: ERROR noutputs %i != ninputs %i\n",noutputs,ninputs);
  }
  if(d_nchan<2)
  {
    result=false;
    ePrintf("gr_align_on_samplenumbers_ss: ERROR nchan %i<2 \n",d_nchan);
  }
  if((int)d_ninputs!=ninputs)
  {
    //Only resize and reset the status if there really changed something
    //Don't reset the status if the user just called stop() and start(), although maybe we should.
    d_state.resize(ninputs);
    d_ninputs=ninputs;
    for(unsigned int i=0;i<d_ninputs;i++)
    {
      d_state[i].sync_found=false;
      d_state[i].sync_end_found=false;
    }
    d_in_presync=false;
  }
  return result;
}

#ifdef ALIGN_ADVANCED_IMPLEMENTATION
int
gr_align_on_samplenumbers_ss::general_work (int noutput_items,
        gr_vector_int &ninput_items,
        gr_vector_const_void_star &input_items,
        gr_vector_void_star &output_items)
{
#ifdef DEBUG_TOCONSUME
  static int dcount=0;
  bool dprint=false;
  dcount++;
  if(dcount>200)
  {
    dcount=0;
    dprint=true;
  } 
#endif
  const size_t item_size = output_signature()->sizeof_stream_item (0);
  const unsigned ninputs = input_items.size();
  const unsigned noutputs = output_items.size();

  int align_interval=d_align_interval*2*d_nchan;
  if(d_align_interval<0)
  {
    //align once per noutput_items
    align_interval=noutput_items;
    align_interval=align_interval/(2*d_nchan);
    align_interval=align_interval*(2*d_nchan);
  }

  int min_ninput_items=noutput_items;//numeric_limits<int>::max();
  int noutput_items_produced=0;
  for(unsigned int i=0;i<ninputs;i++)
  { 
    d_state[i].ninput_items=ninput_items[i];
    d_state[i].ninput_items_used=0;
    min_ninput_items=std::min(ninput_items[i],min_ninput_items);
  }
  for(int j=0;j<noutput_items-align_interval+1;j+=align_interval)
  {
    int common_end;
    if(min_ninput_items>=align_interval)
      common_end=align_interval;
    else
    {
      common_end=min_ninput_items/(d_nchan*2);
      common_end=common_end*(d_nchan*2);
    }
    if (common_end<=0) break;
    
    bool all_diffs_zero=true;
    //bool sync_found=false;
    int diff_comp_end_max=0;
    for(unsigned int i=0;i<ninputs;i++)
    {
      unsigned short * uin=&(((unsigned short*)input_items[i])[d_state[i].ninput_items_used]);
      unsigned int  x_high16bits = uin[0];
      unsigned int  x_low16bits = uin[1];
      d_state[i].ucounter_begin = x_high16bits<<16 | x_low16bits;
      d_state[i].diff=d_state[0].ucounter_begin-d_state[i].ucounter_begin;//Result is a signed value,Will wrap around on 32 bit boundary
      int common_last=std::max(common_end-d_nchan*2,0);
      x_high16bits = uin[d_nchan*2];
      x_low16bits = uin[d_nchan*2+1];
      unsigned int ucounter_begin2 = x_high16bits<<16 | x_low16bits;
#ifdef DEBUG_TOCONSUME
      if((d_state[i].ucounter_begin+1)!=(ucounter_begin2))
        if(ucounter_begin2==0)
          ePrintf("SYNC counters are 0\n");
        else
           ePrintf("Error: counter not continuous.\n ucounter_begin[%i]=%i +1 !=  ucounter_begin2=%i\n",i,d_state[i].ucounter_begin,ucounter_begin2);
#endif
      x_high16bits = uin[common_last];
      x_low16bits = uin[common_last+1];
      d_state[i].ucounter_end = x_high16bits<<16 | x_low16bits;
      d_state[i].diff_end=d_state[0].ucounter_end-d_state[i].ucounter_end;//Result is a signed value,Will wrap around on 32 bit boundary
      d_state[i].diff_comp_end=d_state[i].ucounter_end-d_state[0].ucounter_end;
      diff_comp_end_max=std::max(d_state[i].diff_comp_end,diff_comp_end_max);
#ifdef DEBUG_TOCONSUME
      if(d_state[i].diff>256000000 || d_state[i].diff_end>256000000 || d_state[i].diff_comp_end>256000000)
      {
        tcPrintf("diff[%i]=%i diff_end=%i diff_comp_end=%i\n",i,d_state[i].diff,d_state[i].diff_end,d_state[i].diff_comp_end);
      }
#endif
      all_diffs_zero=all_diffs_zero && (0==d_state[i].diff_end);
      if(d_state[i].ucounter_end<d_state[i].ucounter_begin+(unsigned)(common_last/(d_nchan*2))) //(unsigned)(common_last/(d_nchan*2)))
      {
        //printf("sync 1 ");// found ucounter_end[%i]=%i ucounter_begin[%i]=%i \n",i,d_state[i].ucounter_end,i,d_state[i].ucounter_begin);
        //sync_found=true;//sync_found or 32 bit counter  wraparound (0xffffffff -> 0x00000000)
        if(!d_in_presync)
        {
#ifdef DEBUG_TOCONSUME
          printf("presync START with %i\n",i);
#endif
         for(unsigned int k=0;k<ninputs;k++)
         {
          d_state[k].sync_found=false;
          d_state[i].sync_end_found=false;
         }
         d_in_presync=true;
         d_state[i].sync_found=true;    
        } else
        {
          //d_in_presync=true;
#ifdef DEBUG_TOCONSUME
          if(d_state[i].sync_found)
            printf("presync CONTINUE with %i\n",i);
          else
            printf("presync NEXT with %i\n",i);
#endif
          d_state[i].sync_found=true;  
          d_state[i].sync_end_found=false;  
        }             
      } else
      {
        if(d_in_presync && d_state[i].sync_found)
        {
          d_state[i].sync_end_found=true;
          bool all_syncs_found=true;
          for(unsigned int k=0;k<ninputs;k++)
            all_syncs_found=all_syncs_found && d_state[k].sync_end_found;
          d_in_presync=!all_syncs_found;
          if(!d_in_presync)
          {
            for(unsigned int k=0;k<ninputs;k++)
            {
              d_state[k].sync_found=false;
              d_state[i].sync_end_found=false;
            }
#ifdef DEBUG_TOCONSUME
            printf("presync END\n");
#endif
          }
        }
      }
    }
    if(d_in_presync || all_diffs_zero)
    {
      for(unsigned int i=0;i<ninputs;i++)
      {
         memcpy(&(((unsigned short*)output_items[i])[j]),&(((const unsigned short*)input_items[i])[d_state[i].ninput_items_used]),common_end*item_size);
         //consume(i,common_end);
         d_state[i].ninput_items-=common_end;
         d_state[i].ninput_items_used+=common_end;
         min_ninput_items=std::min(d_state[i].ninput_items,min_ninput_items);
#ifdef DEBUG_TOCONSUME
         if(common_end<256)
           tcPrintf("common_end %i\n",common_end);
#endif
      }
      //min_ninput_items-=common_end;
      noutput_items_produced+=common_end;
      //return common_end;
    } else
    {
      //printf("sync 2");
      for(unsigned int i=0;i<ninputs;i++)
      {
        int toconsume=std::min((d_state[i].diff_end+diff_comp_end_max)*d_nchan*2,d_state[i].ninput_items);
        toconsume=toconsume/(d_nchan*2);
        toconsume=toconsume*(d_nchan*2);
        d_state[i].ninput_items-=toconsume;
        d_state[i].ninput_items_used+=toconsume;
        min_ninput_items=std::min(d_state[i].ninput_items,min_ninput_items);
#ifdef DEBUG_TOCONSUME
      static int toconsume_counter=0;
      toconsume_counter++;
      //if(toconsume_counter>10)
      {
        tcPrintf("toconsume=%i diff_end[%i]*d_nchan*2=%i diff_comp_end_max*d_nchan*2=%i ninput_items[%i]=%i\n",toconsume,i,d_state[i].diff_end*d_nchan*2,diff_comp_end_max*d_nchan*2,i,ninput_items[i]);
        toconsume_counter=0;
      }
#endif
        //printf("toconsume[%i]=%i\n",i,toconsume);
        //consume(i,toconsume);//skip the difference in samplenumber items
      }
      //return 0;
    }
  }
  for(unsigned int i=0;i<ninputs;i++)
    consume(i,d_state[i].ninput_items_used);
#ifdef DEBUG_TOCONSUME
  if(noutput_items_produced<256)
    tcPrintf("noutput_items_produced %i\n",noutput_items_produced);
#endif
  return noutput_items_produced;
}


#else /*ALIGN_ADVANCED_IMPLEMENTATION*/
int
gr_align_on_samplenumbers_ss::general_work (int noutput_items,
        gr_vector_int &ninput_items,
        gr_vector_const_void_star &input_items,
        gr_vector_void_star &output_items)
{
#ifdef DEBUG_TOCONSUME
  static int dcount=0;
  bool dprint=false;
  dcount++;
  if(dcount>2000)
  {
    dcount=0;
    dprint=true;
  } 
#endif
  const size_t item_size = output_signature()->sizeof_stream_item (0);
  const unsigned ninputs = input_items.size();
  
  int common_end=noutput_items;
  //int diff_min=INT_MAX;
  //int diff_max=INT_MIN;
  for(unsigned int i=0;i<ninputs;i++)
  {
    unsigned short * uin=(unsigned short*)input_items[i];
    unsigned int  x_high16bits = uin[0];
    unsigned int  x_low16bits = uin[1];
    d_state[i].ucounter_begin = x_high16bits<<16 | x_low16bits;
    d_state[i].diff=d_state[0].ucounter_end-d_state[i].ucounter_end;//Result is a signed value,Will wrap around on 32 bit boundary
    x_high16bits = uin[d_nchan*2];
    x_low16bits = uin[d_nchan*2+1];
    unsigned int ucounter_begin2 = x_high16bits<<16 | x_low16bits;
    if((d_state[i].ucounter_begin+1)!=(ucounter_begin2)){
      if(ucounter_begin2==0)
      {
#ifdef DEBUG_TOCONSUME
        ePrintf("SYNC counters are 0\n");
#endif
      }
      else
      {
        ePrintf("Error: counter not continuous.\n ucounter_begin[%i]=%i +1 !=  ucounter_begin2=%i\n",i,d_state[i].ucounter_begin,ucounter_begin2);
      }
    }
      
    //diff_comp[i]=ucounter[i]-ucounter[0];
    //diff_min=std::min(diff[i],diff_min);
    //diff_max=std::max(diff[i],diff_max);
    common_end=std::max(std::min(ninput_items[i],common_end),0);
  }
  common_end=common_end/(d_nchan*2);
  common_end=common_end*(d_nchan*2);
#ifdef DEBUG_TOCONSUME
  if(common_end<d_nchan*2)
  {
    printf(" common_end %i\n",common_end);
    for(int j=0;j<ninputs;j++)
      printf("ninput_items[%i]=%i\n",j,ninput_items[j]);
  }
#endif
  bool all_diffs_zero=true;
  bool sync_found=false;
  int diff_comp_end_max=0;
  for(unsigned int i=0;i<ninputs;i++)
  {
    unsigned short * uin=(unsigned short*)input_items[i];
    int common_last=common_end-(d_nchan*2);
    unsigned int  x_high16bits = uin[common_last];
    unsigned int  x_low16bits = uin[common_last+1];
    d_state[i].ucounter_end = x_high16bits<<16 | x_low16bits;
    d_state[i].diff_end=d_state[0].ucounter_end-d_state[i].ucounter_end;//Result is a signed value,Will wrap around on 32 bit boundary
    d_state[i].diff_comp_end=d_state[i].ucounter_end-d_state[0].ucounter_end;
    //diff_end_min=std::min(diff_end[i],diff_end_min);
    //diff_end_max=std::max(diff_end[i],diff_end_max);
    diff_comp_end_max=std::max(d_state[i].diff_comp_end,diff_comp_end_max);
#ifdef DEBUG_TOCONSUME
    if(d_state[i].diff_end!=d_state[i].diff)
    {
      //samples_lost_or_syncstart=true;
      printf("Us%i %i %i ",i,d_state[i].diff_end,d_state[i].diff);
    }
#endif
    all_diffs_zero=all_diffs_zero && (0==d_state[i].diff_end);
    if((d_state[i].ucounter_end<d_state[i].ucounter_begin+(unsigned)(common_last/(d_nchan*2))) || (0==d_state[i].ucounter_end) || (0==d_state[i].ucounter_begin)) //(unsigned)(common_last/(d_nchan*2)))
    {
      sync_found=true;//sync_found or 32 bit counter  wraparound (0xffffffff -> 0x00000000)
#ifdef DEBUG_TOCONSUME
      tcPrintf("SYNC diff_end[%i]=%i ucounter_end[%i]=%i ucounter_begin[%i]=%i \n",i,d_state[i].diff_end,i,d_state[i].ucounter_end,i,d_state[i].ucounter_begin);
      tcPrintf("ucounter_end=%i < %i = ucounter_begin+(unsigned)(common_last/(d_nchan*2) \n",d_state[i].ucounter_end,d_state[i].ucounter_begin+(unsigned)(common_last/(d_nchan*2)));

      printf("ucounter_end[%i]=%i ucounter_begin[%i]=%i\n",i,d_state[i].ucounter_end,i,d_state[i].ucounter_begin);      
      int expected_sync_position=common_last - d_state[i].ucounter_end*(d_nchan*2);
      if(0==uin[expected_sync_position] && 0==uin[expected_sync_position+1])
      {
        printf("sync found on input %i at position %i \n",i,expected_sync_position);
        //sync_start[i]=expected_sync_position;
      } else
      {
        printf("sync found on input %i position unclear, expected at %i value there %i\n",i,expected_sync_position,uin[expected_sync_position]<<16 | uin[expected_sync_position+1]);
        //sync_start[i]=-1;
      }
    } else
    {
      tcPrintf("NOsync diff_end[%i]=%i ucounter_end[%i]=%i ucounter_begin[%i]=%i \n",i,d_state[i].diff_end,i,d_state[i].ucounter_end,i,d_state[i].ucounter_begin);
#endif    
    }
  }
  bool problem=false;
  for(unsigned int i=0;i<ninputs;i++)
    if((d_state[i].diff_end+diff_comp_end_max) >0x4000000)
      {
        problem=true;
        ePrintf("Warning: counter diff greater as 64 Million\n");
        ePrintf("         You might want to swap master and slave.\n");
        ePrintf("          i=%i,d_state[i].diff_end+diff_comp_end_max=%i,d_state[i].diff_end=%i,diff_comp_end_max=%i,ucounter[i]=%i,ucounter[0]=%i\n",
                          i,d_state[i].diff_end+diff_comp_end_max,d_state[i].diff_end,diff_comp_end_max,d_state[i].ucounter_end,d_state[0].ucounter_end);
        //ePrintf("        toconsume=%i\n",toconsume); 
      }
  if(sync_found || all_diffs_zero || problem)
  {
#ifdef DEBUG_TOCONSUME
    if(all_diffs_zero) tcPrintf("ZERO\n");
    if(sync_found) tcPrintf("SYNC\n");
#endif
    for(unsigned int i=0;i<ninputs;i++)
    {
       memcpy(output_items[i],input_items[i],common_end*item_size);
       consume(i,common_end);
#ifdef DEBUG_TOCONSUME
      if(common_end<256)
        tcPrintf("common_end %i\n",common_end);
#endif
    }
    return common_end;
  } else
  {
    //int minconsume=0;//common_end/(2*d_nchan*2);
    //min_consume=min_consume*d_nchan*2;  
    for(unsigned int i=0;i<ninputs;i++)
    {
      int toconsume=std::min((d_state[i].diff_end+diff_comp_end_max)*d_nchan*2,ninput_items[i]);
      toconsume=toconsume/(d_nchan*2);
      toconsume=toconsume*(d_nchan*2);
#ifdef DEBUG_TOCONSUME
      //printf("dcount %i\n",dcount);
      static int toconsume_counter=0;
      toconsume_counter++;
      //if(toconsume_counter>10)
      {
        tcPrintf("toconsume=%i diff_end[[%i]*d_nchan*2=%i diff_comp_end_max)*d_nchan*2=%i ninput_items[%i]=%i\n",
                  toconsume,i,d_state[i].diff_end*d_nchan*2,diff_comp_end_max*d_nchan*2,i,ninput_items[i]);
        toconsume_counter=0;
      }
#endif
      consume(i,toconsume);//skip the difference in samplenumber items
      //printf("toconsume%i %i diff_comp_end_max %i diff_end[[%i] %i\n",i,toconsume,diff_comp_end_max,i,d_state[i].diff_end);
    }
    return 0;
  }
  return -1;//Should never come here
}
#endif /*ALIGN_ADVANCED_IMPLEMENTATION*/
