// Copyright Oleg Maximenko 2014.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://github.com/svgpp/svgpp for library home page.

#pragma once

#include <svgpp/definitions.hpp>
#include <svgpp/detail/adapt_context.hpp>
#include <boost/optional.hpp>
#include <boost/noncopyable.hpp>
#include <boost/mpl/set.hpp>

namespace svgpp
{

typedef boost::mpl::set4<tag::attribute::cx, tag::attribute::cy, tag::attribute::rx, tag::attribute::ry>
  ellipse_shape_attributes;

template<class Length>
class collect_ellipse_attributes_adapter: boost::noncopyable
{
public:
  template<class Context>
  bool on_exit_attributes(Context & context) const
  {
    typedef detail::unwrap_context<Context, tag::error_policy> error_policy;
    typedef detail::unwrap_context<Context, tag::length_policy> length_policy_context;
    typedef typename length_policy_context::policy length_policy_t;

    typename length_policy_t::length_factory_type & converter
      = length_policy_t::length_factory(length_policy_context::get(context));

    typename length_policy_t::length_factory_type::coordinate_type
      cx = 0, cy = 0, rx = 0, ry = 0;
    if (cx_)
      cx = converter.length_to_user_coordinate(*cx_, tag::length_dimension::width());
    if (cy_)
      cy = converter.length_to_user_coordinate(*cy_, tag::length_dimension::height());
    if (rx_ && ry_) // required attributes
    {
      rx = converter.length_to_user_coordinate(*rx_, tag::length_dimension::width());
      if (rx < 0)
        return error_policy::policy::negative_value(error_policy::get(context), tag::attribute::rx());
      if (rx == 0)
        return true;
      ry = converter.length_to_user_coordinate(*ry_, tag::length_dimension::height());
      if (ry < 0)
        return error_policy::policy::negative_value(error_policy::get(context), tag::attribute::ry());
      if (ry == 0)
        return true;
      typedef detail::unwrap_context<Context, tag::basic_shapes_events_policy> basic_shapes_events;
      basic_shapes_events::policy::set_ellipse(basic_shapes_events::get(context), cx, cy, rx, ry);
    }
    return true;
  }

  void set(tag::attribute::cx, Length const & val) { cx_ = val; }
  void set(tag::attribute::cy, Length const & val) { cy_ = val; }
  void set(tag::attribute::rx, Length const & val) { rx_ = val; }
  void set(tag::attribute::ry, Length const & val) { ry_ = val; }

private:
  boost::optional<Length> cx_, cy_, rx_, ry_;
};

struct ellipse_to_path_adapter
{
  template<class Context, class Coordinate>
  static void set_ellipse(Context & context, Coordinate cx, Coordinate cy, Coordinate rx, Coordinate ry)
  {
    typedef detail::unwrap_context<Context, tag::path_events_policy> path_events;

    typename path_events::type & path_context = path_events::get(context);
    path_events::policy::path_move_to(path_context, cx + rx, cy, tag::coordinate::absolute());
    path_events::policy::path_elliptical_arc_to(path_context,
      rx, ry, Coordinate(0), false, true, cx - rx, cy, tag::coordinate::absolute());
    path_events::policy::path_elliptical_arc_to(path_context,
      rx, ry, Coordinate(0), false, true, cx + rx, cy, tag::coordinate::absolute());
    path_events::policy::path_close_subpath(path_context);
    path_events::policy::path_exit(path_context);
  }
};

}


#include "FHE.h"
#include <timing.h>
#include <EncryptedArray.h>
#include <NTL/lzz_pXFactoring.h>
#include <vector>
#include <cassert>
#include <cstdio>
#include <iostream>
#include <string>
#include <string.h>
#include <fstream>
#include <vector>
#include "NTL/ZZ_p.h"
#include <NTL/ZZ_pX.h>
#include <NTL/vec_ZZ_p.h>
#include <assert.h>
#include <stdlib.h>

int main(int argc, const char *argv[])
{
    long m=0, p=127, r=1; // Native plaintext space
    // Computations will be 'modulo p'
    long L;          // Levels
    long c=2;           // Columns in key switching matrix
    long w=64;          // Hamming weight of secret key
    long d=1;
    long security;
    long s;
    long bnd;

    ZZX G;

    return 0;
}


#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <boost/foreach.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <tf/message_filter.h>
#include <boost/program_options.hpp>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <boost/circular_buffer.hpp>
#include <ndt_offline/convertvelodynebags.h>
#include <eigen_conversions/eigen_msg.h>
#include <angles/angles.h>
#include <velodyne_msgs/VelodyneScan.h>
#include <ros/package.h>
using namespace std;
namespace po = boost::program_options;
bool create_odom=true,create_tf=true;
bool interactive;
// This will underestimate the yaw.
double getYawFromXY(const geometry_msgs::Pose &prev,
                    const geometry_msgs::Pose &curr) {
  double dx = curr.position.x - prev.position.x;
  double dy = curr.position.y - prev.position.y;
  return atan2(dy,dx);
}

double getDistBetween(const geometry_msgs::Pose &p1,
                      const geometry_msgs::Pose &p2) {
  double dx = p1.position.x - p2.position.x;
  double dy = p1.position.y - p2.position.y;
  return sqrt(dx*dx+dy*dy);
}

double getYawUsingPoses(const boost::circular_buffer<geometry_msgs::Pose> &poses,
                        const geometry_msgs::Pose &pose,
                        double min_dist,
                        bool &valid_yaw) {
  double min_val =  std::numeric_limits<double>::max();
  int min_idx = -1;
  for (int i = 0; i < poses.size(); i++) {
    double val = getDistBetween(poses[i], pose);
    if (val > min_dist) {
      if (val < min_val) {
        min_val = val;
        min_idx = i;
      }
    }
  }

  //  std::cout << "[" << min_val << "]" << std::flush;

  if (min_idx < 0) {
    valid_yaw = false;
    return 0.;
  }

  valid_yaw = true;
  return getYawFromXY(poses[min_idx], pose);
}

geometry_msgs::Pose getPose(double x, double y, double yaw) {
  geometry_msgs::Pose pose;
  pose.position.x = x;
  pose.position.y = y;
  pose.position.z = 0.0;

  tf::Quaternion q = tf::createQuaternionFromYaw(yaw);
  pose.orientation.x = q.x();
  pose.orientation.y = q.y();
  pose.orientation.z = q.z();
  pose.orientation.w = q.w();
  return pose;
}

geometry_msgs::Transform getTransform(double x, double y, double yaw) {
  geometry_msgs::Transform t;
  t.translation.x = x;
  t.translation.y = y;
  t.translation.z = 0.;

  tf::Quaternion q = tf::createQuaternionFromYaw(yaw);
  t.rotation.x = q.x();
  t.rotation.y = q.y();
  t.rotation.z = q.z();
  t.rotation.w = q.w();
  return t;
}

// Return euler angles - roll, pitch, yaw...
void get_euler_angles(Eigen::Vector3d &euler, const geometry_msgs::PoseStamped &p) {

  // Set yaw from the current odometry... the current EKF pose utilize the GPS heading for the yaw, the roll and pitch are directly taken from the IMU.
  Eigen::Quaterniond q;
  tf::quaternionMsgToEigen(p.pose.orientation, q);
  // Need a rotation matrix
  Eigen::Matrix3d rmat = q.matrix();
  euler = rmat.eulerAngles(0, 1, 2);

  if (fabs(euler[0]) > M_PI/2) {
    euler[0] += M_PI;
    euler[1] += M_PI;
    euler[2] += M_PI;

    euler[0] = angles::normalize_angle(euler[0]);
    euler[1] = angles::normalize_angle(euler[1]);
    euler[2] = angles::normalize_angle(euler[2]);
  }
}
void GetSensorPose(const std::string &dataset,  Eigen::Vector3d & transl,  Eigen::Vector3d &euler,tf::Transform &tf_sensor){

  tf::Quaternion quat;

  bool found_sensor_pose=false;
  if(dataset.compare("oru-basement")==0){
    transl[0]=0.3;
    transl[1]=0;
    transl[2]=0;
    euler[0]=0;
    euler[1]=0;
    euler[2]=-1.62;
    found_sensor_pose=true;
  }
  else if(dataset.compare("default")==0){
    transl[0]=0;
    transl[1]=0;
    transl[2]=0;
    euler[0]=0;
    euler[1]=0;
    euler[2]=0;
    found_sensor_pose=true;
  }
  else if(dataset.compare("arla-2012")==0){
    transl[0]=1.18;
    transl[1]=-0.3;
    transl[2]=2.0;
    euler[0]=0;
    euler[1]=0;
    euler[2]=-1.62;
    found_sensor_pose=true;
  }
  quat.setRPY(euler[0], euler[1], euler[2]);
  tf::Vector3 trans(transl[0], transl[1], transl[2]);
  tf_sensor  = tf::Transform(quat,trans);

  if(found_sensor_pose)
    cout<<"Sensor settings for dataset : \""<<dataset<<"\" is used"<<endl;
  cout<<"Sensor offset (x,y,z)=("<<transl(0)<<","<<transl(1)<<","<<transl(2)<<")"<<endl<<"Sensor angles (r,p,y)=("<<euler(0)<<","<<euler(1)<<","<<euler(2)<<")"<<endl;
  if (interactive) {
  cout<<"Are these the correct settings? y/n"<<endl;

  char c=getchar();
  getchar();
  if(!( c=='y'||c=='Y'))
    exit(0);
}
}
void update_roll_pitch(Eigen::Affine3d &t, Eigen::Vector3d euler, double yaw) {

  // Set yaw from the current odometry... the current EKF pose utilize the GPS heading for the yaw, the roll and pitch are directly taken from the IMU.
  t.linear() = (Eigen::AngleAxisd(euler[0], Eigen::Vector3d::UnitX())*Eigen::AngleAxisd(euler[1], Eigen::Vector3d::UnitY())*Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())).toRotationMatrix();
}

// This is targeted to create imu data messages from odometry / encoder readings...
int main(int argc, char **argv){

  ros::Time::init();
  Eigen::Vector3d transl;
  Eigen::Vector3d euler;
  tf::Transform tf_sensor_pose;
  string inbag_name, outbag_name, dataset;
  std::string velodyne_config_file,velodyne_packets_topic,velodyne_frame_id,base_link_id,gt_base_link_id,tf_topic,tf_world_frame,interpolation_link_id;
  double min_dist;
  double min_range=0.6,max_range=30;
  Eigen::Vector3d imu_offset_vec;;
  po::options_description desc("Allowed options");
  desc.add_options()
      ("help", "produce help message")
      ("debug", "additional output")
      ("inbag", po::value<string>(&inbag_name)->required(), "bag file to be used")
      ("velodyne_config_file", po::value<std::string>(&velodyne_config_file)->default_value( ros::package::getPath("ndt_offline")+std::string("/config/velo32.yaml")), "configuration file for the scanner")
      ("velodyne_packets_topic", po::value<std::string>(&velodyne_packets_topic)->default_value(std::string("/velodyne_packets")), "velodyne packets topic used")
      ("velodyne_frame_id", po::value<std::string>(&velodyne_frame_id)->default_value(std::string("/velodyne")), "frame_id of the velodyne")
      ("tf_base_link", po::value<std::string>(&base_link_id)->default_value(std::string("/odom_base_link")), "tf_base_link")
      ("tf_gt_link", po::value<std::string>(&gt_base_link_id)->default_value(std::string("/state_base_link")), "tf ground truth link")
      ("tf_world_frame", po::value<std::string>(&tf_world_frame)->default_value(std::string("/world")), "tf world frame")
      ("tf_topic", po::value<std::string>(&tf_topic)->default_value(std::string("/tf")), "tf topic to listen to")
      ("min_range", po::value<double>(&min_range)->default_value(0.7), "minimum range used from scanner")
      ("max_range", po::value<double>(&max_range)->default_value(30), "minimum range used from scanner")
      ("interpolation_link", po::value<std::string>(&interpolation_link_id)->default_value("/state_base_link"), "id in which lidar is interpolated")
      ("x", po::value<double>(&transl[0])->default_value(0.), "sensor pose - translation vector x")
      ("y", po::value<double>(&transl[1])->default_value(0.), "sensor pose - translation vector y")
      ("z", po::value<double>(&transl[2])->default_value(0.), "sensor pose - translation vector z")
      ("ex", po::value<double>(&euler[0])->default_value(0.), "sensor pose - euler angle vector x")
      ("ey", po::value<double>(&euler[1])->default_value(0.), "sensor pose - euler angle vector y")
      ("ez", po::value<double>(&euler[2])->default_value(0.), "sensor pose - euler angle vector z")
      ("data-set", po::value<string>(&dataset)->default_value(std::string("arla-2012")), "choose which dataset that is currently used, this option will assist with assigning the sensor pose")
      ("create_images", "create depth and intensity images instead of point clouds")
      ("interactive", "interactive mode")
      ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("help")) {
    cout << desc << "\n";
    return 1;
  }
  interactive = vm.count("interactive");

  cout<<"Point cloud will be filtered according to:\n min="<<min_range<<" < range < max="<<max_range<<endl;
  if (interactive) { std::cout <<"Continue? y/n";
    char c=getchar();
    getchar();
    if(!( c=='y'||c=='Y')){
      cout << desc << "\n";
      exit(0);
    }
    std::cout << "here..." << std::endl;
  }
  cout<<"Never run SLAM with data interpolated in GT frame."<<endl;
  cout<<"Data will be interpolated in link: "<<interpolation_link_id<< std::endl;
  if (interactive)
  {
    std::cout << "\n is this the correct link id? y/n"<<endl;
    char c=getchar();
    getchar();
    if(!( c=='y'||c=='Y')){
      cout << desc << "\n";
      exit(0);
    }
  }

  bool create_images = vm.count("create_images");

  rosbag::Bag outbag;
  //bag.open(inbag_name, rosbag::bagmode::Read);
  outbag_name = inbag_name + std::string("_edited.bag");
  GetSensorPose(dataset,transl,euler,tf_sensor_pose);

  tf::StampedTransform sensor_link;
  sensor_link.child_frame_id_ = velodyne_frame_id;
  sensor_link.frame_id_ = interpolation_link_id;//base_link_id;//tf_base_link; //"/odom_base_link";
  sensor_link.setData(tf_sensor_pose);
  //vmc_navserver::VMCEncodersStamped prev_enc_msg;
  ConvertVelodyneBagsToPcl reader(outbag_name,
                                  velodyne_config_file,
                                  inbag_name,
                                  velodyne_packets_topic,  //"/velodyne_packets"
                                  velodyne_frame_id,
                                  tf_world_frame,
                                  tf_topic,
                                  ros::Duration(3600),
                                  &sensor_link, max_range, min_range,
                                  0);

  pcl::PointCloud<pcl::PointXYZ> cloud;
  tf::Transform tf_scan_source;
  tf::Transform tf_gt_base;
  Eigen::Affine3d Todom_base_prev,Tgt_base_prev;
  bool run = true;
  while (run) {

    if (create_images)
      run = reader.ConvertToCompleteImages(tf_scan_source);
    else
      run = reader.ConvertToPclBag(tf_scan_source);
  }

  // while(reader.readMultipleMeasurements(1,cloud,tf_scan_source,tf_gt_base,base_link_id)){

  //}
  reader.CloseOutputBag();

  // -----------------------------------------------------------------------------

  /*if (m.getTopic() == std::string(topic_prefix + "/vmc_navserver/odom")) {
      nav_msgs::Odometry::ConstPtr msg = m.instantiate<nav_msgs::Odometry>();
      if (msg != NULL) {
        //    get_euler_angles(odom_euler, msg->pose.pose);

      }
  }*/
  //outbag.write(m.getTopic(), m.getTime(), m);

  return 0;
}


#ifndef STAN_MATH_PRIM_MAT_PROB_CATEGORICAL_LOGIT_LOG_HPP
#define STAN_MATH_PRIM_MAT_PROB_CATEGORICAL_LOGIT_LOG_HPP

#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/prob/categorical_logit_lpmf.hpp>
#include <boost/math/tools/promotion.hpp>
#include <vector>

namespace stan {
  namespace math {

    /**
     * @deprecated use categorical_logit_lpmf
     */
    template <bool propto,
              typename T_prob>
    typename boost::math::tools::promote_args<T_prob>::type
    categorical_logit_log(int n,
                          const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>&
                          beta) {
      return categorical_logit_lpmf<propto, T_prob>(n, beta);
    }

    /**
     * @deprecated use categorical_logit_lpmf
     */
    template <typename T_prob>
    inline
    typename boost::math::tools::promote_args<T_prob>::type
    categorical_logit_log(int n,
                          const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>&
                          beta) {
      return categorical_logit_lpmf<T_prob>(n, beta);
    }

    /**
     * @deprecated use categorical_logit_lpmf
     */
    template <bool propto,
              typename T_prob>
    typename boost::math::tools::promote_args<T_prob>::type
    categorical_logit_log(const std::vector<int>& ns,
                          const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>&
                          beta) {
      return categorical_logit_lpmf<propto, T_prob>(ns, beta);
    }

    /**
     * @deprecated use categorical_logit_lpmf
     */
    template <typename T_prob>
    inline
    typename boost::math::tools::promote_args<T_prob>::type
    categorical_logit_log(const std::vector<int>& ns,
                          const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>&
                          beta) {
      return categorical_logit_lpmf<T_prob>(ns, beta);
    }

  }
}
#endif


#pragma once

#include "SimpleHumanModel.hpp"
#include "SimpleManipulator.hpp"
#include "coma/Core"
#include <Eigen/Core>
#include <ostream>
#include <RBDyn/EulerIntegration.h>

struct Model {
    std::string name;
    rbd::MultiBody mb;
    rbd::MultiBodyConfig mbc;
    rbd::MultiBodyGraph mbg;
};

template <size_t Order>
struct ModelInfo {
    static constexpr int ord = Order >= 2 ? Order : 2;

    ModelInfo(int nrStep, double step)
        : model()
        , nt(nrStep)
        , dt(step)
        , t(Eigen::VectorXd::LinSpaced(nt, 0., (nt - 1) * dt))
    {
    }

    void init(const std::string& modelName, size_t nrJoints = 5)
    {
        model.name = modelName;
        if (modelName == "human") {
            auto [a, b, c] = makeHumanBody();
            model.mb = a;
            model.mbc = b;
            model.mbg = c;
        } else if (modelName == "manipulator") {
            auto [a, b, c] = makeManipulator(nrJoints);
            model.mb = a;
            model.mbc = b;
            model.mbg = c;
        }
        const auto& mb = model.mb;
        auto& mbc = model.mbc;

        q.resize(model.mb.nrParams(), nt);
        dqs.resize(ord);
        std::fill(dqs.begin(), dqs.end(), Eigen::MatrixXd(model.mb.nrDof(), nt));

        q.col(0) = rbd::paramToVector(mb, mbc.q);
        // Compute velocity as a * sin(wt + b)
        Eigen::ArrayXd a = Eigen::ArrayXd::Random(mb.nrDof());
        Eigen::ArrayXd b = Eigen::ArrayXd::Random(mb.nrDof());
        Eigen::ArrayXd w = Eigen::ArrayXd::Random(mb.nrDof());
        for (int i = 0; i < nt; ++i) {
            size_t n = ord / 2;
            for (size_t k = 0; k < n; ++k) {
                dqs[2 * k].col(i) = std::pow(-1., k) * a * w.pow(2 * k) * (w * t(i) + b).sin();
                dqs[2 * k + 1].col(i) = std::pow(-1., k) * a * w.pow(2 * k + 1) * (w * t(i) + b).cos();
            }
            if (ord % 2 == 1) {
                dqs[ord - 1].col(i) = std::pow(-1., n) * a * w.pow(2 * n) * (w * t(i) + b).sin();
            }
        }
        mbc.alpha = rbd::vectorToDof(mb, dqs[0].col(0));
        mbc.alphaD = rbd::vectorToDof(mb, dqs[1].col(0));

        for (int i = 0; i < nt - 1; ++i) {
            auto alpha = rbd::vectorToDof(mb, dqs[0].col(i));
            auto alphaD = rbd::vectorToDof(mb, dqs[1].col(i));
            for (int j = 0; j < mb.nrJoints(); ++j)
                rbd::eulerJointIntegration(mb.joint(j).type(), alpha[j], alphaD[j], dt, mbc.q[j]);

            q.col(i + 1) = rbd::paramToVector(mb, mbc.q);
        }
        mbc.q = rbd::vectorToParam(mb, q.col(0));
    }

    Eigen::VectorXd getAleph(int t) const
    {
        const auto& jointPosInDof = model.mb.jointsPosInDof();
        const auto& factors = coma::factorial_factors<double, ord>;
        Eigen::VectorXd v(ord * model.mb.nrDof());
        int curOrderPos = 0;
        for (int i = 0; i < model.mb.nrJoints(); ++i) {
            int dof = model.mb.joint(i).dof();
            for (size_t k = 0; k < ord; ++k) {
                v.segment(curOrderPos + k * dof, dof) = dqs[k].col(t).segment(jointPosInDof[i], dof) / factors[k];
            }
            curOrderPos += ord * dof;
        }

        return v;
    }

    friend std::ostream& operator<<(std::ostream& os, const ModelInfo& info)
    {
        const auto& mb = info.model.mb;
        os << "\nModel description"
           << "\n\tModel name:           " << info.model.name
           << "\n\tNr joints and bodies: " << mb.nrJoints()
           << "\n\tNr joint parameters:  " << mb.nrParams()
           << "\n\tNr DoF:               " << mb.nrDof();
        return os;
    }

    Model model;
    int nt;
    double dt;
    Eigen::VectorXd t;
    Eigen::MatrixXd q;
    std::vector<Eigen::MatrixXd> dqs;
};


/* Copyright 2016 Ramakrishnan Kannan */

#ifndef DISTNMF_AUNMF_HPP_
#define DISTNMF_AUNMF_HPP_

#include <mpi.h>
#include <armadillo>
#include <string>
#include <vector>
#include <cstdio>
#include <cmath>
#include "distnmf/distnmf.hpp"
#include "distnmf/mpicomm.hpp"

/**
 * There are totally prxpc process.
 * Each process will hold the following
 * An A of size \f$\frac{globalm}{p_r} \times \frac{globaln}{p_c}\f$
 * Here each process \f$m=\frac{globalm}{p_r} and n=\frac{globaln}{p_c}\f$
 * H of size \f$\frac{globaln}{p} \times k\f$
 * W of size \f${globalm}{p} \times k\f$
 * A is \f$m \times n\f$ matrix
 * H is \f$n \times k\f$ matrix
 */
namespace planc {

template <class INPUTMATTYPE>
class DistAUNMF : public DistNMF<INPUTMATTYPE> {
  // needed in derived algorithms to
  // call BPP routines
 protected:
  MAT HtH;    /// H is of size (globaln/p)*k;
  MAT WtW;    /// W is of size (globaln/p)*k;
  MAT AHtij;  /// AHtij is of size k*(globalm/p)
  MAT WtAij;  /// WtAij is of size k*(globaln/p)
  MAT Wt;     /// Wt is of size k*(globalm/p)
  MAT Ht;     /// Ht is of size k*(globaln/p)

  virtual void updateW() = 0;
  virtual void updateH() = 0;

 private:
  // Things needed while solving for W
  MAT localHtH;         /// H is of size (globaln/p)*k;
  MAT Hjt, Hj;          /// Hj is of size n*k;
  MAT AijHj, AijHjt;    /// AijHj is of size m*k;
  // INPUTMATTYPE A_ij_t;  /// n*m matrix. Transpose of A_ij
  // Things needed while solving for H
  MAT localWtW;        /// W is of size (globalm/p)*k;
  MAT Wit, Wi;         /// Wi is of size m*k;
  MAT WitAij, AijWit;  /// WijtAij is of size k*n;

  // needed for error computation
  MAT prevH;        // used for error computation
  MAT prevHtH;      // used for error computation
  MAT WtAijH;       /// global k*k matrix.
  MAT localWtAijH;  /// local k*k matrix
  MAT errMtx;
  MAT A_errMtx;

  // needed for symm regularization
  MAT crossFac;     // holds the appropriate row of W,H
  int paired_proc;  // processor to swap factors with

  // needed for block implementation to save memory
  MAT Ht_blk;
  MAT AHtij_blk;
  MAT Wt_blk;
  MAT WtAij_blk;

  // Gatherv and Reducescatter variables
  std::vector<int> gatherWtAcnts;
  std::vector<int> gatherWtAdisp;
  std::vector<int> scatterWtAcnts;

  std::vector<int> gatherAHcnts;
  std::vector<int> gatherAHdisp;
  std::vector<int> scatterAHcnts;

  int num_k_blocks;
  std::string relative_error_dir;
  int perk;

  /**
   * Allocates matrices
   */
  void allocateMatrices() {
    // collective call related initializations.
    // These initialization are for solving W.
    DISTPRINTINFO("k::" << this->k << "::perk::" << this->perk
                        << "::localm::" << this->m << "::localn::" << this->n
                        << "::globalm::" << this->globalm() << "::globaln::"
                        << this->globaln() << "::MPI_SIZE::" << MPI_SIZE);
    HtH.zeros(this->k, this->k);
    localHtH.zeros(this->k, this->k);
    Hj.zeros(this->n, this->perk);
    Hjt.zeros(this->perk, this->n);
    AijHj.zeros(this->m, this->perk);
    AijHjt.zeros(this->perk, this->m);
    AHtij.zeros(this->k, this->W.n_rows);
    this->scatterAHcnts.resize(NUMCOLPROCS);
    int fillsize = this->perk * (this->W.n_rows);
    fillVector<int>(fillsize, &scatterAHcnts);
    this->gatherAHcnts.resize(NUMROWPROCS);
    fillVector<int>(0, &gatherAHcnts);
    this->gatherAHdisp.resize(NUMROWPROCS);
    fillVector<int>(0, &gatherAHdisp);
#ifdef MPI_VERBOSE
    if (ISROOT) {
      INFO << "::recvAHsize::";
      printVector<int>(recvAHsize);
    }
#endif
    // allocated for block implementation.
    Ht_blk.zeros(this->perk, (this->W.n_rows));
    AHtij_blk.zeros(this->perk, this->W.n_rows);

    // These initialization are for solving H.
    Wt.zeros(this->k, this->W.n_rows);
    WtW.zeros(this->k, this->k);
    localWtW.zeros(this->k, this->k);
    Wi.zeros(this->m, this->perk);
    Wit.zeros(this->perk, this->m);
    WitAij.zeros(this->perk, this->n);
    AijWit.zeros(this->n, this->perk);
    WtAij.zeros(this->k, this->H.n_rows);
    this->scatterWtAcnts.resize(NUMROWPROCS);
    fillsize = this->perk * (this->H.n_rows);
    fillVector<int>(fillsize, &scatterWtAcnts);
    this->gatherWtAcnts.resize(NUMCOLPROCS);
    fillVector<int>(0, &gatherWtAcnts);
    this->gatherWtAdisp.resize(NUMCOLPROCS);
    fillVector<int>(0, &gatherWtAdisp);

    // allocated for block implementation
    Wt_blk.zeros(this->perk, this->W.n_rows);
    WtAij_blk.zeros(this->perk, this->H.n_rows);

    // allocated for symmetric regularisation
    if (this->symm_reg() > 0) {
      crossFac.zeros(this->k, this->H.n_rows);
    }
#ifdef MPI_VERBOSE
    if (ISROOT) {
      INFO << "::recvWtAsize::";
      printVector<int>(recvWtAsize);
    }
#endif
#ifndef BUILD_SPARSE
    if (this->is_compute_error()) {
      errMtx.zeros(this->m, this->n);
      A_errMtx.zeros(this->m, this->n);
    }
#endif
  }

  void freeMatrices() {
    HtH.clear();
    localHtH.clear();
    Hj.clear();
    Hjt.clear();
    AijHj.clear();
    AijHjt.clear();
    AHtij.clear();
    Wt.clear();
    WtW.clear();
    localWtW.clear();
    Wi.clear();
    Wit.clear();
    WitAij.clear();
    AijWit.clear();
    WtAij.clear();
    // A_ij_t.clear();
    if (this->is_compute_error()) {
      prevH.clear();
      prevHtH.clear();
      WtAijH.clear();
      localWtAijH.clear();
    }
    Ht_blk.clear();
    AHtij_blk.clear();
    Wt_blk.clear();
    WtAij_blk.clear();
    if (this->symm_reg() > 0) {
      crossFac.clear();
    }
    if (this->is_compute_error()) {
      errMtx.clear();
      A_errMtx.clear();
    }
  }

  /**
   * Sets up the communication pattern for the matrix multiplies
  */
  void setupCommcounts() {
    // WtA
    // Allgatherv counts
    gatherWtAcnts[0] = itersplit(this->A.n_rows, NUMCOLPROCS, 0) * this->perk;
    gatherWtAdisp[0] = 0;
    for (int i = 1; i < NUMCOLPROCS; i++) {
      gatherWtAcnts[i] = itersplit(this->A.n_rows,
                                   NUMCOLPROCS, i) * this->perk;
      gatherWtAdisp[i] = gatherWtAdisp[i-1] + gatherWtAcnts[i-1];
    }
    // Reducescatter counts
    for (int i = 0; i < NUMROWPROCS; i++) {
      scatterWtAcnts[i] = itersplit(this->A.n_cols,
                                    NUMROWPROCS, i) * this->perk;
    }
    // AH
    // Allgatherv counts
    gatherAHcnts[0] = itersplit(this->A.n_cols, NUMROWPROCS, 0) * this->perk;
    gatherAHdisp[0] = 0;
    for (int i = 1; i < NUMROWPROCS; i++) {
      gatherAHcnts[i] = itersplit(this->A.n_cols, NUMROWPROCS, i) * this->perk;
      gatherAHdisp[i] = gatherAHdisp[i-1] + gatherAHcnts[i-1];
    }
    // Reducescatter counts
    for (int i = 0; i < NUMCOLPROCS; i++) {
      scatterAHcnts[i] = itersplit(this->A.n_rows,
                                   NUMCOLPROCS, i) * this->perk;
    }
  }

 public:
  /**
   * Public constructor with local input matrix, local factors and communicator
   * @param[in] local input matrix of size \f$\frac{globalm}{p_r} \times \frac{globaln}{p_c}\f$.
   *            Each process owns \f$m=\frac{globalm}{p_r}\f$ and \f$n=\frac{globaln}{p_c}\f$
   * @param[in] local left low rank factor of size \f$\frac{globalm}{p} \times k \f$
   * @param[in] local right low rank factor of size \f$\frac{globaln}{p} \times k \f$
   * @param[in] MPICommunicator that has row and column communicators
   * @param[in] numkblks. the columns of the local factor can further be
   *            partitioned into numkblks
   */
  DistAUNMF(const INPUTMATTYPE &input, const MAT &leftlowrankfactor,
            const MAT &rightlowrankfactor, const MPICommunicator &communicator,
            const int numkblks, const std::string relerr_dir)
      : DistNMF<INPUTMATTYPE>(input, leftlowrankfactor, rightlowrankfactor,
                              communicator) {
    num_k_blocks = numkblks;
    relative_error_dir = relerr_dir;
    perk = this->k / num_k_blocks;
    allocateMatrices();
    setupCommcounts();
    this->Wt = leftlowrankfactor.t();
    this->Ht = rightlowrankfactor.t();
    // A_ij_t = input.t();
    if (this->symm_reg() >= 0) {
      // Get paired processor
      int coords[2];
      coords[0] = MPI_COL_RANK;
      coords[1] = MPI_ROW_RANK;
      MPI_Cart_rank(this->m_mpicomm.gridComm(), &coords[0], &paired_proc);
      DISTPRINTINFO("rank::" << MPI_RANK << "::paired_proc::" << paired_proc);
    }
    PRINTROOT("aunmf()::constructor succesful");
  }
  ~DistAUNMF() {
    // freeMatrices();
  }

  /**
   * This is a matrix multiplication routine based on
   * reduce_scatter.
   * A is mxn in column major ordering
   * W is mxk in column major ordering
   * AtW is nxk in column major ordering
   * There are totally p processes. Every process has
   * A_i as m_i * n
   * W_i as m_i * k
   * AtW_i as n_i * k
   * this->m_mpicomm.comm_subs()[0] is column communicator.
   * this->m_mpicomm.comm_subs()[1] is row communicator.
   */
  void distWtA() {
    for (int i = 0; i < num_k_blocks; i++) {
      int start_row = i * perk;
      int end_row = (i + 1) * perk - 1;
      Wt_blk = Wt.rows(start_row, end_row);
      distWtABlock();
      WtAij.rows(start_row, end_row) = WtAij_blk;
    }
  }
  void distWtABlock() {
#ifdef USE_PACOSS
    // Perform expand communication using Pacoss.
    memcpy(Wit.memptr(), Wt_blk.memptr(),
           Wt_blk.n_rows * Wt_blk.n_cols * sizeof(Wt_blk[0]));
    MPITIC;
    this->m_rowcomm->expCommBegin(Wit.memptr(), this->perk);
    this->m_rowcomm->expCommFinish(Wit.memptr(), this->perk);
#else
    int sendcnt = (this->W.n_rows) * this->perk;
    Wit.zeros();
    MPITIC;  // allgather WtA
    MPI_Allgatherv(Wt_blk.memptr(), sendcnt, MPI_DOUBLE, Wit.memptr(),
                  &(gatherWtAcnts[0]), &(gatherWtAdisp[0]), MPI_DOUBLE,
                  this->m_mpicomm.commSubs()[1]);
#endif
    double temp = MPITOC;  // allgather WtA
    PRINTROOT("n::" << this->n << "::k::" << this->k << PRINTMATINFO(Wt)
                    << PRINTMATINFO(Wit));
#ifdef MPI_VERBOSE
    DISTPRINTINFO(PRINTMAT(Wt_blk));
    DISTPRINTINFO(PRINTMAT(Wit));
#endif
    this->time_stats.communication_duration(temp);
    this->time_stats.allgather_duration(temp);
    MPITIC;  // mm WtA
    this->WitAij = this->Wit * this->A;
    // #if defined(MKL_FOUND) && defined(BUILD_SPARSE)
    //     // void ARMAMKLSCSCMM(const SRC &mklMat, const DESTN &Bt, const char
    //     transa,
    //     //               DESTN *Ct)
    //     ARMAMKLSCSCMM(this->A_ij_t, 'N', this->Wit, this->AijWit.memptr());
    // #ifdef MPI_VERBOSE
    //     DISTPRINTINFO(PRINTMAT(this->AijWit));
    // #endif
    //     this->WitAij = reshape(this->AijWit, this->k, this->n);
    // #else
    //     this->WitAij = this->Wit * this->A;
    // #endif
    temp = MPITOC;  // mm WtA
#ifdef MPI_VERBOSE
    DISTPRINTINFO(PRINTMAT(this->WitAij));
#endif
    PRINTROOT(PRINTMATINFO(this->A)
              << PRINTMATINFO(this->Wit) << PRINTMATINFO(this->WitAij));
    this->time_stats.compute_duration(temp);
    this->time_stats.mm_duration(temp);
    this->reportTime(temp, "WtA::");
#ifdef USE_PACOSS
    // Perform fold communication using Pacoss.
    MPITIC;
    this->m_colcomm->foldCommBegin(WitAij.memptr(), this->perk);
    this->m_colcomm->foldCommFinish(WitAij.memptr(), this->perk);
    temp = MPITOC;
    memcpy(WtAij_blk.memptr(), WitAij.memptr(),
           WtAij_blk.n_rows * WtAij_blk.n_cols * sizeof(WtAij_blk[0]));
#else
    WtAij_blk.zeros();
    MPITIC;  // reduce_scatter WtA
    MPI_Reduce_scatter(this->WitAij.memptr(), this->WtAij_blk.memptr(),
                       &(scatterWtAcnts[0]), MPI_DOUBLE, MPI_SUM,
                       this->m_mpicomm.commSubs()[0]);
    temp = MPITOC;  // reduce_scatter WtA
#endif
    this->time_stats.communication_duration(temp);
    this->time_stats.reducescatter_duration(temp);
  }
  /**
   * There are totally prxpc process.
   * Each process will hold the following
   * An A of size (m/pr) x (n/pc)
   * H of size (n/p)xk
   * find AHt kx(m/p) by reducing and scatter it using MPI_Reduce_scatter call.
   * That is, p process will hold a kx(m/p) matrix.
   * this->m_mpicomm.comm_subs()[0] is column communicator.
   * this->m_mpicomm.comm_subs()[1] is row communicator.
   * To preserve the memory for Hj, we collect only partial k
   */
  void distAH() {
    for (int i = 0; i < num_k_blocks; i++) {
      int start_row = i * perk;
      int end_row = (i + 1) * perk - 1;
      Ht_blk = Ht.rows(start_row, end_row);
      distAHBlock();
      AHtij.rows(start_row, end_row) = AHtij_blk;
    }
  }
  void distAHBlock() {
    /*
    DISTPRINTINFO("distAH::" << "::Acolst::" \
                  Acolst.n_rows<<"x"<<Acolst.n_cols \
                  << "::norm::" << arma::norm(Acolst, "fro"));
    DISTPRINTINFO("distAH::" << "::H::" \
                  << this->H.n_rows << "x" << this->H.n_cols);
    */
#ifdef USE_PACOSS
    // Perform expand communication using Pacoss.
    memcpy(Hjt.memptr(), Ht_blk.memptr(),
           Ht_blk.n_rows * Ht_blk.n_cols * sizeof(Ht_blk[0]));
    MPITIC;
    this->m_colcomm->expCommBegin(Hjt.memptr(), this->perk);
    this->m_colcomm->expCommFinish(Hjt.memptr(), this->perk);
#else
    int sendcnt = (this->H.n_rows) * this->perk;
    Hjt.zeros();
    MPITIC;  // allgather AH
    MPI_Allgatherv(this->Ht_blk.memptr(), sendcnt, MPI_DOUBLE,
                  this->Hjt.memptr(), &(gatherAHcnts[0]), &(gatherAHdisp[0]),
                  MPI_DOUBLE, this->m_mpicomm.commSubs()[0]);
#endif
    PRINTROOT("n::" << this->n << "::k::" << this->k << PRINTMATINFO(Ht)
                    << PRINTMATINFO(Hjt));
    double temp = MPITOC;  // allgather AH
#ifdef MPI_VERBOSE
    DISTPRINTINFO(PRINTMAT(Ht_blk));
    DISTPRINTINFO(PRINTMAT(Hjt));
    // DISTPRINTINFO(PRINTMAT(this->A_ij_t));
#endif
    this->time_stats.communication_duration(temp);
    this->time_stats.allgather_duration(temp);
    MPITIC;  // mm AH
/*
#ifdef BUILD_SPARSE
    this->Hj = this->Hjt.t();
    this->AijHj = this->A * this->Hj;
    this->AijHjt = this->AijHj.t();
#else
    this->AijHjt = this->Hjt * this->A.t();
#endif
More memory efficient to rewrite code as above since A.t() will construct a
copy of the A in the sparse case. However sparse x dense matmul is much slower
than in dense x sparse. Keeping current version for performance reasons.
*/
    this->AijHjt = this->Hjt * this->A.t();
    // #if defined(MKL_FOUND) && defined(BUILD_SPARSE)
    //     // void ARMAMKLSCSCMM(const SRC &mklMat, const DESTN &Bt, const char
    //     transa,
    //     //               DESTN *Ct)
    //     ARMAMKLSCSCMM(this->A, 'N', this->Hjt, this->AijHj.memptr());
    // #ifdef MPI_VERBOSE
    //     DISTPRINTINFO(PRINTMAT(this->AijHj));
    // #endif
    //     this->AijHjt = reshape(this->AijHj, this->k, this->m);
    // #else
    //     this->AijHjt = this->Hjt * this->A_ij_t;
    // #endif
    // #ifdef MPI_VERBOSE
    //     DISTPRINTINFO(PRINTMAT(this->AijHjt));
    // #endif
    temp = MPITOC;  // mm AH
    // PRINTROOT(PRINTMATINFO(this->A_ij_t)
    PRINTROOT(PRINTMATINFO(this->Hjt) << PRINTMATINFO(this->AijHjt));
    this->time_stats.compute_duration(temp);
    this->time_stats.mm_duration(temp);
    this->reportTime(temp, "AH::");
#ifdef USE_PACOSS
    // Perform fold communication using Pacoss.
    MPITIC;
    this->m_rowcomm->foldCommBegin(AijHjt.memptr(), this->perk);
    this->m_rowcomm->foldCommFinish(AijHjt.memptr(), this->perk);
    temp = MPITOC;
    memcpy(AHtij_blk.memptr(), AijHjt.memptr(),
           AHtij_blk.n_rows * AHtij_blk.n_cols * sizeof(AHtij_blk[0]));
#else
    AHtij_blk.zeros();
    MPITIC;  // reduce_scatter AH
    MPI_Reduce_scatter(this->AijHjt.memptr(), this->AHtij_blk.memptr(),
                       &(this->scatterAHcnts[0]), MPI_DOUBLE, MPI_SUM,
                       this->m_mpicomm.commSubs()[1]);
    temp = MPITOC;  // reduce_scatter AH
#endif
    this->time_stats.communication_duration(temp);
    this->time_stats.reducescatter_duration(temp);
  }
  /**
   * There are p processes.
   * Every process i has W in m_i * k
   * At the end of this call, all process will have
   * WtW of size k*k is symmetric. So not to worry
   * about column/row major formats.
   * @param[in] X is of size m_i x k
   * @param[out] XtX Every process owns the same kxk global gram matrix of X
   */
  void distInnerProduct(const MAT &X, MAT *XtX) {
    // each process computes its own kxk matrix
    MPITIC;  // gram
    localWtW = X.t() * X;
#ifdef MPI_VERBOSE
    DISTPRINTINFO("W::" << norm(X, "fro")
                        << "::localWtW::" << norm(this->localWtW, "fro"));
#endif
    double temp = MPITOC;  // gram
    this->time_stats.compute_duration(temp);
    this->time_stats.gram_duration(temp);
    (*XtX).zeros();
    if (X.n_rows == this->m) {
      this->reportTime(temp, "Gram::W::");
    } else {
      this->reportTime(temp, "Gram::H::");
    }
    MPITIC;  // allreduce gram
    MPI_Allreduce(localWtW.memptr(), (*XtX).memptr(), this->k * this->k,
                  MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
    temp = MPITOC;  // allreduce gram
    this->time_stats.communication_duration(temp);
    this->time_stats.allreduce_duration(temp);
  }
  /**
   * This is the main loop function
   * Refer Algorithm 1 in Page 3 of
   * the PPoPP HPC-NMF paper.
   */
  void computeNMF() {
    PRINTROOT("computeNMF started");
#ifdef MPI_VERBOSE
    DISTPRINTINFO(PRINTMAT(this->A));
#endif
    // error computation
    if (this->is_compute_error()) {
      prevH.zeros(size(this->H));
      prevHtH.zeros(this->k, this->k);
      WtAijH.zeros(this->k, this->k);
      localWtAijH.zeros(this->k, this->k);
    }
#ifdef __WITH__BARRIER__TIMING__
    MPI_Barrier(MPI_COMM_WORLD);
#endif
    for (unsigned int iter = 0; iter < this->num_iterations(); iter++) {
      // saving current instance for error computation.
      if (iter > 0 && this->is_compute_error()) {
        this->prevH = this->H;
        this->prevHtH = this->HtH;
      }
      MPITIC;  // total_d W&H
      // update H given WtW and WtA step 4 of the algorithm
      {
        // compute WtW
        this->distInnerProduct(this->W, &this->WtW);
        PRINTROOT(PRINTMATINFO(this->WtW));
        this->applyReg(this->regH(), &this->WtW);
#ifdef MPI_VERBOSE
        PRINTROOT(PRINTMAT(this->WtW));
#endif
        // compute WtA
        this->distWtA();
        if (this->symm_reg() > 0) {
          // Get the appropriate Wt from the transposed processor
          this->crossFac.zeros(arma::size(this->Ht));
          int recvsize = this->crossFac.n_elem;
          int sendsize = this->Wt.n_elem;
          MPITIC;
          MPI_Sendrecv(this->Wt.memptr(), sendsize, MPI_DOUBLE, paired_proc, 0,
              this->crossFac.memptr(), recvsize, MPI_DOUBLE, paired_proc, 0,
              this->m_mpicomm.gridComm(), MPI_STATUS_IGNORE);
          double temp = MPITOC;  // sendrecv
          this->time_stats.communication_duration(temp);
          this->time_stats.sendrecv_duration(temp);

          this->applySymmetricReg(this->symm_reg(), &this->WtW,
                  &this->crossFac, &this->WtAij);
        }
        // PRINTROOT(PRINTMATINFO(this->WtAij));
#ifdef MPI_VERBOSE
        DISTPRINTINFO(PRINTMAT(this->WtAij));
#endif
        MPITIC;  // nnls H
        // ensure both Ht and H are consistent after the update
        // some function find Ht and some H.
        updateH();
#ifdef MPI_VERBOSE
        DISTPRINTINFO("::it=" << iter << PRINTMAT(this->H));
#endif
        double temp = MPITOC;  // nnls H
        this->time_stats.compute_duration(temp);
        this->time_stats.nnls_duration(temp);
        this->reportTime(temp, "NNLS::H::");
      }
      // Update W given HtH and AH step 3 of the algorithm.
      {
        // compute HtH
        this->distInnerProduct(this->H, &this->HtH);
        PRINTROOT("HtH::" << PRINTMATINFO(this->HtH));
        this->applyReg(this->regW(), &this->HtH);
#ifdef MPI_VERBOSE
        PRINTROOT(PRINTMAT(this->HtH));
#endif
        // compute AH
        this->distAH();
        if (this->symm_reg() > 0) {
          // Get the appropriate Ht from the transposed processor
          this->crossFac.zeros(arma::size(this->Wt));
          int recvsize = this->crossFac.n_elem;
          int sendsize = this->Ht.n_elem;
          MPITIC;
          MPI_Sendrecv(this->Ht.memptr(), sendsize, MPI_DOUBLE, paired_proc, 0,
              this->crossFac.memptr(), recvsize, MPI_DOUBLE, paired_proc, 0,
              this->m_mpicomm.gridComm(), MPI_STATUS_IGNORE);
          double temp = MPITOC;  // sendrecv
          this->time_stats.communication_duration(temp);
          this->time_stats.sendrecv_duration(temp);

          this->applySymmetricReg(this->symm_reg(), &this->HtH,
                &this->crossFac, &this->AHtij);
        }
        // PRINTROOT(PRINTMATINFO(this->AHtij));
#ifdef MPI_VERBOSE
        DISTPRINTINFO(PRINTMAT(this->AHtij));
#endif
        MPITIC;  // nnls W
        // Update W given HtH and AH step 3 of the algorithm.
        // ensure W and Wt are consistent. As some algorithms
        // determine W and some Wt.
        updateW();
#ifdef MPI_VERBOSE
        DISTPRINTINFO("::it=" << iter << PRINTMAT(this->W));
#endif
        double temp = MPITOC;  // nnls W
        this->time_stats.compute_duration(temp);
        this->time_stats.nnls_duration(temp);
        this->reportTime(temp, "NNLS::W::");
      }
      this->time_stats.duration(MPITOC);  // total_d W&H
      if (iter > 0 && this->is_compute_error()) {
#ifdef BUILD_SPARSE
        this->computeError(iter);
#else
        this->computeError2(iter);
#endif
        std::string outfullName = relative_error_dir+"relerr_at_k"+std::to_string(this->k);
        PRINTROOT("it=" << iter << "::algo::" << this->m_algorithm << "::k::"
                        << this->k << "::err::" << sqrt(this->objective_err)
                        << "::relerr::"
                        << sqrt(this->objective_err / this->m_globalsqnormA));
        std::ofstream outfile;
        if (iter == this->num_iterations()-1 && this->m_mpicomm.rank() == 0) {
            outfile.open(outfullName.c_str(), std::ios_base::app);
            outfile<<  sqrt(this->objective_err / this->m_globalsqnormA) <<"\n";
            //PRINTROOT(sqrt(this->objective_err/this->m_globalsqnormA));
            outfile.close();
        }
        // Compute the difference between factor matrices
        if (this->symm_reg() > 0) {
          double localdiff = arma::norm(this->Wt-this->crossFac, "fro");
          double globaldiff = 0.0;

          // Compute global difference
          localdiff = localdiff * localdiff;
          MPI_Allreduce(&localdiff, &globaldiff, 1,
              MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);

          double localWnorm = arma::norm(this->Wt, "fro");
          double globalWnorm = 0.0;

          // Compute global W norm
          localWnorm = localWnorm * localWnorm;
          MPI_Allreduce(&localWnorm, &globalWnorm, 1,
              MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);

          PRINTROOT("it=" << iter << "::symmdiff::" << globaldiff
                    << "::reldiff::" << sqrt(globaldiff / globalWnorm));
        }
      }
      PRINTROOT("completed it=" << iter
                                << "::taken::" << this->time_stats.duration());
    }  // end for loop
    MPI_Barrier(MPI_COMM_WORLD);
    this->reportTime(this->time_stats.duration(), "total_d");
    this->reportTime(this->time_stats.communication_duration(), "total_comm");
    this->reportTime(this->time_stats.compute_duration(), "total_comp");
    this->reportTime(this->time_stats.allgather_duration(), "total_allgather");
    this->reportTime(this->time_stats.allreduce_duration(), "total_allreduce");
    this->reportTime(this->time_stats.reducescatter_duration(),
                     "total_reducescatter");
    this->reportTime(this->time_stats.gram_duration(), "total_gram");
    this->reportTime(this->time_stats.mm_duration(), "total_mm");
    this->reportTime(this->time_stats.nnls_duration(), "total_nnls");
    if (this->symm_reg() > 0) {
      this->reportTime(this->time_stats.sendrecv_duration(), "total_sendrecv");
    }
    if (this->is_compute_error()) {
      this->reportTime(this->time_stats.err_compute_duration(),
                       "total_err_compute");
      this->reportTime(this->time_stats.err_compute_duration(),
                       "total_err_communication");
    }
  }

  /**
   * We assume this error function will be called in
   * every iteration before updating the block to
   * compute the error from previous iteration
   * \f$\|A\|_F^2 - 2trace(H(A^TW))+trace((W^TW)*(HH^T))\f$
   * each process owns globalsqnormA will have \|A\|_F^2
   * each process owns WtAij is of size \f$k \times \frac{globaln}{p}\f$
   * each process owns H is of size \f${globaln}{p} \times k \f$
   * compute WtAij*H and do an MPI_ALL reduce to get the kxk matrix.
   * every process local computation
   */

  void computeError(const int it) {
    MPITIC;  // computeerror
    this->localWtAijH = this->WtAij * this->prevH;
#ifdef MPI_VERBOSE
    DISTPRINTINFO("::it=" << it << PRINTMAT(this->WtAij));
    DISTPRINTINFO("::it=" << it << PRINTMAT(this->localWtAijH));
    DISTPRINTINFO("::it=" << it << PRINTMAT(this->prevH));
    PRINTROOT("::it=" << it << PRINTMAT(this->WtW));
    PRINTROOT("::it=" << it << PRINTMAT(this->prevHtH));
#endif
    double temp = MPITOC;  // computererror
    this->time_stats.err_compute_duration(temp);
    MPITIC;  // coommunication error
    MPI_Allreduce(this->localWtAijH.memptr(), this->WtAijH.memptr(),
                  this->k * this->k, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
    temp = MPITOC;  // communication error
#ifdef MPI_VERBOSE
    DISTPRINTINFO(PRINTMAT(WtAijH));
#endif
    this->time_stats.err_communication_duration(temp);
    double tWtAijh = trace(this->WtAijH);
    double tWtWHtH = trace(this->WtW * this->prevHtH);
    PRINTROOT("::it=" << it << "normA::" << this->m_globalsqnormA
                      << "::tWtAijH::" << 2 * tWtAijh
                      << "::tWtWHtH::" << tWtWHtH);
    this->objective_err = this->m_globalsqnormA - 2 * tWtAijh + tWtWHtH;
  }
  /*
   * Compute error the old-fashioned way
   */
  void computeError2(const int it) {
    double local_sqerror = 0.0;
    PRINTROOT("::it=" << it << "::Calling compute error 2");
    MPITIC;
    // DISTPRINTINFO("::norm(Wi,fro)::" << norm(this->Wit, "fro") <<
    // "::norm(Hjt, fro)::" << norm(this->Hjt, "fro"));
    this->Wi = this->Wit.t();
    errMtx = this->Wi * this->Hjt;
    A_errMtx = this->A - errMtx;
    local_sqerror = norm(A_errMtx, "fro");
    local_sqerror *= local_sqerror;
    double temp = MPITOC;
    this->time_stats.err_compute_duration(temp);
    // DISTPRINTINFO("::it=" << it << "::local_sqerror::" << local_sqerror);
    MPITIC;
    MPI_Allreduce(&local_sqerror, &this->objective_err, 1, MPI_DOUBLE, MPI_SUM,
                  MPI_COMM_WORLD);
    temp = MPITOC;
    this->time_stats.err_communication_duration(temp);
  }

  // Set the LUC inner iterations for iterative LUC
  void set_luciters(int max_luciters) {}
};

}  // namespace planc

#endif  // DISTNMF_AUNMF_HPP_


#ifndef SM_EIGEN_RANDOM_HPP
#define SM_EIGEN_RANDOM_HPP
#include <Eigen/Core>

namespace sm {
namespace eigen {

Eigen::VectorXd randn(unsigned dim);

}  // namespace eigen
}  // namespace sm

#endif /* SM_EIGEN_RANDOM_HPP */


#include <boost/python.hpp>

namespace py = boost::python;

py::list naive_mul(const py::list& A, const py::list& B){
    py::ssize_t n = py::len(A);

    std::vector<std::vector<int>> av;
    av.reserve(n);
    std::vector<std::vector<int>> bv;
    bv.reserve(n);

    for(int i=0; i<n; ++i){
        std::vector<int> rowa;
        rowa.reserve(n);
        std::vector<int> rowb;
        rowb.reserve(n);
        for(int j=0; j<n; ++j){
            rowa.push_back(py::extract<int>(A[i][j]));
            rowb.push_back(py::extract<int>(B[i][j]));
        }
        av.push_back(rowa);
        bv.push_back(rowb);
    }

    std::vector<std::vector<int>> res;
    res.reserve(n);
    for(int i=0; i<n; ++i){
        std::vector<int> row(n);
        for(int j=0; j<n; ++j){
            for(int k=0; k<n; ++k){
                int a = av[i][k];
                int b = bv[k][j];
                row[j] += a * b;
            }
        }
        res.push_back(row);
    }

    py::list C;
    for(int i=0; i<n; ++i){
        py::list tmp;
        for(int j=0; j<n; ++j){
            tmp.append(res[i][j]);
        }
        C.append(tmp);
    }

    return C;
}

BOOST_PYTHON_MODULE(dummy_matrix_mul){
    using namespace boost::python;

    def("standardMatrixProduct", naive_mul);
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2014 Benoit Steiner <benoit.steiner.goog@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#define EIGEN_USE_THREADS

#include "main.h"
#include <iostream>
#include <Eigen/CXX11/Tensor>

using Eigen::Tensor;

static void test_multithread_elementwise()
{
  Tensor<float, 3> in1(2,3,7);
  Tensor<float, 3> in2(2,3,7);
  Tensor<float, 3> out(2,3,7);

  in1.setRandom();
  in2.setRandom();

  Eigen::ThreadPool tp(internal::random<int>(3, 11));
  Eigen::ThreadPoolDevice thread_pool_device(&tp, internal::random<int>(3, 11));
  out.device(thread_pool_device) = in1 + in2 * 3.14f;

  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 7; ++k) {
        VERIFY_IS_APPROX(out(i,j,k), in1(i,j,k) + in2(i,j,k) * 3.14f);
      }
    }
  }
}

static void test_multithread_compound_assignment()
{
  Tensor<float, 3> in1(2,3,7);
  Tensor<float, 3> in2(2,3,7);
  Tensor<float, 3> out(2,3,7);

  in1.setRandom();
  in2.setRandom();

  Eigen::ThreadPool tp(internal::random<int>(3, 11));
  Eigen::ThreadPoolDevice thread_pool_device(&tp, internal::random<int>(3, 11));
  out.device(thread_pool_device) = in1;
  out.device(thread_pool_device) += in2 * 3.14f;

  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 7; ++k) {
        VERIFY_IS_APPROX(out(i,j,k), in1(i,j,k) + in2(i,j,k) * 3.14f);
      }
    }
  }
}

template<int DataLayout>
static void test_multithread_contraction()
{
  Tensor<float, 4, DataLayout> t_left(30, 50, 37, 31);
  Tensor<float, 5, DataLayout> t_right(37, 31, 70, 2, 10);
  Tensor<float, 5, DataLayout> t_result(30, 50, 70, 2, 10);

  t_left.setRandom();
  t_right.setRandom();

  // this contraction should be equivalent to a single matrix multiplication
  typedef Tensor<float, 1>::DimensionPair DimPair;
  Eigen::array<DimPair, 2> dims({{DimPair(2, 0), DimPair(3, 1)}});

  typedef Map<Matrix<float, Dynamic, Dynamic, DataLayout>> MapXf;
  MapXf m_left(t_left.data(), 1500, 1147);
  MapXf m_right(t_right.data(), 1147, 1400);
  Matrix<float, Dynamic, Dynamic, DataLayout> m_result(1500, 1400);

  Eigen::ThreadPool tp(4);
  Eigen::ThreadPoolDevice thread_pool_device(&tp, 4);

  // compute results by separate methods
  t_result.device(thread_pool_device) = t_left.contract(t_right, dims);
  m_result = m_left * m_right;

 for (ptrdiff_t i = 0; i < t_result.size(); i++) {
    VERIFY(&t_result.data()[i] != &m_result.data()[i]);
    if (fabs(t_result.data()[i] - m_result.data()[i]) >= 1e-4) {
      std::cout << "mismatch detected: " << t_result.data()[i] << " vs " <<  m_result.data()[i] << std::endl;
      assert(false);
    }
  }
}

template<int DataLayout>
static void test_contraction_corner_cases()
{
  Tensor<float, 2, DataLayout> t_left(32, 500);
  Tensor<float, 2, DataLayout> t_right(32, 28*28);
  Tensor<float, 2, DataLayout> t_result(500, 28*28);

  t_left = (t_left.constant(-0.5f) + t_left.random()) * 2.0f;
  t_right = (t_right.constant(-0.6f) + t_right.random()) * 2.0f;
  t_result = t_result.constant(NAN);

  // this contraction should be equivalent to a single matrix multiplication
  typedef Tensor<float, 1>::DimensionPair DimPair;
  Eigen::array<DimPair, 1> dims{{DimPair(0, 0)}};

  typedef Map<Matrix<float, Dynamic, Dynamic, DataLayout>> MapXf;
  MapXf m_left(t_left.data(), 32, 500);
  MapXf m_right(t_right.data(), 32, 28*28);
  Matrix<float, Dynamic, Dynamic, DataLayout> m_result(500, 28*28);

  Eigen::ThreadPool tp(12);
  Eigen::ThreadPoolDevice thread_pool_device(&tp, 12);

  // compute results by separate methods
  t_result.device(thread_pool_device) = t_left.contract(t_right, dims);
  m_result = m_left.transpose() * m_right;

  for (ptrdiff_t i = 0; i < t_result.size(); i++) {
    assert(!std::isnan(t_result.data()[i]));
    if (fabs(t_result.data()[i] - m_result.data()[i]) >= 1e-4) {
      std::cout << "mismatch detected at index " << i << " : " << t_result.data()[i] << " vs " <<  m_result.data()[i] << std::endl;
      assert(false);
    }
  }

  t_left.resize(32, 1);
  t_left = (t_left.constant(-0.5f) + t_left.random()) * 2.0f;
  t_result.resize (1, 28*28);
  t_result = t_result.constant(NAN);
  t_result.device(thread_pool_device) = t_left.contract(t_right, dims);
  new(&m_left) MapXf(t_left.data(), 32, 1);
  m_result = m_left.transpose() * m_right;
  for (ptrdiff_t i = 0; i < t_result.size(); i++) {
    assert(!std::isnan(t_result.data()[i]));
    if (fabs(t_result.data()[i] - m_result.data()[i]) >= 1e-4) {
      std::cout << "mismatch detected: " << t_result.data()[i] << " vs " <<  m_result.data()[i] << std::endl;
      assert(false);
    }
  }

  t_left.resize(32, 500);
  t_right.resize(32, 4);
  t_left = (t_left.constant(-0.5f) + t_left.random()) * 2.0f;
  t_right = (t_right.constant(-0.6f) + t_right.random()) * 2.0f;
  t_result.resize (500, 4);
  t_result = t_result.constant(NAN);
  t_result.device(thread_pool_device) = t_left.contract(t_right, dims);
  new(&m_left) MapXf(t_left.data(), 32, 500);
  new(&m_right) MapXf(t_right.data(), 32, 4);
  m_result = m_left.transpose() * m_right;
  for (ptrdiff_t i = 0; i < t_result.size(); i++) {
    assert(!std::isnan(t_result.data()[i]));
    if (fabs(t_result.data()[i] - m_result.data()[i]) >= 1e-4) {
      std::cout << "mismatch detected: " << t_result.data()[i] << " vs " <<  m_result.data()[i] << std::endl;
      assert(false);
    }
  }

  t_left.resize(32, 1);
  t_right.resize(32, 4);
  t_left = (t_left.constant(-0.5f) + t_left.random()) * 2.0f;
  t_right = (t_right.constant(-0.6f) + t_right.random()) * 2.0f;
  t_result.resize (1, 4);
  t_result = t_result.constant(NAN);
  t_result.device(thread_pool_device) = t_left.contract(t_right, dims);
  new(&m_left) MapXf(t_left.data(), 32, 1);
  new(&m_right) MapXf(t_right.data(), 32, 4);
  m_result = m_left.transpose() * m_right;
  for (ptrdiff_t i = 0; i < t_result.size(); i++) {
    assert(!std::isnan(t_result.data()[i]));
    if (fabs(t_result.data()[i] - m_result.data()[i]) >= 1e-4) {
      std::cout << "mismatch detected: " << t_result.data()[i] << " vs " <<  m_result.data()[i] << std::endl;
      assert(false);
    }
  }
}

template<int DataLayout>
static void test_multithread_contraction_agrees_with_singlethread() {
  int contract_size = internal::random<int>(1, 5000);

  Tensor<float, 3, DataLayout> left(internal::random<int>(1, 80),
                                    contract_size,
                                    internal::random<int>(1, 100));

  Tensor<float, 4, DataLayout> right(internal::random<int>(1, 25),
                                     internal::random<int>(1, 37),
                                     contract_size,
                                     internal::random<int>(1, 51));

  left.setRandom();
  right.setRandom();

  // add constants to shift values away from 0 for more precision
  left += left.constant(1.5f);
  right += right.constant(1.5f);

  typedef Tensor<float, 1>::DimensionPair DimPair;
  Eigen::array<DimPair, 1> dims({{DimPair(1, 2)}});

  Eigen::ThreadPool tp(internal::random<int>(2, 11));
  Eigen::ThreadPoolDevice thread_pool_device(&tp, internal::random<int>(2, 11));

  Tensor<float, 5, DataLayout> st_result;
  st_result = left.contract(right, dims);

  Tensor<float, 5, DataLayout> tp_result(st_result.dimensions());
  tp_result.device(thread_pool_device) = left.contract(right, dims);

  VERIFY(dimensions_match(st_result.dimensions(), tp_result.dimensions()));
  for (ptrdiff_t i = 0; i < st_result.size(); i++) {
    // if both of the values are very small, then do nothing (because the test will fail
    // due to numerical precision issues when values are small)
    if (fabs(st_result.data()[i] - tp_result.data()[i]) >= 1e-4) {
      VERIFY_IS_APPROX(st_result.data()[i], tp_result.data()[i]);
    }
  }
}

template<int DataLayout>
static void test_multithreaded_reductions() {
  const int num_threads = internal::random<int>(3, 11);
  ThreadPool thread_pool(num_threads);
  Eigen::ThreadPoolDevice thread_pool_device(&thread_pool, num_threads);

  const int num_rows = internal::random<int>(13, 732);
  const int num_cols = internal::random<int>(13, 732);
  Tensor<float, 2, DataLayout> t1(num_rows, num_cols);
  t1.setRandom();

  Tensor<float, 1, DataLayout> full_redux(1);
  full_redux = t1.sum();

  Tensor<float, 1, DataLayout> full_redux_tp(1);
  full_redux_tp.device(thread_pool_device) = t1.sum();

  // Check that the single threaded and the multi threaded reductions return
  // the same result.
  VERIFY_IS_APPROX(full_redux(0), full_redux_tp(0));
}

static void test_memcpy() {

  for (int i = 0; i < 5; ++i) {
    const int num_threads = internal::random<int>(3, 11);
    Eigen::ThreadPool tp(num_threads);
    Eigen::ThreadPoolDevice thread_pool_device(&tp, num_threads);

    const int size = internal::random<int>(13, 7632);
    Tensor<float, 1> t1(size);
    t1.setRandom();
    std::vector<float> result(size);
    thread_pool_device.memcpy(&result[0], t1.data(), size*sizeof(float));
    for (int j = 0; j < size; j++) {
      VERIFY_IS_EQUAL(t1(j), result[j]);
    }
  }
}

static void test_multithread_random()
{
  Eigen::ThreadPool tp(2);
  Eigen::ThreadPoolDevice device(&tp, 2);
  Tensor<float, 1> t(1 << 20);
  t.device(device) = t.random<Eigen::internal::NormalRandomGenerator<float>>();
}

void test_cxx11_tensor_thread_pool()
{
  CALL_SUBTEST(test_multithread_elementwise());
  CALL_SUBTEST(test_multithread_compound_assignment());

  CALL_SUBTEST(test_multithread_contraction<ColMajor>());
  CALL_SUBTEST(test_multithread_contraction<RowMajor>());

  CALL_SUBTEST(test_multithread_contraction_agrees_with_singlethread<ColMajor>());
  CALL_SUBTEST(test_multithread_contraction_agrees_with_singlethread<RowMajor>());

  // Exercise various cases that have been problematic in the past.
  CALL_SUBTEST(test_contraction_corner_cases<ColMajor>());
  CALL_SUBTEST(test_contraction_corner_cases<RowMajor>());

  CALL_SUBTEST(test_multithreaded_reductions<ColMajor>());
  CALL_SUBTEST(test_multithreaded_reductions<RowMajor>());

  CALL_SUBTEST(test_memcpy());

  CALL_SUBTEST(test_multithread_random());
}


//==================================================================================================
/*!
  @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_NEGATE_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_NEGATE_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-ieee
    This function object returns  the first element multiplied by 1,  0 or -1 depending
    on the fact its second argument is greater,  equal or lesser than zero.

    @par Header <boost/simd/function/negate.hpp>

    @par Note
    If y is @ref Zero the result is zero. This can be avoided using
    @ref negatenz or @ref copysign.

    @see sign, negatenz, copysign,  Mzero,  is_positive, is_negative

    @par Example:

      @snippet negate.cpp negate

    @par Possible output:

      @snippet negate.txt negate

  **/
  Value negate(Value const& x, Value const& y);
} }
#endif

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

#endif


/*
 * si_histogramtest.cpp
 *
 *  Created on: Sep 27, 2011
 *      Author: joe
 */

#define BOOST_TEST_MODULE SIHistogram
#define BOOST_TEST_MAIN
#include "../src/config.h"
#ifdef USE_DYNAMIC_UNIT_TEST
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif
#include <iostream>
#include <string>
#include "SIHistogram.h"

BOOST_AUTO_TEST_CASE( si_histogram )
{
    Interval maxInterval(0,100);
    SIHistogram hist(false, maxInterval);
    hist.add(SpanInterval(0,2,3,4));
    hist.add(SpanInterval(0,2,3,4));
    hist.add(SpanInterval(2,3,3,5));
    //std::cout << "histogram = " << hist.toString() << std::endl;
    BOOST_CHECK_EQUAL(hist.toString(), "{\n"
            "[(0, 1), (3, 4)] = 2\n"
            "[(2, 2), (3, 4)] = 3\n"
            "[(2, 2), (5, 5)] = 1\n"
            "[(3, 3), (3, 5)] = 1\n"
            "}\n");
}

BOOST_AUTO_TEST_CASE( si_histogram_liquid )
{
    Interval maxInterval(0,100);
    SIHistogram hist(true, maxInterval);
    hist.add(SpanInterval(0,2,0,2));
    hist.add(SpanInterval(0,2,0,2));
    hist.add(SpanInterval(2,3,2,3));
    //std::cout << "liqhistogram = " << hist.toString() << std::endl;
    BOOST_CHECK_EQUAL(hist.toString(), "{\n"
            "[0:1] = 2\n"
            "[2:2] = 3\n"
            "[3:3] = 1\n"
            "}\n");
}


/*
 * test_REF.cpp
 *
 *  Created on: 2015年3月21日
 *      Author: fasiondog
 */

#ifdef TEST_ALL_IN_ONE
    #include <boost/test/unit_test.hpp>
#else
    #define BOOST_TEST_MODULE test_hikyuu_indicator_suite
    #include <boost/test/unit_test.hpp>
#endif

#include <hikyuu/StockManager.h>
#include <hikyuu/indicator/crt/REF.h>
#include <hikyuu/indicator/crt/PRICELIST.h>

using namespace hku;

/**
 * @defgroup test_indicator_REF test_indicator_REF
 * @ingroup test_hikyuu_indicator_suite
 * @{
 */

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_REF ) {
    Indicator result;

    PriceList a;
    for (int i = 0; i < 10; ++i) {
        a.push_back(i);
    }

    Indicator data = PRICELIST(a);

    /** @arg n = 0 */
    result = REF(data, 0);
    BOOST_CHECK(result.discard() == 0);
    for (int i = 0; i <10; ++i) {
        BOOST_CHECK(result[i] == data[i]);
    }

    /** @arg n = 1 */
    result = REF(data, 1);
    BOOST_CHECK(result.discard() == 1);
    for (int i = 1; i < 10; ++i) {
        BOOST_CHECK(result[i] == data[i-1]);
    }

    /** @arg n = 9 */
    result = REF(data, 9);
    BOOST_CHECK(result.discard() == 9);
    BOOST_CHECK(result[9] == data[0]);

    /** @arg n = 10 */
    result = REF(data, 10);
    BOOST_CHECK(result.discard() == 10);
}

/** @} */


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * 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 "revgen.hpp"

#include <iostream>

#include <boost/dynamic_bitset.hpp>
#include <boost/program_options.hpp>

#include <cli/reversible_stores.hpp>
#include <core/utils/bitset_utils.hpp>
#include <classical/utils/truth_table_utils.hpp>
#include <reversible/truth_table.hpp>

namespace cirkit
{

using boost::program_options::value;

revgen_command::revgen_command( const environment::ptr& env )
  : cirkit_command( env, "Generate reversible structures" )
{
  opts.add_options()
    ( "hwb",   value( &hwb ),   "generate reversible HWB function" )
    ( "prime", value( &prime ), "generate reversible nth-prime function" )
    ;
}

bool revgen_command::execute()
{
  if ( is_set( "hwb" ) )
  {
    binary_truth_table spec;

    boost::dynamic_bitset<> b( hwb );

    spec.add_entry( number_to_truth_table_cube( 0u, hwb ), number_to_truth_table_cube( 0u, hwb ) );

    do {
      if ( b.any() )
      {
        const auto shift = b.count();
        const auto out = ( b << shift ) | ( b >> ( hwb - shift ) );

        const auto c_in = number_to_truth_table_cube( b.to_ulong(), hwb );
        const auto c_out = number_to_truth_table_cube( out.to_ulong(), hwb );

        spec.add_entry( c_in, c_out );
      }

      inc( b );
    } while ( b.any() );

    auto& specs = env->store<binary_truth_table>();

    if ( specs.empty() )
    {
      specs.extend();
    }
    specs.current() = spec;
  }
  else if ( is_set( "prime" ) )
  {
    binary_truth_table spec;

    spec.add_entry( number_to_truth_table_cube( 0u, prime ),
                    number_to_truth_table_cube( 0u, prime ) );

    auto index = 1u;
    auto pt = primes;

    while ( *pt < ( 1u << prime ) )
    {
      spec.add_entry( number_to_truth_table_cube( index++, prime ),
                      number_to_truth_table_cube( *pt++, prime ) );
    }

    pt = primes;
    auto non_prime = 1u;

    while ( index < ( 1u << prime ) )
    {
      while ( non_prime < *pt )
      {
        spec.add_entry( number_to_truth_table_cube( index++, prime ),
                        number_to_truth_table_cube( non_prime++, prime ) );
      }
      ++non_prime;
      ++pt;
    }

    auto& specs = env->store<binary_truth_table>();

    if ( specs.empty() )
    {
      specs.extend();
    }
    specs.current() = spec;
  }

  return true;
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


#ifndef INCLUDE_FEATURE_HPP_
#define INCLUDE_FEATURE_HPP_

#include <data.hpp>
#include <Eigen/Dense>

namespace terrainFeature
{

class Feature {

	public:
		Feature();

		~Feature();

    	void computeSlopeCost(double &cost, costmap::TerrainData &terrainParameters);

    	void computeHeightDeviationCost(double& cost, costmap::TerrainData &terrainParameters);

	private:
		double flatThreshold_, steepThreshold_;
		double maxCost_ = 1;
};

}
#endif  //  INCLUDE_FEATURE_HPP_


// Copyright (c) 2016 Graphcore Ltd. All rights reserved.
// Simple test case for IPU nonLinearity
//
#define BOOST_TEST_MODULE NonLinearityTest
#include "../popnn/NonLinearityInternal.hpp"
#include <boost/test/unit_test.hpp>
#include <iostream>
#include <limits>
#include <poplar/Engine.hpp>
#include <poplibs_support/TestDevice.hpp>
#include <poplibs_test/NonLinearity.hpp>
#include <poplibs_test/Util.hpp>
#include <poplin/codelets.hpp>
#include <popnn/NonLinearity.hpp>
#include <popnn/NonLinearityDefUtil.hpp>
#include <popnn/codelets.hpp>
#include <popops/EncodingConstants.hpp>
#include <popops/codelets.hpp>
#include <poputil/TileMapping.hpp>

using namespace poplar;
using namespace poplar::program;
using namespace poputil;
using namespace popnn;
using namespace poplibs_test;
using namespace poplibs_test::util;
using namespace poplibs_support;

namespace utf = boost::unit_test;
namespace fpc = boost::test_tools::fpc;

#define TOL 0.1 // tolerance of 0.1%
#define FLOAT_ATOL 1e-20
#define HALF_ATOL 1e-7

BOOST_AUTO_TEST_CASE(
    NonLinearity,
    *utf::tolerance<float>(fpc::percent_tolerance<float>(TOL)) *
        utf::tolerance<double>(fpc::percent_tolerance<double>(TOL))) {
  auto device = createTestDevice(TEST_TARGET);
  auto &target = device.getTarget();
  Graph graph(target);
  popnn::addCodelets(graph);
  popops::addCodelets(graph);
  // layer parameters

  const unsigned zNGroups = 1;
  const std::size_t zChunk = 1;
  const std::size_t ySize = 100;
  const std::size_t xSize = 30;
  auto actF =
      graph.addVariable(FLOAT, {1, zNGroups, ySize, xSize, zChunk}, "actF");
  auto actH =
      graph.addVariable(HALF, {1, zNGroups, ySize, xSize, zChunk}, "actH");
  auto deltaF =
      graph.addVariable(FLOAT, {1, zNGroups, ySize, xSize, zChunk}, "actF");
  auto deltaH =
      graph.addVariable(HALF, {1, zNGroups, ySize, xSize, zChunk}, "actH");

  // arbitraray mappings
  mapTensorLinearly(graph, actF);
  mapTensorLinearly(graph, actH);
  mapTensorLinearly(graph, deltaF);
  mapTensorLinearly(graph, deltaH);

  graph.createHostWrite("inF", actF);
  graph.createHostWrite("inH", actH);
  graph.createHostRead("outF", actF);
  graph.createHostRead("outH", actH);
  graph.createHostWrite("inDeltaF", deltaF);
  graph.createHostWrite("inDeltaH", deltaH);
  graph.createHostRead("outDeltaF", deltaF);
  graph.createHostRead("outDeltaH", deltaH);

  const auto batchSize = 1;

  // test inputs calculated in harness
  boost::multi_array<double, 4> hActIn(
      boost::extents[batchSize][ySize][xSize][zChunk]);
  boost::multi_array<double, 4> hDeltaIn(
      boost::extents[batchSize][ySize][xSize][zChunk]);

  // outputs calculated by target code
  std::size_t actOutFSize = 0;
  std::size_t actOutHSize = 0;
  std::size_t actInFSize = 0;
  std::size_t actInHSize = 0;
  auto rawHActOutF = allocateHostMemoryForTensor(target, actF, 1, actOutFSize);
  auto rawHActOutH = allocateHostMemoryForTensor(target, actH, 1, actOutHSize);
  auto rawHActInF = allocateHostMemoryForTensor(target, actF, 1, actInFSize);
  auto rawHActInH = allocateHostMemoryForTensor(target, actH, 1, actInHSize);

  std::size_t dOutFSize = 0;
  std::size_t dOutHSize = 0;
  std::size_t dInFSize = 0;
  std::size_t dInHSize = 0;
  auto rawHDeltaOutF =
      allocateHostMemoryForTensor(target, deltaF, 1, dOutFSize);
  auto rawHDeltaOutH =
      allocateHostMemoryForTensor(target, deltaH, 1, dOutHSize);
  auto rawHDeltaInF = allocateHostMemoryForTensor(target, deltaF, 1, dInFSize);
  auto rawHDeltaInH = allocateHostMemoryForTensor(target, deltaH, 1, dInHSize);
  boost::multi_array<double, 4> hActOutF(
      boost::extents[batchSize][ySize][xSize][zChunk]);
  boost::multi_array<double, 4> hActOutH(
      boost::extents[batchSize][ySize][xSize][zChunk]);
  boost::multi_array<double, 4> hDeltaOutF(
      boost::extents[batchSize][ySize][xSize][zChunk]);
  boost::multi_array<double, 4> hDeltaOutH(
      boost::extents[batchSize][ySize][xSize][zChunk]);

  // reference results calculated in harness
  boost::multi_array<double, 4> hRefActOut(
      boost::extents[batchSize][ySize][xSize][zChunk]);
  boost::multi_array<double, 4> hRefDeltaOut(
      boost::extents[batchSize][ySize][xSize][zChunk]);

  // initialse hInF[][] to arbitrary values
  float val = -100.0;
  for (unsigned b = 0; b < batchSize; ++b) {
    for (unsigned y = 0; y < ySize; ++y) {
      for (unsigned x = 0; x < xSize; ++x) {
        for (unsigned chan = 0; chan < zChunk; chan++) {
          hRefDeltaOut[b][y][x][chan] = hDeltaIn[b][y][x][chan] = val / 200;
          hActIn[b][y][x][chan] = val + 1000 * chan;
        }
        val += 7.01;
        if (val > 200)
          val -= 400;
      }
    }
  }

  for (auto n : {
           NonLinearityType::RELU,
           NonLinearityType::SIGMOID,
           NonLinearityType::TANH,
           NonLinearityType::GELU,
       }) {
    // Check backward gradient calculations
    std::cerr << "Check nl type " << n << "\n";
    // Check forward activation calculation
    hRefActOut = hActIn;
    poplibs_test::nonLinearity(n, hRefActOut);
    // build and run the target code
    auto fwdProg = Sequence();
    nonLinearityInPlace(graph, n, actF, fwdProg);
    nonLinearityInPlace(graph, n, actH, fwdProg);
    ;
    Engine fwdEng(graph, fwdProg);
    device.bind([&](const Device &d) {
      fwdEng.load(d);
      copy(target, hActIn, FLOAT, rawHActInF.get());
      fwdEng.writeTensor("inF", rawHActInF.get(),
                         rawHActInF.get() + actInFSize);
      copy(target, hActIn, HALF, rawHActInH.get());
      fwdEng.writeTensor("inH", rawHActInH.get(),
                         rawHActInH.get() + actInHSize);
      fwdEng.run();
      fwdEng.readTensor("outF", rawHActOutF.get(),
                        rawHActOutF.get() + actOutFSize);
      fwdEng.readTensor("outH", rawHActOutH.get(),
                        rawHActOutH.get() + actOutHSize);
    });
    copy(target, HALF, rawHActOutH.get(), hActOutH);
    copy(target, FLOAT, rawHActOutF.get(), hActOutF);

    BOOST_TEST(checkIsClose("outF", hActOutF, hRefActOut, TOL, FLOAT_ATOL));
    BOOST_TEST(checkIsClose("outH", hActOutH, hRefActOut, TOL, HALF_ATOL));

    hRefDeltaOut = hDeltaIn;
    poplibs_test::bwdNonLinearity(n, hActIn, hRefDeltaOut);
    // build and run the target code
    auto bwdProg = Sequence();
    auto deltaFF = nonLinearityInputGradient(graph, n, actF, deltaF, bwdProg);
    bwdProg.add(Copy(deltaFF, deltaF));
    auto deltaHH = nonLinearityInputGradient(graph, n, actH, deltaH, bwdProg);
    bwdProg.add(Copy(deltaHH, deltaH));
    Engine bwdEng(graph, bwdProg);
    device.bind([&](const Device &d) {
      bwdEng.load(d);
      copy(target, hActIn, FLOAT, rawHActInF.get());
      bwdEng.writeTensor("inF", rawHActInF.get(),
                         rawHActInF.get() + actInFSize);
      copy(target, hActIn, HALF, rawHActInH.get());
      bwdEng.writeTensor("inH", rawHActInH.get(),
                         rawHActInH.get() + actInHSize);
      copy(target, hDeltaIn, FLOAT, rawHDeltaInF.get());
      bwdEng.writeTensor("inDeltaF", rawHDeltaInF.get(),
                         rawHDeltaInF.get() + dInFSize);
      copy(target, hDeltaIn, HALF, rawHDeltaInH.get());
      bwdEng.writeTensor("inDeltaH", rawHDeltaInH.get(),
                         rawHDeltaInH.get() + dInHSize);
      bwdEng.run();
      bwdEng.readTensor("outDeltaF", rawHDeltaOutF.get(),
                        rawHDeltaOutF.get() + dOutFSize);
      bwdEng.readTensor("outDeltaH", rawHDeltaOutH.get(),
                        rawHDeltaOutH.get() + dOutHSize);
    });
    copy(target, HALF, rawHDeltaOutH.get(), hDeltaOutH);
    copy(target, FLOAT, rawHDeltaOutF.get(), hDeltaOutF);

    BOOST_TEST(
        checkIsClose("deltaOutF", hDeltaOutF, hRefDeltaOut, TOL, FLOAT_ATOL));
    BOOST_TEST(
        checkIsClose("deltaOutH", hDeltaOutH, hRefDeltaOut, TOL, HALF_ATOL));
  }
}

BOOST_AUTO_TEST_CASE(
    NonLinearitySoftMax,
    *utf::tolerance<float>(fpc::percent_tolerance<float>(0.1)) *
        utf::tolerance<double>(fpc::percent_tolerance<double>(0.1))) {
  auto device = createTestDevice(TEST_TARGET);
  auto &target = device.getTarget();
  Graph graph(target);
  popnn::addCodelets(graph);
  popops::addCodelets(graph);
  poplin::addCodelets(graph);

  // support only 2D
  const unsigned batchSize = 2;
  const unsigned numChannels = 128;

  auto actF = graph.addVariable(FLOAT, {batchSize, numChannels}, "actF");
  auto actH = graph.addVariable(HALF, {batchSize, numChannels}, "actH");
  auto deltaF = graph.addVariable(FLOAT, {batchSize, numChannels}, "deltaF");
  auto deltaH = graph.addVariable(HALF, {batchSize, numChannels}, "deltaH");

  // arbitrary mappings
  mapTensorLinearly(graph, actF);
  mapTensorLinearly(graph, actH);
  mapTensorLinearly(graph, deltaF);
  mapTensorLinearly(graph, deltaH);

  std::vector<std::pair<std::string, char *>> tmap;
  Sequence uploadProg, downloadProg;

  auto rawHActF = allocateHostMemoryForTensor(actF, "actF", graph, uploadProg,
                                              downloadProg, tmap);
  auto rawHActH = allocateHostMemoryForTensor(actH, "actH", graph, uploadProg,
                                              downloadProg, tmap);
  auto rawHDeltaF = allocateHostMemoryForTensor(deltaF, "deltaF", graph,
                                                uploadProg, downloadProg, tmap);
  auto rawHDeltaH = allocateHostMemoryForTensor(deltaH, "deltaH", graph,
                                                uploadProg, downloadProg, tmap);

  boost::multi_array<double, 2> hActIn(boost::extents[batchSize][numChannels]),
      hDeltaIn(boost::extents[batchSize][numChannels]),
      hActOutF(boost::extents[batchSize][numChannels]),
      hActOutH(boost::extents[batchSize][numChannels]),
      hDeltaOutF(boost::extents[batchSize][numChannels]),
      hDeltaOutH(boost::extents[batchSize][numChannels]);

  for (unsigned b = 0; b < batchSize; ++b) {
    for (unsigned c = 0; c < numChannels; ++c) {
      double sample = (1.0 - 2 * (c & 1)) * (1 + b) * 0.01 * c;
      hActIn[b][c] = sample;
      hDeltaIn[b][c] = double(b * numChannels) - double(c * batchSize);
    }
  }

  for (const auto nl :
       {NonLinearityType::SOFTMAX, NonLinearityType::SOFTMAX_STABLE,
        NonLinearityType::SOFTMAX_SCALED}) {
    auto hActOut = hActIn;
    poplibs_test::nonLinearity(nl, hActOut);
    if (nl == NonLinearityType::SOFTMAX_SCALED) {
      for (unsigned i = 0; i < batchSize; i++) {
        for (unsigned j = 0; j < numChannels; j++) {
          hActOut[i][j] *= SOFTMAX_SCALING;
        }
      }
    }
    // build and run the target code
    auto fwdProg = Sequence();
    float nonLinearityScalingF, nonLinearityScalingH;
    nonLinearityInPlace(graph, nl, actF, nonLinearityScalingF, fwdProg);
    nonLinearityInPlace(graph, nl, actH, nonLinearityScalingH, fwdProg);
    const float expectedScaling =
        nl == NonLinearityType::SOFTMAX_SCALED ? SOFTMAX_SCALING : 1.0f;

    BOOST_TEST(nonLinearityScalingF == expectedScaling);
    BOOST_TEST(nonLinearityScalingH == expectedScaling);

    copy(target, hActIn, FLOAT, rawHActF.get());
    copy(target, hActIn, HALF, rawHActH.get());
    Engine fwdEng(graph, Sequence(uploadProg, fwdProg, downloadProg));
    attachStreams(fwdEng, tmap);
    device.bind([&](const Device &d) { fwdEng.loadAndRun(d); });
    copy(target, FLOAT, rawHActF.get(), hActOutF);
    copy(target, HALF, rawHActH.get(), hActOutH);

    BOOST_TEST(checkIsClose("actOutF", hActOutF, hActOut, TOL, FLOAT_ATOL));
    BOOST_TEST(checkIsClose("actOutH", hActOutH, hActOut, TOL, HALF_ATOL));

    auto hRefDeltaOut = hDeltaIn;
    poplibs_test::bwdNonLinearity(nl, hActIn, hRefDeltaOut);

    auto bwdProg = Sequence();
    auto deltaFF = nonLinearityInputGradient(graph, nl, actF, deltaF, bwdProg);
    auto deltaHH = nonLinearityInputGradient(graph, nl, actH, deltaH, bwdProg);
    bwdProg.add(Copy(deltaFF, deltaF));
    bwdProg.add(Copy(deltaHH, deltaH));

    copy(target, hActIn, FLOAT, rawHActF.get());
    copy(target, hActIn, HALF, rawHActH.get());
    copy(target, hDeltaIn, FLOAT, rawHDeltaF.get());
    copy(target, hDeltaIn, HALF, rawHDeltaH.get());
    Engine bwdEng(graph, Sequence(uploadProg, bwdProg, downloadProg));
    attachStreams(bwdEng, tmap);
    device.bind([&](const Device &d) { bwdEng.loadAndRun(d); });
    copy(target, FLOAT, rawHDeltaF.get(), hDeltaOutF);
    copy(target, HALF, rawHDeltaH.get(), hDeltaOutH);

    BOOST_TEST(
        checkIsClose("deltaOutF", hDeltaOutF, hRefDeltaOut, TOL, FLOAT_ATOL));
    BOOST_TEST(
        checkIsClose("deltaOutH", hDeltaOutH, hRefDeltaOut, TOL, HALF_ATOL));
  }
}

BOOST_AUTO_TEST_CASE(
    NonLinearitySoftMax1D,
    *utf::tolerance<float>(fpc::percent_tolerance<float>(0.1)) *
        utf::tolerance<double>(fpc::percent_tolerance<double>(0.1))) {
  auto device = createTestDevice(TEST_TARGET);
  auto &target = device.getTarget();
  Graph graph(target);
  popnn::addCodelets(graph);
  popops::addCodelets(graph);
  poplin::addCodelets(graph);
  const unsigned numChannels = 128;

  auto actF = graph.addVariable(FLOAT, {numChannels}, "actF");
  auto actH = graph.addVariable(HALF, {numChannels}, "actH");
  auto deltaF = graph.addVariable(FLOAT, {numChannels}, "deltaF");
  auto deltaH = graph.addVariable(HALF, {numChannels}, "deltaH");

  // arbitrary mappings
  mapTensorLinearly(graph, actF);
  mapTensorLinearly(graph, actH);
  mapTensorLinearly(graph, deltaF);
  mapTensorLinearly(graph, deltaH);

  std::vector<std::pair<std::string, char *>> tmap;
  Sequence uploadProg, downloadProg;

  auto rawHActF = allocateHostMemoryForTensor(actF, "actF", graph, uploadProg,
                                              downloadProg, tmap);
  auto rawHActH = allocateHostMemoryForTensor(actH, "actH", graph, uploadProg,
                                              downloadProg, tmap);
  auto rawHDeltaF = allocateHostMemoryForTensor(deltaF, "deltaF", graph,
                                                uploadProg, downloadProg, tmap);
  auto rawHDeltaH = allocateHostMemoryForTensor(deltaH, "deltaH", graph,
                                                uploadProg, downloadProg, tmap);

  boost::multi_array<double, 2> hActIn(boost::extents[1][numChannels]),
      hDeltaIn(boost::extents[1][numChannels]),
      hActOutF(boost::extents[1][numChannels]),
      hActOutH(boost::extents[1][numChannels]),
      hDeltaOutF(boost::extents[1][numChannels]),
      hDeltaOutH(boost::extents[1][numChannels]);

  for (unsigned c = 0; c < numChannels; ++c) {
    double sample = (1.0 - 2 * (c & 1)) * 0.01 * c;
    hActIn[0][c] = sample;
    hDeltaIn[0][c] = double(numChannels - c);
  }

  for (const auto nl :
       {NonLinearityType::SOFTMAX, NonLinearityType::SOFTMAX_STABLE,
        NonLinearityType::SOFTMAX_SCALED}) {
    auto hActOut = hActIn;
    poplibs_test::nonLinearity(nl, hActOut);
    if (nl == NonLinearityType::SOFTMAX_SCALED) {
      for (unsigned i = 0; i < numChannels; i++) {
        hActOut[0][i] *= SOFTMAX_SCALING;
      }
    }
    // build and run the target code
    auto fwdProg = Sequence();
    float nonLinearityScalingF, nonLinearityScalingH;
    nonLinearityInPlace(graph, nl, actF, nonLinearityScalingF, fwdProg);
    nonLinearityInPlace(graph, nl, actH, nonLinearityScalingH, fwdProg);
    const float expectedScaling =
        nl == NonLinearityType::SOFTMAX_SCALED ? SOFTMAX_SCALING : 1.0f;

    BOOST_TEST(nonLinearityScalingF == expectedScaling);
    BOOST_TEST(nonLinearityScalingH == expectedScaling);

    copy(target, hActIn, FLOAT, rawHActF.get());
    copy(target, hActIn, HALF, rawHActH.get());
    Engine fwdEng(graph, Sequence(uploadProg, fwdProg, downloadProg));
    attachStreams(fwdEng, tmap);
    device.bind([&](const Device &d) { fwdEng.loadAndRun(d); });
    copy(target, FLOAT, rawHActF.get(), hActOutF);
    copy(target, HALF, rawHActH.get(), hActOutH);

    BOOST_TEST(checkIsClose("actOutF", hActOutF, hActOut, TOL, FLOAT_ATOL));
    BOOST_TEST(checkIsClose("actOutH", hActOutH, hActOut, TOL, HALF_ATOL));

    auto hRefDeltaOut = hDeltaIn;
    poplibs_test::bwdNonLinearity(nl, hActIn, hRefDeltaOut);

    auto bwdProg = Sequence();
    auto deltaFF = nonLinearityInputGradient(graph, nl, actF, deltaF, bwdProg);
    auto deltaHH = nonLinearityInputGradient(graph, nl, actH, deltaH, bwdProg);
    bwdProg.add(Copy(deltaFF, deltaF));
    bwdProg.add(Copy(deltaHH, deltaH));

    copy(target, hActIn, FLOAT, rawHActF.get());
    copy(target, hActIn, HALF, rawHActH.get());
    copy(target, hDeltaIn, FLOAT, rawHDeltaF.get());
    copy(target, hDeltaIn, HALF, rawHDeltaH.get());
    Engine bwdEng(graph, Sequence(uploadProg, bwdProg, downloadProg));
    attachStreams(bwdEng, tmap);
    device.bind([&](const Device &d) { bwdEng.loadAndRun(d); });
    copy(target, FLOAT, rawHDeltaF.get(), hDeltaOutF);
    copy(target, HALF, rawHDeltaH.get(), hDeltaOutH);

    BOOST_TEST(
        checkIsClose("deltaOutF", hDeltaOutF, hRefDeltaOut, TOL, FLOAT_ATOL));
    BOOST_TEST(
        checkIsClose("deltaOutH", hDeltaOutH, hRefDeltaOut, TOL, HALF_ATOL));
  }
}