/* -*- c++ -*- */
/*
 * gr-satnogs: SatNOGS GNU Radio Out-Of-Tree Module
 *
 *  Copyright (C) 2017,2018 Libre Space Foundation <http://librespacefoundation.org/>
 *
 *  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, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program 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 program.  If not, see <http://www.gnu.org/licenses/>.
 */

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

#include <gnuradio/io_signature.h>
#include "noaa_apt_sink_impl.h"

#include <cmath>

namespace gr {
namespace satnogs {
// Noaa apt sync pattern A
// (see https://sourceforge.isae.fr/attachments/download/1813/apt_synch.gif)
const bool noaa_apt_sink_impl::synca_seq[] = {false, false, false, false,
                                              true, true, false, false,   // Pulse 1
                                              true, true, false, false,   // Pulse 2
                                              true, true, false, false,   // Pulse 3
                                              true, true, false, false,   // Pulse 4
                                              true, true, false, false,   // Pulse 5
                                              true, true, false, false,   // Pulse 6
                                              true, true, false, false,   // Pulse 7
                                              false, false, false, false,
                                              false, false, false, false
                                             };

// Noaa apt sync pattern B
// (see https://sourceforge.isae.fr/attachments/download/1813/apt_synch.gif)
const bool noaa_apt_sink_impl::syncb_seq[] = {false, false, false, false,
                                              true, true, true, false, false,
                                              true, true, true, false, false,
                                              true, true, true, false, false,
                                              true, true, true, false, false,
                                              true, true, true, false, false,
                                              true, true, true, false, false,
                                              true, true, true, false, false,
                                              false
                                             };



noaa_apt_sink::sptr
noaa_apt_sink::make(const char *filename_png, size_t width, size_t height,
                    bool sync, bool flip)
{
  return gnuradio::get_initial_sptr(
           new noaa_apt_sink_impl(filename_png, width, height, sync,
                                  flip));
}


/*
 * The private constructor
 */
noaa_apt_sink_impl::noaa_apt_sink_impl(const char *filename_png,
                                       size_t width, size_t height,
                                       bool sync, bool flip) :
  gr::sync_block("noaa_apt_sink",
                 gr::io_signature::make(1, 1, sizeof(float)),
                 gr::io_signature::make(0, 0, 0)),
  f_average_alpha(0.25),
  d_filename_png(filename_png),
  d_width(width),
  d_height(height),
  d_synchronize_opt(sync),
  d_flip(flip),
  d_history_length(40),
  d_has_sync(false),
  d_image_received(false),
  d_current_x(0),
  d_current_y(0),
  d_num_images(0),
  f_max_level(0.0),
  f_min_level(1.0),
  f_average(0.0)
{
  set_history(d_history_length);
  d_full_image = png::image<png::gray_pixel>(d_width, d_height);
}

void
noaa_apt_sink_impl::write_image(png::image<png::gray_pixel> image,
                                std::string filename)
{
  // In case the flip option is set
  if (d_flip) {
    size_t width = image.get_width();
    size_t height = image.get_height();

    // An image of same size is created ...
    png::image<png::gray_pixel> flipped(width, height);

    // ... and all the lines are copied over reverse  order
    for (size_t y = 0; y < height; y++) {
      for (size_t x = 0; x < width; x++) {
        png::gray_pixel pixel = image.get_pixel(x, height - y - 1);
        flipped.set_pixel(x, y, pixel);
      }
    }
    // Write out the flipped image
    flipped.write(filename);
  }
  // In case the flip option is not set
  else {
    // Write out the original
    image.write(filename);
  }
}

noaa_apt_sink_impl::~noaa_apt_sink_impl()
{

}
bool
noaa_apt_sink_impl::stop()
{
  if (!d_image_received) {
    write_image(d_full_image, d_filename_png);
  }
  return true;
}

void noaa_apt_sink_impl::set_pixel(size_t x, size_t y, float sample)
{
  // We can encounter NaN here since skip_to read the history whithout checking
  if (std::isnan(sample)) {
    sample = 0.0;
  }

  // Adjust dynamic range, using minimum and maximum values
  sample = (sample - f_min_level) / (f_max_level - f_min_level) * 255;
  // Set the pixel in the full image
  d_full_image.set_pixel(x, y, sample);
}

void
noaa_apt_sink_impl::skip_to(size_t new_x, size_t pos, const float *samples)
{
  // Check if the skip is forward or backward
  if (new_x > d_current_x) {
    // In case it is forward there will be a new_x - d_current_x sized hole
    // in the image. Holes up 39 pixels can be filled from the modules history
    size_t dist = std::min(size_t(39), new_x - d_current_x);
    // Fill the hole using the previous samples of pos
    for (size_t i = 0; i < dist; i++) {
      set_pixel(new_x - dist + i, d_current_y, samples[pos - dist + i]);
    }
  }
  // Jump to new location
  d_current_x = new_x;
}


noaa_apt_sync_marker
noaa_apt_sink_impl::is_marker(size_t pos, const float *samples)
{
  // Initialize counters for 'hacky' correlation
  size_t count_a = 0;
  size_t count_b = 0;

  for (size_t i = 0; i < 40; i++) {
    // history of previous 39 samples + current one
    // -> start 39 samples in the past
    float sample = samples[pos - 39 + i];
    // Remove DC-offset (aka. the average value of the sync pattern)
    sample = sample - f_average;

    // Very basic 1/0 correlation between pattern constan and history
    if ((sample > 0 && synca_seq[i]) || (sample < 0 && !synca_seq[i])) {
      count_a += 1;
    }
    if ((sample > 0 && syncb_seq[i]) || (sample < 0 && !syncb_seq[i])) {
      count_b += 1;
    }
  }

  // Prefer sync pattern a as it is detected more reliable
  if (count_a > 35) {
    return noaa_apt_sync_marker::SYNC_A;
  }
  else if (count_b > 35) {
    return noaa_apt_sync_marker::SYNC_B;
  }
  else {
    return noaa_apt_sync_marker::NONE;
  }
}


int
noaa_apt_sink_impl::work(int noutput_items,
                         gr_vector_const_void_star &input_items,
                         gr_vector_void_star &output_items)
{
  const float *in = (const float *) input_items[0];
  /* If we have already produced one image, ignore the remaining observation*/
  if (d_image_received) {
    return noutput_items;
  }

  // Structure of in[]:
  // - d_history_length many historical samples
  // - noutput_items many samples to process
  for (size_t i = d_history_length - 1;
       i < noutput_items + d_history_length - 1; i++) {

    // Get the current sample
    float sample = in[i];

    // For some reason the first sample on a Raspberry Pi can be NaN
    if (std::isnan(sample)) {
      continue;
    }

    // Update min and max level to adjust dynamic range in set pixel
    f_max_level = std::fmax(f_max_level, sample);
    f_min_level = std::fmin(f_min_level, sample);

    // Update exponential smoothing average used in sync pattern detection
    f_average = f_average_alpha * sample + (1.0 - f_average_alpha) * f_average;

    // If line sync is enabled
    if (d_synchronize_opt) {
      // Check if the history for the current sample is a sync pattern
      noaa_apt_sync_marker marker = is_marker(i, in);

      // For pattern a
      if (marker == noaa_apt_sync_marker::SYNC_A) {
        // Skip to right location, pattern starts 40 samples in the past
        skip_to(39, i, in);
        // If this is the first sync, reset min and max
        if (!d_has_sync) {
          f_max_level = 0.0;
          f_min_level = 1.0;
          d_has_sync = true;
        }
      }
      // For pattern b
      else if (marker == noaa_apt_sync_marker::SYNC_B) {
        // Skip to right location, pattern starts 40 samples in the past
        skip_to(d_width / 2 + 39, i, in);
        // If this is the first sync, reset min and max
        if (!d_has_sync) {
          f_max_level = 0.0;
          f_min_level = 1.0;
          d_has_sync = true;
        }
      }
    }

    // Set the the pixel at the current position
    set_pixel(d_current_x, d_current_y, sample);

    // Increment x position
    d_current_x += 1;
    // If we are beyond the end of line
    if (d_current_x >= d_width) {
      // Increment y position
      d_current_y += 1;
      // Reset x position to line start
      d_current_x = 0;

      // Split the image if there are enough lines decoded
      if (d_current_y >= d_height) {
        d_current_y = 0;
        d_num_images += 1;
        // Write out the full image
        write_image(d_full_image, d_filename_png);
        d_image_received = true;
      }
    }
  }

  // Tell gnu radio how many samples were consumed
  return noutput_items;
}

} /* namespace satnogs */
} /* namespace gr */