/* -*- c++ -*- */
/* 
 * Copyright 2017 <+YOU OR YOUR COMPANY+>.
 * 
 * This 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.
 * 
 * This software 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 this software; 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 <gnuradio/io_signature.h>
#include "sync_decim_impl.h"
#define DECIM 2
#define lout log && std::cout
//#define SYNC_COUNTER_MAX 5//higher value -> slower sync, less cpu load
#define SYNC_COUNTER_MAX 200//higher value -> slower sync, less cpu load
//#include <pmt.h>
namespace gr {
  namespace multirds {
    
    sync_decim::sptr
    sync_decim::make(float threshold,float min_diff,bool log)
    {
      return gnuradio::get_initial_sptr
      (new sync_decim_impl(threshold, min_diff, log));
    }
    
    /*
     * The private constructor
     */
    sync_decim_impl::sync_decim_impl(float threshold,float min_diff,bool log)
    : gr::sync_decimator("sync_decim",
        gr::io_signature::make(1, 1, sizeof(float)),
        gr::io_signature::make(1, 1, sizeof(float)), DECIM),
        threshold(threshold),
        min_diff(min_diff),
        log(log)
        
        {
          message_port_register_in(pmt::mp("ctrl"));
          set_msg_handler(pmt::mp("ctrl"), boost::bind(&sync_decim_impl::parse_ctrl_msg, this, _1));
          message_port_register_out(pmt::mp("ctrl"));
          //init persistant vars
          last_input=0;
          mode=COPY;
	  dosync_counter=0;
          weakout_counter=0;
          outbit_counter=0;
        }
        /*
        * Our virtual destructor.
        */
        sync_decim_impl::~sync_decim_impl()
        {
        }
        
        void sync_decim_impl::parse_ctrl_msg(pmt::pmt_t pdu) {
          if(!pmt::is_pair(pdu)) {
          lout << "wrong input message (not a PDU)" << std::endl;
          return;
          }
          pmt::pmt_t meta = pmt::car(pdu);  // meta declares type 0:RDS, 1:sync/nosync
          pmt::pmt_t sync = pmt::cdr(pdu);
          if(1L==pmt::to_long(meta) && pmt::eqv(sync,pmt::PMT_F)){
            if (mode != COPY){
              pmt::pmt_t meta(pmt::from_long(4));
              pmt::pmt_t data(pmt::from_long(COPY));
              pmt::pmt_t pdu(pmt::cons(meta, data));  // make PDU: (metadata, data) pair
              message_port_pub(pmt::mp("ctrl"), pdu);
              lout<< "switched to copy"<<std::endl;
              }
            mode=COPY;

          }
        }
        
        
        int
        sync_decim_impl::work(int noutput_items,
                              gr_vector_const_void_star &input_items,
                              gr_vector_void_star &output_items)
        {
          //if (noutput_items>8){
          const float *in = (const float *) input_items[0];
          float *out = (float *) output_items[0];
          if(outbit_counter>2000){//2000 ^= ~every 2 seconds
            lout<<"mode: "<<mode<<"\t";
            lout<<"weakout_counter:"<<weakout_counter<<std::endl;
            outbit_counter=0;
            weakout_counter=0;
          }
          for (int i = 0; i < noutput_items; i++) {
            if(mode==COPY){
              out[i]=in[DECIM*i];
            }
            else if(mode==SKIP){
              if(i==0){
                out[i]=last_input-in[DECIM*i];}
                else{
                  out[i]=in[DECIM*i-1]-in[DECIM*i];}
            }
            else if(mode==NOSKIP){
              out[i]=in[DECIM*i]-in[DECIM*i+1];
            }
            //lout<<std::fixed << std::setprecision(3)<<out[i]<<".";
            if(mode!=COPY and std::abs(out[i])<threshold){
              weakout_counter++;
            }
          }
          last_input=in[(noutput_items-1)*DECIM+1];//to use for next iteration of work
          //lout<<noutput_items<<std::endl;
          
          /*synchronize:*/
          if(mode==COPY and dosync_counter>=SYNC_COUNTER_MAX){
            //lout<<noutput_items<<std::endl;
	    dosync_counter=0;
            float out_noskip;
            float out_skip;
            int skip_is_better_counter=0;
            if (noutput_items>4)//TODO: what if there are never more than 9 outputs requested
            {
              for (int i = 0; i < noutput_items; i++) {//TODO set max loop length -> cpu load
                if(i==0){
                  out_skip=last_input-in[DECIM*i];}
                  else{
                    out_skip=in[DECIM*i-1]-in[DECIM*i];}
                    
                out_noskip=in[DECIM*i]-in[DECIM*i+1];
                
                /*if (std::abs(out_skip)>std::abs(out_noskip)){
		  skip_is_better_counter++;
                }
                else{
		  skip_is_better_counter--;
                }*/
                if (std::abs(out_skip)>threshold){
                  skip_is_better_counter++;
                }
                if (std::abs(out_noskip) >threshold){
                  skip_is_better_counter--;
                }
                
                //lout<<"state:"<< mode;
                //lout<<"noutputs:"<<noutput_items;
                
                //lout<<"in:"<<in[DECIM*i];
                //lout<<"\t,out_noskip:"<<out_noskip;
                //lout<<"\t,out_skip:"<<out_skip<<std::endl;
                
              }
              if (skip_is_better_counter>3){//2017-05-02 lowered from 6
  
                pmt::pmt_t meta(pmt::from_long(4));
                pmt::pmt_t data(pmt::from_long(SKIP));
                pmt::pmt_t pdu(pmt::cons(meta, data));  // make PDU: (metadata, data) pair
                message_port_pub(pmt::mp("ctrl"), pdu);
                mode=SKIP;
                lout<<"switched to skip"<< std::endl;
              }
              else if (skip_is_better_counter<-3){
                pmt::pmt_t meta(pmt::from_long(4));
                pmt::pmt_t data(pmt::from_long(NOSKIP));
                pmt::pmt_t pdu(pmt::cons(meta, data));  // make PDU: (metadata, data) pair
                message_port_pub(pmt::mp("ctrl"), pdu);
                mode=NOSKIP;
                lout<<"switched to noskip"<< std::endl;
              }
            }
            else{//wait for longer input
              dosync_counter=SYNC_COUNTER_MAX-2;
            }
          }
	  else if(mode==COPY){
	      dosync_counter++;
          }
          
          // Tell runtime system how many output items we produced.
          outbit_counter+=noutput_items;
          return noutput_items;
            
          /*}
          else{return 0;}*/
        }/*end of work*/
  } /* namespace multirds */
} /* namespace gr */