// Copyright (C) 2008-2018 Lorenzo Caminiti
// Distributed under the Boost Software License, Version 1.0 (see accompanying
// file LICENSE_1_0.txt or a copy at http://www.boost.org/LICENSE_1_0.txt).
// See: http://www.boost.org/doc/libs/release/libs/contract/doc/html/index.html

//[n1962_circle
#include <boost/contract.hpp>
#include <cassert>

class shape {
public:
    virtual ~shape() {}

    virtual unsigned compute_area(boost::contract::virtual_* v = 0) const = 0;
};

unsigned shape::compute_area(boost::contract::virtual_* v) const {
    unsigned result;
    boost::contract::check c = boost::contract::public_function(v, result, this)
        .postcondition([&] (int const& result) {
            BOOST_CONTRACT_ASSERT(result > 0);
        })
    ;
    assert(false);
    return result;
}

class circle
    #define BASES public shape
    : BASES
{
    friend class boost::contract::access;

    typedef BOOST_CONTRACT_BASE_TYPES(BASES) base_types;
    #undef BASES

    BOOST_CONTRACT_OVERRIDE(compute_area);

public:
    static int const pi = 3; // Truncated to int from 3.14...

    explicit circle(unsigned a_radius) : radius_(a_radius) {
        boost::contract::check c = boost::contract::constructor(this)
            .postcondition([&] {
                BOOST_CONTRACT_ASSERT(radius() == a_radius);
            })
        ;
    }

    virtual unsigned compute_area(boost::contract::virtual_* v = 0) const
            /* override */ {
        unsigned result;
        boost::contract::check c = boost::contract::public_function<
                override_compute_area>(v, result, &circle::compute_area, this)
            .postcondition([&] (unsigned const& result) {
                BOOST_CONTRACT_ASSERT(result == pi * radius() * radius());
            })
        ;

        return result = pi * radius() * radius();
    }

    unsigned radius() const {
        boost::contract::check c = boost::contract::public_function(this);
        return radius_;
    }

private:
    unsigned radius_;
};

int main() {
    circle c(2);
    assert(c.radius() == 2);
    assert(c.compute_area() == 12);
    return 0;
}
//]


/*-----------------------------------------------------------
Author: Rahul Bhadani
Initial Date: January 2020

This class implements a Gazebo sensor plugin that calculates
the minimum distance of all the obstacle among all the laser
points from front laser points

-------------------------------------------------------------*/

#include<ros/ros.h>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <tf/transform_broadcaster.h>

#include <ignition/math/Box.hh>
#include <ignition/math/Pose3.hh>
#include <ignition/math/Vector3.hh>
#include <ignition/math/Quaternion.hh>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>
#include <gazebo/sensors/SensorTypes.hh>
#include <gazebo/sensors/RaySensor.hh>
#include <gazebo/sensors/Sensor.hh>
#include <gazebo_plugins/gazebo_ros_laser.h>
#include <gazebo/transport/TransportTypes.hh>
#include <gazebo_plugins/PubQueue.h>

#include <iostream>
#include <cstdlib>
#include <string>
#include <Eigen/Dense>
#include <Eigen/QR>

using namespace std;

namespace gazebo
{
    class distance: public SensorPlugin
    {
        private:

            physics::WorldPtr world; // Point to the World
            string distance_topic; // distance topic that will publish the minimum distance
            string angle_topic; // distance topic that will publish the angle of minimum distance
            string relvel_topic; // relative velocity topic
            string ns; // The Namespace

            ros::Publisher distance_publisher; // ROS publisher for minimum distance information (lead object)
            ros::Publisher relvel_publisher; // ROS publisher for relative velocity of the lead object
            ros::Publisher angle_publisher; // ROS publisher for angle of minimum distance information
            ros::NodeHandle* rosnode; // ROS Node
            sensor_msgs::LaserScan laser_data; // stores received laser scan data
            std_msgs::Float64 minimum_distance; // saves minimum distance found
            std_msgs::Float64 angle_of_min_distance; //saved the angle of minimum distance
            geometry_msgs::Twist relvel_data;
            double angle_min;
            double angle_max;

            sensors::RaySensorPtr parent_sensor;
            event::ConnectionPtr new_laserscan_connection;

            gazebo::transport::NodePtr gazebo_node;
            gazebo::transport::SubscriberPtr laser_scan_sub;

            ros::Time lastUpdate;
            std_msgs::Float64 old_dist;
            vector<double> old_relvel;
            vector<double> old_distance;
            vector<double> old_time;
            int rv_length;
            const unsigned int N_POINTS = 64;
        public:

            distance()
            {
                this->ns = "";
                this->distance_topic = this->ns + "/lead_dist";
                this->angle_topic = this->ns + "/lead_angle";
                this->relvel_topic = this->ns + "/rel_vel";
                this->angle_min = -M_PI;
                this->angle_max = M_PI;
                this->old_dist.data = 0.0;
                lastUpdate = ros::Time();
                rv_length = 0;

                old_relvel = vector<double>(N_POINTS);
                old_time = vector<double>(N_POINTS);
                old_distance = vector<double>(N_POINTS);
                for (int i = 0; i < old_relvel.size() ; ++i)
                {
                    old_relvel.at(i) = 0.0;
                    old_distance.at(i) = 0.0;
                    old_time.at(i) = 0.0;
                }

            }

            void polyfit(	const std::vector<double> &t,
                    const std::vector<double> &v,
                    std::vector<double> &coeff,
                    int order

                    )
            {
                if(rv_length+1 < N_POINTS)
                {
                    return;
                }

                // Create Matrix Placeholder of size n x k, n= number of datapoints, k = order of polynomial, for exame k = 3 for cubic polynomial
                Eigen::MatrixXd T(t.size(), order + 1);
                Eigen::VectorXd V = Eigen::VectorXd::Map(&v.front(), v.size());
                Eigen::VectorXd result;

                // check to make sure inputs are correct
                assert(t.size() == v.size());
                assert(t.size() >= order + 1);
                // Populate the matrix
                for(size_t i = 0 ; i < t.size(); ++i)
                {
                    for(size_t j = 0; j < order + 1; ++j)
                    {
                        T(i, j) = pow(t.at(i) - t.at(0), j);
                    }
                }
                //std::cout<<T<<std::endl;

                // Solve for linear least square fit
                result  = T.householderQr().solve(V);
                coeff.resize(order+1);
                for (int k = 0; k < order+1; k++)
                {
                    coeff[k] = result[k];
                }

            }

            /* Returns nth point as average of the last n points, no argument is passed since it uses class variables*/
            void movingAverage(int n_iterations)
            {
                if(rv_length < N_POINTS)
                {
                    return;
                }
                double sum = 0.0;
                for (int i = 0; i < old_relvel.size() ; ++i)
                {
                    sum = sum + old_relvel.at(i);

                }
                old_relvel.at( old_relvel.size() -1 ) =  sum/rv_length;
                if (n_iterations <= 1)
                {
                    return;
                }
                else
                {
                    movingAverage(n_iterations - 1);
                }

            }

            double MA()
            {

                if(rv_length < N_POINTS-1)
                {
                    //ROS_INFO_STREAM("rv_length: "<< rv_length);
                    //ROS_INFO_STREAM("N_POINTS: "<< N_POINTS);
                    //ROS_INFO_STREAM("Returning as it is ");
                    return (old_relvel.at( old_relvel.size() -1 ));
                }
                double sum = 0.0;
                for (int i = 0; i < old_relvel.size() ; ++i)
                {
                    sum = sum + old_relvel.at(i);

                }
                return(sum/rv_length);
            }

            double MADistance()
            {

                if(rv_length < N_POINTS-1)
                {
                    return (old_distance.at( old_distance.size() -1 ));
                }
                double sum = 0.0;
                for (int i = 0; i < old_distance.size() ; ++i)
                {
                    sum = sum + old_distance.at(i);

                }
                return(sum/rv_length);
            }

            void OnNewScan(ConstLaserScanStampedPtr &_msg)
            {
                vector<double> allranges;
                this->parent_sensor->Ranges(allranges);
                int raycount = this->parent_sensor->RayCount();
                // Get the minimum distance
                double min_dist = this->parent_sensor->RangeMax();
                double range_min = this->parent_sensor->RangeMin();
                double min_angle = this->parent_sensor->AngleMin()();
                double angle_tmp = min_angle;
                double angle_incr = this->parent_sensor->AngleResolution();

                double temp_val = 100.0;
                for(vector<double>::const_iterator it = allranges.begin(); it != allranges.end(); it++, angle_tmp += angle_incr)
                {
                    if(isinf(*it))
                    {
                        temp_val = 100.0;
                    }
                    else
                    {
                        temp_val = *it;
                    }

                    if(min_dist > temp_val && temp_val > range_min && angle_tmp > this->angle_min && angle_tmp < this->angle_max)
                    {
                        min_dist = temp_val;
                        min_angle = angle_tmp;
                    }
                }

                if(isinf(min_dist))
                {
                    ROS_ERROR_STREAM("Infinity Distance Detected. Setting to zero.");
                }
                this->minimum_distance.data = min_dist;
                this->angle_of_min_distance.data = min_angle;

                ros::Time nowtime = ros::Time::now();
                ros::Duration diff = nowtime - lastUpdate;
                double elapsedTime = diff.toSec();
                double nanoSecs = diff.toNSec()*1e-9;
                //elapsedTime = elapsedTime + nanoSecs;
                lastUpdate = nowtime;

         //       ROS_INFO_STREAM("elapsedTime Sec: " << elapsedTime);
         //       ROS_INFO_STREAM("nanoSec: " << nanoSecs);

                // Check if the old time is same as new time.

                double oldtimedata = old_time.at(old_relvel.size() -1);

                /* Store the data point old_relvel array */
                if (oldtimedata != nowtime.toSec())
                {
                    for (int i = 1; i <= rv_length; ++i)
                    {
                        old_distance.at(i-1) = old_distance.at(i);
                        old_relvel.at(i-1) = old_relvel.at(i);
                        old_time.at(i-1) = old_time.at(i);
                    }
                    old_distance.at(rv_length) = min_dist;
                    old_relvel.at(rv_length) = (min_dist - this->old_dist.data)/(2.2*elapsedTime);
                    old_time.at(rv_length) = nowtime.toSec();
                }

                if(rv_length < N_POINTS-1)
                {
                    rv_length = rv_length + 1;
                }

                #if 0
                if (rv_length > 2)
                {
                    double acceleration = (old_relvel.at(rv_length) - old_relvel.at(rv_length -1) )/(elapsedTime);
                    // cap the relative acceleration to be within +- 3.0 m/s^2
                    if(abs(acceleration) > 3.0)
                    {
                        old_relvel.at(rv_length) = old_relvel.at(rv_length -1) + copysign(3.0*elapsedTime, acceleration);
                    }
                    if( isinf(old_relvel.at(rv_length) ))
                    {
                        ROS_ERROR_STREAM("Estimated relative velocity is set at infinity.");
                    }
                }
                #endif
                /* Store ends */

                //movingAverage(5); // 5 times movies average
                // placeholder for storing polynomial coefficient
                std::vector<double> coeff;
                std::vector<double> dist_coeff;

                #if 0
                double vfitted = 0.0;
                double dfitted = 0.0;

                //	ROS_INFO_STREAM("rv length: "<<rv_length);
                if(rv_length >= N_POINTS-1)
                {
                    polyfit(old_time, old_relvel, coeff, 3);
                    polyfit(old_time, old_distance, dist_coeff, 3);

                    if(isinf(coeff[0]))
                    {
                        ROS_ERROR_STREAM("Coefficient 0 for relative veocity fitting is inf");
                    }
                    if(isinf(coeff[1]))
                    {
                        ROS_ERROR_STREAM("Coefficient 1 for relative veocity fitting is inf");
                    }
                    if(isinf(coeff[2]))
                    {
                        ROS_ERROR_STREAM("Coefficient 2 for relative veocity fitting is inf");
                    }
                    if(isinf(dist_coeff[0]))
                    {
                        ROS_ERROR_STREAM("Coefficient 1 for relative distance fitting is inf");
                    }
                    if(isinf(dist_coeff[1]))
                    {
                        ROS_ERROR_STREAM("Coefficient 2 for relative distance fitting is inf");
                    }
                    if(isinf(dist_coeff[2]))
                    {
                        ROS_ERROR_STREAM("Coefficient 0 for relative distance fitting is inf");
                    }
                    //		std::cout <<"Coefficients are "<< coeff[0] <<", "<<coeff[1] <<", "<<coeff[2] <<", "<<coeff[3]<<std::endl;
                    for(int p = 0; p < old_time.size(); ++ p)
                    {
                        double vfitted = coeff[0] + coeff[1]*(old_time.at(p)-old_time.at(0))  + coeff[2]*(pow(old_time.at(p) -old_time.at(0) , 2)) + coeff[3]*(pow(old_time.at(p) - old_time.at(0), 3)) ;
                        double dfitted = dist_coeff[0] + dist_coeff[1]*(old_time.at(p)-old_time.at(0)) + dist_coeff[2]*(pow(old_time.at(p) -old_time.at(0) , 2)) + dist_coeff[3]*(pow(old_time.at(p) - old_time.at(0), 3)) ;
                        //		std::cout << "Old = "<< old_relvel.at(p) <<", new ="<<vfitted<<std::endl;
                        old_relvel.at(p) = vfitted;
                        old_distance.at(p) = dfitted;
                    }

                }

                if(rv_length >= N_POINTS-1)
                {
                    vfitted = coeff[0] + coeff[1]*(old_time.at( rv_length -1 )-old_time.at(0)) + coeff[2]*(pow(old_time.at( rv_length -1) -old_time.at(0) , 2)) + coeff[3]*(pow(old_time.at( rv_length -1 ) - old_time.at(0), 3)) ;
                    dfitted = dist_coeff[0] + dist_coeff[1]*(old_time.at( rv_length -1 )-old_time.at(0)) + dist_coeff[2]*(pow(old_time.at( rv_length -1) -old_time.at(0) , 2)) + dist_coeff[3]*(pow(old_time.at( rv_length -1 ) - old_time.at(0), 3)) ;
                }

                //this->relvel_data.linear.z = vfitted; // old_relvel.at( rv_length) ;
                if( isinf(this->relvel_data.linear.z  ))
                {
                    ROS_ERROR_STREAM("Estimated (filtered) relative velocity is set at infinity.");
                }

                //ROS_INFO_STREAM("this->relvel.linear.z: "<<this->relvel_data.linear.z);
                //this->minimum_distance.data = dfitted;

                #endif
                this->relvel_data.linear.z =  MA() ; //old_relvel.at( rv_length) ;
                this->minimum_distance.data = old_distance.at( rv_length) - 4.5  ;
                if( isinf(this->minimum_distance.data))
                {
                    ROS_ERROR_STREAM("Estimated (filtered) relative distance is set at infinity.");
                }

   //             ROS_INFO_STREAM("this->relvel_data.linear.z: "<<this->relvel_data.linear.z);
   //             ROS_INFO_STREAM("this->minimum_distance.data: "<<this->minimum_distance.data);
                this->distance_publisher.publish(this->minimum_distance);
                //ROS_INFO_STREAM("distance published");
                this->angle_publisher.publish(this->angle_of_min_distance);
                //ROS_INFO_STREAM("angle published");
                this->relvel_publisher.publish(this->relvel_data);
                //ROS_INFO_STREAM("relvel published");
                this->old_dist.data = min_dist;
                //ROS_INFO_STREAM("last value saved:"<< min_dist);
            }

            void Load(sensors::SensorPtr parent, sdf::ElementPtr sdf)
            {
                ROS_INFO_STREAM("Minimum distance estimator SensorPlugin Loaded.");
                this->gazebo_node = gazebo::transport::NodePtr(new gazebo::transport::Node());
                this->gazebo_node->Init("default");

                // Get the name of the parent sensor
                GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST;
                this->parent_sensor = dynamic_pointer_cast<sensors::RaySensor>(parent);

                if(!this->parent_sensor)
                {
                    gzthrow("[distance SensorPlugin] distance plugin requires a Ray Sensor as its parent");
                }

                this->world = physics::get_world(this->parent_sensor->WorldName());
                string grandparent = this->parent_sensor->ParentName();
                size_t pos = grandparent.find("::");
                this->ns = "/" + grandparent.substr (0, pos);
                ROS_INFO_STREAM("[distance SensorPlugin] Namespace retrieved is: "<<this->ns);
                this->distance_topic = this->ns + "/lead_dist";
                this->angle_topic = this->ns + "/lead_angle";
                this->relvel_topic = this->ns + "/rel_vel";

                if(sdf->HasElement("angleMin"))
                {
                    this->angle_min = sdf->GetElement("angleMin")->Get<double>();
                }
                else
                {
                    gzwarn << "[distance SensorPlugin] Using default minimum angle: " << this->angle_min << "\n";
                }
                if(sdf->HasElement("angleMax"))
                {
                    this->angle_max = sdf->GetElement("angleMax")->Get<double>();
                }
                else
                {
                    gzwarn << "[distance SensorPlugin] Using default maximum angle: " << this->angle_max << "\n";
                }

                ROS_INFO_STREAM("[distance SensorPlugin] Plugin's parent name: "<<this->parent_sensor->Name());
                ROS_INFO_STREAM("[distance SensorPlugin] Plugin's granparent: "<<this->parent_sensor->ParentName() );
                ROS_INFO_STREAM("[distance SensorPlugin] Plugin's full scope name: "<<this->parent_sensor->ScopedName() );
                this->rosnode = new ros::NodeHandle(this->ns+"/distance");
                this->distance_publisher = this->rosnode->advertise<std_msgs::Float64>(this->distance_topic, 1);
                this->angle_publisher = this->rosnode->advertise<std_msgs::Float64>(this->angle_topic, 1);
                this->relvel_publisher = this->rosnode->advertise<geometry_msgs::Twist>(this->relvel_topic, 1);

                // Subscribe Gazebo-topic and work on laser data and produce minimum of all distances.
                this->laser_scan_sub = this->gazebo_node->Subscribe(this->parent_sensor->Topic(), &distance::OnNewScan, this);
                ROS_INFO_STREAM("SENSOR PLUGIN LOADED");
            }
    };
    GZ_REGISTER_SENSOR_PLUGIN(distance)
}


/**
 * @file serialization.hpp
 * @author Ryan Curtin
 *
 * Miscellaneous utility functions for serialization tests.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_TESTS_SERIALIZATION_HPP
#define MLPACK_TESTS_SERIALIZATION_HPP

#include <boost/serialization/serialization.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <mlpack/core.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

namespace mlpack {

// Test function for loading and saving Armadillo objects.
template<typename CubeType,
         typename IArchiveType,
         typename OArchiveType>
void TestArmadilloSerialization(arma::Cube<CubeType>& x)
{
  // First save it.
  // Use type_info name to get unique file name for serialization test files.
  std::string fileName = FilterFileName(typeid(IArchiveType).name());
  std::ofstream ofs(fileName, std::ios::binary);
  bool success = true;

  {
    OArchiveType o(ofs);

    try
    {
      o << BOOST_SERIALIZATION_NVP(x);
    }
    catch (boost::archive::archive_exception& e)
    {
      success = false;
    }
  }

  BOOST_REQUIRE_EQUAL(success, true);
  ofs.close();

  // Now load it.
  arma::Cube<CubeType> orig(x);
  success = true;
  std::ifstream ifs(fileName, std::ios::binary);

  {
    IArchiveType i(ifs);

    try
    {
      i >> BOOST_SERIALIZATION_NVP(x);
    }
    catch (boost::archive::archive_exception& e)
    {
      success = false;
    }
  }
  ifs.close();

  remove(fileName.c_str());

  BOOST_REQUIRE_EQUAL(success, true);

  BOOST_REQUIRE_EQUAL(x.n_rows, orig.n_rows);
  BOOST_REQUIRE_EQUAL(x.n_cols, orig.n_cols);
  BOOST_REQUIRE_EQUAL(x.n_elem_slice, orig.n_elem_slice);
  BOOST_REQUIRE_EQUAL(x.n_slices, orig.n_slices);
  BOOST_REQUIRE_EQUAL(x.n_elem, orig.n_elem);

  for (size_t slice = 0; slice != x.n_slices; ++slice)
  {
    const auto& origSlice = orig.slice(slice);
    const auto& xSlice = x.slice(slice);
    for (size_t i = 0; i < x.n_cols; ++i)
    {
      for (size_t j = 0; j < x.n_rows; ++j)
      {
        if (double(origSlice(j, i)) == 0.0)
          BOOST_REQUIRE_SMALL(double(xSlice(j, i)), 1e-8);
        else
          BOOST_REQUIRE_CLOSE(double(origSlice(j, i)), double(xSlice(j, i)),
              1e-8);
      }
    }
  }
}

// Test all serialization strategies.
template<typename CubeType>
void TestAllArmadilloSerialization(arma::Cube<CubeType>& x)
{
  TestArmadilloSerialization<CubeType, boost::archive::xml_iarchive,
      boost::archive::xml_oarchive>(x);
  TestArmadilloSerialization<CubeType, boost::archive::text_iarchive,
      boost::archive::text_oarchive>(x);
  TestArmadilloSerialization<CubeType, boost::archive::binary_iarchive,
      boost::archive::binary_oarchive>(x);
}

// Test function for loading and saving Armadillo objects.
template<typename MatType,
         typename IArchiveType,
         typename OArchiveType>
void TestArmadilloSerialization(MatType& x)
{
  // First save it.
  std::string fileName = FilterFileName(typeid(IArchiveType).name());
  std::ofstream ofs(fileName, std::ios::binary);
  bool success = true;

  {
    OArchiveType o(ofs);

    try
    {
      o << BOOST_SERIALIZATION_NVP(x);
    }
    catch (boost::archive::archive_exception& e)
    {
      success = false;
    }
  }

  BOOST_REQUIRE_EQUAL(success, true);
  ofs.close();

  // Now load it.
  MatType orig(x);
  success = true;
  std::ifstream ifs(fileName, std::ios::binary);

  {
    IArchiveType i(ifs);

    try
    {
      i >> BOOST_SERIALIZATION_NVP(x);
    }
    catch (boost::archive::archive_exception& e)
    {
      success = false;
    }
  }
  ifs.close();

  remove(fileName.c_str());

  BOOST_REQUIRE_EQUAL(success, true);

  BOOST_REQUIRE_EQUAL(x.n_rows, orig.n_rows);
  BOOST_REQUIRE_EQUAL(x.n_cols, orig.n_cols);
  BOOST_REQUIRE_EQUAL(x.n_elem, orig.n_elem);

  for (size_t i = 0; i < x.n_cols; ++i)
    for (size_t j = 0; j < x.n_rows; ++j)
      if (double(orig(j, i)) == 0.0)
        BOOST_REQUIRE_SMALL(double(x(j, i)), 1e-8);
      else
        BOOST_REQUIRE_CLOSE(double(orig(j, i)), double(x(j, i)), 1e-8);
}

// Test all serialization strategies.
template<typename MatType>
void TestAllArmadilloSerialization(MatType& x)
{
  TestArmadilloSerialization<MatType, boost::archive::xml_iarchive,
      boost::archive::xml_oarchive>(x);
  TestArmadilloSerialization<MatType, boost::archive::text_iarchive,
      boost::archive::text_oarchive>(x);
  TestArmadilloSerialization<MatType, boost::archive::binary_iarchive,
      boost::archive::binary_oarchive>(x);
}

// Save and load an mlpack object.
// The re-loaded copy is placed in 'newT'.
template<typename T, typename IArchiveType, typename OArchiveType>
void SerializeObject(T& t, T& newT)
{
  std::string fileName = FilterFileName(typeid(T).name());
  std::ofstream ofs(fileName, std::ios::binary);
  bool success = true;

  {
    OArchiveType o(ofs);

    try
    {
      o << BOOST_SERIALIZATION_NVP(t);
    }
    catch (boost::archive::archive_exception& e)
    {
      std::cerr << e.what() << std::endl;
      success = false;
    }
  }
  ofs.close();

  BOOST_REQUIRE_EQUAL(success, true);

  std::ifstream ifs(fileName, std::ios::binary);

  {
    IArchiveType i(ifs);

    try
    {
      i >> BOOST_SERIALIZATION_NVP(newT);
    }
    catch (boost::archive::archive_exception& e)
    {
      std::cout << e.what() << "\n";
      success = false;
    }
  }
  ifs.close();

  remove(fileName.c_str());

  BOOST_REQUIRE_EQUAL(success, true);
}

// Test mlpack serialization with all three archive types.
template<typename T>
void SerializeObjectAll(T& t, T& xmlT, T& textT, T& binaryT)
{
  SerializeObject<T, boost::archive::xml_iarchive,
      boost::archive::xml_oarchive>(t, xmlT);
  SerializeObject<T, boost::archive::text_iarchive,
      boost::archive::text_oarchive>(t, textT);
  SerializeObject<T, boost::archive::binary_iarchive,
      boost::archive::binary_oarchive>(t, binaryT);
}

// Save and load a non-default-constructible mlpack object.
template<typename T, typename IArchiveType, typename OArchiveType>
void SerializePointerObject(T* t, T*& newT)
{
  std::string fileName = FilterFileName(typeid(T).name());
  std::ofstream ofs(fileName, std::ios::binary);
  bool success = true;

  {
    OArchiveType o(ofs);
    try
    {
      o << BOOST_SERIALIZATION_NVP(t);
    }
    catch (boost::archive::archive_exception& e)
    {
      std::cout << e.what() << "\n";
      success = false;
    }
  }
  ofs.close();

  BOOST_REQUIRE_EQUAL(success, true);

  std::ifstream ifs(fileName, std::ios::binary);

  {
    IArchiveType i(ifs);

    try
    {
      i >> BOOST_SERIALIZATION_NVP(newT);
    }
    catch (std::exception& e)
    {
      std::cout << e.what() << "\n";
      success = false;
    }
  }
  ifs.close();

  remove(fileName.c_str());

  BOOST_REQUIRE_EQUAL(success, true);
}

template<typename T>
void SerializePointerObjectAll(T* t, T*& xmlT, T*& textT, T*& binaryT)
{
  SerializePointerObject<T, boost::archive::text_iarchive,
      boost::archive::text_oarchive>(t, textT);
  SerializePointerObject<T, boost::archive::binary_iarchive,
      boost::archive::binary_oarchive>(t, binaryT);
  SerializePointerObject<T, boost::archive::xml_iarchive,
      boost::archive::xml_oarchive>(t, xmlT);
}

// Utility function to check the equality of two Armadillo matrices.
void CheckMatrices(const arma::mat& x,
                   const arma::mat& xmlX,
                   const arma::mat& textX,
                   const arma::mat& binaryX);

void CheckMatrices(const arma::Mat<size_t>& x,
                   const arma::Mat<size_t>& xmlX,
                   const arma::Mat<size_t>& textX,
                   const arma::Mat<size_t>& binaryX);

void CheckMatrices(const arma::cube& x,
                   const arma::cube& xmlX,
                   const arma::cube& textX,
                   const arma::cube& binaryX);

} // namespace mlpack

#endif


/** \file ITL_Runner.h */

#pragma once

// MTL4 headers
#include <boost/numeric/itl/itl.hpp>
#include <boost/numeric/mtl/mtl.hpp>

// AMDiS headers
#include "solver/LinearSolver.hpp"
#include "solver/ITL_Preconditioner.hpp"

namespace AMDiS
{
  template <class MatrixType, class VectorType>
  struct PreconPair
  {
    /// Pointer to the left preconditioner
    ITL_PreconditionerBase<MatrixType, VectorType>* l = NULL;

    /// Pointer to the right preconditioner
    ITL_PreconditionerBase<MatrixType, VectorType>* r = NULL;

    ~PreconPair()
    {
      if (l)
      {
        l->exit();
        delete l;
        l = NULL;
      }

      if (r)
      {
        r->exit();
        delete r;
        r = NULL;
      }
    }
  };

  /** \ingroup Solver
   *
   * \brief
   * Wrapper class for different MTL4 itl-solvers. These solvers
   * are parametrized by Matrix- and VectorType.
   **/
  template <class ITLSolver, class MatrixType, class VectorType>
  struct ITL_Runner : public RunnerBase<MatrixType, VectorType>
  {
    using Super = RunnerBase<MatrixType, VectorType>;

    /// Constructor.
    ITL_Runner(LinearSolverInterface* oemPtr)
      : oem(*oemPtr),
        solver(oem.getName())
    {
      setPrecon(preconPair);
    }

    /// Destructor.
    ~ITL_Runner() {}

    /// Implementation of \ref RunnerBase::init()
    virtual void init(SolverMatrix<Matrix<DOFMatrix*>> const& A,
                      MatrixType const& fullMatrix) override
    {
      preconPair.l->init(A, fullMatrix);
      preconPair.r->init(A, fullMatrix);
    }

    /// Implementation of \ref RunnerBase::solve()
    virtual int solve(MatrixType const& A , VectorType& x,
                      VectorType const& b) override
    {
      FUNCNAME("ITL_Runner::solve()");

      TEST_EXIT(preconPair.l)("there is no left preconditioner\n");
      TEST_EXIT(preconPair.r)("there is no right preconditioner\n");

      using value_type = typename mtl::Collection<MatrixType>::value_type;

      VectorType r(num_rows(b));
      r = b;
      if (two_norm(x) != 0)
      {
        r = A * x ;
        r -= b;
      }
      int error = 0;
      if (oem.getInfo() == 0)
      {
        // iteration that does not print residual per iteration
        itl::basic_iteration<value_type>
        iter(r, oem.getMaxIterations(), oem.getRelative(), oem.getTolerance());

        error = solver(A, x, b, *(preconPair.l), *(preconPair.r), iter);
        oem.setErrorCode(error);
        oem.setIterations(iter.iterations());
        oem.setResidual(iter.resid());
        oem.setRelativeResidual(iter.relresid());
      }
      else
      {
        // print information about the solution process
        itl::cyclic_iteration<value_type>
        iter(r, oem.getMaxIterations(), oem.getRelative(), oem.getTolerance(),
             oem.getPrint_cycle());

        error = solver(A, x, b, *(preconPair.l), *(preconPair.r), iter);
        oem.setErrorCode(error);
        oem.setIterations(iter.iterations());
        oem.setResidual(iter.resid());
        oem.setRelativeResidual(iter.relresid());
      }

      return error;
    }

    /// Implementation of \ref RunnerBase::adjoint_solve()
    virtual int adjoint_solve(MatrixType const& /*A*/ ,
                              VectorType& /*x*/,
                              VectorType const& /*b*/) override
    {
      FUNCNAME("ITL_Runner::adjoint_solve()");
      ERROR_EXIT("Adjoint solve of preconditioner not yet implemented!\n");
      return 1;
    }

    /// Implementation of \ref RunnerInterface::exit()
    virtual void exit() override
    {
      preconPair.l->exit();
      preconPair.r->exit();
    }

    /// Implementation of \ref RunnerInterface::getLeftPrecon()
    virtual PreconditionerInterface* getLeftPrecon() override
    {
      return preconPair.l;
    }

    /// Implementation of \ref RunnerInterface::getRightPrecon()
    virtual PreconditionerInterface* getRightPrecon() override
    {
      return preconPair.r;
    }

  protected:
    /// create left/right preconditioners from parameters given in the init-file
    void setPrecon(PreconPair<MatrixType, VectorType>& pair)
    {
      FUNCNAME("ITL_Runner::setPrecon()");

      // Creator for the left preconditioner
      CreatorInterfaceName<ITL_PreconditionerBase<MatrixType, VectorType>>* leftCreator(NULL);

      // Creator for the right preconditioner
      CreatorInterfaceName<ITL_PreconditionerBase<MatrixType, VectorType>>* rightCreator(NULL);

      std::string preconType("no");
      std::string initFileStr = oem.getName() + "->left precon";
      Parameters::get(initFileStr, preconType);
      leftCreator = dynamic_cast<CreatorInterfaceName<ITL_PreconditionerBase<MatrixType, VectorType>>*>(
                      CreatorMap<ITL_PreconditionerBase<MatrixType, VectorType>>::getCreator(preconType, initFileStr) );
      TEST_EXIT(leftCreator != NULL)
      ("There is no creator for the given left preconditioner '%s'\n", preconType.c_str());
      leftCreator->setName(initFileStr);

      preconType = "no";
      initFileStr = oem.getName() + "->right precon";
      Parameters::get(initFileStr, preconType);
      rightCreator = dynamic_cast<CreatorInterfaceName<ITL_PreconditionerBase<MatrixType, VectorType>>*>(
                       CreatorMap<ITL_PreconditionerBase<MatrixType, VectorType>>::getCreator(preconType, initFileStr) );
      TEST_EXIT(rightCreator != NULL)
      ("There is no creator for the given right preconditioner '%s'\n", preconType.c_str());
      rightCreator->setName(initFileStr);

      pair.l = leftCreator->create();
      pair.r = rightCreator->create();
    }

  protected:
    LinearSolverInterface& oem;
    ITLSolver solver;

  private:
    PreconPair<MatrixType, VectorType> preconPair;
  };

} // end namespace AMDiS


// This file is part of LatNet Builder.
//
// Copyright (C) 2012-2021  The LatNet Builder author's, supervised by Pierre L'Ecuyer, Universite de Montreal.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "netbuilder/Types.h"
#include "netbuilder/DigitalNet.h"
#include "netbuilder/Task/MinimumObserver.h"
#include "netbuilder/Task/RandomSearch.h"

#include "netbuilder/FigureOfMerit/CoordUniformFigureOfMerit.h"
#include "latbuilder/Kernel/PAlphaTilde.h"
#include "latticetester/ProductWeights.h"

#include "latbuilder/TextStream.h"
using LatBuilder::TextStream::operator<<;

#include <boost/ref.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/tail_quantile.hpp>

using namespace NetBuilder;
using namespace boost::accumulators;

//! [observer]
template <NetConstruction NC>
class QuantilesObserver : public Task::MinimumObserver<NC>
{
    public:

        typedef accumulator_set<Real, features<tag::count, tag::min, tag::max, tag::mean, tag::tail_quantile<boost::accumulators::left>>> QuantilesAccumulator;

        QuantilesObserver(typename NetConstructionTraits<NC>::SizeParameter sizeParameter, int verbose = 0):
            Task::MinimumObserver<NC>(sizeParameter, verbose),
            m_numSamples(0),
            m_acc(tag::tail<boost::accumulators::left>::cache_size = m_numSamples)
        {};

        virtual void reset(bool hard = true) override
        {
            Task::MinimumObserver<NC>::reset(hard);
            if (hard)
                m_acc = QuantilesAccumulator(tag::tail<boost::accumulators::left>::cache_size = 0);
        }

        void setNumSamples(size_t numSamples)
        {
            m_numSamples = numSamples;
            m_acc = QuantilesAccumulator(tag::tail<boost::accumulators::left>::cache_size = m_numSamples);
        }

        virtual bool observe(std::unique_ptr<DigitalNet<NC>> net, const Real& merit) override
        {
            m_acc(merit);
            return Task::MinimumObserver<NC>::observe(std::move(net), merit);
        }

        const QuantilesAccumulator& accumulator()
        {
            return m_acc;
        }

        private:
            size_t m_numSamples;
            QuantilesAccumulator m_acc;
};
//! [observer]

template <typename T1, typename T2>
void printTableRow(const T1& x1, const T2& x2)
{
   using namespace std;
   cout << fixed << setprecision(3) << x1 << '\t'
      << scientific << setprecision(8) << x2 << std::endl;
}

int main(int argc, const char *argv[])
{
    //! [search_params]
    size_t numSamples = 1000;
    Dimension s = 10;
    NetConstructionTraits<NetConstruction::EXPLICIT>::SizeParameter sizeParam(10, 10);
    unsigned int alpha = 2;
    auto kernel = LatBuilder::Kernel::PAlphaTilde(alpha);
    auto weights = std::make_unique<LatticeTester::ProductWeights>(.7);
    auto figure = std::make_unique<FigureOfMerit::CoordUniformFigureOfMerit<LatBuilder::Kernel::PAlphaTilde, EmbeddingType::UNILEVEL>>(std::move(weights), kernel);
    std::cout << figure->format() << std::endl;
    //! [search_params]

    //! [task]
    auto task = Task::RandomSearch<NetConstruction::EXPLICIT, EmbeddingType::UNILEVEL, QuantilesObserver>(s, sizeParam, std::move(figure), (unsigned int) numSamples);
    task.observer().setNumSamples(numSamples);
    task.execute();
    //! [task]

    //! [output]
    const auto& acc = task.observer().accumulator();
    unsigned int numBins = 20;
    printTableRow("# mean:", mean(acc));
    printTableRow("prob", "quantile");
    printTableRow(0.0, boost::accumulators::min(acc));
    for (unsigned int i = 1; i < numBins; i++) {
        double p = double(i) / numBins;
        Real q = quantile(acc, quantile_probability = p);
        printTableRow(p, q);
    }
    printTableRow(1.0, boost::accumulators::max(acc));
    //! [output]

    return 0;
}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This is an example illustrating the use of the deep learning tools from the
    dlib C++ Library.  I'm assuming you have already read the dnn_introduction_ex.cpp
    example.  So in this example program I'm going to go over a number of more
    advanced parts of the API, including:
        - Using multiple GPUs
        - Training on large datasets that don't fit in memory
        - Defining large networks
        - Accessing and configuring layers in a network
*/

#include <dlib/dnn.h>
#include <iostream>
#include <dlib/data_io.h>

using namespace std;
using namespace dlib;

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

// Let's start by showing how you can conveniently define large and complex
// networks.  The most important tool for doing this are C++'s alias templates.
// These let us define new layer types that are combinations of a bunch of other
// layers.  These will form the building blocks for more complex networks.

// So let's begin by defining the building block of a residual network (see
// Figure 2 in Deep Residual Learning for Image Recognition by He, Zhang, Ren,
// and Sun).  We are going to decompose the residual block into a few alias
// statements.  First, we define the core block.

// Here we have parameterized the "block" layer on a BN layer (nominally some
// kind of batch normalization), the number of filter outputs N, and the stride
// the block operates at.
template <
    int N,
    template <typename> class BN,
    int stride,
    typename SUBNET
    >
using block  = BN<con<N,3,3,1,1,relu<BN<con<N,3,3,stride,stride,SUBNET>>>>>;

// Next, we need to define the skip layer mechanism used in the residual network
// paper.  They create their blocks by adding the input tensor to the output of
// each block.  So we define an alias statement that takes a block and wraps it
// with this skip/add structure.

// Note the tag layer.  This layer doesn't do any computation.  It exists solely
// so other layers can refer to it.  In this case, the add_prev1 layer looks for
// the tag1 layer and will take the tag1 output and add it to the input of the
// add_prev1 layer.  This combination allows us to implement skip and residual
// style networks.  We have also set the block stride to 1 in this statement.
// The significance of that is explained next.
template <
    template <int,template<typename>class,int,typename> class block,
    int N,
    template<typename>class BN,
    typename SUBNET
    >
using residual = add_prev1<block<N,BN,1,tag1<SUBNET>>>;

// Some residual blocks do downsampling.  They do this by using a stride of 2
// instead of 1.  However, when downsampling we need to also take care to
// downsample the part of the network that adds the original input to the output
// or the sizes won't make sense (the network will still run, but the results
// aren't as good).  So here we define a downsampling version of residual.  In
// it, we make use of the skip1 layer.  This layer simply outputs whatever is
// output by the tag1 layer.  Therefore, the skip1 layer (there are also skip2,
// skip3, etc. in dlib) allows you to create branching network structures.

// residual_down creates a network structure like this:
/*
         input from SUBNET
             /     \
            /       \
         block     downsample(using avg_pool)
            \       /
             \     /
           add tensors (using add_prev2 which adds the output of tag2 with avg_pool's output)
                |
              output
*/
template <
    template <int,template<typename>class,int,typename> class block,
    int N,
    template<typename>class BN,
    typename SUBNET
    >
using residual_down = add_prev2<avg_pool<2,2,2,2,skip1<tag2<block<N,BN,2,tag1<SUBNET>>>>>>;

// Now we can define 4 different residual blocks we will use in this example.
// The first two are non-downsampling residual blocks while the last two
// downsample.  Also, res and res_down use batch normalization while ares and
// ares_down have had the batch normalization replaced with simple affine
// layers.  We will use the affine version of the layers when testing our
// networks.
template <typename SUBNET> using res       = relu<residual<block,8,bn_con,SUBNET>>;
template <typename SUBNET> using ares      = relu<residual<block,8,affine,SUBNET>>;
template <typename SUBNET> using res_down  = relu<residual_down<block,8,bn_con,SUBNET>>;
template <typename SUBNET> using ares_down = relu<residual_down<block,8,affine,SUBNET>>;

// Now that we have these convenient aliases, we can define a residual network
// without a lot of typing.  Note the use of a repeat layer.  This special layer
// type allows us to type repeat<9,res,SUBNET> instead of
// res<res<res<res<res<res<res<res<res<SUBNET>>>>>>>>>.  It will also prevent
// the compiler from complaining about super deep template nesting when creating
// large networks.
const unsigned long number_of_classes = 10;
using net_type = loss_multiclass_log<fc<number_of_classes,
                            avg_pool_everything<
                            res<res<res<res_down<
                            repeat<9,res, // repeat this layer 9 times
                            res_down<
                            res<
                            input<matrix<unsigned char>>
                            >>>>>>>>>>;

// And finally, let's define a residual network building block that uses
// parametric ReLU units instead of regular ReLU.
template <typename SUBNET>
using pres  = prelu<add_prev1<bn_con<con<8,3,3,1,1,prelu<bn_con<con<8,3,3,1,1,tag1<SUBNET>>>>>>>>;

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

int main(int argc, char** argv) try
{
    if (argc != 2)
    {
        cout << "This example needs the MNIST dataset to run!" << endl;
        cout << "You can get MNIST from http://yann.lecun.com/exdb/mnist/" << endl;
        cout << "Download the 4 files that comprise the dataset, decompress them, and" << endl;
        cout << "put them in a folder.  Then give that folder as input to this program." << endl;
        return 1;
    }

    std::vector<matrix<unsigned char>> training_images;
    std::vector<unsigned long> training_labels;
    std::vector<matrix<unsigned char>> testing_images;
    std::vector<unsigned long> testing_labels;
    load_mnist_dataset(argv[1], training_images, training_labels, testing_images, testing_labels);

    // dlib uses cuDNN under the covers.  One of the features of cuDNN is the
    // option to use slower methods that use less RAM or faster methods that use
    // a lot of RAM.  If you find that you run out of RAM on your graphics card
    // then you can call this function and we will request the slower but more
    // RAM frugal cuDNN algorithms.
    set_dnn_prefer_smallest_algorithms();

    // Create a network as defined above.  This network will produce 10 outputs
    // because that's how we defined net_type.  However, fc layers can have the
    // number of outputs they produce changed at runtime.
    net_type net;
    // So if you wanted to use the same network but override the number of
    // outputs at runtime you can do so like this:
    net_type net2(num_fc_outputs(15));

    // Now, let's imagine we wanted to replace some of the relu layers with
    // prelu layers.  We might do it like this:
    using net_type2 = loss_multiclass_log<fc<number_of_classes,
                                avg_pool_everything<
                                pres<res<res<res_down< // 2 prelu layers here
                                tag4<repeat<9,pres,    // 9 groups, each containing 2 prelu layers
                                res_down<
                                res<
                                input<matrix<unsigned char>>
                                >>>>>>>>>>>;

    // prelu layers have a floating point parameter.  If you want to set it to
    // something other than its default value you can do so like this:
    net_type2 pnet(prelu_(0.2),
                   prelu_(0.25),
                   repeat_group(prelu_(0.3),prelu_(0.4)) // Initialize all the prelu instances in the repeat
                                                         // layer.  repeat_group() is needed to group the
                                                         // things that are part of repeat's block.
                   );
    // As you can see, a network will greedily assign things given to its
    // constructor to the layers inside itself.  The assignment is done in the
    // order the layers are defined, but it will skip layers where the
    // assignment doesn't make sense.

    // Now let's print the details of the pnet to the screen and inspect it.
    cout << "The pnet has " << pnet.num_layers << " layers in it." << endl;
    cout << pnet << endl;
    // These print statements will output this (I've truncated it since it's
    // long, but you get the idea):
    /*
        The pnet has 131 layers in it.
        layer<0>    loss_multiclass_log
        layer<1>    fc       (num_outputs=10) learning_rate_mult=1 weight_decay_mult=1 bias_learning_rate_mult=1 bias_weight_decay_mult=0
        layer<2>    avg_pool (nr=0, nc=0, stride_y=1, stride_x=1, padding_y=0, padding_x=0)
        layer<3>    prelu    (initial_param_value=0.2)
        layer<4>    add_prev1
        layer<5>    bn_con   eps=1e-05 learning_rate_mult=1 weight_decay_mult=0 bias_learning_rate_mult=1 bias_weight_decay_mult=1
        layer<6>    con      (num_filters=8, nr=3, nc=3, stride_y=1, stride_x=1, padding_y=1, padding_x=1) learning_rate_mult=1 weight_decay_mult=1 bias_learning_rate_mult=1 bias_weight_decay_mult=0
        layer<7>    prelu    (initial_param_value=0.25)
        layer<8>    bn_con   eps=1e-05 learning_rate_mult=1 weight_decay_mult=0 bias_learning_rate_mult=1 bias_weight_decay_mult=1
        layer<9>    con      (num_filters=8, nr=3, nc=3, stride_y=1, stride_x=1, padding_y=1, padding_x=1) learning_rate_mult=1 weight_decay_mult=1 bias_learning_rate_mult=1 bias_weight_decay_mult=0
        layer<10>   tag1
        ...
        layer<34>   relu
        layer<35>   bn_con   eps=1e-05 learning_rate_mult=1 weight_decay_mult=0 bias_learning_rate_mult=1 bias_weight_decay_mult=1
        layer<36>   con      (num_filters=8, nr=3, nc=3, stride_y=2, stride_x=2, padding_y=0, padding_x=0) learning_rate_mult=1 weight_decay_mult=1 bias_learning_rate_mult=1 bias_weight_decay_mult=0
        layer<37>   tag1
        layer<38>   tag4
        layer<39>   prelu    (initial_param_value=0.3)
        layer<40>   add_prev1
        layer<41>   bn_con   eps=1e-05 learning_rate_mult=1 weight_decay_mult=0 bias_learning_rate_mult=1 bias_weight_decay_mult=1
        ...
        layer<118>  relu
        layer<119>  bn_con   eps=1e-05 learning_rate_mult=1 weight_decay_mult=0 bias_learning_rate_mult=1 bias_weight_decay_mult=1
        layer<120>  con      (num_filters=8, nr=3, nc=3, stride_y=2, stride_x=2, padding_y=0, padding_x=0) learning_rate_mult=1 weight_decay_mult=1 bias_learning_rate_mult=1 bias_weight_decay_mult=0
        layer<121>  tag1
        layer<122>  relu
        layer<123>  add_prev1
        layer<124>  bn_con   eps=1e-05 learning_rate_mult=1 weight_decay_mult=0 bias_learning_rate_mult=1 bias_weight_decay_mult=1
        layer<125>  con      (num_filters=8, nr=3, nc=3, stride_y=1, stride_x=1, padding_y=1, padding_x=1) learning_rate_mult=1 weight_decay_mult=1 bias_learning_rate_mult=1 bias_weight_decay_mult=0
        layer<126>  relu
        layer<127>  bn_con   eps=1e-05 learning_rate_mult=1 weight_decay_mult=0 bias_learning_rate_mult=1 bias_weight_decay_mult=1
        layer<128>  con      (num_filters=8, nr=3, nc=3, stride_y=1, stride_x=1, padding_y=1, padding_x=1) learning_rate_mult=1 weight_decay_mult=1 bias_learning_rate_mult=1 bias_weight_decay_mult=0
        layer<129>  tag1
        layer<130>  input<matrix>
    */

    // Now that we know the index numbers for each layer, we can access them
    // individually using layer<index>(pnet).  For example, to access the output
    // tensor for the first prelu layer we can say:
    layer<3>(pnet).get_output();
    // Or to print the prelu parameter for layer 7 we can say:
    cout << "prelu param: "<< layer<7>(pnet).layer_details().get_initial_param_value() << endl;

    // We can also access layers by their type.  This next statement finds the
    // first tag1 layer in pnet, and is therefore equivalent to calling
    // layer<10>(pnet):
    layer<tag1>(pnet);
    // The tag layers don't do anything at all and exist simply so you can tag
    // parts of your network and access them by layer<tag>().  You can also
    // index relative to a tag.  So for example, to access the layer immediately
    // after tag4 you can say:
    layer<tag4,1>(pnet); // Equivalent to layer<38+1>(pnet).

    // Or to access the layer 2 layers after tag4:
    layer<tag4,2>(pnet);
    // Tagging is a very useful tool for making complex network structures.  For
    // example, the add_prev1 layer is implemented internally by using a call to
    // layer<tag1>().

    // Ok, that's enough talk about defining and inspecting networks.  Let's
    // talk about training networks!

    // The dnn_trainer will use SGD by default, but you can tell it to use
    // different solvers like adam with a weight decay of 0.0005 and the given
    // momentum parameters.
    dnn_trainer<net_type,adam> trainer(net,adam(0.0005, 0.9, 0.999));
    // Also, if you have multiple graphics cards you can tell the trainer to use
    // them together to make the training faster.  For example, replacing the
    // above constructor call with this one would cause it to use GPU cards 0
    // and 1.
    //dnn_trainer<net_type,adam> trainer(net,adam(0.0005, 0.9, 0.999), {0,1});

    trainer.be_verbose();
    // While the trainer is running it keeps an eye on the training error.  If
    // it looks like the error hasn't decreased for the last 2000 iterations it
    // will automatically reduce the learning rate by 0.1.  You can change these
    // default parameters to some other values by calling these functions.  Or
    // disable the automatic shrinking entirely by setting the shrink factor to 1.
    trainer.set_iterations_without_progress_threshold(2000);
    trainer.set_learning_rate_shrink_factor(0.1);
    // The learning rate will start at 1e-3.
    trainer.set_learning_rate(1e-3);
    trainer.set_synchronization_file("mnist_resnet_sync", std::chrono::seconds(100));

    // Now, what if your training dataset is so big it doesn't fit in RAM?  You
    // make mini-batches yourself, any way you like, and you send them to the
    // trainer by repeatedly calling trainer.train_one_step().
    //
    // For example, the loop below stream MNIST data to out trainer.
    std::vector<matrix<unsigned char>> mini_batch_samples;
    std::vector<unsigned long> mini_batch_labels;
    dlib::rand rnd(time(0));
    // Loop until the trainer's automatic shrinking has shrunk the learning rate to 1e-6.
    // Given our settings, this means it will stop training after it has shrunk the
    // learning rate 3 times.
    while(trainer.get_learning_rate() >= 1e-6)
    {
        mini_batch_samples.clear();
        mini_batch_labels.clear();

        // make a 128 image mini-batch
        while(mini_batch_samples.size() < 128)
        {
            auto idx = rnd.get_random_32bit_number()%training_images.size();
            mini_batch_samples.push_back(training_images[idx]);
            mini_batch_labels.push_back(training_labels[idx]);
        }

        // Tell the trainer to update the network given this mini-batch
        trainer.train_one_step(mini_batch_samples, mini_batch_labels);

        // You can also feed validation data into the trainer by periodically
        // calling trainer.test_one_step(samples,labels).  Unlike train_one_step(),
        // test_one_step() doesn't modify the network, it only computes the testing
        // error which it records internally.  This testing error will then be print
        // in the verbose logging and will also determine when the trainer's
        // automatic learning rate shrinking happens.  Therefore, test_one_step()
        // can be used to perform automatic early stopping based on held out data.
    }

    // When you call train_one_step(), the trainer will do its processing in a
    // separate thread.  This allows the main thread to work on loading data
    // while the trainer is busy executing the mini-batches in parallel.
    // However, this also means we need to wait for any mini-batches that are
    // still executing to stop before we mess with the net object.  Calling
    // get_net() performs the necessary synchronization.
    trainer.get_net();

    net.clean();
    serialize("mnist_res_network.dat") << net;

    // Now we have a trained network.  However, it has batch normalization
    // layers in it.  As is customary, we should replace these with simple
    // affine layers before we use the network.  This can be accomplished by
    // making a network type which is identical to net_type but with the batch
    // normalization layers replaced with affine.  For example:
    using test_net_type = loss_multiclass_log<fc<number_of_classes,
                                avg_pool_everything<
                                ares<ares<ares<ares_down<
                                repeat<9,ares,
                                ares_down<
                                ares<
                                input<matrix<unsigned char>>
                                >>>>>>>>>>;
    // Then we can simply assign our trained net to our testing net.
    test_net_type tnet = net;
    // Or if you only had a file with your trained network you could deserialize
    // it directly into your testing network.
    deserialize("mnist_res_network.dat") >> tnet;

    // And finally, we can run the testing network over our data.

    std::vector<unsigned long> predicted_labels = tnet(training_images);
    int num_right = 0;
    int num_wrong = 0;
    for (size_t i = 0; i < training_images.size(); ++i)
    {
        if (predicted_labels[i] == training_labels[i])
            ++num_right;
        else
            ++num_wrong;

    }
    cout << "training num_right: " << num_right << endl;
    cout << "training num_wrong: " << num_wrong << endl;
    cout << "training accuracy:  " << num_right/(double)(num_right+num_wrong) << endl;

    predicted_labels = tnet(testing_images);
    num_right = 0;
    num_wrong = 0;
    for (size_t i = 0; i < testing_images.size(); ++i)
    {
        if (predicted_labels[i] == testing_labels[i])
            ++num_right;
        else
            ++num_wrong;

    }
    cout << "testing num_right: " << num_right << endl;
    cout << "testing num_wrong: " << num_wrong << endl;
    cout << "testing accuracy:  " << num_right/(double)(num_right+num_wrong) << endl;

}
catch(std::exception& e)
{
    cout << e.what() << endl;
}


// Author(s): Jeroen Keiren
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// 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)
//
/// \file sumelm_test.cpp
/// \brief Add your file description here.

#include "mcrl2/lps/linearise.h"
#include "mcrl2/lps/parse.h"
#include "mcrl2/lps/specification.h"
#include "mcrl2/lps/sumelm.h"
#include <boost/test/included/unit_test_framework.hpp>
#include <iostream>
#include <string>

using namespace mcrl2;
using namespace mcrl2::data;
using namespace mcrl2::lps;

void print_specifications(const specification& s0, const specification& s1)
{
  if (!(s0 == s1))
  {
    std::clog << "=== Specifications differ ===" << std::endl;
    std::clog << "    Input specification  : " << lps::pp(s0) << std::endl
              << "    Output specification : " << lps::pp(s1) << std::endl;
  }
  else
  {
    std::clog << "=== Specification are the same ===" << std::endl;
  }
}

/*
 * Test case which tries to test all possibilities for substitutions. This is a
 * test for issue #367
 */
BOOST_AUTO_TEST_CASE(bug_367)
{
  std::clog << "Test case 1" << std::endl;
  const std::string text(
    "sort S = struct s1 | s2;\n"
    "map f : S -> Bool;\n"
    "act a : S # Bool;\n"
    "proc P = sum c : S, b : Bool . (b == f(c) && c == s2) -> a(c, b) . P;\n"
    "init P;\n"
  );

  specification s0 = parse_linear_process_specification(text);
  specification s1 = s0;
  sumelm_algorithm<>(s1).run();
  const action_summand_vector& summands1 = s1.process().action_summands();
  for (action_summand_vector::const_iterator i = summands1.begin(); i != summands1.end(); ++i)
  {
    BOOST_CHECK(i->summation_variables().empty());
    BOOST_CHECK(data::find_all_variables(i->condition()).empty());
    BOOST_CHECK(lps::find_all_variables(i->multi_action()).empty());
  }

  print_specifications(s0,s1);
}

/// Sum variable y does not occur in the summand, and therefore must be removed.
BOOST_AUTO_TEST_CASE(no_occurrence_of_variable)
{
  std::clog << "Test case 2" << std::endl;
  const std::string text(
    "act a,b;\n"
    "proc P(s3_P: Pos) = sum y_P: Int. (s3_P == 1) -> a . P(2)\n"
    "                  + (s3_P == 2) -> b . P(1);\n"
    "init P(1);\n"
  );

  specification s0 = parse_linear_process_specification(text);
  specification s1 = s0;
  sumelm_algorithm<>(s1).run();
  const action_summand_vector& summands1 = s1.process().action_summands();
  for (action_summand_vector::const_iterator i = summands1.begin(); i != summands1.end(); ++i)
  {
    BOOST_CHECK(i->summation_variables().empty());
  }

  print_specifications(s0,s1);
}

/*
 * The sum variable y is equal to 4 in the condition, hence 4 should be
 * substituted for y in the summand, and sum y should be removed
 * In this case, this boils down to removing sum y, and removing 4==y from the
 * condition. The rest of the specification can be left untouched
 */
BOOST_AUTO_TEST_CASE(reverse_equality)
{
  std::clog << "Test case 3" << std::endl;
  const std::string text(
    "act a;\n"
    "proc P = sum y:Int . (4 == y) -> a . P;\n"
    "init P;\n"
  );

  specification s0 = parse_linear_process_specification(text);
  specification s1 = s0;
  sumelm_algorithm<>(s1).run();
  const action_summand_vector& summands1 = s1.process().action_summands();
  for (action_summand_vector::const_iterator i = summands1.begin(); i != summands1.end(); ++i)
  {
    BOOST_CHECK(i->summation_variables().empty());
    BOOST_CHECK(data::find_all_variables(i->condition()).empty());
  }

  print_specifications(s0,s1);
}

/*
 * This is the same as test case 3, except with the equality in different order.
 */
BOOST_AUTO_TEST_CASE(equality)
{
  std::clog << "Test case 4" << std::endl;
  const std::string text(
    "act a;\n"
    "proc P = sum y:Int . (y == 4) -> a . P;\n"
    "init P;\n"
  );

  specification s0 = parse_linear_process_specification(text);
  specification s1 = s0;
  sumelm_algorithm<>(s1).run();
  const action_summand_vector& summands1 = s1.process().action_summands();
  for (action_summand_vector::const_iterator i = summands1.begin(); i != summands1.end(); ++i)
  {
    BOOST_CHECK(i->summation_variables().empty());
    BOOST_CHECK(data::find_all_variables(i->condition()).empty());
  }

  print_specifications(s0,s1);
}

/*
 * Test whether sum variables are correctly removed from actions and timing.
 */
BOOST_AUTO_TEST_CASE(actions_and_time)
{
  std::clog << "Test case 5" << std::endl;
  const std::string text(
    "act a,b:Int;\n"
    "proc P = sum y:Int . (y == 4) -> a(y)@y . b(y*2)@(y+1) . P;\n"
    "init P;\n"
  );

  // FIXME, this test case requires the parser to allow parsing of free
  // variables.
  specification s0=remove_stochastic_operators(linearise(text));
  specification s1 = s0;
  sumelm_algorithm<>(s1).run();
  std::set<variable> parameters = mcrl2::data::find_all_variables(s1.process().process_parameters());
  const action_summand_vector& summands1 = s1.process().action_summands();
  for (action_summand_vector::const_iterator i = summands1.begin(); i != summands1.end(); ++i)
  {
    BOOST_CHECK(i->summation_variables().empty());

    // Check that the only data variables in the condition and time
    // are process parameters
    std::set<variable> condition_vars = data::find_all_variables(i->condition());
    for (const auto & condition_var : condition_vars)
    {
      BOOST_CHECK(parameters.find(condition_var) != parameters.end());
    }

    if (i->has_time())
    {
      std::set<variable> time_vars = data::find_all_variables(i->multi_action().time());
      for (const auto & time_var : time_vars)
      {
        BOOST_CHECK(parameters.find(time_var) != parameters.end());
      }
    }
  }

  print_specifications(s0,s1);
}

/*
 * Test that a sum variable is not removed when it occurs in both sides of a
 * variable.
 */
BOOST_AUTO_TEST_CASE(both_sides_of_equality)
{
  std::clog << "Test case 6" << std::endl;
  const std::string text(
    "act a;\n"
    "proc P = sum y:Int . (y == y + 1) -> a . P;\n"
    "init P;\n"
  );

  specification s0 = parse_linear_process_specification(text);
  specification s1 = s0;
  sumelm_algorithm<>(s1).run();
  int sumvar_count = 0;
  const action_summand_vector& summands1 = s1.process().action_summands();
  for (const auto & i : summands1)
  {
    if (!i.summation_variables().empty())
    {
      ++sumvar_count;
    }
  }
  BOOST_CHECK(sumvar_count == 1);
  BOOST_CHECK(s0 == s1);

  print_specifications(s0,s1);
}

/*
 * The sum variable e occurs in the condition in the following ways:
 * - As righthandside of an equality
 * - Both as lefthandside and righthandside of an equality
 * - As lefthandside of an equality
 * The sum variable d occurs in the lefthandside of an equality in the
 * condition.
 * The sum variable f occurs in the righthandside of an equality in the
 * condition.
 *
 * Result:
 * - The summation over d is removed by substituting e for d
 * - The summation over e is removed by substituting f for e
 * This should leave only a sum f:D . f == g(f) -> a . X(f).
 */
BOOST_AUTO_TEST_CASE(three_sums_remove_two)
{
  std::clog << "Test case 7" << std::endl;
  const std::string text(
    "sort D = struct d1 | d2 | d3;\n"
    "map g : D -> D;\n"
    "act a;\n"
    "proc P(c:D) = sum d:D . sum e:D . sum f:D . (d == e && e == g(e) && e == f) -> a . P(d);\n"
    "init P(d1);\n"
  );

  specification s0 = parse_linear_process_specification(text);
  specification s1 = s0;
  sumelm_algorithm<>(s1).run();
  int sumvar_count = 0;
  const action_summand_vector& summands1 = s1.process().action_summands();
  for (const auto & i : summands1)
  {
    if (!i.summation_variables().empty())
    {
      ++sumvar_count;
    }
  }
  BOOST_CHECK(sumvar_count == 1);

  print_specifications(s0,s1);
}

/*
 * Sum variable d occurs twice in the lefthandside of an equality
 * The strongest set of substitutions possible is e := d; f := d, hence removing
 * two summations.
 * (Test introduced after a bugreport by M. Voorhoeve)
 */
BOOST_AUTO_TEST_CASE(twice_lhs)
{
  std::clog << "Test case 8" << std::endl;
  const std::string text(
    "sort D = struct d1 | d2 | d3;\n"
    "act a;\n"
    "proc P(c:D) = sum d:D . sum e:D . sum f:D . (d == e && d == f) -> a . P(d);\n"
    "init P(d1);\n"
  );

  specification s0 = parse_linear_process_specification(text);
  specification s1 = s0;
  sumelm_algorithm<>(s1).run();
  int sumvar_count = 0;
  const action_summand_vector& summands1 = s1.process().action_summands();
  for (action_summand_vector::const_iterator i = summands1.begin(); i != summands1.end(); ++i)
  {
    if (!i->summation_variables().empty())
    {
      ++sumvar_count;
      BOOST_CHECK(data::find_all_variables(i->condition()).empty());
    }
  }
  BOOST_CHECK(sumvar_count == 1);

  print_specifications(s0,s1);
}

/*
 * Test introduced after a bug report by Bas Ploeger. In contrary to what was
 * assumed before, sum variables may not be unconditionally removed from delta summands.
 */
BOOST_AUTO_TEST_CASE(no_unconditional_sum_removal_from_delta)
{
  std::clog << "Test case 9" << std::endl;
  const std::string text(
    "proc P = sum y:Nat . (y > 10) -> delta;\n"
    "init P;\n"
  );

  specification s0 = parse_linear_process_specification(text);
  specification s1 = s0;
  sumelm_algorithm<>(s1).run();
  int sumvar_count = 0;
  const deadlock_summand_vector& summands1 = s1.process().deadlock_summands();
  for (const auto & i : summands1)
  {
    if (!i.summation_variables().empty())
    {
      ++sumvar_count;
    }
  }
  BOOST_CHECK(sumvar_count == 1);

  print_specifications(s0,s1);
}

///Test case for issue #380
BOOST_AUTO_TEST_CASE(bug_380)
{
  std::clog << "Test case 10" << std::endl;
  const std::string text(
    "act a:Nat;\n"
    "proc P(n0: Nat) = sum n: Nat. (n == n0 && n == 1) -> a(n0) . P(n);\n"
    "init P(0);\n"
  );

  specification s0 = parse_linear_process_specification(text);
  specification s1 = s0;
  sumelm_algorithm<>(s1).run();
  int sumvar_count = 0;
  const action_summand_vector& summands1 = s1.process().action_summands();
  for (action_summand_vector::const_iterator i = summands1.begin(); i != summands1.end(); ++i)
  {
    BOOST_CHECK(i->condition() != sort_bool::true_());
    if (!i->summation_variables().empty())
    {
      ++sumvar_count;
    }
  }
  BOOST_CHECK(sumvar_count == 0);

  print_specifications(s0, s1);
}

///Test case for issue #380
BOOST_AUTO_TEST_CASE(test_boolean_variables)
{
  std::clog << "Test case 11" << std::endl;
  const std::string text(
    "act a:Bool#Bool;\n"
    "proc P = sum b,c: Bool. (b && !c) -> a(b,c) . P;\n"
    "init P;\n"
  );

  specification s0 = parse_linear_process_specification(text);
  specification s1 = s0;
  sumelm_algorithm<>(s1).run();

  action_summand_vector v(s1.process().action_summands());
  for(action_summand_vector::const_iterator i = v.begin(); i != v.end(); ++i)
  {
    BOOST_CHECK_EQUAL(i->summation_variables().size(), 0u);
  }

  print_specifications(s0, s1);
}

boost::unit_test::test_suite* init_unit_test_suite(int argc, char* argv[])
{
  return nullptr;
}


/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <algorithm>
using std::min;

#include <utility>
using std::pair;

#include <armadillo>
using arma::Mat;
using arma::Row;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

namespace armadilloJava {
  class ExpectedInPlaceGenMatColIndGenRowVec : public Expected {
    public:
      ExpectedInPlaceGenMatColIndGenRowVec() {
        cout << "Compute ExpectedInPlaceGenMatColIndGenRowVec(): " << endl;

          vector<vector<pair<string, void*>>> inputs = Input::getTestParameters({
            InputClass::GenMat,
            InputClass::ColInd,
            InputClass::GenRowVec
          });

          for (vector<pair<string, void*>> input : inputs) {
            _fileSuffix = "";

            int n = 0;
            for (pair<string, void*> value : input) {
              switch (n) {
                case 0:
                  _fileSuffix += value.first;
                  _genMat = *static_cast<Mat<double>*>(value.second);
                  break;
                case 1:
                  _fileSuffix += "," + value.first;
                  _colInd = *static_cast<int*>(value.second);
                  break;
                case 2:
                  _fileSuffix += "," + value.first;
                  _genRowVec = *static_cast<Row<double>*>(value.second);
                  break;
              }
              ++n;
            }

            cout << "Using input: " << _fileSuffix << endl;

            _copyOfGenMat = _genMat;
            _copyOfColInd = _colInd;
            _copyOfGenRowVec = _genRowVec;

            expectedMatDiagEqual();

            _genMat = _copyOfGenMat;
            _colInd = _copyOfColInd;
            _genRowVec = _copyOfGenRowVec;
            expectedMatDiagPlus();

            _genMat = _copyOfGenMat;
            _colInd = _copyOfColInd;
            _genRowVec = _copyOfGenRowVec;
            expectedMatDiagMinus();

            _genMat = _copyOfGenMat;
            _colInd = _copyOfColInd;
            _genRowVec = _copyOfGenRowVec;
            expectedMatDiagElemTimes();

            _genMat = _copyOfGenMat;
            _colInd = _copyOfColInd;
            _genRowVec = _copyOfGenRowVec;
            expectedMatDiagElemDivide();

            _genMat = _copyOfGenMat;
            _colInd = _copyOfColInd;
            _genRowVec = _copyOfGenRowVec;
            expectedMatColEqual();

            _genMat = _copyOfGenMat;
            _colInd = _copyOfColInd;
            _genRowVec = _copyOfGenRowVec;
            expectedMatColPlus();

            _genMat = _copyOfGenMat;
            _colInd = _copyOfColInd;
            _genRowVec = _copyOfGenRowVec;
            expectedMatColMinus();

            _genMat = _copyOfGenMat;
            _colInd = _copyOfColInd;
            _genRowVec = _copyOfGenRowVec;
            expectedMatColElemTimes();

            _genMat = _copyOfGenMat;
            _colInd = _copyOfColInd;
            _genRowVec = _copyOfGenRowVec;
            expectedMatColElemDivide();
          }

          cout << "done." << endl;
        }

    protected:
      Mat<double> _genMat;
      Mat<double> _copyOfGenMat;

      int _colInd;
      int _copyOfColInd;

      Row<double> _genRowVec;
      Row<double> _copyOfGenRowVec;

      void expectedMatDiagEqual() {
        if(_colInd >= _genMat.n_cols) {
          return;
        }

        if (_genRowVec.n_elem != min(_genMat.n_rows, _genMat.n_cols - _colInd)) {
          return;
        }

        cout << "- Compute expectedMatDiagEqual() ... ";

        _genMat.diag(_colInd) = _genRowVec;
        save<double>("Mat.diagSuperEqual", _genMat);

        cout << "done." << endl;
      }

      void expectedMatDiagPlus() {
        if(_colInd >= _genMat.n_cols) {
          return;
        }

        if (_genRowVec.n_elem != min(_genMat.n_rows, _genMat.n_cols - _colInd)) {
          return;
        }

        cout << "- Compute expectedMatDiagPlus() ... ";

        _genMat.diag(_colInd) += _genRowVec;
        save<double>("Mat.diagSuperPlus", _genMat);

        cout << "done." << endl;
      }

      void expectedMatDiagMinus() {
        if(_colInd >= _genMat.n_cols) {
          return;
        }

        if (_genRowVec.n_elem != min(_genMat.n_rows, _genMat.n_cols - _colInd)) {
          return;
        }

        cout << "- Compute expectedMatDiagMinus() ... ";

        _genMat.diag(_colInd) -= _genRowVec;
        save<double>("Mat.diagSuperMinus", _genMat);

        cout << "done." << endl;
      }

      void expectedMatDiagElemTimes() {
        if(_colInd >= _genMat.n_cols) {
          return;
        }

        if (_genRowVec.n_elem != min(_genMat.n_rows, _genMat.n_cols - _colInd)) {
          return;
        }

        cout << "- Compute expectedMatDiagElemTimes() ... ";

        _genMat.diag(_colInd) %= _genRowVec;
        save<double>("Mat.diagSuperElemTimes", _genMat);

        cout << "done." << endl;
      }

      void expectedMatDiagElemDivide() {
        if(_colInd >= _genMat.n_cols) {
          return;
        }

        if (_genRowVec.n_elem != min(_genMat.n_rows, _genMat.n_cols - _colInd)) {
          return;
        }

        cout << "- Compute expectedMatDiagElemDivide() ... ";

        _genMat.diag(_colInd) /= _genRowVec;
        save<double>("Mat.diagSuperElemDivide", _genMat);

        cout << "done." << endl;
      }

      void expectedMatColEqual() {
        if(_colInd >= _genMat.n_cols) {
          return;
        }

        if(!_genRowVec.is_colvec() || _genMat.n_rows != _genRowVec.n_rows) {
          return;
        }

        cout << "- Compute expectedMatColEqual() ... ";

        _genMat.col(_colInd) = _genRowVec;
        save<double>("Mat.colEqual", _genMat);

        cout << "done." << endl;
      }

      void expectedMatColPlus() {
        if(_colInd >= _genMat.n_cols) {
          return;
        }

        if(!_genRowVec.is_colvec() || _genMat.n_rows != _genRowVec.n_rows) {
          return;
        }

        cout << "- Compute expectedMatColPlus() ... ";

        _genMat.col(_colInd) += _genRowVec;
        save<double>("Mat.colPlus", _genMat);

        cout << "done." << endl;
      }

      void expectedMatColMinus() {
        if(_colInd >= _genMat.n_cols) {
          return;
        }

        if(!_genRowVec.is_colvec() || _genMat.n_rows != _genRowVec.n_rows) {
          return;
        }

        cout << "- Compute expectedMatColMinus() ... ";

        _genMat.col(_colInd) -= _genRowVec;
        save<double>("Mat.colMinus", _genMat);

        cout << "done." << endl;
      }

      void expectedMatColElemTimes() {
        if(_colInd >= _genMat.n_cols) {
          return;
        }

        if(!_genRowVec.is_colvec() || _genMat.n_rows != _genRowVec.n_rows) {
          return;
        }

        cout << "- Compute expectedMatColElemTimes() ... ";

        _genMat.col(_colInd) %= _genRowVec;
        save<double>("Mat.colElemTimes", _genMat);

        cout << "done." << endl;
      }

      void expectedMatColElemDivide() {
        if(_colInd >= _genMat.n_cols) {
          return;
        }

        if(!_genRowVec.is_colvec() || _genMat.n_rows != _genRowVec.n_rows) {
          return;
        }

        cout << "- Compute expectedMatColElemDivide() ... ";

        _genMat.col(_colInd) /= _genRowVec;
        save<double>("Mat.colElemDivide", _genMat);

        cout << "done." << endl;
      }
  };
}


/*
 * =====================================================================================
 *
 *       Filename:  astron_main_atp.cpp
 *
 *    Description:
 *
 *        Version:  1.0
 *        Created:  06/28/2014 02:04:16 PM
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Anup Pillai (), anupgpillai@gmail.com
 *   Organization:  IISER Pune
 *
 * =====================================================================================
 */
#define DIR1 "/media/chivda/BigDaddy/"
#include <iostream>
#include <stdlib.h>
#include <cmath>
#include <vector>
//#include "/home/goofy/DATA/SCRIPTS/CPP/Astron/astron_proj.hpp"
#include "astron_ATPext_model.hpp"
//#include "astron_ip3_model.hpp"
//#include "astron_ip3_atp_model.hpp"
//#include "astron_utility_functions.hpp"
//#include "/opt/boost/boost_1_55_0/boost/numeric/ublas/vector.hpp" /*  Specifying the full path in qoutes for a header file */
#include <boost/numeric/odeint.hpp> /*  Specifying the file within < > lets c++ search for it at -I path  */

using namespace std;
using namespace boost::numeric::odeint;

typedef std::vector< double > state_type;

//-----------------
int main(int argc, char **argv)
{
   ATPext atp_ext(25E-04);

   typedef runge_kutta4 < state_type > stepper_type;
   stepper_type rk4;

   double atp_conc0 = 0.0;
   double adp_conc0 = 0.0;
   double amp_conc0 = 0.0;
   double ado_conc0 = 0.0;
   double ino_conc0 = 0.0;

   state_type x = {atp_conc0, adp_conc0, amp_conc0, ado_conc0, ino_conc0};

   double dt = 100.0; // 40 in micro sec
   double atp_pulse = 0.0;
   int pulse_given=0;
   for (double t=0.0; t<= (1000 * 1 * 60); t+= dt)
   {
      x[0] = x[0] + atp_pulse;
      if ( t >=3000 & (t < 4000) & (pulse_given == 0) )
      {
         atp_pulse = 1000.0;   // micro M
         pulse_given = 1;
      }
      else
      {
         atp_pulse = 0.0;
      }
      rk4.do_step(boost::ref(atp_ext),std::make_pair(x.begin(),x.end()),t,dt);
      cout << t << " " << x[0] << " " << x[1] << " " << x[2] << " " << x[3] << " " << x[4] << " " << atp_pulse << "\n";
      //hh.printparam();
      //xwrite(x,t);
      //if (t >=1000) cin.get(); // Press enter key to continue
      //cin.get();
   }

   return 0;
}


///////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2019.                        //
//  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)             //
///////////////////////////////////////////////////////////////////

#include <algorithm>
#include <cstddef>
#include <cstdint>
#include <ctime>
#include <iomanip>
#include <iostream>
#include <limits>
#include <iterator>
#include <vector>

#include <boost/multiprecision/cpp_int.hpp>

class random_pcg32_fast_base
{
protected:
  using itype = std::uint64_t;

  static constexpr bool is_mcg = false;

public:
  virtual ~random_pcg32_fast_base() = default;

protected:
  explicit random_pcg32_fast_base(const itype = itype()) { }

  random_pcg32_fast_base(const random_pcg32_fast_base&) = default;

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

  template<typename ArithmeticType>
  static ArithmeticType rotr(const ArithmeticType& value_being_shifted,
                             const std::size_t     bits_to_shift)
  {
    const std::size_t left_shift_amount =
      std::numeric_limits<ArithmeticType>::digits - bits_to_shift;

    const ArithmeticType part1 = ((bits_to_shift > 0U) ? ArithmeticType(value_being_shifted >> bits_to_shift)     : value_being_shifted);
    const ArithmeticType part2 = ((bits_to_shift > 0U) ? ArithmeticType(value_being_shifted << left_shift_amount) : 0U);

    return ArithmeticType(part1 | part2);
  }

  template<typename xtype,
           typename itype>
  struct xsh_rr_mixin
  {
    static xtype output(const itype internal_value)
    {
      using bitcount_t = std::size_t;

      constexpr bitcount_t bits         = bitcount_t(sizeof(itype) * 8U);
      constexpr bitcount_t xtypebits    = bitcount_t(sizeof(xtype) * 8U);
      constexpr bitcount_t sparebits    = bits - xtypebits;
      constexpr bitcount_t wantedopbits =   ((xtypebits >= 128U) ? 7U
                                          : ((xtypebits >=  64U) ? 6U
                                          : ((xtypebits >=  32U) ? 5U
                                          : ((xtypebits >=  16U) ? 4U
                                          :                        3U))));

      constexpr bitcount_t opbits       = ((sparebits >= wantedopbits) ? wantedopbits : sparebits);
      constexpr bitcount_t amplifier    = wantedopbits - opbits;
      constexpr bitcount_t mask         = (1ULL << opbits) - 1U;
      constexpr bitcount_t topspare     = opbits;
      constexpr bitcount_t bottomspare  = sparebits - topspare;
      constexpr bitcount_t xshift       = (topspare + xtypebits) / 2U;

      const bitcount_t rot =
        ((opbits != 0U) ? (bitcount_t(internal_value >> (bits - opbits)) & mask)
                        : 0U);

      const bitcount_t amprot = (rot << amplifier) & mask;

      const itype internal_value_xor = internal_value ^ itype(internal_value >> xshift);

      const xtype result = rotr(xtype(internal_value_xor >> bottomspare), amprot);

      return result;
    }
  };
};

class random_pcg32_fast : public random_pcg32_fast_base
{
private:
  static constexpr itype default_multiplier = static_cast<itype>(6364136223846793005ULL);
  static constexpr itype default_increment  = static_cast<itype>(1442695040888963407ULL);

public:
  using result_type = std::uint32_t;

  static constexpr itype default_seed = static_cast<itype>(0xCAFEF00DD15EA5E5ULL);

  explicit random_pcg32_fast(const itype state = default_seed)
    : random_pcg32_fast_base(state),
      my_inc  (default_increment),
      my_state(is_mcg ? state | itype(3U) : bump(state + increment())) { }

  random_pcg32_fast(const random_pcg32_fast& other)
    : random_pcg32_fast_base(other),
      my_inc  (other.my_inc),
      my_state(other.my_state) { }

  virtual ~random_pcg32_fast() = default;

  random_pcg32_fast& operator=(const random_pcg32_fast& other)
  {
    static_cast<void>(random_pcg32_fast_base::operator=(other));

    if(this != &other)
    {
      my_inc   = other.my_inc;
      my_state = other.my_state;
    }

    return *this;
  }

  void seed(const itype state = default_seed)
  {
    my_inc = default_increment;

    my_state = (is_mcg ? state | itype(3U) : bump(state + increment()));
  }

  result_type operator()()
  {
    const result_type value =
      xsh_rr_mixin<result_type, itype>::output(base_generate0());

    return value;
  }

private:
  itype my_inc;
  itype my_state;

  itype multiplier() const
  {
    return default_multiplier;
  }

  itype increment() const
  {
    return default_increment;
  }

  itype bump(const itype state)
  {
    return itype(state * multiplier()) + increment();
  }

  itype base_generate0()
  {
    const itype old_state = my_state;

    my_state = bump(my_state);

    return old_state;
  }
};

template<typename UnsignedIntegralIteratorType,
         typename RandomEngineType>
void get_random_big_uint(RandomEngineType& rng, UnsignedIntegralIteratorType it_out)
{
  using local_random_value_type = typename RandomEngineType::result_type;

  using local_uint_type = typename std::iterator_traits<UnsignedIntegralIteratorType>::value_type;

  constexpr std::size_t digits_of_uint___type = static_cast<std::size_t>(std::numeric_limits<local_uint_type>::digits);
  constexpr std::size_t digits_of_random_type = static_cast<std::size_t>(std::numeric_limits<local_random_value_type>::digits);

  local_random_value_type next_random = rng();

  *it_out = next_random;

  for(std::size_t i = digits_of_random_type; i < digits_of_uint___type; i += digits_of_random_type)
  {
    (*it_out) <<= digits_of_random_type;

    next_random = rng();

    (*it_out) |= next_random;
  }
}

using big_uint_backend_type =
  boost::multiprecision::cpp_int_backend<8192UL << 1U,
                                         8192UL << 1U,
                                         boost::multiprecision::unsigned_magnitude>;

using big_uint_type = boost::multiprecision::number<big_uint_backend_type>;

namespace local
{
  std::vector<big_uint_type> a(1024U);
  std::vector<big_uint_type> b(a.size());
}

int main()
{
  random_pcg32_fast rng;

  rng.seed(std::clock());

  for(auto i = 0U; i < local::a.size(); ++i)
  {
    get_random_big_uint(rng, local::a.begin() + i);
    get_random_big_uint(rng, local::b.begin() + i);
  }

  std::size_t count = 0U;

  float total_time = 0.0F;

  const std::clock_t start = std::clock();

  do
  {
    const std::size_t index = count % local::a.size();

    local::a[index] * local::b[index];

    ++count;
  }
  while((total_time = (float(std::clock() - start) / float(CLOCKS_PER_SEC))) < 10.0F);

  // Boost.Multiprecision 1.71
  // bits: 16384, kops_per_sec: 4.7
  // bits: 32768, kops_per_sec: 1.2
  // bits: 65536, kops_per_sec: 0.3
  // bits: 131072, kops_per_sec: 0.075
  // bits: 262144, kops_per_sec: 0.019
  // bits: 524288, kops_per_sec: 0.0047

  // Boost.Multiprecision + kara mult
  // bits: 16384, kops_per_sec: 4.8
  // bits: 32768, kops_per_sec: 1.6
  // bits: 65536, kops_per_sec: 0.5
  // bits: 131072, kops_per_sec: 0.15
  // bits: 262144, kops_per_sec: 0.043
  // bits: 524288, kops_per_sec: 0.011

  const float kops_per_sec = (float(count) / total_time) / 1000.0F;

  std::cout << "bits: "
            << std::numeric_limits<big_uint_type>::digits
            << ", kops_per_sec: "
            << kops_per_sec
            << count << std::endl;
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
#pragma once
#include <iostream>
#include <stdint.h>
#include <vector>
#include <Eigen/Dense>

#include <boost/shared_ptr.hpp>

#include <dpMM/dpMM.hpp>
#include <dpMM/cat.hpp>
#include <dpMM/dir.hpp>
#include <dpMM/niw.hpp>
#include <dpMM/sampler.hpp>
#include <dpMM/basemeasure.hpp>

using namespace Eigen;
using std::endl; using std::cout;
using boost::shared_ptr;

#ifdef CUDA
  #define SET_CUDA
  #undef CUDA
#endif

template<typename T>
class DirMM : public DpMM<T>
{
public:
  DirMM(const Dir<Cat<T>, T>& alpha, const shared_ptr<BaseMeasure<T> >&
      theta, uint32_t K0);
  DirMM(const Dir<Cat<T>, T>& alpha, const
      vector<shared_ptr<BaseMeasure<T> > >& thetas);
  DirMM(const DirMM<T>& dirMM);
  virtual ~DirMM();

  virtual void reset();
  virtual void initialize(const Matrix<T,Dynamic,Dynamic>& x);
  virtual void initialize(const shared_ptr<ClGMMData<T> >& cld)
    {cout<<"not supported"<<endl; assert(false);};

  virtual void sampleLabels();
  virtual void sampleParameters();
  virtual void sampleFromPrior();

  virtual T logJoint();
  virtual const Matrix<T,Dynamic,Dynamic>& x() const {return x_;};
  virtual const VectorXu& labels() const {return z_;};
  virtual const VectorXu& getLabels() {return z_;};
  virtual void setLabels(const VectorXu& z){z_ = z;};
  virtual uint32_t getK() const { return K_;};
  virtual const shared_ptr<BaseMeasure<T> >& getTheta(uint32_t k) const
    { assert(k<K_); return this->thetas_[k];};
  virtual const vector<shared_ptr<BaseMeasure<T> > >& getThetas() const
    { return this->thetas_;};
  virtual const shared_ptr<BaseMeasure<T> >& getTheta0() const
    { return this->theta0_;};

  virtual const Dir<Cat<T>, T>& Alpha() const { return dir_;};
  virtual const Cat<T>& Pi() const { return pi_;};

//  virtual MatrixXu mostLikelyInds(uint32_t n);
  virtual MatrixXu mostLikelyInds(uint32_t n, Matrix<T,Dynamic,Dynamic>& logLikes);

  Matrix<T,Dynamic,1> getCounts();

  bool isInit() const { return sampler_!= NULL;};

protected:
  uint32_t K0_;  // that is the number of clusters that are initialized with data at the beginning (K0_ <= K_)
  uint32_t K_;
  Dir<Cat<T>, T> dir_;
  Cat<T> pi_;
#ifdef CUDA
  SamplerGpu<T>* sampler_;
#else
  Sampler<T>* sampler_;
#endif
  Matrix<T,Dynamic,Dynamic> pdfs_;
//  Cat cat_;
  shared_ptr<BaseMeasure<T> > theta0_;
  vector<shared_ptr<BaseMeasure<T> > > thetas_;

  Matrix<T,Dynamic,Dynamic> x_;
  VectorXu z_;
};

// --------------------------------------- impl -------------------------------

template<typename T>
DirMM<T>::DirMM(const Dir<Cat<T>,T>& alpha, const
    shared_ptr<BaseMeasure<T> >& theta, uint32_t K0) :
  K0_(K0), K_(alpha.K_), dir_(alpha), pi_(dir_.sample()),
  sampler_(NULL),
  theta0_(theta)
{};

template<typename T>
DirMM<T>::DirMM(const Dir<Cat<T>,T>& alpha,
    const vector<shared_ptr<BaseMeasure<T> > >& thetas) :
  K_(alpha.K_), dir_(alpha), pi_(dir_.sample()),
  sampler_(NULL), thetas_(thetas),
  theta0_(
      shared_ptr<BaseMeasure<T> >(thetas[0]->copy()))
{
//  cout<<thetas_.size()<<endl;
//  for(uint32_t k=0; k<thetas_.size(); ++k)
//    cout<< "   "<<thetas_[k].get()<<endl;
//  cout<<thetas_[0].get()<<endl;
//  cout<<thetas_[0]->copy()<<endl;
//
//  theta0_ = shared_ptr<BaseMeasure<T> >(thetas_[0]->copy());
};

template<typename T>
DirMM<T>::DirMM(const DirMM<T>& dirMM)
  : K_(dirMM.getK()), dir_(dirMM.Alpha()), pi_(dirMM.Pi()),
  sampler_(NULL),
  theta0_(shared_ptr<BaseMeasure<T> >(
        dirMM.getTheta0()->copy()))
{
  for(uint32_t k=0; k<  dirMM.getK(); ++k)
  {
    thetas_.push_back(shared_ptr<BaseMeasure<T> >(
          dirMM.getTheta(k)->copy()));
  }
  if(dirMM.isInit())
  {
    x_ = dirMM.x();
    // bad boy
    z_ = const_cast<DirMM<T>* >(&dirMM)->labels();
    pdfs_.setZero(x_.cols(),K_);
#ifdef CUDA
    sampler_ = new SamplerGpu<T>(x_.cols(),K_,dir_.pRndGen_);
#else
    sampler_ = new Sampler<T>(dir_.pRndGen_);
#endif
  }
};

template<typename T>
DirMM<T>::~DirMM()
{
  if (sampler_ != NULL) delete sampler_;
};

template <typename T>
Matrix<T,Dynamic,1> DirMM<T>::getCounts()
{
  return counts<T,uint32_t>(z_,K_);
};

template<typename T>
void DirMM<T>::initialize(const Matrix<T,Dynamic,Dynamic>& x)
{
//  cout<<"init"<<endl;
  x_ = x;
  // randomly init labels from prior
  z_.setZero(x.cols());
//  cout<<"sample pi"<<endl;
  pi_ = dir_.sample();
  if (K0_ < K_)
  {
    Matrix<T,Dynamic,1> pdf = pi_.pdf();
    pdf.bottomRows(K_-K0_).setZero();
    pdf = pdf / pdf.sum(); // renormalize
    pi_.pdf(pdf);
  }
//  cout<<"init pi="<<pi_.pdf().transpose()<<endl;
  pi_.sample(z_);

  pdfs_.setZero(x.cols(),K_);
#ifdef CUDA
  sampler_ = new SamplerGpu<T>(x.cols(),K_,dir_.pRndGen_);
#else
  sampler_ = new Sampler<T>(dir_.pRndGen_);
#endif

  // init the parameters
//  if(thetas_.size() == 0)
//  {
  thetas_.clear(); // destrey eny previous thetas
//  cout<<"creating thetas"<<endl;
  for (uint32_t k=0; k<K_; ++k)
    thetas_.push_back(shared_ptr<BaseMeasure<T> >(theta0_->copy()));
//  }
//#pragma omp parallel for
//  for(uint32_t k=0; k<K_; ++k)
//    thetas_[k]->posterior(x_,z_,k);
//  for (uint32_t k=0; k<K_; ++k)
//    thetas_[k].initialize(x_,z_);
};

template<typename T>
void DirMM<T>::reset()
{
  x_ = Matrix<T,Dynamic,Dynamic>::Zero(0,theta0_->getDim());
  z_ = VectorXu::Zero(0);
  pi_ = dir_.sample();
  pdfs_.setZero(0,K_);
  delete sampler_;
  sampler_ = NULL;

  thetas_.clear(); // delete any previous thetas
  for (uint32_t k=0; k<K_; ++k)
    thetas_.push_back(shared_ptr<BaseMeasure<T> >(theta0_->copy()));
};

template<typename T>
void DirMM<T>::sampleLabels()
{
  // obtain posterior categorical under labels
  pi_ = dir_.posterior(z_).sample();
//  cout<<pi_.pdf().transpose()<<endl;

#pragma omp parallel for
  for(int32_t i=0; i<z_.size(); ++i)
  {
    //TODO: could buffer this better
    // compute categorical distribution over label z_i
    VectorXd logPdf_z = pi_.pdf().array().log();
    for(uint32_t k=0; k<K_; ++k)
    {
//      cout<<thetas_[k]->logLikelihood(x_.col(i))<<" ";
      logPdf_z[k] += thetas_[k]->logLikelihood(x_.col(i));
    }
//    cout<<endl;
    // make pdf sum to 1. and exponentiate
    pdfs_.row(i) = (logPdf_z.array()-logSumExp(logPdf_z)).exp().matrix().transpose();
//    cout<<pi_.pdf().transpose()<<endl;
//    cout<<pdfs_.row(i)<<" |.|="<<pdfs_.row(i).sum()<<endl;;
//    cout<<" z_i="<<z_[i]<<endl;
  }
  // sample z_i
  sampler_->sampleDiscPdf(pdfs_,z_);
};

template<typename T>
void DirMM<T>::sampleParameters()
{
//  Matrix<T,Dynamic,1> Ns = getCounts();
//#pragma omp parallel for
  for(uint32_t k=0; k<K_; ++k)
  {
//    cout<<"k:"<<k<<" "<<Ns(k)<<" ";
    thetas_[k]->posterior(x_,z_,k);
//    thetas_[k]->print();
  }
};

template<typename T>
void DirMM<T>::sampleFromPrior()
{
//  Matrix<T,Dynamic,1> Ns = getCounts();
//#pragma omp parallel for
//
// simulate sampling from prior by not giving the posteriors any data
  Matrix<T,Dynamic,Dynamic> x = Matrix<T,Dynamic,Dynamic>::Zero(1,1);
  VectorXu z = VectorXu::Ones(1)*(K_+1);
  for(uint32_t k=0; k<K_; ++k)
  {
//    cout<<"k:"<<k<<" "<<Ns(k)<<" ";
    thetas_[k]->posterior(x,z,k);
//    thetas_[k]->print();
  }
};

template<typename T>
T DirMM<T>::logJoint()
{
  T logJoint = dir_.logPdf(pi_);
  cout<<"  [logJoint="<<logJoint<<" -> ";
#pragma omp parallel for reduction(+:logJoint)
  for (int32_t k=0; k<K_; ++k)
    logJoint = logJoint + thetas_[k]->logPdfUnderPrior();
  cout<<" "<<logJoint<<" -> ";
#pragma omp parallel for reduction(+:logJoint)
  for (int32_t i=0; i<z_.size(); ++i)
    logJoint = logJoint + thetas_[z_[i]]->logLikelihood(x_.col(i));
  cout<<" "<<logJoint<<"]"<<endl;
  return logJoint;
};

template<typename T>
MatrixXu DirMM<T>::mostLikelyInds(uint32_t n, Matrix<T,Dynamic,Dynamic>& logLikes)
{
  MatrixXu inds = MatrixXu::Zero(n,K_);
  logLikes = Matrix<T,Dynamic,Dynamic>::Ones(n,K_);

#pragma omp parallel for
  for (int32_t k=0; k<K_; ++k)
  {
    for (uint32_t i=0; i<z_.size(); ++i)
      if(z_(i) == k)
      {
        T logLike = thetas_[z_[i]]->logLikelihood(x_.col(i));
        for (uint32_t j=0; j<n; ++j)
          if(logLikes(j,k) < logLike)
          {
            for(uint32_t l=n-1; l>j; --l)
            {
              logLikes(l,k) = logLikes(l-1,k);
              inds(l,k) = inds(l-1,k);
            }
            logLikes(j,k) = logLike;
            inds(j,k) = i;
//            cout<<"after update "<<logLike<<endl;
//            Matrix<T,Dynamic,Dynamic> out(n,K_*2);
//            out<<logLikes.cast<T>(),inds.cast<T>();
//            cout<<out<<endl;
            break;
          }
      }
  }
  cout<<"::mostLikelyInds: logLikes"<<endl;
  cout<<logLikes<<endl;
  cout<<"::mostLikelyInds: inds"<<endl;
  cout<<inds<<endl;
  return inds;
};

#ifdef SET_CUDA
  #define CUDA
#endif

//template<class T>
//T DirMM<T>::avgIntraClusterDeviation()
//{
//  Matrix<T,Dynamic,1> deviates(K_);
//  deviates.setZero(K_);
//#pragma omp parallel for
//  for (uint32_t k=0; k<K_; ++k)
//  {
//    T N_k = 0.0;
//    for (uint32_t i=0; i<N_; ++i)
//      if(z_(i) == k)
//      {
//        T dot = thetas_[k]->transpose()*spx_->col(i);
//        deviates(k) += acos(min(1.0,max(-1.0,dot)));
//        N_k ++;
//      }
//    if(N_k > 0.0) deviates(k) /= N_k;
//  }
//  return deviates.sum()/static_cast<T>(K_);
//}