/*!
 * \file rtl_tcp_signal_source_c.cc
 * \brief An rtl_tcp signal source reader.
 * \author Anthony Arnold, 2015. anthony.arnold(at)uqconnect.edu.au
 *
 * This module contains logic taken from gr-omsosdr
 * <http://git.osmocom.org/gr-osmosdr>
 * -------------------------------------------------------------------------
 *
 * Copyright (C) 2010-2015  (see AUTHORS file for a list of contributors)
 *
 * GNSS-SDR is a software defined Global Navigation
 *          Satellite Systems receiver
 *
 * This file is part of GNSS-SDR.
 *
 * GNSS-SDR 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.
 *
 * GNSS-SDR 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 GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
 *
 * -------------------------------------------------------------------------
 */

#include "rtl_tcp_signal_source_c.h"
#include "rtl_tcp_commands.h"
#include <glog/logging.h>
#include <boost/thread/thread.hpp>
#include <map>

using google::LogMessage;

namespace ip = boost::asio::ip;
using boost::asio::ip::tcp;

// Buffer constants
// TODO: Make these configurable
enum {
  RTL_TCP_BUFFER_SIZE = 1024 * 16, // 16 KB
  RTL_TCP_PAYLOAD_SIZE = 1024 * 4  //  4 KB
};

rtl_tcp_signal_source_c_sptr
rtl_tcp_make_signal_source_c(const std::string &address,
                             short port,
                             bool flip_iq)
{
    return gnuradio::get_initial_sptr (new rtl_tcp_signal_source_c (address,
                                                                    port,
                                                                    flip_iq));
}


rtl_tcp_signal_source_c::rtl_tcp_signal_source_c(const std::string &address,
                                                 short port,
                                                 bool flip_iq)
   : gr::sync_block ("rtl_tcp_signal_source_c",
                   gr::io_signature::make(0, 0, 0),
                   gr::io_signature::make(1, 1, sizeof(gr_complex))),
     socket_ (io_service_),
     data_ (RTL_TCP_PAYLOAD_SIZE),
     flip_iq_(flip_iq),
     buffer_ (RTL_TCP_BUFFER_SIZE),
     unread_ (0)
{
   boost::system::error_code ec;

   // 1. Setup lookup table
   for (unsigned i = 0; i < 0xff; i++) {
       lookup_[i] = ((float)(i & 0xff) - 127.4f) * (1.0f / 128.0f);
   }

   // 2. Set socket options
   ip::address addr = ip::address::from_string (address, ec);
   if (ec) {
      std::cout << address << " is not an IP address" << std::endl;
      LOG (ERROR) << address << " is not an IP address";
      return;
   }
   ip::tcp::endpoint ep (addr, port);
   socket_.open (ep.protocol( ), ec);
   if (ec) {
       std::cout << "Failed to open socket." << std::endl;
       LOG (ERROR)  << "Failed to open socket.";
   }

   socket_.set_option (boost::asio::socket_base::reuse_address (true), ec);
   if (ec) {
       std::cout << "Failed to set reuse address option: " << ec << std::endl;
       LOG (WARNING)  << "Failed to set reuse address option";
   }
   socket_.set_option (boost::asio::socket_base::linger (true, 0), ec);
   if (ec) {
       std::cout << "Failed to set linger option: " << ec << std::endl;
       LOG (WARNING)  << "Failed to set linger option";
   }

   // 3. Connect socket

   socket_.connect(ep, ec);
   if (ec) {
       std::cout << "Failed to connect to " << addr << ":" << port
                 << "(" << ec << ")" << std::endl;
       LOG (ERROR)  << "Failed to connect to " << addr << ":" << port
                      << "(" << ec << ")";
       return;
   }
   std::cout << "Connected to " << addr << ":" << port << std::endl;
   LOG (INFO)  << "Connected to " << addr << ":" << port;

   // 4. Set nodelay
   socket_.set_option (tcp::no_delay (true), ec);
   if (ec) {
       std::cout << "Failed to set no delay option." << std::endl;
       LOG (WARNING)  << "Failed to set no delay option";
   }

   // 5. Receive dongle info
   ec = info_.read (socket_);
   if (ec) {
       std::cout << "Failed to read dongle info." << std::endl;
       LOG (WARNING)  << "Failed to read dongle info";
   }
   else if (info_.is_valid ()) {
       std::cout << "Found " << info_.get_type_name() << " tuner."  << std::endl;
       LOG (INFO)  << "Found " << info_.get_type_name() << " tuner.";
   }

   // 6. Start reading
   boost::asio::async_read (socket_, boost::asio::buffer (data_),
			    boost::bind (&rtl_tcp_signal_source_c::handle_read,
					 this, _1, _2));
   boost::thread (boost::bind (&boost::asio::io_service::run, &io_service_));
}

rtl_tcp_signal_source_c::~rtl_tcp_signal_source_c()
{
   io_service_.stop ();
}

int rtl_tcp_signal_source_c::work (int noutput_items,
                                   gr_vector_const_void_star &/*input_items*/,
				    gr_vector_void_star &output_items)
{
  gr_complex *out = reinterpret_cast <gr_complex *>( output_items[0] );
  int i = 0;
  if (io_service_.stopped ()) {
      return -1;
  }

  {
    boost::mutex::scoped_lock lock (mutex_);
    not_empty_.wait (lock, boost::bind (&rtl_tcp_signal_source_c::not_empty,
                                        this));

    for ( ; i < noutput_items && unread_ > 1; i++ ) {
      float re = buffer_[--unread_];
      float im = buffer_[--unread_];
      if (flip_iq_) {
          out[i] = gr_complex (im, re);
      }
      else {
          out[i] = gr_complex (re, im);
      }
    }
  }
  not_full_.notify_one ();
  return i == 0 ? -1 : i;
}


void rtl_tcp_signal_source_c::set_frequency (int frequency) {
   boost::system::error_code ec =
      rtl_tcp_command (RTL_TCP_SET_FREQUENCY, frequency, socket_);
   if (ec) {
      std::cout << "Failed to set frequency" << std::endl;
      LOG (WARNING) << "Failed to set frequency";
   }
}

void rtl_tcp_signal_source_c::set_sample_rate (int sample_rate) {
   boost::system::error_code ec =
      rtl_tcp_command (RTL_TCP_SET_SAMPLE_RATE, sample_rate, socket_);
   if (ec) {
      std::cout << "Failed to set sample rate" << std::endl;
      LOG (WARNING) << "Failed to set sample rate";
   }
}


void rtl_tcp_signal_source_c::set_agc_mode (bool agc) {
    boost::system::error_code ec =
       rtl_tcp_command (RTL_TCP_SET_GAIN_MODE, !agc, socket_);
    if (ec) {
        std::cout << "Failed to set gain mode" << std::endl;
        LOG (WARNING) << "Failed to set gain mode";
    }
    ec =
       rtl_tcp_command (RTL_TCP_SET_AGC_MODE, agc, socket_);
    if (ec) {
        std::cout << "Failed to set gain mode" << std::endl;
        LOG (WARNING) << "Failed to set gain mode";
    }
}

void rtl_tcp_signal_source_c::set_gain (int gain) {
    unsigned clipped = static_cast<unsigned> (info_.clip_gain (gain) * 10.0);
    boost::system::error_code ec =
       rtl_tcp_command (RTL_TCP_SET_GAIN, clipped, socket_);
    if (ec) {
        std::cout << "Failed to set gain" << std::endl;
        LOG (WARNING) << "Failed to set gain";
    }
}

void rtl_tcp_signal_source_c::set_if_gain (int gain) {
    // from gr-osmosdr
    struct range {
       double start, stop, step;
    };
    if (info_.get_tuner_type () != rtl_tcp_dongle_info::TUNER_E4000) {
        return;
    }

    std::vector<range> ranges = {
        { -3, 6, 9 },
        { 0, 9, 3 },
        { 0, 9, 3 },
        { 0, 2, 1 },
        { 3, 15, 3},
        { 3, 15, 3}
    };

    std::map <int, double> gains;
    for (int i = 0; i < static_cast<int>(ranges.size ()); i++) {
        gains[i+1] = ranges[i].start;
    }

    for (int i = ranges.size() - 1; i >= 0; i--) {
        const range &r = ranges[i];
        double error = gain;

        for (double g = r.start; g < r.stop; g += r.step) {
            double sum = 0;
            for (int j = 0; j < static_cast<int> ( gains.size() ); j++) {
                if (i == j) {
                    sum += g;
                }
                else {
                    sum += gains[j + 1];
                }
            }
            double err = std::abs (gain - sum);
            if (err < error) {
                error = err;
                gains[i+1] = g;
            }
        }
    }
    for (unsigned stage = 1; stage <= gains.size(); stage++) {
        int stage_gain = static_cast<int>( gains[stage] * 10 );
        unsigned param = (stage << 16) | (stage_gain & 0xffff);
        boost::system::error_code ec =
           rtl_tcp_command (RTL_TCP_SET_IF_GAIN, param, socket_);
        if (ec) {
            std::cout << "Failed to set if gain" << std::endl;
            LOG (WARNING) << "Failed to set if gain";
        }
    }
}

void
rtl_tcp_signal_source_c::handle_read (const boost::system::error_code &ec,
                                       size_t bytes_transferred)
{
   if (ec) {
      std::cout << "Error during read: " << ec << std::endl;
      LOG (WARNING) << "Error during read: " << ec;
      boost::mutex::scoped_lock lock (mutex_);
      io_service_.stop ();
      not_empty_.notify_one ();
   }
   else {
     {
        // Unpack read data
        boost::mutex::scoped_lock lock (mutex_);
        not_full_.wait (lock,
                        boost::bind (&rtl_tcp_signal_source_c::not_full,
                                     this));

        for (size_t i = 0; i < bytes_transferred; i++) {
          while (!not_full( )) {
            // uh-oh, buffer overflow
            // wait until there's space for more
            not_empty_.notify_one (); // needed?
            not_full_.wait (lock,
                            boost::bind (&rtl_tcp_signal_source_c::not_full,
                                         this));
          }

          buffer_.push_front (lookup_[data_[i]]);
          unread_++;
        }
      }
      // let woker know that more data is available
      not_empty_.notify_one ();
      // Read some more
      boost::asio::async_read (socket_,
                               boost::asio::buffer (data_),
			       boost::bind (&rtl_tcp_signal_source_c::handle_read,
					    this, _1, _2));
   }
}
