// ======================================================================
/*!
 * \file NFmiLatLonArea.cpp
 * \brief Implementation of class NFmiLatLonArea
 */
// ======================================================================
/*!
 * \class NFmiLatLonArea
 *
 * Undocumented
 *
 */
// ======================================================================

#include "NFmiLatLonArea.h"
#include "NFmiAreaFactory.h"
#include <boost/functional/hash.hpp>
#include <fmt/format.h>
#include <macgyver/Exception.h>

#include <iostream>

// ----------------------------------------------------------------------
/*!
 * Destructor
 */
// ----------------------------------------------------------------------

NFmiLatLonArea::~NFmiLatLonArea() = default;
// ----------------------------------------------------------------------
/*!
 * Void constructor
 */
// ----------------------------------------------------------------------

NFmiLatLonArea::NFmiLatLonArea()
    : NFmiArea(),
      itsBottomLeftLatLon(),
      itsTopRightLatLon(),
      itsXScaleFactor(),
      itsYScaleFactor(),
      itsWorldRect()
{
}

// ----------------------------------------------------------------------
/*!
 * Constructor
 *
 * \param theBottomLeftLatLon Undocumented
 * \param theTopRightLatLon Undocumented
 * \param theTopLeftXY Undocumented
 * \param theBottomRightXY Undocumented
 */
// ----------------------------------------------------------------------

NFmiLatLonArea::NFmiLatLonArea(const NFmiPoint &theBottomLeftLatLon,
                               const NFmiPoint &theTopRightLatLon,
                               const NFmiPoint &theTopLeftXY,
                               const NFmiPoint &theBottomRightXY,
                               bool usePacificView)
    : NFmiArea(theTopLeftXY, theBottomRightXY, usePacificView),
      itsBottomLeftLatLon(theBottomLeftLatLon),
      itsTopRightLatLon(theTopRightLatLon),
      itsXScaleFactor(),
      itsYScaleFactor(),
      itsWorldRect()
{
  try
  {
    Init();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Copy constructor
 *
 * \param theLatLonArea The other object being copied
 */
// ----------------------------------------------------------------------

NFmiLatLonArea::NFmiLatLonArea(const NFmiLatLonArea &theLatLonArea)

    = default;

// ----------------------------------------------------------------------
/*!
 * \return Undocumented
 */
// ----------------------------------------------------------------------

NFmiArea *NFmiLatLonArea::Clone() const
{
  try
  {
    return new NFmiLatLonArea(*this);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}
// ----------------------------------------------------------------------
/*!
 * \param fKeepWorldRect Undocumented
 */
// ----------------------------------------------------------------------

void NFmiLatLonArea::Init(bool fKeepWorldRect)
{
  try
  {
    if (!fKeepWorldRect)
      itsWorldRect =
          NFmiRect(LatLonToWorldXY(itsBottomLeftLatLon),
                   LatLonToWorldXY(itsTopRightLatLon));  // 28.8.2001/Marko&Esa Lisätty laskuihin.

    itsXScaleFactor = (Right() - Left()) / (itsTopRightLatLon.X() - itsBottomLeftLatLon.X());
    itsYScaleFactor = (Top() - Bottom()) / (itsTopRightLatLon.Y() - itsBottomLeftLatLon.Y());

    NFmiArea::Init(fKeepWorldRect);

    const char *fmt = "+proj=eqc +R={} +wktext +no_defs +type=crs";
    itsProjStr = fmt::format(fmt, kRearth);
    itsSpatialReference = std::make_shared<Fmi::SpatialReference>(itsProjStr);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theXYPoint Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiLatLonArea::ToLatLon(const NFmiPoint &theXYPoint) const
{
  try
  {
    double dlon = (theXYPoint.X() - Left()) / itsXScaleFactor;
    NFmiLongitude lon(itsBottomLeftLatLon.X() + dlon, PacificView());
    NFmiLatitude lat(itsBottomLeftLatLon.Y() + (theXYPoint.Y() - Bottom()) / itsYScaleFactor);
    return NFmiPoint(lon.Value(), lat.Value());
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theLatLonPoint Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiLatLonArea::ToXY(const NFmiPoint &theLatLonPoint) const
{
  try
  {
    double usedLongitude = FixLongitude(theLatLonPoint.X());
    double X = Left() + (usedLongitude - itsBottomLeftLatLon.X()) * itsXScaleFactor;
    double Y = Top() + (theLatLonPoint.Y() - itsTopRightLatLon.Y()) *
                           itsYScaleFactor;  // Tässä on edelleen virhe
    return NFmiPoint(X, Y);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theXYPoint Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiLatLonArea::XYToWorldXY(const NFmiPoint &theXYPoint) const
{
  try
  {
    return LatLonToWorldXY(ToLatLon(theXYPoint));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theXYPoint Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiLatLonArea::WorldXYToXY(const NFmiPoint &theWorldXYPoint) const
{
  try
  {
    return ToXY(WorldXYToLatLon(theWorldXYPoint));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \return Undocumented
 */
// ----------------------------------------------------------------------

double NFmiLatLonArea::XScale() const
{
  try
  {
    return 1. / itsXScaleFactor;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}
// ----------------------------------------------------------------------
/*!
 * \return Undocumented
 */
// ----------------------------------------------------------------------

double NFmiLatLonArea::YScale() const
{
  try
  {
    return 1. / itsYScaleFactor;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}
// ----------------------------------------------------------------------
/*!
 * \param theBottomLeftLatLon Undocumented
 * \param theTopRightLatLon Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

NFmiArea *NFmiLatLonArea::NewArea(const NFmiPoint &theBottomLeftLatLon,
                                  const NFmiPoint &theTopRightLatLon,
                                  bool allowPacificFix) const
{
  try
  {
    if (allowPacificFix)
    {
      PacificPointFixerData fixedPointData =
          NFmiArea::PacificPointFixer(theBottomLeftLatLon, theTopRightLatLon);
      return new NFmiLatLonArea(fixedPointData.itsBottomLeftLatlon,
                                fixedPointData.itsTopRightLatlon,
                                TopLeft(),
                                BottomRight(),
                                fixedPointData.fIsPacific);
    }
    else
      return new NFmiLatLonArea(
          theBottomLeftLatLon, theTopRightLatLon, TopLeft(), BottomRight(), PacificView());
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Write the object to the given output stream
 *
 * \param file The output stream to write to
 * \return The output stream written to
 */
// ----------------------------------------------------------------------

std::ostream &NFmiLatLonArea::Write(std::ostream &file) const
{
  try
  {
    NFmiArea::Write(file);
    file << itsBottomLeftLatLon;
    file << itsTopRightLatLon;

    // Dummies to replace old removed variables
    file << "0 0\n0 0\n";

    file << itsXScaleFactor << " ";
    file << itsYScaleFactor << std::endl;
    return file;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Read new object contents from the given stream
 *
 * \param file The input stream to read from
 * \return The input stream read from
 */
// ----------------------------------------------------------------------

std::istream &NFmiLatLonArea::Read(std::istream &file)
{
  try
  {
    double dummy;

    NFmiArea::Read(file);
    file >> itsBottomLeftLatLon;
    file >> itsTopRightLatLon;
    PacificView(NFmiArea::IsPacificView(itsBottomLeftLatLon, itsTopRightLatLon));
    file >> dummy >> dummy >> dummy >> dummy;  // old removed variables

    file >> itsXScaleFactor;
    file >> itsYScaleFactor;

    itsWorldRect =
        NFmiRect(LatLonToWorldXY(itsBottomLeftLatLon), LatLonToWorldXY(itsTopRightLatLon));

    Init();

    // 28.8.2001/Marko&Esa Tätä kutsuttaessa kaikki dataosat päivittyvät,
    // jolloin voidaan esim. editoida tiedostoissa olevia areoiden projektio
    // parametreja siten (tätä ei voitu tehdä ennen koska skaalauskertoimet eivät päivittyneet)

    return file;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

const std::string NFmiLatLonArea::AreaStr() const
{
  try
  {
    std::ostringstream out;
    out << "latlon:" << BottomLeftLatLon().X() << ',' << BottomLeftLatLon().Y() << ','
        << TopRightLatLon().X() << ',' << TopRightLatLon().Y();
    return out.str();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Return Well Known Text representation of the GCS
 *  GEOGCS["FMI_Sphere",
 *         DATUM["FMI_2007",SPHEROID["FMI_Sphere",6371220,0]],
 *         PRIMEM["Greenwich",0],
 *         UNIT["Degree",0.0174532925199433]],
 *  UNIT["Metre",1.0]
 */
// ----------------------------------------------------------------------

const std::string NFmiLatLonArea::WKT() const
{
  try
  {
    const char *fmt =
        R"(PROJCS["unknown",GEOGCS["FMI_Sphere",DATUM["FMI_2007",SPHEROID["FMI_Sphere",{},0]],PRIMEM["Greenwich",0],UNIT["Degree",0.0174532925199433]],PROJECTION["Equirectangular"],PARAMETER["standard_parallel_1",0],PARAMETER["central_meridian",0],PARAMETER["false_easting",0],PARAMETER["false_northing",0],UNIT["metre",1, AUTHORITY["EPSG","9001"]],AXIS["Easting",EAST],AXIS["Northing",NORTH]])";

    return fmt::format(fmt, kRearth);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Equality comparison with an area derived from NFmiLatLonArea.
 *
 * \param theArea The other area being compared to
 * \return True, if the NFmiLatLonArea parts are equivalent
 * \todo Use static_cast instead of C-style cast
 */
// ----------------------------------------------------------------------

bool NFmiLatLonArea::operator==(const NFmiArea &theArea) const
{
  try
  {
    return *this == static_cast<const NFmiLatLonArea &>(theArea);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Equality comparison
 *
 * \param theArea The other area being compared to
 * \return True, if the areas are equivalent
 * \todo Investigate whether NFmiArea::operator== should also be called.
 */
// ----------------------------------------------------------------------

bool NFmiLatLonArea::operator==(const NFmiLatLonArea &theArea) const
{
  try
  {
    if ((itsBottomLeftLatLon == theArea.itsBottomLeftLatLon) &&
        (itsTopRightLatLon == theArea.itsTopRightLatLon) &&
        (itsXScaleFactor == theArea.itsXScaleFactor) &&
        (itsYScaleFactor == theArea.itsYScaleFactor) && (itsWorldRect == theArea.itsWorldRect))
      return true;

    return false;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Hash value
 */
// ----------------------------------------------------------------------

std::size_t NFmiLatLonArea::HashValue() const
{
  try
  {
    std::size_t hash = NFmiArea::HashValue();
    boost::hash_combine(hash, itsBottomLeftLatLon.HashValue());
    boost::hash_combine(hash, itsTopRightLatLon.HashValue());
    boost::hash_combine(hash, boost::hash_value(itsXScaleFactor));
    boost::hash_combine(hash, boost::hash_value(itsYScaleFactor));
    boost::hash_combine(hash, itsWorldRect.HashValue());
    return hash;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}
// ======================================================================


// file: dotnect_platform.cpp, style: README.me
//
// License http://opensource.org/licenses/BSD-3-Clause
// Copyright (c) 2016 14U2g4ocMy5aB2cY4cmCtbXD6qyNQzujuA (serves donations as well)
// All rights reserved.
//
// -subscribes to dotnect_dgram_socket to get orientation and to kinect_tracking to get translation (3D point)
// -averages the 3D kinect point via moving averages: SMA, LMA and EMA (moving_average.h)
// -the information is published again on ros topics
// -implements the dotnect_markers menu actions
//
// NAME     DESCRIPTION
// dotnect_base_link  map coordinate system (frame) to latch dotnect_base_link
// frame_0   robot base
// frame_1   camera body
// frame_2   android refrence used by the rotation-vector sensor
// frame_3   smartphone body

// qr0to2    quaternion that rotates frame_0 to frame_2 (each cosy unit vector of frame_2) coordinate-rotation or
//           quaternion that describes frame_2 relative to frame_0 from a coordinate-transformation point of view;
//
// q0to2     quaternion that describes frame_0 relative to frame_2 from a coordinate-transformation point of view;
//           transformation of a vector v from f0 to f2 would be 2^v = 2^0_R_H(q0to2) * 0^v (Syntax: Craig -
//           Introdcution to Robotics)
//           q0to2 and qr0to2 are conjugates to each other and their rotation matrices derived by R_H(q) are the
//           transpose of one another
//
//           The conversion of q to euler angles (XYZ) is taken from https://github.com/ItsmeJulian/quat2eul
//           The conversion of q to rotation matrix would use the euler-hamilton function R_H(qr0to2) to get 3x3 rotation
//            matrix can found in
//             -matlab quat2rotm
//             -wolfram alpha e.g. https://www.wolframalpha.com/input/?i=quaternion:+0.7071%2B0i%2B0j%2B0.7071k
//             -wikipedia
//             https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Quaternion-derived_rotation_matrix
//
#include <stdio.h>
#include <cstring>
// boost
#include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp>
// ros
#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Float64MultiArray.h>
#include <std_msgs/Int32.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
// local
#include "custom_defines.h"
#include "data_container.h"
#include "dotnect_custom_msgs/DotnectUi.h"
#include "dotnect_custom_msgs/Float64Array.h"
#include "dotnect_custom_msgs/Int32Array.h"
#include "moving_average.h"
#include "tracking_class.h"
#include "val3_fstream_socket.h"

using namespace std;

// output user args
void printInfo(char **argv)
{
  printf(ACCYAN "$ dotnect_platform <1:int> <2:int> <3:int> <4:string> <5-7:dbl dbl dbl> "
                "<8-10:dbl dbl dbl>" ACRESETN);
  printf("     <1:xma> decide for moving average type (XMA)\n");
  printf("             0: simple moving average SMA\n");
  printf("             1: linear moving average LMA\n");
  printf("             2: exponential moving average EMA (alpha adjust via @FLAG_B)\n");
  printf("     <2:pos/vel> 0: position is averaged\n");
  printf("                 1: position delta = position now - position before\n");
  printf("                 2: velocity\n");
  printf("                 3: velocity delta\n");
  printf("     <3:nPoints> how many past measurements to consider (only SMA/LMA, if XMA set to 1)\n");
  printf("                 (low: fast respond, high: gradual respond)\n");
  printf("                 estimate: nPoints = freq *  time_intervall (e.g. 30 nPoints = 30Hz *1s)\n\n");
  printf("     <4:show_urdf_>  0: disable robot-model/IK-solver/jump-range-validation, show hovering smartphone "
         "instead\n");
  printf("                   1: enbale ...\n");
  printf("     <5-7:offset> initial end-effector position relative to robots base frame_0\n");
  printf("     <8-10:scale> scale end-effector reaction relative to kinect point delta (default: 1 1 1)\n");
}

// subscription name pool
string getSensorTopic(int type) {
  switch (type) {
    case 1: return "/dotnect_dgram_socket/acc";
    case 2: return "/dotnect_dgram_socket/mag";
    case 3: return "/dotnect_dgram_socket/orient";
    case 4: return "/dotnect_dgram_socket/gyro";
    case 5: return "/dotnect_dgram_socket/light";
    case 6: return "/dotnect_dgram_socket/pres";
    case 7: return "/dotnect_dgram_socket/temp";
    case 8: return "/dotnect_dgram_socket/prox";
    case 9: return "/dotnect_dgram_socket/grav";
    case 10: return "/dotnect_dgram_socket/linacc";
    case 11: return "/dotnect_dgram_socket/rot";
    case 12: return "/dotnect_dgram_socket/humid";
    case 13: return "/dotnect_dgram_socket/ambtmp";
    case 14: return "/dotnect_dgram_socket/rot_euler";
    default:
      printf(ACYELLOW NNAME": topic name not stored" ACRESETN);
      return "";
  }
}

// static global object
DataContainer *getContainerObj()
{
  static DataContainer x;
  return &x;
}

// static global object
Val3FstreamSocket *getGenerateVal3()
{
  static Val3FstreamSocket x;
  return &x;
}

// callbacks from dotnect_markers
void callbInt32A(const dotnect_custom_msgs::Int32Array::ConstPtr &msg_)
{
  printf(ACYELLOW NNAME": marker handle state changed" ACRESETN);
}

// callbacks from subscriptions
void callbMAFloat64(const std_msgs::Float64MultiArray::ConstPtr &msg_)
{
  if ((msg_->data)[2] == 2)
  {
    if ((msg_->data)[1] == 11)
    {
      // get real part of rotation-vector sensor to have complete quaternion vector
      static vector<double> tmp;
      double qw =
          sqrt(1 - (msg_->data)[3] * (msg_->data)[3] - (msg_->data)[4] * (msg_->data)[4] - (msg_->data)[5] * (msg_->data)[5]);
      tmp = msg_->data;
      tmp.push_back(qw);
      getContainerObj()->setVec(tmp);
    }
    else
    {
      // data sets into v_dbls_
      getContainerObj()->setVec(msg_->data);
    }
  }
  else if ((msg_->data)[2] == 1)
  {
    // data sets into v_ints_
    getContainerObj()->setInt((msg_->data)[1], (msg_->data)[3]);
  }
  else if ((msg_->data)[2] == 0)
  {
    // data is processed immideately
    getGenerateVal3()->manageSocket((msg_->data)[3]);
  }
}

// let ros flush data to our subscriptions
void ros_spin()
{
  ros::spin();
}

// publish joint states for possible urdf models
class JointsPublisher
{
public:
  JointsPublisher(Val3FstreamSocket *val3_fstream_socket_) : val3_fstream_socket_(val3_fstream_socket_)
  {
    pub_ = nh_.advertise<sensor_msgs::JointState>("joint_states_test", 5);
    joint_state_msg_.name.resize(6);
    joint_state_msg_.name[0] = "a1";
    joint_state_msg_.name[1] = "a2";
    joint_state_msg_.name[2] = "a3";
    joint_state_msg_.name[3] = "a4";
    joint_state_msg_.name[4] = "a5";
    joint_state_msg_.name[5] = "a6";
    joint_state_msg_.position.resize(6);
  }
  ~JointsPublisher()
  {
  }
  void publishJoints(vector<double> &radAng)
  {
    if (radAng.size() != 6)
    {
      printf(ACRED NNAME": joints size %lu" ACRESET, radAng.size());
      return;
    }
    write_lock_(mtx_);
    joint_state_msg_.position[0] = radAng[0];
    joint_state_msg_.position[1] = radAng[1];
    joint_state_msg_.position[2] = radAng[2];
    joint_state_msg_.position[3] = radAng[3];
    joint_state_msg_.position[4] = radAng[4];
    joint_state_msg_.position[5] = radAng[5];
    pub_.publish(joint_state_msg_);
  }

private:
  sensor_msgs::JointState joint_state_msg_;
  ros::NodeHandle nh_;
  ros::Publisher pub_;
  Val3FstreamSocket *val3_fstream_socket_;
  boost::shared_mutex mtx_;
  typedef boost::unique_lock<boost::shared_mutex> write_lock_;
};

// static global object
JointsPublisher *getJointStatePublisher()
{
  static JointsPublisher x(getGenerateVal3());
  return &x;
}

// Class used for message generation, advertising and publishing
// Each topic has one object, so not multi-threadsafe
class MultiArrayPublisher
{
public:
  MultiArrayPublisher(DataContainer *container, string topic_name, int k) : container_(container), k_(k)
  {
    pub_ = nh_.advertise<std_msgs::Float64MultiArray>(topic_name, 10);
    printf(NNAME ": advertised %s %s \n", k == 0 ? "timed" : "", topic_name.c_str());
    msg_.layout.dim.push_back(std_msgs::MultiArrayDimension());
    msg_.layout.dim[0].stride = 1;
    msg_.layout.dim[0].label = topic_name;
    msg_timed_ = msg_;
  }
  ~MultiArrayPublisher()
  {
  }
  // convert quaternion to euler angles of xyz sequence
  // extract from github.com/ItsmeJulian/quattoEul
  void quattoEul(double &qx, double &qy, double &qz, double &qw, bool toEulDeg)
  {
    double psi = atan2(2 * (qx * qw - qy * qz), (qw * qw - qx * qx - qy * qy + qz * qz));
    double theta = asin(2 * (qx * qz + qy * qw));
    double phi = atan2(2 * (qz * qw - qx * qy), (qw * qw + qx * qx - qy * qy - qz * qz));
    if (toEulDeg)
    {
      psi *= 180 / M_PI;
      theta *= 180 / M_PI;
      phi *= 180 / M_PI;
    }
    qx = psi;
    qy = theta;
    qz = phi;
    qw = 0.;
  }
  // publish position and rotation (quaternion or euler-angles)
  void publish(double x, double y, double z, double qx, double qy, double qz, double qw, bool toEul, bool toEulDeg)
  {
    // convert to xyz euler angles
    if (toEul)
    {
      quattoEul(qx, qy, qz, qw, toEulDeg);
      msg_.layout.dim[0].size = 6;
    }
    else
    {
      msg_.layout.dim[0].size = 7;
    }
    msg_.data.push_back(x);
    msg_.data.push_back(y);
    msg_.data.push_back(z);
    msg_.data.push_back(qx);
    msg_.data.push_back(qy);
    msg_.data.push_back(qz);
    if (toEul == false)
    {
      msg_.data.push_back(qw);
    }
    pub_.publish(msg_);
  }
  // ros timer triggered
  void publishT(const ros::TimerEvent &)
  {
    if (container_->checkVecFlag(4, k_) == 1)
    {
      container_->getVec(4, k_, v_);
    }
    msg_timed_.layout.dim[0].size = v_.size();
    msg_timed_.data = v_;
    pub_.publish(msg_timed_);
  }
  void publish_joint_state()
  {
    // IK-solver not impl.
  }
private:
  int k_;
  vector<double> v_;
  std_msgs::Float64MultiArray msg_;
  std_msgs::Float64MultiArray msg_timed_;
  DataContainer *container_;
  ros::NodeHandle nh_;
  ros::Publisher pub_;
};

// static global object
MultiArrayPublisher *getMultiArrayPublisherPP()
{
  static MultiArrayPublisher x(getContainerObj(), "/dotnect_platform/point_pixel_postion", 17);
  return &x;
}

// static global object for felix
MultiArrayPublisher *getMultiArrayPublisherTS()
{
  static MultiArrayPublisher x(getContainerObj(), "/dotnect_platform/tele_speaker", 0);
  return &x;
}

// Function is called to visualize the android refrence frame_2
// The broadcaster sends transforms for the android refrence frame originating from the rotation-vector sensor
void findRefFrame2(const ros::TimerEvent&)
{
  ROS_WARN_THROTTLE(1,NNAME "timer on");
  // static vector<double> q2to0(7);
  //
  // if (getContainerObj()->checkVecFlag(2, 11) == 0)
  // {
  //   ROS_WARN_THROTTLE(5,NNAME ": empty /dotnect_dgram_socket/rot. Retrying... (would use identity q)");
  //   q2to0[0] = ros::Time::now().toNSec();  // implicit double(ulong)
  //   q2to0[1] = 18;
  //   q2to0[2] = 20;  // quaternion transforms coordinates from frame_2 to frame_0
  //   q2to0[6] = 1;
  //   getContainerObj()->setVec(q2to0);
  //   return;
  // }
  // ROS_INFO_ONCE("Superpose smartphone-frame with the frame you want as real-world refrence.");
  // ROS_INFO_ONCE("The real-world-refrence frame in digital RViz is the robot base frame_0.");
  // ROS_INFO_ONCE("If you hit the real-world-refrence frame later again, the relation in RViz of frame_3 to frame_0 will "
  //               "be the identity I = δij.");
  // ROS_INFO_ONCE("Info on smartphone-fixed frame "
  //               "https://developer.android.com/guide/topics/sensors/sensors_overview.html#sensors-coords");
  // // ROS_INFO_ONCE("Info on android-refrence frame_2
  // // https://developer.android.com/guide/topics/sensors/sensors_motion.html#sensors-motion-rotate");
  // ROS_INFO_THROTTLE(10, "Superpose, then tick off right-click menu of interactive marker when frame_2 stopped "
  //                       "drifting.");
  //
  // vector<double> rot;
  // getContainerObj()->getVec(2, 11, rot);
  // tf::Transform tf(tf::Quaternion(rot[3], rot[4], rot[5], rot[6]), tf::Vector3(0, 0, 1));
  // tf::TransformBroadcaster br;
  // br.sendTransform(tf::StampedTransform(tf, ros::Time::now(), "frame_0", "frame_2"));
  //
  // q2to0[0] = ros::Time::now().toNSec();  // implicit double(ulong) conversion
  // q2to0[1] = 18;
  // q2to0[2] = 20;
  // q2to0[3] = rot[3];
  // q2to0[4] = rot[4];
  // q2to0[5] = rot[5];
  // q2to0[6] = rot[6];
  // getContainerObj()->setVec(q2to0);
}

// publish dynamic and static RViz markers
class MarkerArrayPublisher
{
public:
  MarkerArrayPublisher(DataContainer *container) : container_(container)
  {
    // point_f1    pos:3x, ori:4, scale:3, color:4, type:1, action:1, duration:2 id:1;
    double arr[19] = { 0, 0, 0, 0, 0, 0, 1, 0.05, 0.05, 0.05, 0.6, 0.6, 0.6, 1, 2, 0, 1, 0, 0 };
    makeMarker("frame_1", arr);
    marker_array_.markers.push_back(marker_);

    show_urdf_ = container->getInt(4);
    if (show_urdf_)
    {
      // smartphone
      double arr[19] = { 0, 0, 0, 0, 0, 0, 1, 0.17, 0.35, 0.018, 0.5, 0.5, 0.5, 0.95, 1, 0, 1, 0, 0 };
      makeMarker("frame_3", arr);
      marker_array_.markers.push_back(marker_);
    }
    else
    {
      // green ball is the initial/current end-effector position
      double arr[19] = { 0, 0, 0, 0, 0, 0, 1, 0.05, 0.05, 0.05, 0, 1, 0, 1, 2, 0, 1, 0, 0 };
      arr[0] = container_->getInt(5);
      arr[1] = container_->getInt(6);
      arr[2] = container_->getInt(7);
      makeMarker("frame_0", arr);
      marker_array_.markers.push_back(marker_);
      // red ball
      double arr2[19] = { 0, 0, 0, 0, 0, 0, 1, 0.05, 0.05, 0.05, 1, 0, 0, 1, 2, 2, 1, 0, 1 };
      makeMarker("frame_0", arr2);
      marker_array_.markers.push_back(marker_);
      // maximum jump range sphere
      double arr3[19] = { 0, 0, 0, 0, 0, 0, 1, 0.5, 0.5, 0.5, 1, 0, 0, 0.5, 2, 2, 1, 0, 2 };
      makeMarker("frame_0", arr3);
      marker_array_.markers.push_back(marker_);
    }
    // advertise marker array
    pub_ = nh_.advertise<visualization_msgs::MarkerArray>("/dotnect_platform/MarkerArrayPublisher", 3);
  }
  ~MarkerArrayPublisher()
  {
  }
  // initalize marker for rviz
  void makeMarker(basic_string<char> frame_id, double *vars)
  {
    marker_.header.frame_id = frame_id;
    marker_.header.stamp = ros::Time::now();
    marker_.ns = NNAME;
    marker_.pose.position.x = vars[0];
    marker_.pose.position.y = vars[1];
    marker_.pose.position.z = vars[2];
    marker_.pose.orientation.y = vars[4];
    marker_.pose.orientation.z = vars[5];
    marker_.pose.orientation.w = vars[6];
    marker_.scale.x = vars[7];
    marker_.scale.y = vars[8];
    marker_.scale.z = vars[9];
    marker_.color.r = vars[10];
    marker_.color.g = vars[11];
    marker_.color.b = vars[12];
    marker_.color.a = vars[13];
    marker_.type = vars[14];
    marker_.action = vars[15];
    marker_.lifetime = ros::Duration(vars[16], vars[17]);
    marker_.id = vars[18];
  }
  void publishMarkerCallback(const ros::TimerEvent &)
  {
    boost::mutex::scoped_lock lock(mtx_);
    if (container_->checkVecFlag(0, 15))
    {
      container_->getVec(0, 15, point_f1_);
      marker_array_.markers[0].pose.position.x = point_f1_[3] * 1e-3;
      marker_array_.markers[0].pose.position.y = point_f1_[4] * 1e-3;
      marker_array_.markers[0].pose.position.z = point_f1_[5] * 1e-3;
    }
    if (show_urdf_)
    {
      // Add or fade redBall depending on container j:21
      if (container_->checkVecFlag(0, 21) == 1)
      {
        double discards = container_->getVecDbl(0, 21, 3);
        marker_array_.markers[2].action = discards;
        marker_array_.markers[3].action = discards;
      }
    }
    // update all timestamps and publish
    for (int i = 0; i < marker_array_.markers.size(); ++i)
    {
      marker_array_.markers[i].header.stamp = ros::Time::now();
    }
    pub_.publish(marker_array_);
  }

private:
  bool show_urdf_;
  std::vector<double> point_f1_;
  DataContainer *container_;
  ros::NodeHandle nh_;
  ros::Publisher pub_;
  visualization_msgs::Marker marker_;
  visualization_msgs::MarkerArray marker_array_;
  boost::mutex mtx_;
};

// static global object
MarkerArrayPublisher *getMarkerArrayPub()
{
  static MarkerArrayPublisher x(getContainerObj());
  return &x;
}

// Regularly publish transforms, called by ros timer
// The tf pose updates are from the the latest interactive marker feedback
// incoming every time the user drag & drops an interactive marker
class TransformHandler
{
public:
  TransformHandler()
  {
    getContainerObj()->getVec(3, 11, v_q2to3_);
    getContainerObj()->getVec(3, 18, v_q2to0_);
    getContainerObj()->getVec(3, 16, point_f1_xma_);

    // default coordinate-transformation from frame_1 to frame_0
    tf::Transform tf;
    tf.setOrigin(tf::Vector3(0, 0, 0));
    tf.setRotation(tf::Quaternion(0, 0, 0, 1));
    tf_1to0_ = tf::StampedTransform(tf, ros::Time::now(), "/frame_0", "/frame_1");
  }
  ~TransformHandler()
  {
  }
  void updateFeedback(...)
  {
    write_lock_ w_lock(mutex_shr_);
    // ...
  }
  // get dynamic transform for point from child to parent, no waitForTransform (just throw then)
  void getTransform(string parent, string child, tf::StampedTransform &tf_1to0_)
  {
    try
    {
      tf_li_.lookupTransform(parent, child, ros::Time(0), tf_1to0_);
    }
    catch (tf::TransformException ex)
    {
      ROS_WARN_THROTTLE(10, "%s Retrying...", ex.what());
    }
  }
  // publish frame_3 on the averaged point_f0_xma
  void publishTransform(const ros::TimerEvent &)
  {
    lock_shared_shared_ r_lock(mutex_shr_);
    if (getContainerObj()->checkVecFlag(3, 11) == 1)
    {
      getContainerObj()->getVec(3, 11, v_q2to3_);
    }
    if (getContainerObj()->checkVecFlag(3, 16) == 1)
    {
      getContainerObj()->getVec(3, 16, point_f1_xma_);
      printf("checkVecFlag(2, 16) %f %f %f\n", point_f1_xma_[3], point_f1_xma_[4], point_f1_xma_[5]);
    }
    if (getContainerObj()->checkVecFlag(3, 18) == 1)
    {
      getContainerObj()->getVec(3, 18, v_q2to0_);
    }
    // lookup coordinate-transformation from frame_1 to frame_0
    getTransform("/frame_0", "/frame_1", tf_1to0_);
    // vector to tf::Vector3
    tf::Vector3 point_f1_xma(point_f1_xma_[3], point_f1_xma_[4], point_f1_xma_[5]);
    // subtract transformed origin as vector is a relative quantity
    tf::Vector3 point_f0_xma(tf_1to0_ * point_f1_xma - tf_1to0_ * tf::Vector3(0, 0, 0));
    // coordinate-transformation from frame_2 to frame_0
    tf::Quaternion q2to0(v_q2to0_[3], v_q2to0_[4], v_q2to0_[5], v_q2to0_[6]);
    // coordinate-transformation from frame_2 to frame_3
    tf::Quaternion q2to3(v_q2to3_[3], v_q2to3_[4], v_q2to3_[5], v_q2to3_[6]);
    // coordinate-transformation from frame_3 to frame_0
    tf::Quaternion q3to0 = q2to0.inverse() * q2to3;
    // the coordinate-rotation with sendTransform from parent frame_0 to child frame_3
    // is equal to the coordinate-transformation quaternion q3to2
    tf::Transform tf(q3to0, point_f0_xma);
    tf_br_.sendTransform(tf::StampedTransform(tf, ros::Time::now(), "frame_0", "frame_3"));
    // trigger convert to val3 syntax that fits staeubli robot control and publish string
    // getGenerateVal3()->sendMovej(point_f0_xma.x(), point_f0_xma.y(), point_f0_xma.z(), q3to0.x(), q3to0.y(), q3to0.z(),
    getMultiArrayPublisherTS()->publish(point_f0_xma.x(), point_f0_xma.y(), point_f0_xma.z(), q3to0.x(), q3to0.y(), q3to0.z(),
                                           q3to0.w(), true, false);
  }

private:
  vector<double> point_f1_xma_;
  vector<double> v_q2to0_;
  vector<double> v_q2to3_;
  tf::StampedTransform tf_1to0_;
  tf::TransformListener tf_li_;
  tf::TransformBroadcaster tf_br_;
  boost::shared_mutex mutex_shr_;
  typedef boost::unique_lock<boost::shared_mutex> write_lock_;  // block new readers and wait until
  typedef boost::shared_lock<boost::shared_mutex> lock_shared_shared_;   // all lock_shared_shared_s are out of scope (released)
};

// static global object
TransformHandler *getTransformHandler()
{
  static TransformHandler x;
  return &x;
}

// pauses caller
bool customSleep(int sec, int nsec)
{
  struct timespec req, rem;
  req.tv_sec = sec;
  req.tv_nsec = nsec;
  if (nanosleep(&req, &rem) != 0)
  {
    printf(ACYELLOW NNAME ": nanosleep rem %d:%d strerror:%s" ACRESETN, (int)rem.tv_sec, (int)rem.tv_nsec, strerror(errno));
    return false;
  }
  return true;
}

// stack mod
void  setupTimer(){

  }

  struct CommaIterator
  :
    public std::iterator<std::output_iterator_tag, void, void, void, void>
  {
    std::ostream *os;
    std::string comma;
    bool first;

    CommaIterator(std::ostream& os, const std::string& comma)
    :
      os(&os), comma(comma), first(true)
    {
    }

    CommaIterator& operator++() { return *this; }
    CommaIterator& operator++(int) { return *this; }
    CommaIterator& operator*() { return *this; }
    template <class T>
    CommaIterator& operator=(const T& t) {
      if(first)
        first = false;
      else
        *os << comma;
      *os << t;
      return *this;
    }
  };

// process ui-client requests
bool processUiService(dotnect_custom_msgs::DotnectUi::Request &req, dotnect_custom_msgs::DotnectUi::Response &res) {
  if (req.object_name == "ui_validjumps") {

  } else if (req.object_name == "ui_rotation_offset") {
    if (req.rint) {
      *(getContainerObj()->getTimerPtr(2)) = getContainerObj()->getNodeHandle()->createTimer(ros::Duration(0.1), findRefFrame2);
      printf(NNAME ": timer findRefFrame2 started.\n");
    } else {
      getContainerObj()->getTimerPtr(2)->stop();
      printf(NNAME ": timer findRefFrame2 stopped.\n");
    }
  } else if (req.object_name == "ui_xma_change") {
    // stop thread
    getContainerObj()->setInt(14, 1);
    customSleep(1,0);
    // start again with new values
      getContainerObj()->setInt(1, req.rints[0]);
      // getContainerObj()->setInt(2, 0);
      getContainerObj()->setInt(3, req.rints[1]);
      *(getContainerObj()->getThreadXMA()) = boost::thread(MOVAV::moving_average, getContainerObj(), 15);
    }

  // print request TODO push to above with on off
  std::ostringstream oss_rbools;
  std::ostringstream oss_rints;
  std::ostringstream oss_rdbls;
  std::copy(req.rbools.begin(), req.rbools.end(), CommaIterator(oss_rbools, ","));
  std::copy(req.rints.begin(), req.rints.end(), CommaIterator(oss_rints, ","));
  std::copy(req.rdbls.begin(), req.rdbls.end(), CommaIterator(oss_rdbls, ","));
  printf(ACGREEN NNAME ": client request:\nobject_name:%s\nbool:%d\nbools:%s\nint:%d\nints:%s\nrdbl:%f\nrdbls:%s\nrstr:%s\n" ACRESETN, req.object_name.c_str(), req.rbool, oss_rbools.str().c_str(), req.rint, oss_rints.str().c_str(), req.rdbl, oss_rdbls.str().c_str(), req.rstr.c_str());

  // answer that request got sorted
  res.rbool = true;
  return true;
}

// main function processes user input, starts timers and threads
int main(int argc, char **argv)
{
  if (argc < 11)
  {
    printInfo(argv);
    exit(0);
  }
  // wait for roslaunch server to prevent nodes printf entangling (only reason)
  customSleep(3, 0);

  ros::init(argc, argv, NNAME);
  ros::NodeHandle nh;

  // printInfo
  int tmp;
  for (int i = 1; i < 11; ++i)
  {
    istringstream(argv[i]) >> tmp;
    getContainerObj()->setInt(i, tmp);
  }

  // add subscriptions
  printf(NNAME ": subscribes on: \n%s\n/dotnect_markers/menu_state_change\n", getSensorTopic(11).c_str());
  ros::Subscriber subContObj11 = nh.subscribe(getSensorTopic(11), 3, callbMAFloat64);
  ros::Subscriber subContObj19 = nh.subscribe("/dotnect_markers/menu_state_change", 3, callbInt32A);
  TrackingClass tracking_class(getContainerObj());
  tracking_class.subscribeImgSrc("/camera/depth/image_raw");

  // create timer to update marker array
  *(getContainerObj()->getTimerPtr(0)) = nh.createTimer(ros::Duration(0.1), &MarkerArrayPublisher::publishMarkerCallback, getMarkerArrayPub());
  // update transform
  *(getContainerObj()->getTimerPtr(1)) = nh.createTimer(ros::Duration(0.1), &TransformHandler::publishTransform, getTransformHandler());
  // flush data to callback functions
  boost::thread t_spinner(ros_spin);
  // start averaging point_f1 to point_f1_xma
  *(getContainerObj()->getThreadXMA()) = boost::thread(MOVAV::moving_average, getContainerObj(), 15);
  // react to incoming verbose signal
  ros::ServiceServer service = nh.advertiseService("dotnect_ui_service", processUiService);

  // shutdown handling
  t_spinner.join();
  printf(NNAME ": t_spinner terminated \n");
  getContainerObj()->setInt(14, 1);
  getContainerObj()->getThreadXMA()->join();
  printf(NNAME ": t_xma_average_ terminated \n");
  getContainerObj()->getTimerPtr(0)->stop();
  getContainerObj()->getTimerPtr(1)->stop();
  getContainerObj()->getTimerPtr(2)->stop();
  printf(NNAME ": timers stopped \n");
  printf(ACCYAN NNAME " returned" ACRESETN);
  return 0;
}
// EOF


#include <boost/hana/ext/std/ratio.hpp>


#ifndef PYTHONIC_NUMPY_REDUCE_HPP
#define PYTHONIC_NUMPY_REDUCE_HPP

#include "pythonic/include/numpy/reduce.hpp"

#include "pythonic/types/ndarray.hpp"
#include "pythonic/__builtin__/None.hpp"
#include "pythonic/__builtin__/ValueError.hpp"
#include "pythonic/utils/neutral.hpp"

#ifdef USE_BOOST_SIMD
#include <boost/simd/include/functions/broadcast.hpp>
#include <boost/simd/memory/functions/aligned_store.hpp>
#endif

#include <algorithm>

namespace pythonic
{

  namespace numpy
  {

    template <class Op, bool vector_form>
    template <class E, class F>
    F _reduce<Op, 1, vector_form>::operator()(E e, F acc)
    {
      for (auto const &value : e)
        Op{}(acc, value);
      return acc;
    }

#ifdef USE_BOOST_SIMD
    template <class Op>
    template <class E, class F>
    F _reduce<Op, 1, true>::operator()(E e, F acc)
    {
      using T = typename E::dtype;
      using vT = typename boost::simd::native<T, BOOST_SIMD_DEFAULT_EXTENSION>;
      static const size_t vN = boost::simd::meta::cardinal_of<vT>::value;
      const long n = e.shape()[0];
      const long bound = n / vN * vN;
      long i = 0;
      if (bound > 0) {
        vT vacc = e.load(i);
        for (i = vN; i < bound; i += vN)
          Op{}(vacc, e.load(i));
        alignas(sizeof(vT)) T stored[vN];
        boost::simd::store(vacc, &stored[0]);
        for (size_t j = 0; j < vN; ++j)
          Op{}(acc, stored[j]);
      }
      for (; i < n; ++i)
        Op{}(acc, e.fast(i));
      return acc;
    }
#endif

    template <class Op, size_t N, bool vector_form>
    template <class E, class F>
    F _reduce<Op, N, vector_form>::operator()(E e, F acc)
    {
      for (auto const &value : e)
        acc = _reduce<Op, N - 1, vector_form>{}(value, acc);
      return acc;
    }

    template <class Op, class E>
    reduce_result_type<E> reduce(E const &expr, types::none_type)
    {
      bool constexpr is_vectorizable =
          E::is_vectorizable and
          not std::is_same<typename E::dtype, bool>::value;
      reduce_result_type<E> p = utils::neutral<Op, typename E::dtype>::value;
      return _reduce<Op, types::numpy_expr_to_ndarray<E>::N, is_vectorizable>{}(
          expr, p);
    }

    template <class Op, class E>
    auto reduce(E const &array, long axis) ->
        typename std::enable_if<E::value == 1,
                                decltype(reduce<Op>(array))>::type
    {
      if (axis != 0)
        throw types::ValueError("axis out of bounds");
      return reduce<Op>(array);
    }

    template <class Op, class E>
    typename std::enable_if<E::value != 1, reduced_type<E>>::type
    reduce(E const &array, long axis)
    {
      if (axis < 0 || size_t(axis) >= E::value)
        throw types::ValueError("axis out of bounds");
      auto shape = array.shape();
      if (axis == 0) {
        types::array<long, E::value - 1> shp;
        std::copy(shape.begin() + 1, shape.end(), shp.begin());
        return _reduce<Op, 1, false /* not on scalars*/>{}(
            array,
            reduced_type<E>{shp, utils::neutral<Op, typename E::dtype>::value});
      } else {
        types::array<long, E::value - 1> shp;
        auto next = std::copy(shape.begin(), shape.begin() + axis, shp.begin());
        std::copy(shape.begin() + axis + 1, shape.end(), next);
        reduced_type<E> sumy{shp, __builtin__::None};
        std::transform(array.begin(), array.end(), sumy.begin(),
                       [axis](typename E::iterator::value_type other) {
                         return reduce<Op>(other, axis - 1);
                       });
        return sumy;
      }
    }
  }
}

#endif


#include <string>
#include <vector>

#include <Eigen/Dense>
#include <yaml-cpp/yaml.h>

#include <maplab-common/eigen-yaml-serialization.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>
#include <maplab-common/yaml-serialization.h>

namespace common {
#define CHECK_TEST(testname, type, rows, cols, filename)    \
  do {                                                      \
    const double kApproxPrecision = 1e-5;                   \
    Eigen::Matrix<type, rows, cols> lhs;                    \
    Eigen::Matrix<type, rows, cols> rhs;                    \
    if (rows == Eigen::Dynamic && cols == Eigen::Dynamic) { \
      lhs.resize(8, 3);                                     \
    } else if (rows == Eigen::Dynamic) {                    \
      lhs.resize(8, Eigen::NoChange);                       \
    } else if (cols == Eigen::Dynamic) {                    \
      lhs.resize(Eigen::NoChange, 3);                       \
    }                                                       \
    lhs.setRandom();                                        \
    YAML::Save(lhs, filename);                              \
    bool ok_string = YAML::Load(filename, &rhs);            \
    EXPECT_TRUE(ok_string);                                 \
    EXPECT_NEAR_EIGEN(lhs, rhs, kApproxPrecision);          \
    std::ofstream ofs(filename);                            \
    YAML::Save(lhs, &ofs);                                  \
    ofs.close();                                            \
    bool ok_load = YAML::Load(filename, &rhs);              \
    EXPECT_TRUE(ok_load);                                   \
    EXPECT_NEAR_EIGEN(lhs, rhs, kApproxPrecision);          \
  } while (0);

TEST(MaplabCommon, EigenYamlSerialization_Fixed_SizeDouble) {
  CHECK_TEST("Fixed size double", double, 7, 6, "double_fixed.yaml");
}
TEST(MaplabCommon, EigenYamlSerialization_Dynamic_SizeDouble) {
  CHECK_TEST(
      "Dynamic size double", double, Eigen::Dynamic, Eigen::Dynamic,
      "double_dynamic.yaml");
}
TEST(MaplabCommon, EigenYamlSerialization_Dynamic_SizeVectorDouble) {
  CHECK_TEST(
      "Dynamic vector col double", double, 1, Eigen::Dynamic,
      "double_dynveccol.yaml");
}
TEST(MaplabCommon, EigenYamlSerialization_Dynamic_SizeVectorRowDouble) {
  CHECK_TEST(
      "Dynamic vector row double", double, Eigen::Dynamic, 1,
      "double_dynvecrow.yaml");
}
TEST(MaplabCommon, EigenYamlSerialization_Fixed_SizeInt) {
  CHECK_TEST("Fixed size int", int, 7, 6, "int_fixed.yaml");
}
TEST(MaplabCommon, EigenYamlSerialization_Dynamic_SizeInt) {
  CHECK_TEST(
      "Dynamic size int", int, Eigen::Dynamic, Eigen::Dynamic,
      "int_dynamic.yaml");
}
TEST(MaplabCommon, EigenYamlSerialization_Dynamic_SizeVectorColInt) {
  CHECK_TEST(
      "Dynamic vector col int", int, 1, Eigen::Dynamic, "int_dynveccol.yaml");
}
TEST(MaplabCommon, EigenYamlSerialization_Dynamic_SizeVectorRowInt) {
  CHECK_TEST(
      "Dynamic vector row int", int, Eigen::Dynamic, 1, "int_dynvecrow.yaml");
}

TEST(MaplabCommon, EigenYamlSerialization_DetectEmptyFile) {
  Eigen::Matrix<double, 5, 4> rhs;
  std::string filename = "destroyed_file.yaml";
  std::ofstream ofs(filename);
  ofs << "";
  ofs.close();

  bool parse_ok = YAML::Load(filename, &rhs);
  EXPECT_FALSE(parse_ok);
}

TEST(MaplabCommon, EigenYamlSerialization_DetectDestroyedFile) {
  Eigen::Matrix<double, 5, 4> rhs;
  std::string filename = "destroyed_file.yaml";
  std::ofstream ofs(filename);
  ofs << "Invalid data to test the YAML parser.";
  ofs.close();

  std::ifstream ifs(filename);
  YAML::Parser parser(ifs);
  bool parse_ok = YAML::Load(filename, &rhs);
  EXPECT_FALSE(parse_ok);
}

TEST(MaplabCommon, EigenYamlSerialization_DeathFromWrongMatrixType) {
  Eigen::Matrix<double, 5, 4> lhs;
  Eigen::Matrix<double, 5, 3> rhs;
  lhs.setRandom();
  std::string filename = "death_from_wrong_matrix_type.yaml";
  YAML::Save(lhs, filename);
  EXPECT_DEATH(YAML::Load(filename, &rhs), "^");
}

}  // namespace common

MAPLAB_UNITTEST_ENTRYPOINT


/**
 * @cond ___LICENSE___
 *
 * Copyright (c) 2016-2018 Zefiros Software.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *
 * @endcond
 */
#include "sync/sync.h"
#include "sync/util/random.h"
#include "sync/util/timer.h"

#include "args/args.h"

#include <armadillo>

#include <numeric>
#include <tuple>

template<typename tMat, typename tRandom>
void RandomizeMatrix(tMat &mat, tRandom &random)
{
    for (auto &entry : mat)
    {
        entry = random.NextDouble();
    }
}

struct ProcessorCube
{
    size_t p;
    size_t q0, q1, q2;

    ProcessorCube(const size_t iP, const size_t iQ0, const size_t iQ1, const size_t iQ2)
        : p(iP),
          q0(iQ0),
          q1(iQ1),
          q2(iQ2)
    {
        SyncLibInternal::Assert(q0 * q1 * q2 == p, "Dimensions mismatch, {} * {} * {} = {} != {}", q0, q1, q2, q0 * q1 * q2, p);
    }

    ProcessorCube(const size_t iP, const size_t iQ)
        : ProcessorCube(iP, iQ, iQ, iQ)
    {}

    std::tuple<size_t, size_t, size_t>operator()(size_t rank) const
    {
        return std::make_tuple(rank / (q1 * q2), (rank % (q1 * q2)) / q2, rank % q2);
    }

    size_t operator()(size_t s, size_t t, size_t u)const
    {
        return s * q1 * q2 + t * q2 + u;
    }

    template<typename tEnv>
    tEnv &Split0(tEnv &env, size_t s0, size_t s1, size_t s2) const
    {
        return env.Split(s2 * q1 + s1, s0);
    }

    template<typename tEnv>
    tEnv &Split1(tEnv &env, size_t s0, size_t s1, size_t s2) const
    {
        return env.Split(s0 * q2 + s2, s1);
    }

    template<typename tEnv>
    tEnv &Split2(tEnv &env, size_t s0, size_t s1, size_t s2) const
    {
        return env.Split(s0 * q1 + s1, s2);
    }

    template<typename tEnv>
    std::tuple<tEnv &, tEnv &, tEnv &> Split(tEnv &env, size_t s0, size_t s1, size_t s2) const
    {
        return std::tuple<tEnv &, tEnv &, tEnv &>(Split0(env, s0, s1, s2), Split1(env, s0, s1, s2), Split2(env, s0, s1, s2));
    }
};

template<typename tEnv>
struct MatMatHelper
{
    static size_t NumLocalBalanced(size_t n, size_t q, size_t s)
    {
        return (n - s + q - 1) / q;
    }

    static size_t OffsetGlobalBalanced(size_t n, size_t q, size_t s)
    {
        size_t offset = 0;

        for (size_t t = 0; t < s; ++t)
        {
            offset += NumLocalBalanced(n, q, t);
        }

        return offset;
    }

    static size_t NumLocal(size_t n, size_t q)
    {
        return (n + q - 1) / q;
    }

    template<typename tMat>
    static auto GetSubViewBalanced(tMat &M,
                                   size_t s0, size_t q0, size_t n0, size_t nl0,
                                   size_t s1, size_t q1, size_t n1, size_t nl1)
    {
        const arma::uword iOffset = OffsetGlobalBalanced(n0, q0, s0);
        const arma::uword jOffset = OffsetGlobalBalanced(n1, q1, s1);

        return M(arma::span(iOffset, iOffset + nl0 - 1), arma::span(jOffset, jOffset + nl1 - 1));
    }

    struct DimInfoContiguous
    {
        size_t n;
        size_t k;
        size_t m;
        size_t nl0;
        size_t nl1;
        size_t nl2;
        size_t nCol02;
        size_t nCol21;
        size_t nCol01;
        size_t nl02;
        size_t nl21;
        size_t nl01;

        static DimInfoContiguous Create(const size_t n, const size_t k, const size_t m, const ProcessorCube &P, const size_t s0,
                                        const size_t s1, const size_t s2)
        {
            const size_t nl0 = NumLocalBalanced(n, P.q0, s0);
            const size_t nl1 = NumLocalBalanced(m, P.q1, s1);
            const size_t nl2 = NumLocalBalanced(k, P.q2, s2);

            const size_t nCol02 = NumLocal(nl2, P.q1);
            const size_t nCol21 = NumLocal(nl1, P.q0);
            const size_t nCol01 = NumLocal(nl1, P.q2);

            const size_t nl02 = nl0 * nCol02;
            const size_t nl21 = nl2 * nCol21;
            const size_t nl01 = nl0 * nCol01;

            return { n, k, m, nl0, nl1, nl2, nCol02, nCol21, nCol01, nl02, nl21, nl01 };
        }
    };

    template<typename tT, typename tSendQueue>
    static void FanOutMatrixContiguous(arma::Mat<tT> &M, tSendQueue &sq, size_t q, size_t v0, size_t vStart, size_t vSkip,
                                       size_t sizeV, size_t sizeTotal)
    {
        const arma::uword start = v0 * sizeV;
        const arma::uword size = std::min<arma::uword>(start + sizeV, sizeTotal) - start;

        if (size == 0)
        {
            return;
        }

        arma::Col<tT>tmp(&M(start), size, false, true);

        for (size_t v = 0, rank1 = vStart; v < q; ++v, rank1 += vSkip)
        {
            if (v == v0)
            {
                continue;
            }

            sq.Send(rank1, start, tmp);
        }
    }

    template<typename tT, typename tSendQueue>
    static void FanInMatrixContiguous(arma::Mat<tT> &M, tSendQueue &sq)
    {
        for (auto [i, mVec] : sq)
        {
            arma::Col<tT>subM(&M(i), mVec.n_elem, false, true);
            subM = mVec;
        }
    }

    template<typename tT, typename tSendQueue>
    static void ReduceCContiuous(arma::Mat<tT> &C_st, tSendQueue &sq, const size_t s2, const size_t startT, const size_t q2,
                                 const DimInfoContiguous &dimInfo, tEnv &env)
    {
        {
            for (size_t t2 = 0, t = startT; t2 < q2; ++t2, ++t)
            {
                if (t2 == s2)
                {
                    continue;
                }

                const arma::uword start = t2 * dimInfo.nl01;
                const arma::uword size = std::min<arma::uword>(start + dimInfo.nl01, dimInfo.nl0 * dimInfo.nl1) - start;
                arma::Col<tT>subC(&C_st(start), size, false, true);

                sq.Send(t, start, subC);
            }
        }

        env.Sync();

        for (auto[i, cVec] : sq)
        {
            arma::Col<tT>subC(&C_st(i), cVec.n_elem, false, true);
            subC += cVec;
        }
    }
};

template<typename tSubEnv, typename... tSubEnvs>
struct MatMatCoreImpl;

template<typename tEnv, typename... tSubEnvs>
struct MatMatAlgorithm
{
    using tHelper = MatMatHelper<tEnv>;

    template<typename tDimInfo>
    static void MatMat(tEnv &env, arma::mat &A_su, arma::mat &B_ut, arma::mat &C_st,
                       tDimInfo &dimInfo, const ProcessorCube &P,
                       typename tEnv::template SendQueue<arma::uword, arma::vec> &sqA, typename tEnv::template SendQueue<arma::uword, arma::vec> &sqB,
                       typename tEnv::template SendQueue<arma::uword, arma::vec> &sqC)
    {
        auto[s0, s1, s2] = P(env.Rank());

        const size_t q0 = P.q0, q1 = P.q1, q2 = P.q2;

        const size_t nl0 = dimInfo.nl0, nl1 = dimInfo.nl1, nl2 = dimInfo.nl2,
                     nl02 = dimInfo.nl02, nl21 = dimInfo.nl21;

        tHelper::FanOutMatrixContiguous(A_su, sqA, q1, s1, s0 * q1 * q2 + s2, q2, nl02, nl0 * nl2);
        tHelper::FanOutMatrixContiguous(B_ut, sqB, q0, s0, s2 + s1 * q2, q1 * q2, nl21, nl2 * nl1);

        env.Sync();

        tHelper::FanInMatrixContiguous(A_su, sqA);
        tHelper::FanInMatrixContiguous(B_ut, sqB);

        MatMatCoreImpl<tSubEnvs..., SyncLib::Environments::NoBSP>::Compute(env, A_su, B_ut, C_st, dimInfo, P, sqC);
    }

    template<typename tDimInfo>
    static void MatMatSplit(tEnv &env, tEnv &env02, tEnv &env21, tEnv &env01,
                            arma::mat &A_su, arma::mat &B_ut, arma::mat &C_st,
                            tDimInfo &dimInfo, const ProcessorCube &P,
                            typename tEnv::template SendQueue<arma::uword, arma::vec> &sqA, typename tEnv::template SendQueue<arma::uword, arma::vec> &sqB,
                            typename tEnv::template SendQueue<arma::uword, arma::vec> &sqC)
    {
        auto[s0, s1, s2] = P(env.Rank());

        tHelper::FanOutMatrixContiguous(A_su, sqA, P.q1, s1, 0, 1, dimInfo.nl02, dimInfo.nl0 * dimInfo.nl2);
        env02.Sync();
        tHelper::FanInMatrixContiguous(A_su, sqA);

        tHelper::FanOutMatrixContiguous(B_ut, sqB, P.q0, s0, 0, 1, dimInfo.nl21, dimInfo.nl2 * dimInfo.nl1);
        env21.Sync();
        tHelper::FanInMatrixContiguous(B_ut, sqB);

        MatMatCoreImpl<tSubEnvs..., SyncLib::Environments::NoBSP>::Compute(env01, A_su, B_ut, C_st, dimInfo, P, sqC);
    }

    struct MatMatBench
    {
        template<typename tDimInfo>
        static void Run(size_t repetitions, SyncLib::Util::Timer<> &timer, tEnv &env, const ProcessorCube &P,
                        tDimInfo &dimInfo, arma::mat &A_su, arma::mat &B_ut, arma::mat &C_st)
        {
            typename tEnv::template SendQueue<arma::uword, arma::vec>sqA(env), sqB(env), sqC(env);

            for (size_t it = 0; it < repetitions; ++it)
            {
                env.Barrier();
                timer.Tic();
                MatMat(env, A_su, B_ut, C_st, dimInfo, P, sqA, sqB, sqC);

                if (env.Rank() == 0)
                {
                    fmt::print("Matrix multiplication with (n, k, m) = ({}, {}, {}) with processor count p={:>2} x {:>2} x {:>2}={} took {:.3f}s\n",
                               dimInfo.n, dimInfo.k, dimInfo.m, P.q0, P.q1, P.q2, P.p, timer.Toc());
                }
            }
        }
    };

    struct MatMatBenchSplit
    {
        template<typename tDimInfo>
        static void Run(size_t repetitions, SyncLib::Util::Timer<> &timer, tEnv &env, const ProcessorCube &P,
                        tDimInfo &dimInfo, arma::mat &A_su, arma::mat &B_ut, arma::mat &C_st)
        {
            const size_t rank = env.Rank();
            const auto[s0, s1, s2] = P(rank);

            auto [env21, env02, env01] = P.Split(env, s0, s1, s2);

            typename tEnv::template SendQueue<arma::uword, arma::vec> sqA(env02), sqB(env21), sqC(env01);

            for (size_t it = 0; it < repetitions; ++it)
            {
                env.Barrier();
                timer.Tic();
                MatMatSplit(env, env02, env21, env01, A_su, B_ut, C_st, dimInfo, P, sqA, sqB, sqC);

                if (env.Rank() == 0)
                {
                    fmt::print("Matrix multiplication with (n, k, m) = ({}, {}, {}) with processor count p={:>2} x {:>2} x {:>2}={} took {:.3f}s\n",
                               dimInfo.n, dimInfo.k, dimInfo.m, P.q0, P.q1, P.q2, P.p, timer.Toc());
                }
            }
        }
    };

    template<typename tBench>
    static void MatMatTest(tEnv &env, ProcessorCube &P, arma::mat &A, arma::mat &B, size_t repetitions)
    {
        const size_t rank = env.Rank();
        const auto[s0, s1, s2] = P(rank);

        size_t n = A.n_rows, m = A.n_cols, k = B.n_rows;
        const auto dimInfo = tHelper::DimInfoContiguous::Create(n, k, m, P, s0, s1, s2);

        arma::mat A_su(dimInfo.nl0, dimInfo.nl2, arma::fill::zeros);
        arma::mat B_ut(dimInfo.nl1, dimInfo.nl2, arma::fill::zeros);
        arma::mat C_st(dimInfo.nl0, dimInfo.nl1, arma::fill::zeros);

        SyncLib::Util::LinearCongruentialRandom random(rank);
        RandomizeMatrix(A_su, random);
        RandomizeMatrix(B_ut, random);

        SyncLib::Util::Timer<> timer;

        tBench::Run(repetitions, timer, env, P, dimInfo, A_su, B_ut, C_st);
        env.Barrier();

        typename tEnv::template SendQueue<double>sumC(env);
        {
            const arma::uword start = s2 * dimInfo.nl01;
            const arma::uword size = std::min<arma::uword>(start + dimInfo.nl01, dimInfo.nl0 * dimInfo.nl1) - start;
            const arma::vec subC(&C_st(start), size, false, true);
            sumC.Broadcast(arma::as_scalar(arma::sum(arma::abs(subC))));
        }
        env.Sync();
        // fmt::print("Sum C on s={:<5}: {:9.4f}\n", rank, std::accumulate(sumC.begin(), sumC.end(), 0.0));
        // fflush(stdout);
        env.Barrier();

        tHelper::GetSubViewBalanced(A, s0, P.q0, n, dimInfo.nl0, s2, P.q2, k, dimInfo.nl2) = A_su;
        tHelper::GetSubViewBalanced(B, s1, P.q1, m, dimInfo.nl1, s2, P.q2, k, dimInfo.nl2) = B_ut;
    }
};

template<typename... tSubEnvs>
struct MatMatCoreImpl<SyncLib::Environments::NoBSP, tSubEnvs...>
{
    template<typename tEnv, typename tMat>
    static void Compute(tEnv &env, const tMat &A_su, const tMat &B_ut, arma::mat &C_st,
                        const typename MatMatHelper<tEnv>::DimInfoContiguous &dimInfo, const ProcessorCube &P,
                        typename tEnv::template SendQueue<arma::uword, arma::vec> &sqC)
    {
        auto[s0, s1, s2] = P(env.Rank());

        C_st = A_su * B_ut.t();

        MatMatHelper<tEnv>::ReduceCContiuous(C_st, sqC, s2, s0 * P.q1 * P.q2 + s1 * P.q2, P.q2, dimInfo, env);
    }
};

template<typename... tSubEnvs>
struct MatMatCoreImpl<SyncLib::Environments::SharedMemoryBSP, tSubEnvs...>
{
    using tSubEnv = SyncLib::Environments::SharedMemoryBSP;
    using tHelper = MatMatHelper<tSubEnv>;

    static void MatMatShared(tSubEnv &env, const arma::mat &A, const arma::mat &B, arma::mat &C, const ProcessorCube &P)
    {
        auto[s0, s1, s2] = P(env.Rank());
        tSubEnv &env01 = P.Split2(env, s0, s1, s2);
        const tHelper::DimInfoContiguous dimInfo = tHelper::DimInfoContiguous::Create(A.n_rows, A.n_cols, B.n_rows, P, s0, s1, s2);

        arma::mat A_su = tHelper::GetSubViewBalanced(A, s0, P.q0, dimInfo.n, dimInfo.nl0, s2, P.q2, dimInfo.k, dimInfo.nl2);
        arma::mat B_ut = tHelper::GetSubViewBalanced(B, s1, P.q1, dimInfo.m, dimInfo.nl1, s2, P.q2, dimInfo.k, dimInfo.nl2);
        arma::mat C_st(A_su.n_rows, B_ut.n_rows);

        tSubEnv::SendQueue<arma::uword, arma::vec> sqC(env01);
        MatMatCoreImpl<tSubEnvs...>::Compute(env01, A_su, B_ut, C_st, dimInfo, P, sqC);

        auto sizeC = arma::size(C_st);

        const arma::uword iOffsetC = tHelper::OffsetGlobalBalanced(dimInfo.n, P.q0, s0);
        const arma::uword jOffsetC = tHelper::OffsetGlobalBalanced(dimInfo.m, P.q1, s1);

        for (arma::uword col = s2 * dimInfo.nCol01, colEnd = std::min<arma::uword>(col + dimInfo.nCol01, dimInfo.nl1);
             col < colEnd; ++col)
        {
            arma::vec subC(&C(iOffsetC, jOffsetC + col), dimInfo.nl0, false, true);
            arma::vec colC(&C_st(0, col), C_st.n_rows, false, true);
            subC = colC;
        }
    }

    template<typename tEnv, typename tMat>
    static void Compute(tEnv &env, const tMat &A_su, const tMat &B_ut, arma::mat &C_st,
                        const typename MatMatHelper<tEnv>::DimInfoContiguous &dimInfo, const ProcessorCube &P,
                        typename tEnv::template SendQueue<arma::uword, arma::vec> &sqC)
    {
        auto[s0, s1, s2] = P(env.Rank());

        using tSubEnv = SyncLib::Environments::SharedMemoryBSP;
        tSubEnv subEnv(2);
        ProcessorCube Psub(subEnv.Size(), 2, 1, 1);
        subEnv.Run(MatMatShared, A_su, B_ut, C_st, Psub);

        MatMatHelper<tEnv>::ReduceCContiuous(C_st, sqC, s2, s0 * P.q1 * P.q2 + s1 * P.q2, P.q2, dimInfo, env);
    }
};

#ifdef IS_WINDOWS
void Pause(bool pause = true)
{
    if (pause)
    {
        system("pause");
    }
}
#else
void Pause(bool)
{
}
#endif

template<typename tEnv>
void RunMatMatTest(tEnv &env, size_t n, size_t m, size_t k, size_t N, size_t M, size_t K, bool split, uint32_t iterations,
                   bool onlyMKL)
{
    ProcessorCube P(env.Size(), N, M, K);
    using tAlgorithm = MatMatAlgorithm<tEnv>;
    arma::mat A(n, m), B(k, m), C(n, k);

    if (!onlyMKL)
    {
        using tSubEnv = SyncLib::Environments::NoBSP;

        if (split)
        {
            env.Run(tAlgorithm::template MatMatTest<typename tAlgorithm::MatMatBenchSplit>, P, A, B, iterations);
        }
        else
        {
            env.Run(tAlgorithm::template MatMatTest<typename tAlgorithm::MatMatBench>, P, A, B, iterations);
        }
    }

    if (onlyMKL && env.Rank() == 0)
    {
        SyncLib::Util::LinearCongruentialRandom random(42);

        RandomizeMatrix(A, random);
        RandomizeMatrix(B, random);

        SyncLib::Util::Timer<>timer;

        for (size_t it = 0; it < iterations; ++it)
        {
            timer.Tic();
            C = A * B.t();
            fmt::print("Single-Threaded multiplication took {}s\n", timer.Toc());
        }

        fmt::print("Sum C sequential: {:9.4f}\n", arma::as_scalar(arma::sum(arma::sum(arma::abs(C), 0), 1)));
        fmt::print("Number of zeros (A, B, C): ({}, {}, {})\n",
                   A.n_elem - arma::nonzeros(A).eval().n_elem,
                   B.n_elem - arma::nonzeros(B).eval().n_elem,
                   C.n_elem - arma::nonzeros(C).eval().n_elem);
    }
}

int main(int argc, char **argv)
{
    SyncLib::Environments::DistributedBSP mpiEnv(argc, argv);

    /*if (mpiEnv.Rank() == 0)
    {
    system("pause");
    }

    mpiEnv.Barrier();*/

    Args args("Matrix-Matrix multiplication: C=AB");
    args.AddOptions(
    {
        {"n", "The number of rows in A, C", Option::U32(), "4096", "4096"},
        {"k", "The number of columns in A, rows in B", Option::U32(), "4096", "4096"},
        {"m", "The number of columns in B, C", Option::U32(), "4096", "4096"},
        {"N", "The divisor of n", Option::U32(), "2", "2",},
        {"K", "The divisor of k", Option::U32(), "2", "2",},
        {"M", "The divisor of m", Option::U32(), "2", "2",},
        {{"o", "only-mkl"}, "Run only with MKL", Option::Boolean(), "true", "false",},
        {{"s", "split"}, "Whether to split the environment into subset-environments", Option::Boolean(), "true", "true"},
        {"i", "The number of repetitions", Option::U32(), "1", "1"}
    });

    args.Parse(argc, argv);

    uint32_t n = args.GetOption("n");
    uint32_t m = args.GetOption("m");
    uint32_t k = args.GetOption("k");

    uint32_t N = args.GetOption("N");
    uint32_t M = args.GetOption("M");
    uint32_t K = args.GetOption("K");

    if (mpiEnv.Size() > 1)
    {
        RunMatMatTest(mpiEnv, n, m, k, N, M, K, args.GetOption("split"), args.GetOption("i"), args.GetOption("only-mkl"));
    }
    else
    {
        SyncLib::Environments::SharedMemoryBSP env(N * M * K);
        RunMatMatTest(env, n, m, k, N, M, K, args.GetOption("split"), args.GetOption("i"), args.GetOption("only-mkl"));
    }

    return EXIT_SUCCESS;
}


/**
 * MIT License
 * Copyright (c) 2018 Patrick Geneva
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <boost/filesystem.hpp>
#include <iostream>
#include <torch/data.h>
#include <torch/torch.h>

#include "data/CityscapesDataset.h"
#include "data/Comma10kDataset.h"
#include "network/models/UNetModel.h"

int main() {

  // See if we can use the GPU or should train using the CPU
  torch::DeviceType device_type;
  if (torch::cuda::is_available()) {
    std::cout << "CUDA available! Training on GPU." << std::endl;
    device_type = torch::kCUDA;
  } else {
    std::cout << "Training on CPU. Are you using the CUDA TorchLib??" << std::endl;
    device_type = torch::kCPU;
  }
  torch::Device device(device_type);

  // Load the data from disk
  std::string rootdir = "/media/patrick/DATA 02/SEGNET/comma10k/";
  // auto dataset =
  //     CityscapesDataset(rootdir, CityscapesDataset::ModeDataSplit::kTrain, CityscapesDataset::ModeGroundtruth::kSegmentation, true);
  auto dataset = Comma10kDataset(rootdir, Comma10kDataset::ModeDataSplit::kTrain, true);

  // Create the network (RGB image => class masks)
  size_t n_channels = 3;
  size_t n_classes = dataset.map_id2class.size();
  UNetModel model(n_channels, n_classes);
  std::cout << "num params: " << model->parameters(true).size() << std::endl;

  // Load from disk if we can
  std::string modelpath = "checkpoint-seg.pt";
  if (boost::filesystem::exists(modelpath)) {
    torch::load(model, modelpath);
  } else {
    std::cerr << "no checkpoint found on disk, creating new model." << std::endl;
  }
  model->to(device);

  // Create the log file for our loss values
  std::string logpath = "loss_history_seg.txt";
  std::ofstream loghist;
  loghist.open(logpath, std::ofstream::out | std::ofstream::app);

  // Finally convert it to a unique pointer dataloader
  auto dataset_mapped = dataset.map(torch::data::transforms::Stack<>());
  auto sampler = torch::data::samplers::RandomSampler(dataset.size().value());
  auto options = torch::data::DataLoaderOptions().enforce_ordering(false).batch_size(5).workers(30);
  auto data_loader = torch::data::make_data_loader(std::move(dataset_mapped), sampler, options);

  // Create the optimizer
  // torch::optim::SGD optimizer(model->parameters(), torch::optim::SGDOptions(0.01).momentum(0.5));
  torch::optim::Adam optimizer(model->parameters(), torch::optim::AdamOptions(0.0002));

  // Loop through our epochs
  size_t max_epochs = 100;
  for (size_t epoch = 1; epoch <= max_epochs; epoch++) {

    // Enable training mode
    model->train();

    // Loop through our batches of training data
    size_t batch_idx = 0;
    for (auto &batch : *data_loader) {

      // Send that data to the device
      auto d_rgb = batch.data.to(device);
      auto d_masks = batch.target.to(device);

      // Zero the gradient from the last run
      optimizer.zero_grad();

      // Send the data through and calc the loss
      auto output = model->forward(d_rgb);

      // There is not a cross entropy loss
      // We do a softmax first along the classes [N, classes, H, W]
      // Also note that our dataloader will add an extra dimension to the labels (so we squeese the class dim)
      // https://discuss.pytorch.org/t/c-loss-functions/27471/5
      d_masks = torch::squeeze(d_masks, 1);
      auto loss = torch::nll_loss2d(torch::log_softmax(output, 1), d_masks);
      loghist << std::fixed << std::setprecision(10) << loss.item<float>() << std::endl;

      // Back propagate!
      loss.backward();
      optimizer.step();

      // Print our the loss every once in a while
      if (batch_idx % 100 == 0) {

        // Debug printout
        size_t items_curr = batch_idx * batch.data.size(0);
        size_t items_total = dataset.size().value();
        std::cout << epoch << " - " << items_curr << "/" << items_total << " | loss = " << loss.item<float>() << std::endl;

        // Softmax the output to get our total class probabilities
        // Thus across all classes, our probabilities should sum to 1
        auto output_probs = torch::softmax(output, 1);

        // Plot the first image, need to change to opencv format [H,W,C]
        // Note that we arg max the softmax network output, then need to add an dimension
        // We scale up the 0..1 range back to the 0..255 that opencv expects (later cast to int)
        torch::Tensor cv_input = 255.0 * batch.data[0].permute({1, 2, 0}).clone().cpu();
        torch::Tensor cv_label = batch.target[0].permute({1, 2, 0}).clone().cpu();
        torch::Tensor cv_output = torch::unsqueeze(output_probs[0].argmax(0), 0).permute({1, 2, 0}).clone().cpu();

        // Convert them all to 0..255 ranges
        cv_input = cv_input.to(torch::kInt8);
        cv_label = cv_label.to(torch::kInt8);
        cv_output = cv_output.to(torch::kInt8);

        // Point the cv::Mats to the transformed locations in memory
        cv::Mat img_input(cv::Size((int)cv_input.size(1), (int)cv_input.size(0)), CV_8UC3, cv_input.data_ptr<int8_t>());
        cv::Mat img_label(cv::Size((int)cv_label.size(1), (int)cv_label.size(0)), CV_8UC1, cv_label.data_ptr<int8_t>());
        cv::Mat img_output(cv::Size((int)cv_output.size(1), (int)cv_output.size(0)), CV_8UC1, cv_output.data_ptr<int8_t>());

        // Convert labeled images to color
        cv::cvtColor(img_label, img_label, cv::COLOR_GRAY2BGR);
        cv::cvtColor(img_output, img_output, cv::COLOR_GRAY2BGR);
        // img_label = 255.0 / (double)n_classes * img_label;
        // img_output = 255.0 / (double)n_classes * img_output;

        // Change both to be colored like the comma10k
        img_label.forEach<cv::Vec3b>([&](cv::Vec3b &px, const int *pos) -> void { px = dataset.map_id2hex[(char)px[0]]; });
        img_output.forEach<cv::Vec3b>([&](cv::Vec3b &px, const int *pos) -> void { px = dataset.map_id2hex[(char)px[0]]; });

        // Finally, stack and display in a window
        cv::Mat outimg1, outimg2, outimg3;
        cv::hconcat(img_input, img_label, outimg1);
        cv::hconcat(img_input, img_output, outimg2);
        cv::vconcat(outimg1, outimg2, outimg3);
        cv::imshow("current status", outimg3);
        cv::waitKey(100);

        // Save the model to disk
        torch::save(model, modelpath);
      }
      batch_idx++;
    }
  }

  // Save model to file
  torch::save(model, modelpath);

  // Close file
  loghist.close();
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_RANK_TWO_UPDATE_INCLUDE
#define MTL_RANK_TWO_UPDATE_INCLUDE

#include <boost/numeric/mtl/operation/rank_one_update.hpp>

namespace mtl { namespace matrix {

/// Rank-two update: rank_two_update(A, x, y) computes A+= x * conj(y)^T + y * conj(x)^T
/** The current implementation works for column and row vectors (although
    the notation above refers to column vectors). **/
template <typename Matrix, typename VectorX, typename VectorY>
inline void rank_two_update(Matrix& matrix, const VectorX& x, const VectorY& y)
{
    rank_one_update(matrix, x, y);
    rank_one_update(matrix, y, x);
}

}} // namespace mtl::matrix

#endif // MTL_RANK_TWO_UPDATE_INCLUDE


/*******
edit_distance: STL and Boost compatible edit distance functions for C++

Copyright (c) 2013 Erik Erlandson

Author:  Erik Erlandson <erikerlandson@yahoo.com>

Distributed under the Boost Software License, Version 1.0.
See accompanying file LICENSE or copy at
http://www.boost.org/LICENSE_1_0.txt
*******/

#if !defined(BOOST_ALGORITHM_SEQUENCE_EDIT_DISTANCE_HPP)
#define BOOST_ALGORITHM_SEQUENCE_EDIT_DISTANCE_HPP

#include <boost/concept/requires.hpp>

#include <boost/algorithm/sequence/detail/edit_distance.hpp>

namespace boost {
namespace algorithm {
namespace sequence {

using boost::as_literal;
using boost::parameter::value_type;
using boost::range_value;
using boost::false_type;

using detail::SequenceAlignmentCost;
using detail::ForwardRangeConvertible;
using detail::edit_cost_impl;
using detail::cost_type;
using detail::none;
using detail::nonconst_default;
using detail::default_equal;

template <typename Sequence1, typename Sequence2, typename Output, typename Cost, typename Equal, typename AllowSub, typename MaxCost>
inline
BOOST_CONCEPT_REQUIRES(
    ((ForwardRangeConvertible<Sequence1>))
    ((ForwardRangeConvertible<Sequence2>))
    ((SequenceAlignmentCost<Cost, Sequence1>)),
(typename cost_type<Cost, typename range_value<Sequence1>::type>::type))
edit_distance_check(Sequence1 const& seq1, Sequence2 const& seq2, Output& output, const Cost& cost, const Equal& equal, const AllowSub& allow_sub, const MaxCost& max_cost, const bool max_cost_exception) {
    return edit_cost_impl(as_literal(seq1), as_literal(seq2), output, cost, equal, allow_sub, max_cost, max_cost_exception);
}

BOOST_PARAMETER_FUNCTION(
(typename cost_type<typename value_type<Args, parameter::tag::cost, unit_cost>::type,
                    typename range_value<typename value_type<Args, parameter::tag::sequence1>::type>::type>::type),
    edit_distance,
    parameter::tag,
    (required
        (sequence1, *)
        (sequence2, *))
    (optional
        (cost, *, unit_cost())
        (equal, *, default_equal())
        (in_out(script), *, nonconst_default<none>())
        (substitution, *, false_type())
        (max_cost, *, none())
        (max_cost_exception, (bool), false))
)
{
    return edit_distance_check(sequence1, sequence2, script, cost, equal, substitution, max_cost, max_cost_exception);
}

}}} // boost::algorithm::sequence

#endif


#include <boost/regex.hpp>
#include <iostream>

int main(int argc, const char * const argv[])
{
    std::string line;
    boost::regex pat( "^Subject: (Re: |Aw: )*(.*)" );

    std::vector<int> values;
    for (int i = 1; i < argc; ++i) {
        const std::string word(argv[i]);
        boost::smatch matches;
        if (boost::regex_match(line, matches, pat))
            std::cout << matches[2] << std::endl;
    }
}


/*!
 * @file     type_traits.hpp
 * @author   Gabriel Hottiger
 * @date     Oct, 2017
 * @brief    Traits for the supported non-STL types.
 */

#pragma once

// Eigen
#include <Eigen/Core>

// STL
#include <type_traits>

namespace parameter_handler {

namespace traits {

//----------------------------------- EIGEN traits -------------------------------------//

//! isEigenMatrix false type
template <typename ValueType_, typename Enable_ = void>
struct is_eigen_matrix : std::false_type {};

//! isEigenMatrix true type
template <typename ValueType_>
struct is_eigen_matrix<ValueType_, typename std::enable_if<std::is_base_of<Eigen::MatrixBase<ValueType_>, ValueType_>::value>::type>
    : std::true_type {};

//! isEigenMartix (All matrices/vectors except Eigen::Vector3 types) false type
template <typename ValueType_, typename Enable_ = void>
struct is_fixed_size_eigen_matrix : std::false_type {};

//! isEigenMartix (All matrices/vectors except Eigen::Vector3 types) true type
template <typename ValueType_>
struct is_fixed_size_eigen_matrix<
    ValueType_, typename std::enable_if<is_eigen_matrix<ValueType_>::value && ValueType_::RowsAtCompileTime != Eigen::Dynamic &&
                                        ValueType_::ColsAtCompileTime != Eigen::Dynamic>::type> : std::true_type {};

}  // end namespace traits

}  // end namespace parameter_handler


// -*- C++ -*-
//
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
//                                   Jiao Lin
//                      California Institute of Technology
//                         (C) 2007 All Rights Reserved
//
// {LicenseText}
//
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//

#include "mccomponents/kernels/sample/phonon/AtomicScatterer.h"

#include <boost/python.hpp>
#include "wrap_vector.h"

namespace wrap_mccomponents {

  void wrap_AtomicScatterer()
  {
    using namespace boost::python;

    typedef mccomponents::kernels::AtomicScatterer w_t;
    typedef w_t::float_t float_t;
    typedef w_t::R_t R_t;

    class_<w_t>
      ("AtomicScatterer",
       init<
       const R_t &, float_t
       > ()
       )
      .def_readwrite("coherent_scattering_length", &w_t::coherent_scattering_length)
      .def_readwrite("coherent_cross_section", &w_t::coherent_cross_section)
      .def_readwrite("incoherent_cross_section", &w_t::incoherent_cross_section)
      .def_readwrite("absorption_cross_section", &w_t::absorption_cross_section)
      ;

    wrap_vector2<w_t>( "AtomicScatterer" );
  }

}

// version
// $Id$

// End of file


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SLIDE_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SLIDE_HPP_INCLUDED

namespace boost { namespace simd
{
#if defined(DOXYGEN_ONLY)
  /*!

  **/
  template<std::size_t N, typename T> T slide(T const& a0, T const& a1) {}

  /// @overload
  template<std::size_t N, typename T> T slide(T const& a0) {}
#endif
} }

#include <boost/simd/function/scalar/slide.hpp>
#include <boost/simd/function/simd/slide.hpp>

#endif


/*
 * SlidingWindowIterator.hpp
 *
 *  Created on: Aug 17, 2017
 *      Author: Péter Fankhauser
 *   Institute: ETH Zurich, ANYbotics
 */

#pragma once

#include "grid_map_core/GridMap.hpp"
#include "grid_map_core/iterators/GridMapIterator.hpp"

#include <Eigen/Core>

namespace grid_map {

/*!
 * Iterator class to iterate trough the entire grid map with access to a layer's
 * data through a sliding window.
 * Note: This iterator only works with maps with zero start index.
 */
class SlidingWindowIterator : public GridMapIterator
{
public:

  enum class EdgeHandling {
    INSIDE, // Only visit indices that are surrounded by a full window.
    CROP, // Crop data matrix with missing cells at edges.
    EMPTY, // Fill in missing edges with empty cells (NAN-value).
    MEAN // Fill in missing edges with MEAN of valid values.
  };

  /*!
   * Constructor.
   * @param gridMap the grid map to iterate on.
   * @param layer the layer on which the data is accessed.
   * @param edgeHandling the method to handle edges of the map.
   * @param windowSize the size of the moving window in number of cells (has to be an odd number!).
   */
  SlidingWindowIterator(const GridMap& gridMap, const std::string& layer,
                        const EdgeHandling& edgeHandling = EdgeHandling::CROP,
                        const size_t windowSize = 3);

  /*!
   * Copy constructor.
   * @param other the object to copy.
   */
  SlidingWindowIterator(const SlidingWindowIterator* other);

  /*!
   * Set the side length of the moving window (in m).
   * @param gridMap the grid map to iterate on.
   * @param windowLength the side length of the window (in m).
   */
  void setWindowLength(const GridMap& gridMap, const double windowLength);

  /*!
   * Increase the iterator to the next element.
   * @return a reference to the updated iterator.
   */
  SlidingWindowIterator& operator ++();

  /*!
   * Return the data of the sliding window.
   * @return the data of the sliding window.
   */
  const Matrix getData() const;

private:
  //! Setup members.
  void setup(const GridMap& gridMap);

  //! Check if data for current index is fully inside map.
  bool dataInsideMap() const;

  //! Edge handling method.
  const EdgeHandling edgeHandling_;

  //! Data.
  const Matrix& data_;

  //! Size of the sliding window.
  size_t windowSize_;

  //! Size of the border of the window around the center cell.
  size_t windowMargin_;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

} /* namespace */


// Copyright (C) by Josh Blum. See LICENSE.txt for licensing information.

#include <boost/test/unit_test.hpp>
#include <iostream>

#include <gras/chrono.hpp>
#include <boost/thread/thread.hpp> //sleep

BOOST_AUTO_TEST_CASE(test_chrono_time)
{
    for (size_t trial_i = 0; trial_i < 100; trial_i++)
    {
        const double sleep_secs = 0.01 + trial_i*0.0001;
        gras::time_ticks_t t0 = gras::time_now();
        boost::this_thread::sleep(boost::posix_time::milliseconds(long(sleep_secs*1000)));
        gras::time_ticks_t t1 = gras::time_now();

        std::cout << "t0 " << t0 << std::endl;
        std::cout << "t1 " << t1 << std::endl;
        BOOST_CHECK(t1 > t0);

        const double delta_time = double(t1-t0)/gras::time_tps();
        std::cout << "delta_time " << delta_time << std::endl;
        const double low_margin = sleep_secs - sleep_secs/2;
        const double high_margin = sleep_secs + sleep_secs/2;
        BOOST_CHECK(delta_time < high_margin and delta_time > low_margin);
    }
}


/* Copyright (C) 2012-2020 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */

#include <NTL/ZZ.h>
#include <NTL/BasicThreadPool.h>
#include <helib/helib.h>
#include <NTL/lzz_pXFactoring.h>

#include <cstdio>

#include "gtest/gtest.h"
#include "test_common.h"

#include <helib/debugging.h>

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

  1. c1.multiplyBy(c0)
  2. c0 += random constant
  3. c2 *= random constant
  4. tmp = c1
  5. ea.shift(tmp, random amount in [-nSlots/2, nSlots/2])
  6. c2 += tmp
  7. ea.rotate(c2, random amount in [1-nSlots, nSlots-1])
  8. c1.negate()
  9. c3.multiplyBy(c2)
  10. c0 -= c3

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

namespace {

::testing::AssertionResult ciphertextMatches(const helib::EncryptedArray& ea,
                                             const helib::SecKey& sk,
                                             const helib::PlaintextArray& p,
                                             const helib::Ctxt& c)
{
  helib::PlaintextArray pp(ea);
  ea.decrypt(c, sk, pp);
  if (equals(ea, pp, p)) {
    return ::testing::AssertionSuccess();
  } else {
    return ::testing::AssertionFailure()
           << "Ciphertext does not match plaintext:" << std::endl
           << "p = " << p << std::endl
           << "pp = " << pp << std::endl;
  }
}

struct Parameters
{
  Parameters(long R,
             long p,
             long r,
             long d,
             long c,
             long k,
             long L,
             long s,
             long m,
             NTL::Vec<long> mvec,
             NTL::Vec<long> gens,
             NTL::Vec<long> ords,
             long seed,
             long nt) :
      R(R),
      p(p),
      r(r),
      d(d),
      c(c),
      k(k),
      L(L),
      s(s),
      m(m),
      mvec(mvec),
      gens(gens),
      ords(ords),
      seed(seed),
      nt(nt){};

  long R; // number of rounds
  long p; // plaintext base
  long r; // lifting
  long d; // degree of the field extension
  // Note: d == 0 => factors[0] defines extension
  long c;              // number of columns in the key-switching matrices
  long k;              // security parameter
  long L;              // # of bits in the modulus chain
  long s;              // minimum number of slots
  long m;              // use specified value as modulus
  NTL::Vec<long> mvec; // use product of the integers as modulus
  // Note: mvec takes priority over m
  NTL::Vec<long> gens; // use specified vector of generators
  NTL::Vec<long> ords; // use specified vector of orders
  // e.g., ords=[4 2 -4], negative means 'bad'
  long seed; // PRG seed
  long nt;   // num threads

  // Let googletest know how to print the Parameters
  friend std::ostream& operator<<(std::ostream& os, const Parameters& params)
  {
    return os << "{"
              << "R=" << params.R << ","
              << "p=" << params.p << ","
              << "r=" << params.r << ","
              << "d=" << params.d << ","
              << "c=" << params.c << ","
              << "k=" << params.k << ","
              << "L=" << params.L << ","
              << "s=" << params.s << ","
              << "m=" << params.m << ","
              << "mvec=" << params.mvec << ","
              << "gens=" << params.gens << ","
              << "ords=" << params.ords << ","
              << "seed=" << params.seed << ","
              << "nt=" << params.nt << "}";
  };
};

class GTestGeneral : public ::testing::TestWithParam<Parameters>
{
protected:
  long R;
  long p;
  long r;
  long d;
  long c;
  long k;
  long w;
  long L;
  long m;

  std::vector<long> gens;
  std::vector<long> ords;

  helib::Context context;

  helib::SecKey secretKey;

  const helib::PubKey& publicKey;

  GTestGeneral() :
      R(GetParam().R),
      p(GetParam().p),
      r(GetParam().r),
      d(GetParam().d),
      c(GetParam().c),
      k(GetParam().k),
      w(64),
      L(GetParam().L),
      m(helib::FindM(k,
                     L,
                     c,
                     p,
                     d,
                     GetParam().s,
                     GetParam().mvec.length() > 0
                         ? helib::computeProd(GetParam().mvec)
                         : GetParam().m,
                     true)),
      gens(helib::convert<std::vector<long>, NTL::Vec<long>>(GetParam().gens)),
      ords(helib::convert<std::vector<long>, NTL::Vec<long>>(GetParam().ords)),
      context(m, p, r, gens, ords),
      secretKey((buildModChain(context, L, c), context)),
      publicKey(secretKey)
  {}

  virtual void SetUp() override
  {
    NTL::SetSeed(NTL::ZZ(GetParam().seed));
    NTL::SetNumThreads(GetParam().nt);
    secretKey.GenSecKey(w); // A Hamming-weight-w secret key
    helib::addSome1DMatrices(
        secretKey); // compute key-switching matrices that we need

    helib::setupDebugGlobals(&secretKey, context.ea);
  };

  virtual void TearDown() override { helib::cleanupDebugGlobals(); }
};

TEST_P(GTestGeneral, correctlyImplementsMixOfOperationsOverFourCiphertexts)
{
  char buffer[32];
  if (!helib_test::noPrint) {
    std::cout << "\n\n******** " << (helib_test::dry ? "(dry run):" : ":");
    std::cout << " R=" << R << ", p=" << p << ", r=" << r << ", d=" << d
              << ", c=" << c << ", k=" << k << ", w=" << w << ", L=" << L
              << ", m=" << m << ", gens=" << helib::vecToStr(gens)
              << ", ords=" << helib::vecToStr(ords) << std::endl;
  }

  NTL::ZZX G;
  if (d == 0)
    G = context.alMod.getFactorsOverZZ()[0];
  else
    G = helib::makeIrredPoly(p, d);

  if (!helib_test::noPrint) {
    context.zMStar.printout();
    std::cout << std::endl;

    std::cout << "security=" << context.securityLevel() << std::endl;
    std::cout << "# small primes = " << context.smallPrimes.card() << "\n";
    std::cout << "# ctxt primes = " << context.ctxtPrimes.card() << "\n";
    std::cout << "# bits in ctxt primes = "
              << long(context.logOfProduct(context.ctxtPrimes) / log(2.0) + 0.5)
              << "\n";
    std::cout << "# special primes = " << context.specialPrimes.card() << "\n";
    std::cout << "# bits in special primes = "
              << long(context.logOfProduct(context.specialPrimes) / log(2.0) +
                      0.5)
              << "\n";
    std::cout << "G = " << G << "\n";
  }
  std::shared_ptr<helib::EncryptedArray> ea_ptr =
      std::make_shared<helib::EncryptedArray>(context, G);
  helib::EncryptedArray& ea(*ea_ptr);
  long nslots = ea.size();

  // Debugging additions
  helib::setupDebugGlobals(&secretKey, ea_ptr);

  helib::PlaintextArray p0(ea);
  helib::PlaintextArray p1(ea);
  helib::PlaintextArray p2(ea);
  helib::PlaintextArray p3(ea);

  helib::random(ea, p0);
  helib::random(ea, p1);
  helib::random(ea, p2);
  helib::random(ea, p3);

  helib::Ctxt c0(publicKey), c1(publicKey), c2(publicKey), c3(publicKey);
  ea.encrypt(c0, publicKey, p0);
  // {ZZX ppp0; ea.encode(ppp0, p0); c0.DummyEncrypt(ppp0);} // dummy encryption
  ea.encrypt(c1, publicKey, p1); // real encryption
  ea.encrypt(c2, publicKey, p2); // real encryption
  ea.encrypt(c3, publicKey, p3); // real encryption

  helib::resetAllTimers();

  HELIB_NTIMER_START(Circuit);

  for (long i = 0; i < R; i++) {

    if (!helib_test::noPrint)
      std::cout << "*** round " << i << "..." << std::endl;

    long shamt = NTL::RandomBnd(2 * (nslots / 2) + 1) - (nslots / 2);
    // random number in [-nslots/2..nslots/2]
    long rotamt = NTL::RandomBnd(2 * nslots - 1) - (nslots - 1);
    // random number in [-(nslots-1)..nslots-1]

    // two random constants
    helib::PlaintextArray const1(ea);
    helib::PlaintextArray const2(ea);
    helib::random(ea, const1);
    helib::random(ea, const2);

    NTL::ZZX const1_poly, const2_poly;
    ea.encode(const1_poly, const1);
    ea.encode(const2_poly, const2);

    mul(ea, p1, p0); // c1.multiplyBy(c0)
    c1.multiplyBy(c0);
    if (!helib_test::noPrint)
      CheckCtxt(c1, "c1*=c0");
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p1, c1));

    add(ea, p0, const1); // c0 += random constant
    c0.addConstant(const1_poly);
    if (!helib_test::noPrint)
      CheckCtxt(c0, "c0+=k1");
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p0, c0));

    mul(ea, p2, const2); // c2 *= random constant
    c2.multByConstant(const2_poly);
    if (!helib_test::noPrint)
      CheckCtxt(c2, "c2*=k2");
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p2, c2));

    helib::PlaintextArray tmp_p(p1); // tmp = c1
    helib::Ctxt tmp(c1);
    sprintf(buffer, "tmp=c1>>=%d", (int)shamt);
    shift(ea,
          tmp_p,
          shamt); // ea.shift(tmp, random amount in [-nSlots/2,nSlots/2])
    ea.shift(tmp, shamt);
    if (!helib_test::noPrint)
      CheckCtxt(tmp, buffer);
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, tmp_p, tmp));

    add(ea, p2, tmp_p); // c2 += tmp
    c2 += tmp;
    if (!helib_test::noPrint)
      CheckCtxt(c2, "c2+=tmp");
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p2, c2));

    sprintf(buffer, "c2>>>=%d", (int)rotamt);
    rotate(ea,
           p2,
           rotamt); // ea.rotate(c2, random amount in [1-nSlots, nSlots-1])
    ea.rotate(c2, rotamt);
    if (!helib_test::noPrint)
      CheckCtxt(c2, buffer);
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p2, c2));

    ::helib::negate(ea, p1); // c1.negate()
    c1.negate();
    if (!helib_test::noPrint)
      CheckCtxt(c1, "c1=-c1");
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p1, c1));

    mul(ea, p3, p2); // c3.multiplyBy(c2)
    c3.multiplyBy(c2);
    if (!helib_test::noPrint)
      CheckCtxt(c3, "c3*=c2");
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p3, c3));

    sub(ea, p0, p3); // c0 -= c3
    c0 -= c3;
    if (!helib_test::noPrint)
      CheckCtxt(c0, "c0=-c3");
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p0, c0));
  }

  c0.cleanUp();
  c1.cleanUp();
  c2.cleanUp();
  c3.cleanUp();

  HELIB_NTIMER_STOP(Circuit);

  if (!helib_test::noPrint) {
    std::cout << std::endl;
    helib::printAllTimers();
    std::cout << std::endl;
  }
  helib::resetAllTimers();
  HELIB_NTIMER_START(Check);

  EXPECT_TRUE(ciphertextMatches(ea, secretKey, p0, c0));
  EXPECT_TRUE(ciphertextMatches(ea, secretKey, p1, c1));
  EXPECT_TRUE(ciphertextMatches(ea, secretKey, p2, c2));
  EXPECT_TRUE(ciphertextMatches(ea, secretKey, p3, c3));

  HELIB_NTIMER_STOP(Check);

  std::cout << std::endl;
  if (!helib_test::noPrint) {
    helib::printAllTimers();
    std::cout << std::endl;
  }
}

// clang-format off
INSTANTIATE_TEST_SUITE_P(variousParameters, GTestGeneral, ::testing::Values(
    //         R, p, r, d, c,  k,   L, s,  m,             mvec,             gens,             ords, seed, nt
    //DEEP
    Parameters(1, 2, 2, 1, 2, 10, 500, 0, 91, NTL::Vec<long>{}, NTL::Vec<long>{}, NTL::Vec<long>{},    0,  1),
    Parameters(1, 2, 1, 2, 2, 10, 500, 0, 91, NTL::Vec<long>{}, NTL::Vec<long>{}, NTL::Vec<long>{},    0,  1),
    Parameters(2, 7, 2, 1, 2, 10, 500, 0, 91, NTL::Vec<long>{}, NTL::Vec<long>{}, NTL::Vec<long>{},    0,  1)
    //FAST
    //Parameters(1, 2, 1, 1, 2, 80, 500, 0, 91, NTL::Vec<long>{}, NTL::Vec<long>{}, NTL::Vec<long>{},    0,  1)
    ));
// clang-format on

// parameters to get an example where phi(m) is very
// close to m:
// m=18631 L=10 R=5

} // anonymous namespace