// Copyright (c) 2021 Graphcore Ltd. All rights reserved.

#include <boost/program_options.hpp>
#include <poplar/Engine.hpp>
#include <poplar/OptionFlags.hpp>
#include <poplibs_support/TestDevice.hpp>
#include <poplibs_test/Util.hpp>
#include <poplin/codelets.hpp>
#include <popops/ElementWise.hpp>
#include <popops/Expr.hpp>
#include <popops/codelets.hpp>
#include <poputil/TileMapping.hpp>
#include <poputil/exceptions.hpp>

#include <sstream>
#include <utility>
#include <vector>

using namespace poplibs_support;
using namespace popops;
using namespace popops::expr;

static DeviceType deviceType;

std::pair<std::vector<float>, std::vector<float>>
executeExpr(const Expr &expression, const poplar::Type &dType, bool inPlace,
            bool enableFusedCodelets = true) {
  auto device = createTestDevice(TEST_TARGET);
  const auto &target = device.getTarget();
  poplar::Graph g(device.getTarget());
  popops::addCodelets(g);

  const std::vector<unsigned> hostInA = {1, 2, 3, 4, 5, 6, 7, 8};

  const std::vector<unsigned> hostInC = hostInA;

  const std::vector<unsigned> hostInB = {8, 7, 6, 5, 4, 3, 2, 1};

  unsigned size = hostInA.size();

  auto a = g.addVariable(dType, {size}, "inputa");
  g.setTileMapping(a, 0);
  auto b = g.addVariable(dType, {size}, "inputb");
  g.setTileMapping(b, 0);
  auto c = g.addVariable(dType, {size}, "inputc");
  g.setTileMapping(c, 0);

  auto rawBufSize = target.getTypeSize(dType) * size;

  std::vector<char> rawInA(rawBufSize);
  std::vector<char> rawInB(rawBufSize);
  std::vector<char> rawInC(rawBufSize);
  std::vector<char> rawOutOpt(rawBufSize);
  std::vector<char> rawOutNoOpt(rawBufSize);

  poplibs_test::util::copy(target, hostInA.data(), size, dType,
                           reinterpret_cast<void *>(rawInA.data()));
  poplibs_test::util::copy(target, hostInC.data(), size, dType,
                           reinterpret_cast<void *>(rawInC.data()));

  poplar::OptionFlags enableOptims = {
      {"enableExpressionOptimizations", "true"}};
  poplar::OptionFlags disableOptims = {
      {"enableExpressionOptimizations", "false"}};

  enableOptims.set("enableGenerateCodelet",
                   enableFusedCodelets ? "true" : "false");

  poplar::program::Sequence progOpt;
  poplar::Tensor tOpt;
  if (!inPlace) {
    tOpt =
        popops::map(g, expression, {a, b}, progOpt, "test1Opt", enableOptims);
  } else {
    popops::mapInPlace(g, expression, {a, b}, progOpt, "test1Opt",
                       enableOptims);
  }

  poplar::program::Sequence progNoOpt;
  poplar::Tensor tNoOpt;
  if (!inPlace) {
    tNoOpt = popops::map(g, expression, {c, b}, progNoOpt, "test1NoOpt",
                         disableOptims);
  } else {
    popops::mapInPlace(g, expression, {c, b}, progNoOpt, "test1NoOpt",
                       disableOptims);
  }
  g.createHostWrite("inA", a);
  g.createHostWrite("inC", c);
  g.createHostWrite("inB", b);
  if (!inPlace) {
    g.createHostRead("tOpt", tOpt);
    g.createHostRead("tNoOpt", tNoOpt);
  } else {
    g.createHostRead("tOpt", a);
    g.createHostRead("tNoOpt", c);
  }

  poplar::program::Sequence controlProg(
      {std::move(progOpt), std::move(progNoOpt)});

  poplar::Engine e(g, controlProg);
  device.bind([&](const poplar::Device &d) {
    e.load(d);
    e.writeTensor("inA", rawInA.data(), rawInA.data() + rawInA.size());
    e.writeTensor("inC", rawInC.data(), rawInC.data() + rawInC.size());
    e.writeTensor("inB", rawInB.data(), rawInB.data() + rawInB.size());
    e.run();
    e.readTensor("tOpt", rawOutOpt.data(), rawOutOpt.data() + rawOutOpt.size());
    e.readTensor("tNoOpt", rawOutNoOpt.data(),
                 rawOutNoOpt.data() + rawOutNoOpt.size());
  });

  std::vector<float> hostOutOpt(size);
  poplibs_test::util::copy(target, dType,
                           reinterpret_cast<void *>(rawOutOpt.data()),
                           hostOutOpt.data(), size);
  std::vector<float> hostOutNoOpt(size);
  poplibs_test::util::copy(target, dType,
                           reinterpret_cast<void *>(rawOutNoOpt.data()),
                           hostOutNoOpt.data(), size);

  return std::make_pair(hostOutOpt, hostOutNoOpt);
}

int main(int argc, char **argv) {
  namespace po = boost::program_options;

  std::string test;
  poplar::Type dataType;
  bool rhsIsSigned, powerOfTwo, inPlace;

  po::options_description desc("Options");
  // clang-format off
  desc.add_options() ("help", "Print help")
    ("device-type",
     po::value<DeviceType>(&deviceType)->required(),
     "Device Type")
    ("rhs-is-signed",
     po::value<bool>(&rhsIsSigned)->required(),
     "Right hand side is signed")
    ("power-of-two",
     po::value<bool>(&powerOfTwo)->required(),
     "right hand side is power of two")
    ("data-type",
     po::value<poplar::Type>(&dataType)->required(),
     "Data Type: unsigned, int")
    ("in-place",
     po::value<bool>(&inPlace)->required(),
     "operation is inplace")
    ("test-type",
     po::value<std::string>(&test)->required(),
     "The test to run: REMAINDER | DIVIDE ");
  // clang-format on

  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n\n";
      return 1;
    }
    po::notify(vm);
  } catch (std::exception &e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }

  // Only allow tests for types for which codelets are instantiated
  if (dataType != poplar::UNSIGNED_INT && dataType != poplar::INT) {
    std::cerr << "Unsupported data type\n";
    return 1;
  }

  std::unique_ptr<popops::expr::Expr> subExpr;

  if (test == "REMAINDER") {
    if (rhsIsSigned) {
      int rhsValue = powerOfTwo ? 4 : 3;
      auto e = Rem(_2, Const(rhsValue));
      subExpr = e.clone();
    } else {
      unsigned rhsValue = powerOfTwo ? 4 : 3;
      auto e = Rem(_2, Const(rhsValue));
      subExpr = e.clone();
    }
  } else if (test == "DIVIDE") {
    if (rhsIsSigned) {
      int rhsValue = powerOfTwo ? 4 : 3;
      auto e = Divide(_2, Const(rhsValue));
      subExpr = e.clone();
    } else {
      unsigned rhsValue = powerOfTwo ? 4 : 3;
      auto e = Divide(_2, Const(rhsValue));
      subExpr = e.clone();
    }
  } else {
    std::cerr << "Unsupported test type \n";
    return 1;
  }
  auto mainExpr = Add(Add(Add(_1, _2), *subExpr), Const(3));
  auto [opt, noOpt] = executeExpr(mainExpr, dataType, inPlace);
  for (unsigned i = 0; i != opt.size(); ++i) {
    if (opt[i] != noOpt[i]) {
      std::cerr << "mismatch at position " << i << " expected " << noOpt[i];
      std::cerr << " actual " << opt[i];
      return 1;
    }
  }
  return 0;
}


//The PCL_NO_PRECOMPILE is necessary to use my own Point T template PointXYZVI within the pcl functions
#include <memory>
//#include <boost/foreach.hpp>
#include <functional>
#include <chrono>

#include <Eigen/Geometry>

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <pcl/pcl_base.h>

#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <tf2/exceptions.h>

#include <pcl/PCLHeader.h>
#include <pcl/filters/crop_box.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/impl/passthrough.hpp>

#include <pcl/filters/voxel_grid.h>
#include "pcl_conversions/pcl_conversions.h"

using namespace std::chrono_literals;

/* The PointFilterCopy node filters unimportent points based on three criteria:
1. RoI: The region of interest is described by a box. All points inside this box are considered relevant. The parameters of the box
are described in world coordinates. Thus, a transformation from sensor to world needs to be given. It is recommended to set the world
coordinate system onto the road surface. This allows you to easily remove the ground points by defining a box just a bit higher then the road surface.
2. Velocity: All points with low absolute velocity are filtered (considered to be background - especially useful, if the sensor itself does not move).
**/
class PointFilterCopy : public rclcpp::Node
{
  public:
    PointFilterCopy()
    : Node("PointFilterCopy")
    {
      subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2> (
      "input_cloud", 10, std::bind(&PointFilterCopy::topic_callback, this, std::placeholders::_1)
      );
      publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("filtered_cloud", 10);

      //tf2 listener
      tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
      transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

      //filter objects
      cropBoxFilter_ = new pcl::CropBox<pcl::PCLPointCloud2>();
      vel_filter_ = new pcl::PassThrough<pcl::PCLPointCloud2>();
      vel_filter_ ->setFilterFieldName("velocity");
      vel_filter_ ->setNegative(true);
      sor_ = std::make_unique<pcl::VoxelGrid<pcl::PCLPointCloud2>>();

      //parameters
      this->declare_parameter<std::string>("input_frame", "input_frame");
      this->declare_parameter<float>("box_min_x", 0.f);
      this->declare_parameter<float>("box_min_y", 0.f);
      this->declare_parameter<float>("box_min_z", -10.f);
      this->declare_parameter<float>("box_max_x", 100.f);
      this->declare_parameter<float>("box_max_y", 30.f);
      this->declare_parameter<float>("box_max_z", 10.f);
      this->declare_parameter<float>("vel_min", -0.1f);
      this->declare_parameter<float>("vel_max", 0.1f);
      this->declare_parameter<float>("voxel_size", 0.3f);

      timer_ = this->create_wall_timer(
      10000ms, std::bind(&PointFilterCopy::setParameters, this));
      setParameters();
    }

    void setParameters()
    {
      this->get_parameter("input_frame", fromFrameRel);
      this->get_parameter("box_min_x", box_min_x);
      this->get_parameter("box_min_y", box_min_y);
      this->get_parameter("box_min_z", box_min_z);
      this->get_parameter("box_max_x", box_max_x);
      this->get_parameter("box_max_y", box_max_y);
      this->get_parameter("box_max_z", box_max_z);
      this->get_parameter("vel_low", vel_low);
      this->get_parameter("vel_high", vel_high);
      this->get_parameter("voxel_size", voxel_size);
      min_pt << box_min_x, box_min_y, box_min_z, 1.f;
      max_pt << box_max_x, box_max_y, box_max_z, 1.f;
      cropBoxFilter_->setMin(min_pt);
      cropBoxFilter_->setMax(max_pt);
      vel_filter_ ->setFilterLimits(vel_low, vel_high);
      sor_->setLeafSize (voxel_size, voxel_size, voxel_size);
      //RCLCPP_INFO(this->get_logger(), "Filter box parameter set to: x %f - %f, y %f - %f, z %f - %f \n"
      //                                " Velocity filter set to: %f - %f",
      //                                 box_min_x,box_max_x, box_min_y,box_max_y, box_min_z,box_max_z, vel_low, vel_high);
    }

  private:

    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;

    //tf2 listener
    std::shared_ptr<tf2_ros::TransformListener> transform_listener_{nullptr};
    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;

    //listener parameters
    std::string fromFrameRel;
    std::string toFrameRel = "world";

    //filter elements
    pcl::CropBox<pcl::PCLPointCloud2> * cropBoxFilter_;
    Eigen::Vector4f min_pt;
    Eigen::Vector4f max_pt;
    pcl::PassThrough<pcl::PCLPointCloud2> * vel_filter_;
    rclcpp::TimerBase::SharedPtr timer_;
    std::unique_ptr<pcl::VoxelGrid<pcl::PCLPointCloud2>> sor_;

    //filter parameter
    float box_min_x;
    float box_min_y;
    float box_min_z;
    float box_max_x;
    float box_max_y;
    float box_max_z;

    float vel_low;
    float vel_high;

    float voxel_size;

    void topic_callback(const sensor_msgs::msg::PointCloud2::SharedPtr point_cloud2_msgs) const
    {
      using std::chrono::high_resolution_clock;
      using std::chrono::duration_cast;
      using std::chrono::duration;
      using std::chrono::milliseconds;

      auto t1 = high_resolution_clock::now();
      //----------deserialize--------------
      //pcl::PointCloud<PointXYZVI> cloud_out;
      //pcl::PointCloud<PointXYZVI> input_cloud;
      //fromROSMsg(*point_cloud2_msgs, input_cloud);

      //---------tf2 transformation------------
      //this stuff should be done with pcl_ros as soon as a filter transformation is available.
      geometry_msgs::msg::TransformStamped transformStamped;
      try {
          transformStamped = tf_buffer_->lookupTransform(
            toFrameRel, fromFrameRel,
            tf2::TimePointZero);
        } catch (tf2::TransformException & ex) {
          RCLCPP_INFO(
            this->get_logger(), "Could not transform %s to %s: %s",
            toFrameRel.c_str(), fromFrameRel.c_str(), ex.what());
          return;
        }

      Eigen::Affine3f transform;
      Eigen::AngleAxisf rotation;
      Eigen::Transform<float,3,Eigen::Affine> t;
      Eigen::Quaternion<float> quat(transformStamped.transform.rotation.w,
                          transformStamped.transform.rotation.x,
                          transformStamped.transform.rotation.y,
                          transformStamped.transform.rotation.z);
      rotation = quat;
      Eigen::Translation<float,3> translation(transformStamped.transform.translation.x, transformStamped.transform.translation.y, transformStamped.transform.translation.z);
      t = translation * rotation;
      cropBoxFilter_->setTransform(t);

      //---------filter-----------
      //I don't like copying the data, but I have not found another way to get the pointcloud behind a shared Ptr.
      //pcl::PointCloud<PointXYZVI>::Ptr boxfilter_cloud(new pcl::PointCloud<PointXYZVI>(input_cloud));
      //pcl::PointCloud<PointXYZVI>::Ptr vel_cloud(new pcl::PointCloud<PointXYZVI>(cloud_out));

      pcl::PCLPointCloud2::Ptr boxfiltered_cloud (new pcl::PCLPointCloud2());
      pcl::PCLPointCloud2::Ptr velocity_filtered_cloud (new pcl::PCLPointCloud2());
      pcl::PCLPointCloud2::Ptr output_cloud (new pcl::PCLPointCloud2());
      pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2();
      //pcl::PCLPointCloud2 cloud_filtered;
      //Performance note: Not sure if PCLPointCloud2ConstPtr copies the data - if so this should be optimized
      pcl_conversions::toPCL(*point_cloud2_msgs, *cloud);
      //pcl::PCLPointCloud2::Ptr input_cloud (new pcl::PCLPointCloud2());
      pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
      cropBoxFilter_ ->setInputCloud(cloudPtr);
      cropBoxFilter_ ->filter(*boxfiltered_cloud);
      //----------velocity based filtering-------------
      pcl::PCLPointCloud2ConstPtr vel_input_cloudPtr(boxfiltered_cloud);
      vel_filter_ ->setInputCloud(vel_input_cloudPtr);
      vel_filter_ ->filter(*velocity_filtered_cloud);
      //----------down sampling------------
      pcl::PCLPointCloud2ConstPtr voxel_input_cloudPtr(velocity_filtered_cloud);
      sor_->setInputCloud (voxel_input_cloudPtr);
      sor_->filter (*output_cloud);

      //----------serialize---------------
      sensor_msgs::msg::PointCloud2 output;
      pcl_conversions::fromPCL(*output_cloud, output);
      publisher_->publish(output);

      //Get runtime info
      auto t2 = high_resolution_clock::now();
      duration<double, std::milli> ms_double = t2 - t1;

      RCLCPP_INFO(
        this->get_logger(), "Pointfilter took %f ms to complete",
            ms_double.count());
    }
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<PointFilterCopy>());
  rclcpp::shutdown();
  return 0;
}


/* -*- c++ -*- */
/*
 * Copyright 2013 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 <gnuradio/filter/pfb_arb_resampler.h>
#include <gnuradio/logger.h>
#include <cstdio>
#include <stdexcept>
#include <boost/math/special_functions/round.hpp>

namespace gr {
  namespace filter {
    namespace kernel {

      pfb_arb_resampler_ccf::pfb_arb_resampler_ccf(float rate,
                                                   const std::vector<float> &taps,
                                                   unsigned int filter_size)
      {
        d_acc = 0; // start accumulator at 0

        /* The number of filters is specified by the user as the
           filter size; this is also the interpolation rate of the
           filter. We use it and the rate provided to determine the
           decimation rate. This acts as a rational resampler. The
           flt_rate is calculated as the residual between the integer
           decimation rate and the real decimation rate and will be
           used to determine to interpolation point of the resampling
           process.
        */
        d_int_rate = filter_size;
        set_rate(rate);

        d_last_filter = (taps.size()/2) % filter_size;

        d_filters = std::vector<fir_filter_ccf*>(d_int_rate);
        d_diff_filters = std::vector<fir_filter_ccf*>(d_int_rate);

        // Create an FIR filter for each channel and zero out the taps
        std::vector<float> vtaps(0, d_int_rate);
        for(unsigned int i = 0; i < d_int_rate; i++) {
          d_filters[i] = new fir_filter_ccf(1, vtaps);
          d_diff_filters[i] = new fir_filter_ccf(1, vtaps);
        }

        // Now, actually set the filters' taps
        set_taps(taps);

        // Delay is based on number of taps per filter arm. Round to
        // the nearest integer.
        float delay = rate * (taps_per_filter() - 1.0) / 2.0;
        d_delay = static_cast<int>(boost::math::iround(delay));

        // This calculation finds the phase offset induced by the
        // arbitrary resampling. It's based on which filter arm we are
        // at the filter's group delay plus the fractional offset
        // between the samples. Calculated here based on the rotation
        // around nfilts starting at start_filter.
        float accum = d_delay * d_flt_rate;
        int   accum_int = static_cast<int>(accum);
        float accum_frac = accum - accum_int;
        int end_filter = static_cast<int>
          (boost::math::iround(fmodf(d_last_filter + d_delay * d_dec_rate + accum_int, \
                                     static_cast<float>(d_int_rate))));

        d_est_phase_change = d_last_filter - (end_filter + accum_frac);
      }

      pfb_arb_resampler_ccf::~pfb_arb_resampler_ccf()
      {
        for(unsigned int i = 0; i < d_int_rate; i++) {
          delete d_filters[i];
          delete d_diff_filters[i];
        }
      }

      void
      pfb_arb_resampler_ccf::create_taps(const std::vector<float> &newtaps,
                                         std::vector< std::vector<float> > &ourtaps,
                                         std::vector<fir_filter_ccf*> &ourfilter)
      {
        unsigned int ntaps = newtaps.size();
        d_taps_per_filter = (unsigned int)ceil((double)ntaps/(double)d_int_rate);

        // Create d_numchan vectors to store each channel's taps
        ourtaps.resize(d_int_rate);

        // Make a vector of the taps plus fill it out with 0's to fill
        // each polyphase filter with exactly d_taps_per_filter
        std::vector<float> tmp_taps;
        tmp_taps = newtaps;
        while((float)(tmp_taps.size()) < d_int_rate*d_taps_per_filter) {
          tmp_taps.push_back(0.0);
        }

        for(unsigned int i = 0; i < d_int_rate; i++) {
          // Each channel uses all d_taps_per_filter with 0's if not enough taps to fill out
          ourtaps[i] = std::vector<float>(d_taps_per_filter, 0);
          for(unsigned int j = 0; j < d_taps_per_filter; j++) {
            ourtaps[i][j] = tmp_taps[i + j*d_int_rate];
          }

          // Build a filter for each channel and add it's taps to it
          ourfilter[i]->set_taps(ourtaps[i]);
        }
      }

      void
      pfb_arb_resampler_ccf::create_diff_taps(const std::vector<float> &newtaps,
                                              std::vector<float> &difftaps)
      {
        // Calculate the differential taps using a derivative filter
        std::vector<float> diff_filter(2);
        diff_filter[0] = -1;
        diff_filter[1] = 1;

        for(unsigned int i = 0; i < newtaps.size()-1; i++) {
          float tap = 0;
          for(unsigned int j = 0; j < diff_filter.size(); j++) {
            tap += diff_filter[j]*newtaps[i+j];
          }
          difftaps.push_back(tap);
        }
        difftaps.push_back(0);
      }

      void
      pfb_arb_resampler_ccf::set_taps(const std::vector<float> &taps)
      {
        std::vector<float> dtaps;
        create_diff_taps(taps, dtaps);
        create_taps(taps, d_taps, d_filters);
        create_taps(dtaps, d_dtaps, d_diff_filters);
      }

      std::vector<std::vector<float> >
      pfb_arb_resampler_ccf::taps() const
      {
        return d_taps;
      }

      void
      pfb_arb_resampler_ccf::print_taps()
      {
        unsigned int i, j;
        for(i = 0; i < d_int_rate; i++) {
          printf("filter[%d]: [", i);
          for(j = 0; j < d_taps_per_filter; j++) {
            printf(" %.4e", d_taps[i][j]);
          }
          printf("]\n");
        }
      }

      void
      pfb_arb_resampler_ccf::set_rate(float rate)
      {
        d_dec_rate = (unsigned int)floor(d_int_rate/rate);
        d_flt_rate = (d_int_rate/rate) - d_dec_rate;
      }

      void
      pfb_arb_resampler_ccf::set_phase(float ph)
      {
        if((ph < 0) || (ph >= 2.0*M_PI)) {
          throw std::runtime_error("pfb_arb_resampler_ccf: set_phase value out of bounds [0, 2pi).\n");
        }

        float ph_diff = 2.0*M_PI / (float)d_filters.size();
        d_last_filter = static_cast<int>(ph / ph_diff);
      }

      float
      pfb_arb_resampler_ccf::phase() const
      {
        float ph_diff = 2.0*M_PI / static_cast<float>(d_filters.size());
        return d_last_filter * ph_diff;
      }

      unsigned int
      pfb_arb_resampler_ccf::taps_per_filter() const
      {
        return d_taps_per_filter;
      }

      float
      pfb_arb_resampler_ccf::phase_offset(float freq, float fs)
      {
        float adj = (2.0*M_PI)*(freq/fs)/static_cast<float>(d_int_rate);
        return -adj * d_est_phase_change;
      }

      int
      pfb_arb_resampler_ccf::filter(gr_complex *output, gr_complex *input,
                                    int n_to_read, int &n_read)
      {
        int i_out = 0, i_in = 0;
        unsigned int j = d_last_filter;;
        gr_complex o0, o1;

        while(i_in < n_to_read) {
          // start j by wrapping around mod the number of channels
          while(j < d_int_rate) {
            // Take the current filter and derivative filter output
            o0 = d_filters[j]->filter(&input[i_in]);
            o1 = d_diff_filters[j]->filter(&input[i_in]);

            output[i_out] = o0 + o1*d_acc;     // linearly interpolate between samples
            i_out++;

            // Adjust accumulator and index into filterbank
            d_acc += d_flt_rate;
            j += d_dec_rate + (int)floor(d_acc);
            d_acc = fmodf(d_acc, 1.0);
          }
          i_in += (int)(j / d_int_rate);
          j = j % d_int_rate;
        }
        d_last_filter = j; // save last filter state for re-entry

        n_read = i_in;   // return how much we've actually read
        return i_out;    // return how much we've produced
      }

      /****************************************************************/

      pfb_arb_resampler_ccc::pfb_arb_resampler_ccc(float rate,
                                                   const std::vector<gr_complex> &taps,
                                                   unsigned int filter_size)
      {
        d_acc = 0; // start accumulator at 0

        /* The number of filters is specified by the user as the
           filter size; this is also the interpolation rate of the
           filter. We use it and the rate provided to determine the
           decimation rate. This acts as a rational resampler. The
           flt_rate is calculated as the residual between the integer
           decimation rate and the real decimation rate and will be
           used to determine to interpolation point of the resampling
           process.
        */
        d_int_rate = filter_size;
        set_rate(rate);

        d_last_filter = (taps.size()/2) % filter_size;

        d_filters = std::vector<fir_filter_ccc*>(d_int_rate);
        d_diff_filters = std::vector<fir_filter_ccc*>(d_int_rate);

        // Create an FIR filter for each channel and zero out the taps
        std::vector<gr_complex> vtaps(0, d_int_rate);
        for(unsigned int i = 0; i < d_int_rate; i++) {
          d_filters[i] = new fir_filter_ccc(1, vtaps);
          d_diff_filters[i] = new fir_filter_ccc(1, vtaps);
        }

        // Now, actually set the filters' taps
        set_taps(taps);

        // Delay is based on number of taps per filter arm. Round to
        // the nearest integer.
        float delay = rate * (taps_per_filter() - 1.0) / 2.0;
        d_delay = static_cast<int>(boost::math::iround(delay));

        // This calculation finds the phase offset induced by the
        // arbitrary resampling. It's based on which filter arm we are
        // at the filter's group delay plus the fractional offset
        // between the samples. Calculated here based on the rotation
        // around nfilts starting at start_filter.
        float accum = d_delay * d_flt_rate;
        int   accum_int = static_cast<int>(accum);
        float accum_frac = accum - accum_int;
        int end_filter = static_cast<int>
          (boost::math::iround(fmodf(d_last_filter + d_delay * d_dec_rate + accum_int, \
                                     static_cast<float>(d_int_rate))));

        d_est_phase_change = d_last_filter - (end_filter + accum_frac);
      }

      pfb_arb_resampler_ccc::~pfb_arb_resampler_ccc()
      {
        for(unsigned int i = 0; i < d_int_rate; i++) {
          delete d_filters[i];
          delete d_diff_filters[i];
        }
      }

      void
      pfb_arb_resampler_ccc::create_taps(const std::vector<gr_complex> &newtaps,
                                         std::vector< std::vector<gr_complex> > &ourtaps,
                                         std::vector<fir_filter_ccc*> &ourfilter)
      {
        unsigned int ntaps = newtaps.size();
        d_taps_per_filter = (unsigned int)ceil((double)ntaps/(double)d_int_rate);

        // Create d_numchan vectors to store each channel's taps
        ourtaps.resize(d_int_rate);

        // Make a vector of the taps plus fill it out with 0's to fill
        // each polyphase filter with exactly d_taps_per_filter
        std::vector<gr_complex> tmp_taps;
        tmp_taps = newtaps;
        while((float)(tmp_taps.size()) < d_int_rate*d_taps_per_filter) {
          tmp_taps.push_back(0.0);
        }

        for(unsigned int i = 0; i < d_int_rate; i++) {
          // Each channel uses all d_taps_per_filter with 0's if not enough taps to fill out
          ourtaps[i] = std::vector<gr_complex>(d_taps_per_filter, 0);
          for(unsigned int j = 0; j < d_taps_per_filter; j++) {
            ourtaps[i][j] = tmp_taps[i + j*d_int_rate];
          }

          // Build a filter for each channel and add it's taps to it
          ourfilter[i]->set_taps(ourtaps[i]);
        }
      }

      void
      pfb_arb_resampler_ccc::create_diff_taps(const std::vector<gr_complex> &newtaps,
                                              std::vector<gr_complex> &difftaps)
      {
        // Calculate the differential taps using a derivative filter
        std::vector<gr_complex> diff_filter(2);
        diff_filter[0] = -1;
        diff_filter[1] = 1;

        for(unsigned int i = 0; i < newtaps.size()-1; i++) {
          gr_complex tap = 0;
          for(unsigned int j = 0; j < diff_filter.size(); j++) {
            tap += diff_filter[j]*newtaps[i+j];
          }
          difftaps.push_back(tap);
        }
        difftaps.push_back(0);
      }

      void
      pfb_arb_resampler_ccc::set_taps(const std::vector<gr_complex> &taps)
      {
        std::vector<gr_complex> dtaps;
        create_diff_taps(taps, dtaps);
        create_taps(taps, d_taps, d_filters);
        create_taps(dtaps, d_dtaps, d_diff_filters);
      }

      std::vector<std::vector<gr_complex> >
      pfb_arb_resampler_ccc::taps() const
      {
        return d_taps;
      }

      void
      pfb_arb_resampler_ccc::print_taps()
      {
        unsigned int i, j;
        for(i = 0; i < d_int_rate; i++) {
          printf("filter[%d]: [", i);
          for(j = 0; j < d_taps_per_filter; j++) {
            printf(" %.4e + j%.4e", d_taps[i][j].real(), d_taps[i][j].imag());
          }
          printf("]\n");
        }
      }

      void
      pfb_arb_resampler_ccc::set_rate(float rate)
      {
        d_dec_rate = (unsigned int)floor(d_int_rate/rate);
        d_flt_rate = (d_int_rate/rate) - d_dec_rate;
      }

      void
      pfb_arb_resampler_ccc::set_phase(float ph)
      {
        if((ph < 0) || (ph >= 2.0*M_PI)) {
          throw std::runtime_error("pfb_arb_resampler_ccc: set_phase value out of bounds [0, 2pi).\n");
        }

        float ph_diff = 2.0*M_PI / (float)d_filters.size();
        d_last_filter = static_cast<int>(ph / ph_diff);
      }

      float
      pfb_arb_resampler_ccc::phase() const
      {
        float ph_diff = 2.0*M_PI / static_cast<float>(d_filters.size());
        return d_last_filter * ph_diff;
      }

      unsigned int
      pfb_arb_resampler_ccc::taps_per_filter() const
      {
        return d_taps_per_filter;
      }

      float
      pfb_arb_resampler_ccc::phase_offset(float freq, float fs)
      {
        float adj = (2.0*M_PI)*(freq/fs)/static_cast<float>(d_int_rate);
        return -adj * d_est_phase_change;
      }

      int
      pfb_arb_resampler_ccc::filter(gr_complex *output, gr_complex *input,
                                    int n_to_read, int &n_read)
      {
        int i_out = 0, i_in = 0;
        unsigned int j = d_last_filter;;
        gr_complex o0, o1;

        while(i_in < n_to_read) {
          // start j by wrapping around mod the number of channels
          while(j < d_int_rate) {
            // Take the current filter and derivative filter output
            o0 = d_filters[j]->filter(&input[i_in]);
            o1 = d_diff_filters[j]->filter(&input[i_in]);

            output[i_out] = o0 + o1*d_acc;     // linearly interpolate between samples
            i_out++;

            // Adjust accumulator and index into filterbank
            d_acc += d_flt_rate;
            j += d_dec_rate + (int)floor(d_acc);
            d_acc = fmodf(d_acc, 1.0);
          }
          i_in += (int)(j / d_int_rate);
          j = j % d_int_rate;
        }
        d_last_filter = j; // save last filter state for re-entry

        n_read = i_in;   // return how much we've actually read
        return i_out;    // return how much we've produced
      }

      /****************************************************************/

      pfb_arb_resampler_fff::pfb_arb_resampler_fff(float rate,
                                                   const std::vector<float> &taps,
                                                   unsigned int filter_size)
      {
        d_acc = 0; // start accumulator at 0

        /* The number of filters is specified by the user as the
           filter size; this is also the interpolation rate of the
           filter. We use it and the rate provided to determine the
           decimation rate. This acts as a rational resampler. The
           flt_rate is calculated as the residual between the integer
           decimation rate and the real decimation rate and will be
           used to determine to interpolation point of the resampling
           process.
        */
        d_int_rate = filter_size;
        set_rate(rate);

        d_last_filter = (taps.size()/2) % filter_size;

        d_filters = std::vector<fir_filter_fff*>(d_int_rate);
        d_diff_filters = std::vector<fir_filter_fff*>(d_int_rate);

        // Create an FIR filter for each channel and zero out the taps
        std::vector<float> vtaps(0, d_int_rate);
        for(unsigned int i = 0; i < d_int_rate; i++) {
          d_filters[i] = new fir_filter_fff(1, vtaps);
          d_diff_filters[i] = new fir_filter_fff(1, vtaps);
        }

        // Now, actually set the filters' taps
        set_taps(taps);

        // Delay is based on number of taps per filter arm. Round to
        // the nearest integer.
        float delay = rate * (taps_per_filter() - 1.0) / 2.0;
        d_delay = static_cast<int>(boost::math::iround(delay));

        // This calculation finds the phase offset induced by the
        // arbitrary resampling. It's based on which filter arm we are
        // at the filter's group delay plus the fractional offset
        // between the samples. Calculated here based on the rotation
        // around nfilts starting at start_filter.
        float accum = d_delay * d_flt_rate;
        int   accum_int = static_cast<int>(accum);
        float accum_frac = accum - accum_int;
        int end_filter = static_cast<int>
          (boost::math::iround(fmodf(d_last_filter + d_delay * d_dec_rate + accum_int, \
                                     static_cast<float>(d_int_rate))));

        d_est_phase_change = d_last_filter - (end_filter + accum_frac);
      }

      pfb_arb_resampler_fff::~pfb_arb_resampler_fff()
      {
        for(unsigned int i = 0; i < d_int_rate; i++) {
          delete d_filters[i];
          delete d_diff_filters[i];
        }
      }

      void
      pfb_arb_resampler_fff::create_taps(const std::vector<float> &newtaps,
                                         std::vector< std::vector<float> > &ourtaps,
                                         std::vector<fir_filter_fff*> &ourfilter)
      {
        unsigned int ntaps = newtaps.size();
        d_taps_per_filter = (unsigned int)ceil((double)ntaps/(double)d_int_rate);

        // Create d_numchan vectors to store each channel's taps
        ourtaps.resize(d_int_rate);

        // Make a vector of the taps plus fill it out with 0's to fill
        // each polyphase filter with exactly d_taps_per_filter
        std::vector<float> tmp_taps;
        tmp_taps = newtaps;
        while((float)(tmp_taps.size()) < d_int_rate*d_taps_per_filter) {
          tmp_taps.push_back(0.0);
        }

        for(unsigned int i = 0; i < d_int_rate; i++) {
          // Each channel uses all d_taps_per_filter with 0's if not enough taps to fill out
          ourtaps[i] = std::vector<float>(d_taps_per_filter, 0);
          for(unsigned int j = 0; j < d_taps_per_filter; j++) {
            ourtaps[i][j] = tmp_taps[i + j*d_int_rate];
          }

          // Build a filter for each channel and add it's taps to it
          ourfilter[i]->set_taps(ourtaps[i]);
        }
      }

      void
      pfb_arb_resampler_fff::create_diff_taps(const std::vector<float> &newtaps,
                                              std::vector<float> &difftaps)
      {
        // Calculate the differential taps using a derivative filter
        std::vector<float> diff_filter(2);
        diff_filter[0] = -1;
        diff_filter[1] = 1;

        for(unsigned int i = 0; i < newtaps.size()-1; i++) {
          float tap = 0;
          for(unsigned int j = 0; j < diff_filter.size(); j++) {
            tap += diff_filter[j]*newtaps[i+j];
          }
          difftaps.push_back(tap);
        }
        difftaps.push_back(0);
      }

      void
      pfb_arb_resampler_fff::set_taps(const std::vector<float> &taps)
      {
        std::vector<float> dtaps;
        create_diff_taps(taps, dtaps);
        create_taps(taps, d_taps, d_filters);
        create_taps(dtaps, d_dtaps, d_diff_filters);
      }

      std::vector<std::vector<float> >
      pfb_arb_resampler_fff::taps() const
      {
        return d_taps;
      }

      void
      pfb_arb_resampler_fff::print_taps()
      {
        unsigned int i, j;
        for(i = 0; i < d_int_rate; i++) {
          printf("filter[%d]: [", i);
          for(j = 0; j < d_taps_per_filter; j++) {
            printf(" %.4e", d_taps[i][j]);
          }
          printf("]\n");
        }
      }

      void
      pfb_arb_resampler_fff::set_rate(float rate)
      {
        d_dec_rate = (unsigned int)floor(d_int_rate/rate);
        d_flt_rate = (d_int_rate/rate) - d_dec_rate;
      }

      void
      pfb_arb_resampler_fff::set_phase(float ph)
      {
        if((ph < 0) || (ph >= 2.0*M_PI)) {
          throw std::runtime_error("pfb_arb_resampler_fff: set_phase value out of bounds [0, 2pi).\n");
        }

        float ph_diff = 2.0*M_PI / (float)d_filters.size();
        d_last_filter = static_cast<int>(ph / ph_diff);
      }

      float
      pfb_arb_resampler_fff::phase() const
      {
        float ph_diff = 2.0*M_PI / static_cast<float>(d_filters.size());
        return d_last_filter * ph_diff;
      }

      unsigned int
      pfb_arb_resampler_fff::taps_per_filter() const
      {
        return d_taps_per_filter;
      }

      float
      pfb_arb_resampler_fff::phase_offset(float freq, float fs)
      {
        float adj = (2.0*M_PI)*(freq/fs)/static_cast<float>(d_int_rate);
        return -adj * d_est_phase_change;
      }

      int
      pfb_arb_resampler_fff::filter(float *output, float *input,
                                    int n_to_read, int &n_read)
      {
        int i_out = 0, i_in = 0;
        unsigned int j = d_last_filter;;
        float o0, o1;

        while(i_in < n_to_read) {
          // start j by wrapping around mod the number of channels
          while(j < d_int_rate) {
            // Take the current filter and derivative filter output
            o0 = d_filters[j]->filter(&input[i_in]);
            o1 = d_diff_filters[j]->filter(&input[i_in]);

            output[i_out] = o0 + o1*d_acc;     // linearly interpolate between samples
            i_out++;

            // Adjust accumulator and index into filterbank
            d_acc += d_flt_rate;
            j += d_dec_rate + (int)floor(d_acc);
            d_acc = fmodf(d_acc, 1.0);
          }
          i_in += (int)(j / d_int_rate);
          j = j % d_int_rate;
        }
        d_last_filter = j; // save last filter state for re-entry

        n_read = i_in;   // return how much we've actually read
        return i_out;    // return how much we've produced
      }

    } /* namespace kernel */
  } /* namespace filter */
} /* namespace gr */


#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include "Components.h"
#include <vector>
#include "Object.h"
#include <fstream>
#include <string>
#include "Transform.hpp"

#include <GL/glu.h>
#include <GLFW/glfw3.h>
#include <armadillo>

//Vectors that will be used as auxiliaries to send them to the figures.
vector <Vertex> verts;
vector<Edge> edgs;
vector<Face>fcs;
vector<Object> models;

//function prototypes
void readFile(string* nombre, string* archivo, string enter);
int paintModels2();
int inicializacion(GLFWwindow* window);
vector <Vertex> bezier(double x);
int main(int argc, char** argv)
{
	//Variables that will be used as auxiliaries to temporarily save the data and then send them to the figures
	string nombre;
	string archivo;
	readFile(&nombre, &archivo, "BMX.obj");
	Object model = Object(nombre, archivo, verts, edgs, fcs);
	models.push_back(model);
	nombre = "";archivo = "";verts.clear();edgs.clear();fcs.clear();
	paintModels2();

	return 0;
}//main (END).

/*The following function reads the data of the obj file receiving as parameters nombre and archivo these are by reference and
  an enter string that is the name of the file to read this goes by value.*/
void readFile(string* nombre, string* archivo, string enter) {
	char cadena[500];
	float a, b, c;
	int d, e, f, aux = 0;
	string dcadena, ecadena, fcadena;
	int indiVer = 1;
	ifstream fe(enter);
	fe >> cadena;
	//Assign the file name to the archivo variable
	*archivo = cadena;
	fe >> cadena;
	fe >> cadena;
	fe >> cadena;
	//Assign the name to the nombre variable
	*nombre = cadena;
	//While not end of file
	while (!fe.eof()) {
		fe >> cadena;
		//Verify that only vertices are read
		if (cadena[1] != 'n' && fe.eof() == 0 && cadena[0] == 'v') {
			fe >> cadena;
			a = atof(cadena);
			fe >> cadena;
			b = atof(cadena);
			fe >> cadena;
			c = atof(cadena);
			//A temporary vertex is created and added to the array of vertices.
			Vertex v(a, b, c, indiVer);
			verts.push_back(v);
			indiVer++;
		}
		//Verify that only the faces are read.
		else if (cadena[1] != 'n' && fe.eof() == 0 && cadena[0] == 'f') {
			vector<Vertex> vertic;
			vector<Edge> edgaux;
			fe >> cadena;
			d = atof(cadena);
			vertic.push_back(verts[d - 1]);
			fe >> cadena;
			e = atof(cadena);
			vertic.push_back(verts[e - 1]);
			fe >> cadena;
			f = atof(cadena);
			vertic.push_back(verts[f - 1]);
			//The three edges are deducted and added to the edge vector.
			Edge j(verts[d - 1], verts[e - 1]);
			Edge k(verts[e - 1], verts[f - 1]);
			Edge l(verts[f - 1], verts[d - 1]);
			edgs.push_back(j);
			edgs.push_back(k);
			edgs.push_back(l);
			//Those same edges are sent to the edgaux temporary vector that will be sent to the face that will be formed by a vector of vertices and one of edges.
			edgaux.push_back(j);
			edgaux.push_back(k);
			edgaux.push_back(l);
			Face fac(vertic, edgaux);
			fcs.push_back(fac);
		}
		else
		{
			fe >> cadena;
			fe >> cadena;
			fe >> cadena;
		}
	}
	fe.close();
}

int paintModels2() {
	arma::frowvec eye = { 0.0, 0.0, 10.0 };
	arma::frowvec camera = { 0.0, 0.0, 0.0 };

	if (!glfwInit())
	{
		fprintf(stderr, "Fallo al inicializar GLFW\n");
		getchar();
		return -1;
	}
	GLFWwindow* window;
	window = glfwCreateWindow(1024, 768, "Motocicleta Freestyle", NULL, NULL);
	if (window == NULL) {
		fprintf(stderr, "Fallo al abrir la ventana de GLFW.\n");
		getchar();
		glfwTerminate();
		return -1;
	}
	glfwMakeContextCurrent(window);
	glfwSetInputMode(window, GLFW_STICKY_KEYS,GL_TRUE);
	glEnable(GL_DEPTH_TEST);
	glDepthFunc(GL_LESS);

	glClearColor(0.0f, 0.0f, 0.0f, 0.0f);

	//  Proyecciones
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	int width, height;
	glfwGetFramebufferSize(window, &width, &height);
	float ar = width / height;

	//  Proyecci�n en paralelo
	glViewport(0, 0, width, height);
	glOrtho(-ar, ar, -1.0, 1.0, -20.0, 20.0);
	//  Proyecci�n en perspectiva
	//    glFrustum(-ar, ar, -ar, ar, 2.0, 4.0);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	Transform Tr = Transform();
	double t_angle = 0.0f;
	int cont = 0;
	int bcont = 0;
	double dt = 13.0;
	std::vector< Vertex > p_vertices;
	for (int f = 0; f < models[0].getSizevectorface();f++) {
		//Cycle to pass each vertex
		for (int v = 0;v < 3;v++) {
			p_vertices.push_back(models[0].getFace(f).getVert(v));
		}
	}

	vector <Vertex> vet = bezier(1.0);
	do {
		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
		glMatrixMode(GL_MODELVIEW);
		glLoadIdentity();
		/*gluLookAt(eye[0], eye[1], eye[2],
			camera[0], camera[1], camera[2],
			0.0, 1.0, 0.0);*/

	    // Dibujar la moto
		arma::dmat transd;
		if (dt > 1) {
			transd = Tr.S(0.05, 0.05, 0.05) * Tr.R(0.0, 0.5, 0.0, 180.0) * Tr.T(dt, 0.0, 0.0);
			dt -= 0.3;
		}
		else{
			if (bcont < 20)
			{
				transd = Tr.S(0.05, 0.05, 0.05) * Tr.R(0.0, 0.5, 0.0, 180.0) * Tr.T(vet[bcont].getX(), vet[bcont].getY(), vet[bcont].getZ());
				dt = vet[bcont].getX();
				bcont++;
			}
			else {

				transd = Tr.S(0.05, 0.05, 0.05) * Tr.R(0.0, 0.5, 0.0, 180.0) * Tr.T(dt, 0.0, 0.0);
				dt -= 0.3;
			}
		}

		std::vector< Vertex > moto_vertices;
		for (unsigned int i = 0; i < p_vertices.size(); i++) {
			arma::dcolvec v = p_vertices[i].h();
			arma::dcolvec vp = transd * v;
			Vertex rv = Vertex();
			rv.set_value(arma::trans(vp));
			moto_vertices.push_back(rv);
		}
		glColor3f(1.0, 1.0, 0.0);
		glBegin(GL_TRIANGLES);
		for (unsigned int i = 0; i < moto_vertices.size(); i++) {
			glVertex3d(moto_vertices[i].getX(), moto_vertices[i].getY(), moto_vertices[i].getZ());
		}
		glEnd();

		glfwSwapBuffers(window);
		glfwPollEvents();

	} while (glfwGetKey(window, GLFW_KEY_ESCAPE) != GLFW_PRESS &&
		glfwWindowShouldClose(window) == 0);

	glfwTerminate();
	return 0;
}

vector <Vertex> bezier(double x) {

	vector<Vertex> vertic;
	arma::dmat MB = { {-1.0, 3.0, -3.0, 1.0},
				  {3.0, -6.0, 3.0, 0.0},
				  {-3.0, 3.0, 0.0, 0.0},
				  {1.0, 0.0, 0.0, 0.0} };
	//MH.print();

	//cout << endl;

	arma::dmat GH = { {x, 0.0, 0.0},
					  {-1.0, 5.0, 0.0},
					  {-3.0,4.0,0.0},
					  {-5.0, 0.0, 0.0} };
	//GH.print();

	double dt = 0.05;
	arma::dmat Qt;
	for (double t = 0.0; t <= 1.0 + dt; t += dt) {
		arma::drowvec T = { powf(t, 3.0), powf(t,2.0), t, 1.0 };
		Qt = T * MB * GH;
		Qt.print();
		Vertex vert(Qt[0], Qt[1], Qt[2], 1.0);
		vertic.push_back(vert);
	}
	return vertic;
}


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/integral_constant.hpp>
#include <boost/hana/map.hpp>
#include <boost/hana/pair.hpp>

#include <memory>
namespace hana = boost::hana;

int main() {
    auto map = hana::make_map(
        hana::make_pair(hana::int_c<0>, 123),
        hana::make_pair(hana::int_c<1>, 456)
    );

    std::unique_ptr<decltype(map)> p1{}, p2{};
    using std::swap;
    swap(p1, p2);
}


#include "dynet/nodes.h"
#include "dynet/dynet.h"
#include "dynet/training.h"
#include "dynet/timing.h"
#include "dynet/dict.h"
#include "dynet/expr.h"

#include <iostream>
#include <fstream>
#include <sstream>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/serialization/access.hpp>

using namespace std;
using namespace dynet;

unsigned REP_DIM = 128;
unsigned INPUT_VOCAB_SIZE = 0;
unsigned OUTPUT_VOCAB_SIZE = 0;

dynet::Dict sd;
dynet::Dict td;
int kSRC_SOS;
int kSRC_EOS;
int kTRG_SOS;
int kTRG_EOS;

struct Encoder {
  LookupParameter p_s;
  LookupParameter p_t;
  vector<Expression> m;

  Encoder() {}

  explicit Encoder(Model& model) {
    p_s = model.add_lookup_parameters(INPUT_VOCAB_SIZE, {REP_DIM});
    p_t = model.add_lookup_parameters(OUTPUT_VOCAB_SIZE, {REP_DIM});
  }

  Expression EmbedSource(const vector<int>& sent, ComputationGraph& cg) {
    m.resize(sent.size() + 2);
    m[0] = lookup(cg, p_s, kSRC_SOS);
    int i = 1;
    for (auto& w : sent)
      m[i++] = lookup(cg, p_s, w);
    m[i] = lookup(cg, p_s, kSRC_EOS);
#define DUMB_ADDITIVE
#ifdef DUMB_ADDITIVE
    return sum(m);
#else
    return sum_cols(tanh(kmh_ngram(concatenate_cols(m), 2)));
#endif
  }

  Expression EmbedTarget(const vector<int>& sent, ComputationGraph& cg) {
    m.resize(sent.size() + 2);
    m[0] = lookup(cg, p_s, kTRG_SOS);
    int i = 1;
    for (auto& w : sent)
      m[i++] = lookup(cg, p_t, w);
    m[i] = lookup(cg, p_s, kTRG_EOS);
#ifdef DUMB_ADDITIVE
    return sum(m);
#else
    return sum_cols(tanh(kmh_ngram(concatenate_cols(m), 2)));
#endif
  }

  friend class boost::serialization::access;
  template<class Archive>
  void serialize(Archive& ar, const unsigned int) {
    ar & p_s;
    ar & p_t;
  }
};

int main(int argc, char** argv) {
  dynet::initialize(argc, argv);
  if (argc != 3 && argc != 4) {
    cerr << "Usage: " << argv[0] << " corpus.txt dev.txt [model.params]\n";
    return 1;
  }
  vector<pair<vector<int>, vector<int>>> training, dev;
  string line;
  kSRC_SOS = sd.convert("<s>");
  kSRC_EOS = sd.convert("</s>");
  kTRG_SOS = td.convert("<s>");
  kTRG_EOS = td.convert("</s>");
  int tlc = 0;
  cerr << "Reading training data from " << argv[1] << "...\n";
  {
    ifstream in(argv[1]);
    assert(in);
    while(getline(in, line)) {
      ++tlc;
      vector<int> src, trg;
      read_sentence_pair(line, src, sd, trg, td);
      training.push_back(make_pair(src, trg));
    }
    cerr << tlc << " lines, " << sd.size() << " source types, " << td.size() << " target types\n";
  }
  sd.freeze(); // no new word types allowed
  td.freeze(); // no new word types allowed
  INPUT_VOCAB_SIZE = sd.size();
  OUTPUT_VOCAB_SIZE = td.size();
#if 0

  int dlc = 0;
  int dtoks = 0;
  cerr << "Reading dev data from " << argv[2] << "...\n";
  {
    ifstream in(argv[2]);
    assert(in);
    while(getline(in, line)) {
      ++dlc;
      dev.push_back(read_sentence(line, &d));
      dtoks += dev.back().size();
      if (dev.back().front() != kSOS && dev.back().back() != kEOS) {
        cerr << "Dev sentence in " << argv[2] << ":" << tlc << " didn't start or end with <s>, </s>\n";
        abort();
      }
    }
    cerr << dlc << " lines, " << dtoks << " tokens\n";
  }
  ostringstream os;
  os << "bilm"
     << '_' << LAYERS
     << '_' << INPUT_DIM
     << '_' << HIDDEN_DIM
     << "-pid" << getpid() << ".params";
  const string fname = os.str();
  cerr << "Parameters will be written to: " << fname << endl;
  double best = 9e+99;
#endif
  Model model;
  Encoder emb;
  if (argc == 4) {
    string fname = argv[3];
    ifstream in(fname);
    boost::archive::text_iarchive ia(in);
    ia >> model >> emb;
  }
  else {
    emb = Encoder(model);
  }

  bool use_momentum = false;
  Trainer* sgd = nullptr;
  if (use_momentum)
    sgd = new MomentumSGDTrainer(model);
  else
    sgd = new SimpleSGDTrainer(model);

  unsigned report_every_i = 100;
  unsigned si = training.size();
  vector<unsigned> order(training.size());
  for (unsigned i = 0; i < order.size(); ++i) order[i] = i;
  bool first = true;
  // int report = 0;
  // unsigned dev_every_i_reports = 10;
  unsigned lines = 0;
  while(1) {
    Timer iteration("completed in");
    double loss = 0;
    unsigned chars = 0;
    for (unsigned i = 0; i < report_every_i; ++i) {
      if (si == training.size()) {
        si = 0;
        if (first) { first = false; } else { sgd->update_epoch(); }
        cerr << "**SHUFFLE\n";
        random_shuffle(order.begin(), order.end());
      }

      // build graph for this instance
      ComputationGraph cg;
      auto& sent_pair = training[order[si]];
      ++si;
      Expression s = emb.EmbedSource(sent_pair.first, cg);
      Expression sim = squared_distance(s, emb.EmbedTarget(sent_pair.second, cg));
      float margin = 2;
      const unsigned K = 20;
      vector<Expression> noise(K);
      for (unsigned j = 0; j < K; ++j) {
        unsigned sample = rand01() * training.size();
        while (sample == order[si] || sample == training.size()) { sample = rand01() * training.size(); }
        Expression sim_n = squared_distance(s, emb.EmbedTarget(training[sample].second, cg));
        noise[j] = pairwise_rank_loss(sim, sim_n, margin);
      }
      Expression l = sum(noise);
      auto iloss = as_scalar(cg.forward(l));
      assert(iloss >= 0);
      if (iloss > 0) {
        loss += iloss;
        cg.backward(l);
        sgd->update();
      }
      ++lines;
    }
    sgd->status();
    cerr << " E = " << (loss) << " ppl=" << exp(loss / chars) << ' ';

#if 0
    lm.RandomSample();
#endif
#if 0
    // show score on dev data?
    report++;
    if (report % dev_every_i_reports == 0) {
      double dloss = 0;
      int dchars = 0;
      for (auto& sent : dev) {
        ComputationGraph cg;
        lm.BuildGraph(sent, sent, cg);
        dloss += as_scalar(cg.forward());
        dchars += sent.size() - 1;
      }
      if (dloss < best) {
        best = dloss;
        ofstream out(fname);
        boost::archive::text_oarchive oa(out);
        oa << model << emb;
      }
      cerr << "\n***DEV [epoch=" << (lines / (double)training.size()) << "] E = " << (dloss / dchars) << " ppl=" << exp(dloss / dchars) << ' ';
    }
#endif
  }
  delete sgd;
}


// Copyright (c) 2019 yshurik
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <boost/assign/list_of.hpp>

#include "base58.h"
#include "rpcserver.h"
#include "txdb.h"
#include "init.h"
#include "main.h"
#include "net.h"
#include "keystore.h"
#include "wallet.h"

#include "pegops.h"
#include "pegopsp.h"
#include "pegdata.h"

#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/split.hpp>

using namespace std;
using namespace boost;
using namespace boost::assign;
using namespace json_spirit;

void printpegshift(const CFractions & frPegShift,
                   const CPegLevel & peglevel,
                   Object & result);

void printpeglevel(const CPegLevel & peglevel,
                   Object & result);

void printpegbalance(const CPegData & pegdata,
                     Object & result,
                     string prefix);

// API calls

Value getpeglevel(const Array& params, bool fHelp)
{
    if (fHelp || params.size() != 3)
        throw runtime_error(
            "getpeglevel "
                "<exchange_pegdata_base64> "
                "<pegshift_pegdata_base64> "
                "<previous_cycle>\n"
            );

    string exchange_pegdata64 = params[0].get_str();
    string pegshift_pegdata64 = params[1].get_str();
    int nCyclePrev = params[2].get_int();

    CPegData pdExchange(exchange_pegdata64);
    if (!pdExchange.IsValid()) {
        string err = "Can not unpack 'exchange' pegdata";
        throw JSONRPCError(RPC_DESERIALIZATION_ERROR, err);
    }

    CPegData pdPegShift(pegshift_pegdata64);
    if (!pdPegShift.IsValid()) {
        string err = "Can not unpack 'pegshift' pegdata";
        throw JSONRPCError(RPC_DESERIALIZATION_ERROR, err);
    }

    int nSupplyNow = pindexBest ? pindexBest->nPegSupplyIndex : 0;
    int nSupplyNext = pindexBest ? pindexBest->GetNextIntervalPegSupplyIndex() : 0;
    int nSupplyNextNext = pindexBest ? pindexBest->GetNextNextIntervalPegSupplyIndex() : 0;

    int nPegInterval = Params().PegInterval(nBestHeight);
    int nCycleNow = nBestHeight / nPegInterval;
    int nBuffer = 3;

    if (nCycleNow <= nCyclePrev) {
        std::stringstream ss;
        ss << "Current cycle "
           << nCycleNow
           << " should be greater than previous "
           << nCyclePrev;
        string err = ss.str();
        throw JSONRPCError(RPC_MISC_ERROR, err);
    }

    if (nBestHeight < nPegStartHeight) { // run on zero levels before the peg
        nBuffer = 0;
        nSupplyNow = 0;
        nSupplyNext = 0;
        nSupplyNextNext = 0;
    }

    string err;
    CPegLevel peglevel("");
    CPegData pdPegPool;

    bool ok = pegops::getpeglevel(
                nCycleNow,
                nCyclePrev,
                nBuffer,
                nSupplyNow,
                nSupplyNext,
                nSupplyNextNext,
                pdExchange,
                pdPegShift,

                peglevel,
                pdPegPool,
                err
    );
    if (!ok) {
        throw JSONRPCError(RPC_MISC_ERROR, err);
    }

    // update exchange datas
    pdExchange.peglevel = peglevel;
    pdExchange.nLiquid = pdExchange.fractions.High(peglevel);
    pdExchange.nReserve = pdExchange.fractions.Low(peglevel);

    Object result;
    result.push_back(Pair("cycle", peglevel.nCycle));

    printpeglevel(peglevel, result);
    printpegbalance(pdPegPool, result, "pegpool_");
    printpegbalance(pdExchange, result, "exchange_");
    printpegshift(pdPegShift.fractions, peglevel, result);

    return result;
}

Value makepeglevel(const Array& params, bool fHelp)
{
    if (fHelp || params.size() != 8)
        throw runtime_error(
            "makepeglevel "
                "<current_cycle> "
                "<previous_cycle> "
                "<pegbuffer> "
                "<pegsupplyindex> "
                "<pegsupplyindex_next> "
                "<pegsupplyindex_nextnext> "
                "<exchange_pegdata_base64> "
                "<pegshift_pegdata_base64>\n"
            );

    int cycle_now = params[0].get_int();
    int cycle_prev = params[1].get_int();
    int buffer = params[2].get_int();
    int supply_now = params[3].get_int();
    int supply_next = params[4].get_int();
    int supply_next_next = params[5].get_int();
    string exchange_pegdata64 = params[6].get_str();
    string pegshift_pegdata64 = params[7].get_str();

    CPegData pdExchange(exchange_pegdata64);
    if (!pdExchange.IsValid()) {
        string err = "Can not unpack 'exchange' pegdata";
        throw JSONRPCError(RPC_DESERIALIZATION_ERROR, err);
    }

    CPegData pdPegShift(pegshift_pegdata64);
    if (!pdPegShift.IsValid()) {
        string err = "Can not unpack 'pegshift' pegdata";
        throw JSONRPCError(RPC_DESERIALIZATION_ERROR, err);
    }

    string err;
    CPegLevel peglevel("");
    CPegData pdPegPool;

    bool ok = pegops::getpeglevel(
                cycle_now,
                cycle_prev,
                buffer,
                supply_now,
                supply_next,
                supply_next_next,
                pdExchange,
                pdPegShift,

                peglevel,
                pdPegPool,
                err
    );
    if (!ok) {
        throw JSONRPCError(RPC_MISC_ERROR, err);
    }

    // update exchange datas
    pdExchange.peglevel = peglevel;
    pdExchange.nLiquid = pdExchange.fractions.High(peglevel);
    pdExchange.nReserve = pdExchange.fractions.Low(peglevel);

    Object result;
    result.push_back(Pair("cycle", peglevel.nCycle));

    printpeglevel(peglevel, result);
    printpegbalance(pdPegPool, result, "pegpool_");
    printpegbalance(pdExchange, result, "exchange_");
    printpegshift(pdPegShift.fractions, peglevel, result);

    return result;
}

Value updatepegbalances(const Array& params, bool fHelp)
{
    if (fHelp || (params.size() != 3))
        throw runtime_error(
            "updatepegbalances "
                "<balance_pegdata_base64> "
                "<pegpool_pegdata_base64> "
                "<peglevel_hex>\n"
            );

    string inp_balance_pegdata64 = params[0].get_str();
    string inp_pegpool_pegdata64 = params[1].get_str();
    string inp_peglevel_hex = params[2].get_str();

    CPegData pdBalance(inp_balance_pegdata64);
    if (!pdBalance.IsValid()) {
        string err = "Can not unpack 'balance' pegdata";
        throw JSONRPCError(RPC_DESERIALIZATION_ERROR, err);
    }

    CPegData pdPegPool(inp_pegpool_pegdata64);
    if (!pdPegPool.IsValid()) {
        string err = "Can not unpack 'pegpool' pegdata";
        throw JSONRPCError(RPC_DESERIALIZATION_ERROR, err);
    }

    CPegLevel peglevelNew(inp_peglevel_hex);
    if (!peglevelNew.IsValid()) {
        string err = "Can not unpack peglevel";
        throw JSONRPCError(RPC_DESERIALIZATION_ERROR, err);
    }

    string err;

    bool ok = pegops::updatepegbalances(
            pdBalance,
            pdPegPool,
            peglevelNew,

            err);

    if (!ok) {
        throw JSONRPCError(RPC_MISC_ERROR, err);
    }

    Object result;

    result.push_back(Pair("completed", true));
    result.push_back(Pair("cycle", peglevelNew.nCycle));

    printpeglevel(peglevelNew, result);
    printpegbalance(pdBalance, result, "balance_");
    printpegbalance(pdPegPool, result, "pegpool_");

    return result;
}

Value movecoins(const Array& params, bool fHelp)
{
    if (fHelp || params.size() != 4)
        throw runtime_error(
            "movecoins "
                "<amount> "
                "<src_pegdata_base64> "
                "<dst_pegdata_base64> "
                "<peglevel_hex>\n"
            );

    int64_t inp_move_amount = params[0].get_int64();
    string inp_src_pegdata64 = params[1].get_str();
    string inp_dst_pegdata64 = params[2].get_str();
    string inp_peglevel_hex = params[3].get_str();

    CPegData pdSrc(inp_src_pegdata64);
    if (!pdSrc.IsValid()) {
        string err = "Can not unpack 'src' pegdata";
        throw JSONRPCError(RPC_DESERIALIZATION_ERROR, err);
    }

    CPegData pdDst(inp_dst_pegdata64);
    if (!pdDst.IsValid()) {
        string err = "Can not unpack 'dst' pegdata";
        throw JSONRPCError(RPC_DESERIALIZATION_ERROR, err);
    }

    CPegLevel peglevel(inp_peglevel_hex);
    if (!peglevel.IsValid()) {
        string err = "Can not unpack peglevel";
        throw JSONRPCError(RPC_DESERIALIZATION_ERROR, err);
    }

    string err;

    bool ok = pegops::movecoins(
                inp_move_amount,
                pdSrc,
                pdDst,
                peglevel,
                true,

                err);

    if (!ok) {
        throw JSONRPCError(RPC_MISC_ERROR, err);
    }

    Object result;

    result.push_back(Pair("cycle", peglevel.nCycle));

    printpeglevel(peglevel, result);
    printpegbalance(pdSrc, result, "src_");
    printpegbalance(pdDst, result, "dst_");

    return result;
}

Value moveliquid(const Array& params, bool fHelp)
{
    if (fHelp || params.size() != 4)
        throw runtime_error(
            "moveliquid "
                "<liquid> "
                "<src_pegdata_base64> "
                "<dst_pegdata_base64> "
                "<peglevel_hex>\n"
            );

    int64_t inp_move_liquid = params[0].get_int64();
    string inp_src_pegdata64 = params[1].get_str();
    string inp_dst_pegdata64 = params[2].get_str();
    string inp_peglevel_hex = params[3].get_str();

    CPegData pdSrc(inp_src_pegdata64);
    if (!pdSrc.IsValid()) {
        string err = "Can not unpack 'src' pegdata";
        throw JSONRPCError(RPC_DESERIALIZATION_ERROR, err);
    }

    CPegData pdDst(inp_dst_pegdata64);
    if (!pdDst.IsValid()) {
        string err = "Can not unpack 'dst' pegdata";
        throw JSONRPCError(RPC_DESERIALIZATION_ERROR, err);
    }

    CPegLevel peglevel(inp_peglevel_hex);
    if (!peglevel.IsValid()) {
        string err = "Can not unpack peglevel";
        throw JSONRPCError(RPC_DESERIALIZATION_ERROR, err);
    }

    string err;

    bool ok = pegops::moveliquid(
            inp_move_liquid,
            pdSrc,
            pdDst,
            peglevel,

            err);

    if (!ok) {
        throw JSONRPCError(RPC_MISC_ERROR, err);
    }

    Object result;

    result.push_back(Pair("cycle", peglevel.nCycle));

    printpeglevel(peglevel, result);
    printpegbalance(pdSrc, result, "src_");
    printpegbalance(pdDst, result, "dst_");

    return result;
}

Value movereserve(const Array& params, bool fHelp)
{
    if (fHelp || params.size() != 4)
        throw runtime_error(
            "movereserve "
                "<reserve> "
                "<src_pegdata_base64> "
                "<dst_pegdata_base64> "
                "<peglevel_hex>\n"
            );

    int64_t inp_move_reserve = params[0].get_int64();
    string inp_src_pegdata64 = params[1].get_str();
    string inp_dst_pegdata64 = params[2].get_str();
    string inp_peglevel_hex = params[3].get_str();

    CPegData pdSrc(inp_src_pegdata64);
    if (!pdSrc.IsValid()) {
        string err = "Can not unpack 'src' pegdata";
        throw JSONRPCError(RPC_DESERIALIZATION_ERROR, err);
    }

    CPegData pdDst(inp_dst_pegdata64);
    if (!pdDst.IsValid()) {
        string err = "Can not unpack 'dst' pegdata";
        throw JSONRPCError(RPC_DESERIALIZATION_ERROR, err);
    }

    CPegLevel peglevel(inp_peglevel_hex);
    if (!peglevel.IsValid()) {
        string err = "Can not unpack peglevel";
        throw JSONRPCError(RPC_DESERIALIZATION_ERROR, err);
    }

    string err;

    bool ok = pegops::movereserve(
            inp_move_reserve,
            pdSrc,
            pdDst,
            peglevel,

            err);

    if (!ok) {
        throw JSONRPCError(RPC_MISC_ERROR, err);
    }

    Object result;

    result.push_back(Pair("cycle", peglevel.nCycle));

    printpeglevel(peglevel, result);
    printpegbalance(pdSrc, result, "src_");
    printpegbalance(pdDst, result, "dst_");

    return result;
}

Value removecoins(const Array& params, bool fHelp)
{
    if (fHelp || params.size() != 2)
        throw runtime_error(
            "removecoins "
                "<from_pegdata_base64> "
                "<remove_pegdata_base64>\n"
            );

    string inp_from_pegdata64 = params[0].get_str();
    string inp_remove_pegdata64 = params[1].get_str();

    CPegData pdFrom(inp_from_pegdata64);
    if (!pdFrom.IsValid()) {
        string err = "Can not unpack 'from' pegdata";
        throw JSONRPCError(RPC_DESERIALIZATION_ERROR, err);
    }

    CPegData pdRemove(inp_remove_pegdata64);
    if (!pdRemove.IsValid()) {
        string err = "Can not unpack 'remove' pegdata";
        throw JSONRPCError(RPC_DESERIALIZATION_ERROR, err);
    }

    pdFrom.fractions    -= pdRemove.fractions;
    pdFrom.nLiquid      -= pdRemove.nLiquid;
    pdFrom.nReserve     -= pdRemove.nReserve;

    Object result;

    printpegbalance(pdFrom, result, "out_");

    return result;
}


// Copyright (c) 2015-2018 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#ifndef denovo_model_hpp
#define denovo_model_hpp

#include <cstddef>
#include <unordered_map>
#include <string>
#include <utility>

#include <boost/optional.hpp>
#include <boost/functional/hash.hpp>

#include "core/types/haplotype.hpp"
#include "../pairhmm/pair_hmm.hpp"
#include "indel_mutation_model.hpp"

namespace octopus {

class DeNovoModel
{
public:
    struct Parameters
    {
        double snv_mutation_rate, indel_mutation_rate;
    };

    enum class CachingStrategy { none, value, address };

    DeNovoModel() = delete;

    DeNovoModel(Parameters parameters,
                std::size_t num_haplotypes_hint = 1000,
                CachingStrategy caching = CachingStrategy::value);

    DeNovoModel(const DeNovoModel&)            = default;
    DeNovoModel& operator=(const DeNovoModel&) = default;
    DeNovoModel(DeNovoModel&&)                 = default;
    DeNovoModel& operator=(DeNovoModel&&)      = default;

    ~DeNovoModel() = default;

    void prime(std::vector<Haplotype> haplotypes);
    void unprime() noexcept;
    bool is_primed() const noexcept;

    // ln p(target | given)
    double evaluate(const Haplotype& target, const Haplotype& given) const;
    double evaluate(unsigned target, unsigned given) const;

private:
    struct AddressPairHash
    {
        std::size_t operator()(const std::pair<const Haplotype*, const Haplotype*>& p) const noexcept
        {
            auto result = boost::hash_value(p.first);
            boost::hash_combine(result, p.second);
            return result;
        }
    };

    using PenaltyVector   = hmm::VariableGapOpenMutationModel::PenaltyVector;
    using GapPenaltyModel = std::pair<PenaltyVector, PenaltyVector>;

    hmm::FlatGapMutationModel flat_mutation_model_;
    IndelMutationModel indel_model_;
    boost::optional<double> min_ln_probability_;
    std::size_t num_haplotypes_hint_;
    std::vector<Haplotype> haplotypes_;
    CachingStrategy caching_;

    mutable PenaltyVector gap_open_penalties_, gap_extend_penalties_;
    mutable std::vector<boost::optional<GapPenaltyModel>> gap_model_index_cache_;
    mutable std::unordered_map<Haplotype, std::unordered_map<Haplotype, double>> value_cache_;
    mutable std::unordered_map<std::pair<const Haplotype*, const Haplotype*>, double, AddressPairHash> address_cache_;
    mutable std::vector<std::vector<boost::optional<double>>> guarded_index_cache_;
    mutable std::vector<std::vector<double>> unguarded_index_cache_;
    mutable std::string padded_given_;
    mutable bool use_unguarded_;

    void set_gap_penalties(const Haplotype& given) const;
    void set_gap_penalties(unsigned given) const;
    hmm::VariableGapExtendMutationModel make_hmm_model_from_cache() const;
    double evaluate_uncached(const Haplotype& target, const Haplotype& given, bool gap_penalties_cached = false) const;
    double evaluate_uncached(unsigned target, unsigned given) const;
    double evaluate_basic_cache(const Haplotype& target, const Haplotype& given) const;
    double evaluate_address_cache(const Haplotype& target, const Haplotype& given) const;
};

} // namespace octopus

#endif


/******************************************************************************
 *
 * Copyright 2018 Xaptum, Inc.
 *
 *    Licensed under the Apache License, Version 2.0 (the "License");
 *    you may not use this file except in compliance with the License.
 *    You may obtain a copy of the License at
 *
 *        http://www.apache.org/licenses/LICENSE-2.0
 *
 *    Unless required by applicable law or agreed to in writing, software
 *    distributed under the License is distributed on an "AS IS" BASIS,
 *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *    See the License for the specific language governing permissions and
 *    limitations under the License
 *
 *****************************************************************************/

#include <xtt/asio.hpp>

#include <sodium.h>

#include <boost/asio.hpp>

#include <cstdlib>

#include <iostream>
#include <fstream>
#include <memory>

const char *daa_gpk_file = "daa_gpk.bin";
const char *basename_file = "basename.bin";
const char *server_certificate_file = "server_certificate.bin";
const char *server_privatekey_file = "server_privatekey.bin";

class xtt_server {
public:
    xtt_server(boost::asio::io_context& io_context,
               short port,
               const std::vector<unsigned char>& certificate,
               const std::vector<unsigned char>& private_key,
               xtt::server_cookie_context& cookie_ctx,
               std::unordered_map<xtt::group_identity, std::unique_ptr<xtt::group_public_key_context>>& gpk_map)
        : acceptor_(io_context, boost::asio::ip::tcp::endpoint(boost::asio::ip::tcp::v4(), port)),
          certificate_(certificate),
          private_key_(private_key),
          cookie_ctx_(cookie_ctx),
          gpk_map_(gpk_map),
          xtt_contexts_(),
          io_context_(io_context)
    {
        do_accept();
    }

private:
    void do_accept()
    {
        acceptor_.async_accept([this](boost::system::error_code ec, boost::asio::ip::tcp::socket socket)
                               {
                                   if (!ec) {
                                       run_handshake(std::move(socket));
                                   }

                                   do_accept();
                               });
    }

    void run_handshake(boost::asio::ip::tcp::socket socket)
    {
        xtt_contexts_.emplace_back(std::move(socket), cookie_ctx_);
        xtt::asio::server_context& xtt_context = xtt_contexts_.back();

        boost::system::error_code cert_ec;
        xtt_context.load_certificate(certificate_, private_key_, cert_ec);
        if (cert_ec) {
            std::cerr << "Error deserializing certificate\n";
            return;
        }

        xtt_context.async_handle_connect([this](xtt::group_identity claimed_gid,
                                                xtt::identity requested_client_id,
                                                auto&& continuation)
                                         {
                                            (void)requested_client_id;

                                             this->async_lookup_gpk(claimed_gid, continuation);
                                         },
                                         [this, &xtt_context](xtt::group_identity claimed_gid,
                                                              xtt::identity requested_client_id,
                                                              auto&& continuation)
                                         {
                                             this->async_assign_id(claimed_gid, requested_client_id, xtt_context, continuation);
                                         },
                                         [this, &xtt_context](const boost::system::error_code& ec)
                                         {
                                             this->handle_handshake(ec, xtt_context);
                                         });
    }

    template <typename AsyncContinuation>
    void
    async_lookup_gpk(const xtt::group_identity& claimed_gid, AsyncContinuation continuation)
    {
        auto gpk_it = gpk_map_.find(claimed_gid);
        if (gpk_map_.end() == gpk_it) {
            std::cerr << "Error: claimed group ID '" << claimed_gid << "' doesn't match any known\n";
            boost::asio::post(io_context_,
                              [continuation]()
                              {
                                  continuation(xtt::asio::get_unknown_gid_ec(),
                                               std::unique_ptr<xtt::group_public_key_context>());
                              });
            return;
        }

        boost::asio::post(io_context_,
                          [continuation, gpk_it]()
                          {
                              continuation(boost::system::error_code(), gpk_it->second->clone());
                          });
    }

    template <typename AsyncContinuation>
    void
    async_assign_id(const xtt::group_identity& claimed_gid,
                    const xtt::identity requested_client_id,
                    xtt::asio::server_context& xtt_context,
                    AsyncContinuation continuation)
    {
        auto clients_pseudonym = xtt_context.get_clients_pseudonym();
        if (!clients_pseudonym) {
            std::cerr << "Unable to get client's pseudonym, while assigning an ID\n";
            boost::asio::post(io_context_,
                              [continuation]()
                              {
                                  continuation(xtt::asio::get_bad_id_ec(),
                                               xtt::identity());
                              });
            return;
        }

        xtt::identity assigned_id;
        // If the client sent xtt_null_client_id assign them id = SHA-256(GID || pseudonym) (truncated to first 16bytes)
        // Otherwise, just echo back what they requested.
        if (requested_client_id.is_null()) {
            std::vector<unsigned char> new_id_serialized(crypto_hash_sha256_BYTES);
            crypto_hash_sha256_state h;
            crypto_hash_sha256_init(&h);
            std::vector<unsigned char> gid_serial = claimed_gid.serialize();
            crypto_hash_sha256_update(&h, gid_serial.data(), gid_serial.size());
            std::vector<unsigned char> pseudonym_serial = clients_pseudonym->serialize();
            crypto_hash_sha256_update(&h, pseudonym_serial.data(), pseudonym_serial.size());
            crypto_hash_sha256_final(&h, new_id_serialized.data());
            new_id_serialized.resize(sizeof(xtt_identity_type));
            auto new_id = xtt::identity::deserialize(new_id_serialized);
            if (!new_id) {
                std::cerr << "Error creating new identity\n";
                boost::asio::post(io_context_,
                                  [continuation]()
                                  {
                                      continuation(xtt::asio::get_bad_id_ec(),
                                                   xtt::identity());
                                  });
                return;
            }

            assigned_id = *new_id;
        } else {
            assigned_id = requested_client_id;
        }

        boost::asio::post(io_context_,
                          [continuation, assigned_id]()
                          {
                              continuation(boost::system::error_code(), assigned_id);
                          });
    }

    void handle_handshake(const boost::system::error_code& ec,
                          xtt::asio::server_context& xtt_context)
    {
        if (!ec) {
            std::cout << "Successfully finished handshake:\n";

            auto clients_pseudonym = xtt_context.get_clients_pseudonym();
            if (!clients_pseudonym) {
                std::cerr << "Error retrieving client's pseudonym!";
                return;
            }
            std::cout << "\tClient's pseudonym:       " << *clients_pseudonym << "\n";

            auto clients_id = xtt_context.get_clients_identity();
            if (!clients_id) {
                std::cerr << "Error retrieving client's assigned id!";
                return;
            }
            std::cout << "\tWe assigned the identity: " << *clients_id << "\n";

            auto clients_longterm_key = xtt_context.get_clients_longterm_key();
            if (!clients_longterm_key) {
                std::cerr << "Error retrieving client's longterm public key!";
                return;
            }
            std::cout << "\tClient has longterm key:  " << *clients_longterm_key << "\n";
        } else {
            std::cout << "Error during handshake: " << ec << std::endl;
        }

        xtt_context.lowest_layer().close(); // we should also remove xtt_context from our vector
    }

private:
    boost::asio::ip::tcp::acceptor acceptor_;

    std::vector<unsigned char> certificate_;
    std::vector<unsigned char> private_key_;

    xtt::server_cookie_context& cookie_ctx_;
    std::unordered_map<xtt::group_identity, std::unique_ptr<xtt::group_public_key_context>>& gpk_map_;

    std::vector<xtt::asio::server_context> xtt_contexts_;

    boost::asio::io_context& io_context_;
};

void parse_cmd_args(int argc, char *argv[], short *port);

int initialize(std::vector<unsigned char>& certificate,
               std::vector<unsigned char>& private_key,
               std::unordered_map<xtt::group_identity, std::unique_ptr<xtt::group_public_key_context>>& gpk_map);

int main(int argc, char *argv[])
{
    // 1) Parse args
    short server_port;
    parse_cmd_args(argc, argv, &server_port);

    // 2) Setup necessary XTT information (used by all handshakes)
    std::vector<unsigned char> certificate;
    std::vector<unsigned char> private_key;
    xtt::server_cookie_context cookie_ctx;
    std::unordered_map<xtt::group_identity, std::unique_ptr<xtt::group_public_key_context>> gpk_map;
    int ret;
    ret = initialize(certificate, private_key, gpk_map);
    if (0 != ret) {
        std::cerr << "Error initializing persistent XTT contexts\n";
        return 1;
    }

    // 3) Start server
    boost::asio::io_context io_context;
    xtt_server serv{io_context, server_port, certificate, private_key, cookie_ctx, gpk_map};

    // 4) Run event loop
    io_context.run();
}

void parse_cmd_args(int argc, char *argv[], short *port)
{
    if (2 != argc) {
        std::cerr<< "usage: " << argv[0] << " <server port>\n";
        exit(1);
    }

    *port = std::atoi(argv[1]);
}

int initialize(std::vector<unsigned char>& certificate,
               std::vector<unsigned char>& private_key,
               std::unordered_map<xtt::group_identity, std::unique_ptr<xtt::group_public_key_context>>& gpk_map)
{
    // 1) Read DAA GPK from file.
    std::ifstream gpk_file(daa_gpk_file, std::ios::in | std::ios::binary);
    std::vector<unsigned char> serialized_gpk((std::istreambuf_iterator<char>(gpk_file)), std::istreambuf_iterator<char>());

    // 2) Read DAA basename from file
    std::ifstream bsn_file(basename_file, std::ios::in | std::ios::binary);
    std::vector<unsigned char> basename((std::istreambuf_iterator<char>(bsn_file)), std::istreambuf_iterator<char>());

    // 3) Initialize DAA context
    auto gpk = xtt::group_public_key_context_lrsw::from_gpk_and_basename(serialized_gpk, basename);
    if (!gpk) {
        std::cerr << "Error deserializing GPK and basename\n";
        return -1;
    }
    std::cout << "Using group public key context: " << *gpk << std::endl;

    // 4) Generate GID from GPK (GID = SHA-256(GPK))
    std::vector<unsigned char> raw_gid(crypto_hash_sha256_BYTES);
    crypto_hash_sha256_state h;
    crypto_hash_sha256_init(&h);
    std::vector<unsigned char> gpk_serial = gpk->get_gpk();
    crypto_hash_sha256_update(&h, gpk_serial.data(), gpk_serial.size());
    crypto_hash_sha256_final(&h, raw_gid.data());
    auto gid = xtt::group_identity::deserialize(raw_gid);
    if (!gid) {
        std::cerr << "Error computing GID from GPK\n";
        return -1;
    }
    std::cout << "\twith GID: " << *gid << std::endl;

    // 4ii) Insert gpk into map
    gpk_map[*gid] = std::move(gpk);

    // 5) Read in my certificate from file
    std::ifstream cert_file(server_certificate_file, std::ios::in | std::ios::binary);
    certificate.assign((std::istreambuf_iterator<char>(cert_file)), std::istreambuf_iterator<char>());

    // 6) Read in my private key from file
    std::ifstream privkey_file(server_privatekey_file, std::ios::in | std::ios::binary);
    private_key.assign((std::istreambuf_iterator<char>(privkey_file)), std::istreambuf_iterator<char>());

    return 0;
}