/* -*- c++ -*- */
/*
 * Copyright 2020
 *   Federico "Larroca" La Rocca <flarroca@fing.edu.uy>
 *
 *   Instituto de Ingenieria Electrica, Facultad de Ingenieria,
 *   Universidad de la Republica, Uruguay.
 *  
 * 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 <volk/volk.h>
#include <random>
#include "ssamp_correction_impl.h"

namespace gr {
  namespace tempest {

    ssamp_correction::sptr
    ssamp_correction::make(int Htotal, int Vtotal, int correct_sampling, float max_deviation)
    {
      return gnuradio::get_initial_sptr
        (new ssamp_correction_impl(Htotal, Vtotal, correct_sampling, max_deviation));
    }


    /*
     * The private constructor
     */
    ssamp_correction_impl::ssamp_correction_impl(int Htotal, int Vtotal, int correct_sampling, float max_deviation)
      : gr::block("ssamp_correction",
              gr::io_signature::make(1, 1, sizeof(gr_complex)),
              gr::io_signature::make(1, 1, sizeof(gr_complex))),
            d_inter(gr::filter::mmse_fir_interpolator_cc()),
            d_gen(std::random_device{}())
    {
      set_relative_rate(1);
      d_correct_sampling = correct_sampling; 
      d_max_deviation = max_deviation;
      d_Htotal = Htotal; 
      d_Vtotal = Vtotal; 
      d_max_deviation_px = (int)std::ceil(d_Htotal*d_max_deviation);
      
      set_history(d_Vtotal*(d_Htotal+d_max_deviation_px)+1);

      d_peak_line_index = 0;
      d_samp_inc_rem = 0;
      d_stop_fine_sampling_synch = 0;
      d_new_interpolation_ratio_rem = 0;
      
      d_current_line_corr = new gr_complex[2*d_max_deviation_px + 1];
      d_historic_line_corr = new gr_complex[2*d_max_deviation_px + 1];
      d_abs_historic_line_corr = new float[2*d_max_deviation_px + 1];

      /** Note: d_current_frame_corr[i] and derivatives will keep the correlation between pixels 
          px[t] and px[t+Htotal*Vtotal+i]. Since a single pixel de-alignment with the next line will 
          mean d_Vtotal pixels de-alignments with the next frame, these arrays are much bigger. 
          However, instead of always calculating the whole of them, I'll only calculate around those
          indicated by the max in the d_abs_historic_line_corr. 
      */
      d_current_frame_corr = new gr_complex[2*(d_max_deviation_px+1)*d_Vtotal + 1];
      d_historic_frame_corr = new gr_complex[2*(d_max_deviation_px+1)*d_Vtotal + 1];
      d_abs_historic_frame_corr = new float[2*(d_max_deviation_px+1)*d_Vtotal + 1];
      
      d_alpha_samp_inc = 1e-1;
      d_samp_phase = 0; 
      d_alpha_corr = 1e-2; 
      d_next_update = 0;

      for (int i = 0; i<2*d_max_deviation_px+1; i++){
          d_historic_line_corr[i] = 0;
          d_abs_historic_line_corr[i] = 0;
      }
      for (int i = 0; i<2*d_max_deviation_px*d_Vtotal+1; i++){
          d_historic_frame_corr[i] = 0;
          d_abs_historic_frame_corr[i] = 0;
      }

      printf("[TEMPEST] Construction of ssamp_correction_impl with Htotal=%i and Vtotal=%i.\n", Htotal, Vtotal);

      //VOLK alignment as recommended by GNU Radio's Manual. It has a similar effect 
      //than set_output_multiple(), thus we will generally get multiples of this value
      //as noutput_items. 
      const int alignment_multiple = volk_get_alignment() / sizeof(gr_complex);
      set_alignment(std::max(1, alignment_multiple));

      // PMT ports
      message_port_register_in(pmt::mp("ratio"));
      message_port_register_in(pmt::mp("en"));

      // PMT handlers 
      //              : Port                Lambda function
      set_msg_handler(  pmt::mp("ratio"),   [this](const pmt::pmt_t& msg) {ssamp_correction_impl::set_ratio_msg(msg); });
      set_msg_handler(  pmt::mp("en"),      [this](const pmt::pmt_t& msg) {ssamp_correction_impl::set_ena_msg(msg); });
    }


    void 
    ssamp_correction_impl::set_ena_msg(pmt::pmt_t msg)
    {
      if (pmt::is_bool(msg)) {
          bool en = pmt::to_bool(msg);
          gr::thread::scoped_lock l(d_mutex);
          d_stop_fine_sampling_synch = !en;
          printf("ssamp_correction_impl Received Sampling Stop.\n");
      } else {
          GR_LOG_WARN(d_logger,
                      "ssamp_correction_impl Received : Non-PMT type received, expecting Boolean PMT\n");
      }
    }

    void 
    ssamp_correction_impl::set_ratio_msg(pmt::pmt_t msg)
    {
      if(pmt::is_pair(msg)) {
          // saca el primero de la pareja
          pmt::pmt_t key = pmt::car(msg);
          // saca el segundo
          pmt::pmt_t val = pmt::cdr(msg);
          if(pmt::eq(key, pmt::string_to_symbol("ratio"))) {
              if(pmt::is_number(val)) {
                  d_new_interpolation_ratio_rem = (double)pmt::to_double(val);
                  printf("ssamp_correction_impl Received : interpolation ratio  = %f \n", d_new_interpolation_ratio_rem);
              }
          }
      }
    }

    /*
     * Our virtual destructor.
     */
    ssamp_correction_impl::~ssamp_correction_impl()
    {
      delete [] d_current_line_corr;
      delete [] d_historic_line_corr;
      delete [] d_abs_historic_line_corr;
      delete [] d_current_frame_corr;
      delete [] d_historic_frame_corr;
      delete [] d_abs_historic_frame_corr;
    }

    void
    ssamp_correction_impl::forecast (int noutput_items, gr_vector_int &ninput_items_required)
    {
      int ninputs = ninput_items_required.size ();
      // make sure we receive at least Hsize+max_deviation+taps_to_interpolate
      for (int i = 0; i < ninputs; i++)
      {
          ninput_items_required[i] = (int)ceil((noutput_items + 1) * (2+d_samp_inc_rem)) + d_inter.ntaps() ;
      }

    }

    void 
    ssamp_correction_impl::set_Htotal_Vtotal(int Htotal, int Vtotal){
      // If the resolution's changed, I reset the whole block
      /**
       * @fcarraustewart
       *  FIXME: This callback double taps onChange 
       * of any of these two variables, Htotal Vtotal,
       *  This happens on all our testXXX.grc files.
       *  Solution .grc file should not have a link 
       * between these two variables?
       */
      d_Htotal = Htotal; 
      d_Vtotal = Vtotal; 
      
      d_max_deviation_px = (int)std::ceil(d_Htotal*d_max_deviation);
      printf("ssamp_correction_impl d_max_deviation_px: %i\n", d_max_deviation_px);
      set_history(d_Vtotal*(d_Htotal+d_max_deviation_px)+1);

      d_peak_line_index = 0;
      d_samp_inc_rem = 0;
      d_stop_fine_sampling_synch = 0;
      d_new_interpolation_ratio_rem = 0;

      /**
       * < Calling delete for each of the following pointers 
       * and assigning them nullptr right here. Before running the new operator.
       * 
       * */
      delete [] d_current_line_corr;
      delete [] d_historic_line_corr;
      delete [] d_abs_historic_line_corr;
      delete [] d_current_frame_corr;
      delete [] d_historic_frame_corr;
      delete [] d_abs_historic_frame_corr;

      d_current_line_corr = new gr_complex[2*d_max_deviation_px + 1];
      d_historic_line_corr = new gr_complex[2*d_max_deviation_px + 1];
      d_abs_historic_line_corr = new float[2*d_max_deviation_px + 1];
      d_current_frame_corr = new gr_complex[2*(d_max_deviation_px+1)*d_Vtotal + 1];
      d_historic_frame_corr = new gr_complex[2*(d_max_deviation_px+1)*d_Vtotal + 1];
      d_abs_historic_frame_corr = new float[2*(d_max_deviation_px+1)*d_Vtotal + 1];
      //I'll estimate the new sampling synchronization asap
      d_next_update = 0;

      for (int i = 0; i<2*d_max_deviation_px+1; i++){
          d_historic_line_corr[i] = 0;
          d_abs_historic_line_corr[i] = 0;
      }
      for (int i = 0; i<2*d_max_deviation_px*d_Vtotal+1; i++){
          d_historic_frame_corr[i] = 0;
          d_abs_historic_frame_corr[i] = 0;
      }

      printf("[TEMPEST] Construction of ssamp_correction_impl with Htotal=%i and Vtotal=%i.\n", Htotal, Vtotal);

    }
    
    int 
    ssamp_correction_impl::interpolate_input(const gr_complex * in, 
                        gr_complex * out, 
                        int size)
    {
      int ii = 0; // input index
      int oo = 0; // output index

      double s, f; 
      int incr; 
      while(oo < size) {
          out[oo++] = d_inter.interpolate(&in[ii], d_samp_phase);

          s = d_samp_phase + d_samp_inc_rem + 1;
          f = floor(s);
          incr = (int)f;
          d_samp_phase = s - f;
          ii += incr;
      }

      // return how many inputs we required to generate d_cp_length+d_fft_length outputs 
      return ii;
    }

    int
    ssamp_correction_impl::general_work (int noutput_items,
                       gr_vector_int &ninput_items,
                       gr_vector_const_void_star &input_items,
                       gr_vector_void_star &output_items)
    {
      const gr_complex *in = (const gr_complex *) input_items[0];
      gr_complex *out = (gr_complex *) output_items[0];
      gr::thread::scoped_lock l(d_mutex);
      int required_for_interpolation = noutput_items; 
      
      d_samp_inc_rem = d_new_interpolation_ratio_rem;
      required_for_interpolation = interpolate_input(&in[0], &out[0], noutput_items);
      consume_each (required_for_interpolation);
      return noutput_items;
    }

  } /* namespace tempest */
} /* namespace gr */