#include "meshview/meshview.hpp"

#include <cmath>
#include <iostream>
#include <Eigen/Geometry>

#include "meshview/util.hpp"

namespace meshview {

Camera::Camera(const Vector3f& center_of_rot, const Vector3f& world_up,
               float dist_to_center, float yaw, float pitch, float roll,
               bool ortho, float fovy, float aspect, float z_close, float z_far)
    : center_of_rot(center_of_rot),
      world_up(world_up),
      dist_to_center(dist_to_center),
      yaw(yaw),
      pitch(pitch),
      roll(roll),
      ortho(ortho),
      fovy(fovy),
      aspect(aspect),
      z_close(z_close),
      z_far(z_far) {
    update_proj();
    update_view();
}

void Camera::rotate_with_mouse(float xoffset, float yoffset) {
    xoffset *= rotate_speed;
    yoffset *= rotate_speed;

    float cr = cos(roll), sr = sin(roll);
    yaw += xoffset * cr + yoffset * sr;
    pitch -= yoffset * cr + xoffset * sr;

    // Clamp pitch
    static const float PITCH_CLAMP = M_PI * 0.49999f;
    if (std::fabs(pitch) > PITCH_CLAMP) {
        pitch = PITCH_CLAMP * (pitch > 0.f ? 1.f : -1.f);
        yaw = yaw += M_PI;
        roll = roll += M_PI;
    }
    update_view();
}

void Camera::roll_with_mouse(float xoffset, float yoffset) {
    xoffset *= rotate_speed;
    roll += xoffset;

    update_view();
}

void Camera::pan_with_mouse(float xoffset, float yoffset) {
    xoffset *= pan_speed * dist_to_center;
    yoffset *= pan_speed * dist_to_center;

    center_of_rot += -xoffset * right + yoffset * up;
    update_view();
}

void Camera::zoom_with_mouse(float amount) {
    if (amount < 0)
        dist_to_center *= scroll_factor;
    else
        dist_to_center *= 1.f / scroll_factor;
    update_view();
    if (ortho) update_proj();
}

void Camera::reset_view() {
    center_of_rot.setZero();
    world_up = Vector3f(0.f, 1.f, 0.f);
    dist_to_center = 3.f;
    yaw = -M_PI / 2;
    pitch = roll = 0.0f;
    update_view();
}

void Camera::reset_proj() {
    fovy = M_PI / 4.f;
    aspect = 5.f / 3.f;
    z_close = 0.01f;
    z_far = 1e3f;
    update_proj();
}

void Camera::update_view() {
    front[0] = cos(yaw) * cos(pitch);
    front[1] = sin(pitch);
    front[2] = sin(yaw) * cos(pitch);
    pos = center_of_rot - front * dist_to_center;
    Eigen::AngleAxisf aa_roll(roll, front);
    right = front.cross(aa_roll * world_up).normalized();
    up = right.cross(front);
    view = util::look_at(pos, front, up);
}

void Camera::update_proj() {
    if (ortho) {
        proj = Eigen::Matrix4f::Identity();
        proj(0, 0) = 4.f / (aspect * dist_to_center);
        proj(1, 1) = 4.f / (dist_to_center);
        proj(2, 2) = -1.f / (z_far - z_close);
        proj(2, 3) = z_close / (z_far - z_close);
    } else {
        float tan_half_fovy = tan(fovy / 2.f);
        proj = util::persp(1.f / (tan_half_fovy * aspect), 1.f / tan_half_fovy,
                           z_close, z_far);
    }
}

}  // namespace meshview


#include <iostream>

#include <cmath>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/operation.hpp>

#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/generator_iterator.hpp>
#include <boost/random/linear_congruential.hpp>

#include "stl_vector_operation_functions.hpp"
namespace bnu=boost::numeric::ublas;
//------------------------
//This is typedef for a random number generator
typedef boost::minstd_rand base_generator_type;

//===============FUNCTIONS==================
/* Function update H_mew vector with the current number of molecules in each state from the state vector X */
void updateH(std::vector<double>& H, const std::vector<int> X)
{
   H[0] = X[0] * X[1];
   H[1] = X[2];
   H[2] = X[2] * X[0];
   H[3] = X[3];
   H[4] = X[2];
   H[5] = X[4];
   H[6] = X[4] * X[0];
   H[7] = X[5];
   H[8] = X[3];
   H[9] = X[5];
   H[10] = X[5];
   H[11] = X[6];
   H[12] = X[3];
   H[13] = X[7];
   H[14] = X[6];
   H[15] = X[7];
}
void updateX(std::vector<int>& X, const int rx_id)
{
   switch(rx_id)
   {
      case 0:
      {
         if((X[0] > 0) && (X[1] > 0)){
            --X[0]; --X[1];
            ++X[2];
         }
         break;
      }
      case 1:
      {
         if(X[2] > 0){
         --X[2];
         ++X[0]; ++X[1];
         }
         break;
      }
      case 2:
      {
         if((X[2] > 0) && (X[0] > 0)){
         --X[2]; --X[0];
         ++X[3];
         }
         break;
      }
      case 3:
      {
         if(X[3] > 0){
         --X[3];
         ++X[2]; ++X[0];
         }
         break;
      }
      case 4:
      {
         if(X[2] > 0){
         --X[2];
         ++X[4];
         }
         break;
      }
      case 5:
      {
         if(X[4] > 0){
         --X[4];
         ++X[2];
         }
         break;
      }
      case 6:
      {
         if((X[4] > 0) && (X[0] > 0)){
         --X[4];
         --X[0];
         ++X[5];
         }
         break;
      }
      case 7:
      {
         if(X[5] > 0){
         --X[5];
         ++X[4];
         ++X[0];
         }
         break;
      }
      case 8:
      {
         if(X[3] > 0){
         --X[3];
         ++X[5];
         }
         break;
      }
      case 9:
      {
         if(X[5] > 0){
         --X[5];
         ++X[3];
         }
         break;
      }
      case 10:
      {
         if(X[5] > 0){
         --X[5];
         ++X[6];
         }
         break;
      }
      case 11:
      {
         if(X[6] > 0){
         --X[6];
         ++X[5];
         }
         break;
      }
      case 12:
      {
         if(X[3] > 0){
         --X[3];
         ++X[7];
         }
         break;
      }
      case 13:
      {
         if(X[7] > 0){
         --X[7];
         ++X[3];
         }
         break;
      }
      case 14:
      {
         if(X[6] > 0){
         --X[6];
         ++X[7];
         }
         break;
      }
      case 15:
      {
         if(X[7] > 0){
         --X[7];
         ++X[6];
         }
         break;
      }
   }
   for(size_t i=0; i<X.size();++i){
      if(X[i] < 0){
         X[i] = 0;
      }
   }
}
//Function to compute Tau from the first random number r1 and aTot (a = ∑H_mew*C_mew)
double getTau(const double r1, const double Atotal)
{
   if (Atotal >1E-308){
      return( (1/Atotal) * log(1/r1) );
   }
   else{
      return(-1);
   }
}
//Function to find the next reaction using a random number r2 and vector A (the vector A = H*C; contains: 0 to M-1 reactions)
int getReaction(const double r2, const double Atotal, const std::vector<double> A)
{
   std::vector<int> Ai; // Vector to hold the indices of A that has a non-zero entry
   for(size_t i=0; i<A.size();i++){
      if(A[i] > 1E-308){
         Ai.push_back(i);
      }
   }
   int j=-1;
   if (!Ai.empty()){     // evaluate if Ai vector is non-zero
      j = Ai[0];  //
      double cum=0;
      for(size_t i=0; i < Ai.size()-1; i++){
         cum = cum + A[Ai[i]];
         if (cum <= (Atotal * r2) ){
            j = Ai[i+1];
         }
      }
   }
   return(j);
}
//-------------------------
//=================MAIN====================
int main(int argc, char** argv)
{
   //Define a random number generator and initialize it with a reproducible seed
   base_generator_type generator(45);
   //Define a uniform random number distribution which produces "double"
   //values b/w 0 and 1 (0 inclusive, 1 exlusive).
   boost::uniform_real<double>uni_dist(0,1);
   boost::variate_generator<base_generator_type&, boost::uniform_real<double> > uni(generator, uni_dist);

   const double avagadro = 6.022e+23;
   //--------------
   double kf1 = 4.59E06;//M^-1 S^-1
   double kb1 = 4.26E03;//S^-1
   double kf2 = 28.4E06;//M^-1 * S^-1
   double kb2 = 3.26E03;//S^-1
   double kf3 = 1.27E06;//M^-1 S^-1
   double kb3 = 45.7;//S^-1
   double a0 = 4.24E03;//S^-1
   double b0 = 900.0;//S^-1
   double a1 = 2.89E03;//S^-1
   double b1 = 39.2;//S^-1
   double a2 = 172.0;//S^-1
   double b2 = 0.727;//S^-1
   double a3 = 17.7;//S^-1
   double b3 = 4.0;//S^-1
   double a4 = 16.8;//S^-1
   double b4 = 190.4;//S^-1
   //-----------------------------
   std::vector<int> X(8);// A vector to hold the initial number of the receptors in each states [Glu,C0,C1,C2,C3,C4,C5,Open]
   std::vector<double> C(16); //A vector to hold the Cmew values
   std::vector<double> H(16); //A vector to hold the Hmew values
   std::vector<double> A(16); //A vector to hold the Amew values
   X = {1000,100,0,0,0,0,0,0};
   double V = 1.0;// Volume of the reaction space
   C = {kf1*V,kb1,kf2*V,kb2,a1,b1,kf3*V,kb3,a2,b2,a4,b4,a0,b0,b3,a3};
   H = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
   double tmax = 1.0;//10 secs
   double dt = 50E-06;// 1 msec
   double t2 = dt;
   double t = 0;
   double r1 = 0.0;
   double r2 = 0.0;
   int reaction = 0;
   double tau = 0.0;
   double Atotal = 1.0;// store Amew total;
   int reactionCount = 0;
   std::cout << t << " " << X;
   updateH(H,X); // calculate Hmew based on the current state
   A = vectorProd(H,C);// calculate Amew = Hmew*Cmew
   Atotal = vectorSum(A);// store Amew total
   while( (t <= tmax) && (Atotal != 0) ){
      //Genrate two random numbers from the distribution by means of STL generator interface.
      r1 = uni();
      r2 = uni();
      tau = getTau(r1,Atotal); //calutate the next time step dttau
      reaction = getReaction(r2,Atotal,A); // determine the next reaction
      if (tau > tmax){
         break;
      }
      else{
         updateH(H,X); // calculate Hmew based on the current state
         updateX(X,reaction); // update the state to reflect the occurance of the reaction identified by mewIndex
         A = vectorProd(H,C);// calculate Amew = Hmew*Cmew
         Atotal = vectorSum(A);// store Amew total
         //std::cout << "Current State " << X;
         //std::cout << "H " << H;
         //std::cout << "C " << C;
         //std::cout << "A " << A;
         //std::cout << "Atotal " << Atotal << std::endl;
         //std::cout << "Atotal * r2 " << Atotal*r2 << std::endl;
         //std::cout << "cumSum (A * r2) " << vectorCumSum(A);
         //std::cout << "Reaction " << reaction << std::endl;
         //std::cout << "Tau " << tau << std::endl;
         //std::cout << "Updated State " << X;
      }
      t = t+tau;
      if (t >= t2){
         std::cout << t << " " << X;
         t2+=dt;
      }
      reactionCount+=1;
      //std::cin.get();
   }
}


#include "body.hpp"
#include "engine.hpp"
#include "rocket.hpp"
#include "rocketmodel.hpp"

#include <external/json.h>

#include <boost/units/cmath.hpp>

#include <gtest/gtest.h>

using nlohmann::json;

using boost::units::si::kilograms;
using boost::units::si::meters;
using boost::units::si::newtons;
using boost::units::si::seconds;

struct TestSimulationBasicRocketMasslessBody : public ::testing::Test
{
	static RocketModel createModel()
	{
		json js = {{"name", "rocket"}, {"stages", {{{"name", "P80"},
													{"length", 11.7},
													{"diameter", 3},
													{"dry_mass", 7330},
													{"gross_mass", 95695},
													{"thrust", 2261000},
													{"isp", 280},
													{"burn_time", 110},
													{"fuel", "solid"}}}},
				   {"fairings", {{"length", 7.88}, {"diameter", 2.6}}}};
		return RocketModel::fromJson(js);
	}

	const RocketModel model = createModel();
	const double payload_mass = 1E3;
	Rocket rocket = {model, payload_mass};
	const Body body = {0 * kilograms, 6378 * meters};
	Engine engine = {rocket, body};
	const Engine::position_t expected_initial_position = {0, body.radius + rocket.totalLength(), 0};
};

TEST_F(TestSimulationBasicRocketMasslessBody, TestInitialConditions)
{
	EXPECT_NEAR(expected_initial_position.x().value(), engine.state().position.x().value(), 1E-6);
	EXPECT_NEAR(expected_initial_position.y().value(), engine.state().position.y().value(), 1E-6);
	EXPECT_NEAR(expected_initial_position.z().value(), engine.state().position.z().value(), 1E-6);
	EXPECT_NEAR(0, engine.state().velocity.norm2().value(), 1E-6);
	EXPECT_EQ(0 * newtons, rocket.currentThrust());
	EXPECT_EQ(1, rocket.stages().front().fillingRate());
	EXPECT_NEAR(rocket.totalLength().value(), engine.altitude().value(), 1E-3);
	EXPECT_EQ(model.stages.front().gross_mass, rocket.stages().front().currentMass());
}

TEST_F(TestSimulationBasicRocketMasslessBody, TestStateAtLaunch)
{
	rocket.fire();
	EXPECT_NEAR(expected_initial_position.x().value(), engine.state().position.x().value(), 1E-6);
	EXPECT_NEAR(expected_initial_position.y().value(), engine.state().position.y().value(), 1E-6);
	EXPECT_NEAR(expected_initial_position.z().value(), engine.state().position.z().value(), 1E-6);
	EXPECT_NEAR(0, engine.state().velocity.norm2().value(), 1E-6);
	EXPECT_EQ(model.stages.front().thrust, rocket.currentThrust());
	const utils::units::acceleration_vector expected_acceleration
		= {0, model.stages.front().thrust / rocket.currentMass(), 0};
	EXPECT_EQ(expected_acceleration, engine.currentAcceleration());
	EXPECT_EQ(1, rocket.stages().front().fillingRate());
	EXPECT_NEAR(rocket.totalLength().value(), engine.altitude().value(), 1E-3);
	EXPECT_EQ(model.stages.front().gross_mass, rocket.stages().front().currentMass());
}

TEST_F(TestSimulationBasicRocketMasslessBody, TestStateAfterLaunch)
{
	const auto step = .1 * seconds;
	rocket.fire();
	engine.tick(step);
	EXPECT_EQ(step, engine.currentTime());
	const auto expected = Engine::position_t{0, body.radius + rocket.totalLength(), 0};
	EXPECT_NEAR(expected.x().value(), engine.state().position.x().value(), 1E-6);
	EXPECT_NEAR(expected.y().value() + .237, engine.state().position.y().value(), 1E-3);
	EXPECT_NEAR(expected.z().value(), engine.state().position.z().value(), 1E-6);
	EXPECT_GT(engine.state().velocity.norm().value(), 0);
	EXPECT_LT(rocket.stages().front().fillingRate(), 1);
	EXPECT_LT(rocket.stages().front().currentMass(), model.stages.front().gross_mass);
	EXPECT_NEAR(rocket.totalLength().value() + .237, engine.altitude().value(), 1E-3);
}

TEST_F(TestSimulationBasicRocketMasslessBody, TestStateTillBurnout)
{
	const auto step = .1 * seconds;
	rocket.fire();

	while(engine.currentTime() <= model.stages.front().burn_time + step)
		engine.tick(step);

	ASSERT_GT(engine.currentTime(), model.stages.front().burn_time);
	EXPECT_EQ(0, rocket.stages().front().fillingRate());
	EXPECT_EQ(0 * newtons, rocket.currentThrust());
	EXPECT_EQ(model.stages.front().dry_mass, rocket.stages().front().currentMass());
}


#include <Eigen/Core>
#include <cpzlib.hh>
#include <iostream>

int main(int argc, char** argv) {
  if (argc != 1) {
    std::cout << argv[0] << " takes no arguments.\n";
    return 1;
  }

  Eigen::Matrix<float, 3, 2> generators;
  Eigen::Matrix<float, 3, 1> center;
  Eigen::Matrix<float, 2, 2> exponents;
  Eigen::Matrix<float, 0, 1> constraints;
  Eigen::Matrix<float, 1, 0> constraint_generators;
  Eigen::Matrix<float, 2, 0> constraint_exponents;
  cpz::ConstrainedPolynomialZonotope<> z(
  center, generators, exponents, constraints, constraint_generators, constraint_exponents);
}


#pragma once

#include "DependencyHelpers.hpp"
#include "Variable.hpp"

#include <boost/graph/adjacency_matrix.hpp>

#include <xtensor/xtensor.hpp>

namespace tsplp::graph
{
    using PiSigmaGraphTraits = boost::adjacency_matrix_traits<boost::directedS>;
    using PiSigmaVertex = PiSigmaGraphTraits::vertex_descriptor;
    using PiSigmaEdge = PiSigmaGraphTraits::edge_descriptor;
    using PiSigmaVertexProperties =
        boost::property<boost::vertex_predecessor_t, PiSigmaGraphTraits::edge_descriptor,
        boost::property<boost::vertex_color_t, boost::default_color_type,
        boost::property<boost::vertex_distance_t, double>>>;
    using PiSigmaEdgeProperties = boost::property<boost::edge_residual_capacity_t, double>;
    using PiSigmaSupportGraphImpl = boost::adjacency_matrix<boost::directedS, PiSigmaVertexProperties, PiSigmaEdgeProperties>;

    class PiSigmaSupportGraph
    {
    private:
        PiSigmaSupportGraphImpl m_graph;
        const xt::xtensor<Variable, 3>& m_variables;
        const DependencyGraph& m_dependencies;
        const Model& m_model;

    public:
        PiSigmaSupportGraph(const xt::xtensor<Variable, 3>& variables, const DependencyGraph& dependencies, const Model& model);

    public:
        enum class ConstraintType
        {
            Pi, Sigma, PiSigma
        };

        std::pair<double, std::vector<std::pair<PiSigmaVertex, PiSigmaVertex>>> FindMinCut(PiSigmaVertex s, PiSigmaVertex t, ConstraintType x);
    };
}


#include <Kokkos_Core.hpp>

#include <boost/program_options.hpp>

#include <mpi.h>

#include "heatdis.hpp"

/* Added to test restart */
#include <signal.h>
#include <sys/types.h>
#include <unistd.h>

using namespace heatdis;

/*
    This sample application is based on the heat distribution code
    originally developed within the FTI project: github.com/leobago/fti
*/

int main(int argc, char* argv[])
{
    std::size_t rank, nbProcs, nbLines, M;
    double wtime, memSize, localerror, globalerror = 1;

    namespace bpo = boost::program_options;
    bpo::options_description desc("Heatdis");

    desc.add_options()(
        "size", bpo::value<std::size_t>()->default_value(100u), "Problem Size");
    desc.add_options()("nsteps", bpo::value<std::size_t>()->default_value(600u),
        "Number of timesteps");
    desc.add_options()("precision",
        bpo::value<double>()->default_value(0.00001), "Minimum precision");
    // desc.add_options()("config", bpo::value<std::string>());
    desc.add_options()(
        "scale", bpo::value<std::string>()->default_value("weak"));

    bpo::variables_map vm;

    // Setup commandline arguments
    bpo::store(bpo::parse_command_line(argc, argv, desc), vm);
    bpo::notify(vm);

    std::size_t nsteps = vm["nsteps"].as<std::size_t>();
    const auto precision = vm["precision"].as<double>();

    int strong = 0, str_ret;

    std::string scale = vm["scale"].as<std::string>();
    if (scale == "strong")
        strong = 1;

    MPI_Init(&argc, &argv);
    int mpi_nbProcs, mpi_rank;
    MPI_Comm_size(MPI_COMM_WORLD, &mpi_nbProcs);
    MPI_Comm_rank(MPI_COMM_WORLD, &mpi_rank);

    nbProcs = mpi_nbProcs, rank = mpi_rank;

    std::size_t mem_size = vm["size"].as<std::size_t>();

    if (mem_size == 0)
    {
        printf("Wrong memory size! See usage\n");
        exit(3);
    }

    Kokkos::initialize(argc, argv);
    {
        if (!strong)
        {
            /* weak scaling */
            M = sqrt((double) (mem_size * 1024.0 * 1024.0 * nbProcs) /
                (2 * sizeof(double)));    // two matrices needed
            nbLines = (M / nbProcs) + 3;
        }
        else
        {
            /* strong scaling */
            M = sqrt((double) (mem_size * 1024.0 * 1024.0 * nbProcs) /
                (2 * sizeof(double) * nbProcs));    // two matrices needed
            nbLines = (M / nbProcs) + 3;
        }

        Kokkos::View<double*> h_view("h", M * nbLines);
        Kokkos::View<double*> g_view("g", M * nbLines);

        initData(nbLines, M, rank, g_view);

        memSize = M * nbLines * 2 * sizeof(double) / (1024 * 1024);

        // printf("nbProcs: %d\n", nbProcs);
        // printf("size of double: %d\n", sizeof(double));

        if (rank == 0)
            if (!strong)
            {
                printf("Local data size is %lu x %lu = %f MB (%lu).\n", M,
                    nbLines, memSize, mem_size);
            }
            else
            {
                printf("Local data size is %lu x %lu = %f MB (%lu).\n", M,
                    nbLines, memSize, mem_size / nbProcs);
            }
        if (rank == 0)
            printf("Target precision : %f \n", precision);
        if (rank == 0)
            printf("Maximum number of iterations : %lu \n", nsteps);

        wtime = MPI_Wtime();
        int i = 0;
        int v = i;
        printf("i: %d --- v: %d\n", i, v);
        if (i < 0)
        {
            i = 0;
        }

        while (i < nsteps)
        {
            localerror = doWork(nbProcs, rank, M, nbLines, g_view, h_view);

            if (((i % ITER_OUT) == 0) && (rank == 0))
            {
                printf("Step : %d, error = %f\n", i, globalerror);
            }
            if ((i % REDUCED) == 0)
            {
                MPI_Allreduce(&localerror, &globalerror, 1, MPI_DOUBLE, MPI_MAX,
                    MPI_COMM_WORLD);
            }

            if (globalerror < precision)
            {
                printf("PRECISION ERROR\n");
                break;
            }
            i++;
        }
        if (rank == 0)
            printf("Execution finished in %lf seconds.\n", MPI_Wtime() - wtime);
    }
    Kokkos::finalize();

    MPI_Finalize();
    return 0;
}


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file boost/numeric/ublasx/operation/cumsum.hpp
 *
 * \brief Compute the cumulative sum of an array.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * 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)
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_CUMSUM_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_CUMSUM_HPP

#include <boost/numeric/ublas/detail/config.hpp>
#include <boost/numeric/ublas/detail/temporary.hpp>
#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/size.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>
#include <cstddef>

//FIXME: could not user boost::[matrix|vector]_temporary_traits since some
//container type (e.g., zero_matrix) does not define such type.

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

//@{ Declarations

/**
 * \brief Compute the cumulative sum of the elements of the given vector
 *  expression.
 * \tparam VectorExprT The type of the vector expression.
 * \param ve The vector expression whose elements are cumsummed up.
 * \return The cumulative sum of the elements of the vector expression.
 *
 * \author Marco Guazzone, <marco.guazzone@gmail.com>
 */
template <typename VectorExprT>
vector<typename vector_traits<VectorExprT>::value_type> cumsum(vector_expression<VectorExprT> const& ve);

/**
 * \brief Compute the cumulative sum of the elements over each column of the
 *  given matrix expression.
 * \tparam MatrixExprT The type of the matrix expression.
 * \param ve The matrix expression whose elements are cumulative summed up over
 *  each column.
 * \return The cumulative sum of the elements over each column of the give
 *  matrix expression.
 *
 * \author Marco Guazzone, <marco.guazzone@gmail.com>
 */
template <typename MatrixExprT>
matrix<typename matrix_traits<MatrixExprT>::value_type> cumsum(matrix_expression<MatrixExprT> const& me);

/**
 * \brief Compute the cumulative sum of the elements over each column of the
 *  given matrix expression.
 * \tparam MatrixExprT The type of the matrix expression.
 * \param ve The matrix expression whose elements are cumulative summed up over
 *  each column.
 * \return The cumulative sum of the elements over each column of the given
 *  matrix expression.
 *
 * \author Marco Guazzone, <marco.guazzone@gmail.com>
 */
template <typename MatrixExprT>
matrix<typename matrix_traits<MatrixExprT>::value_type> cumsum_rows(matrix_expression<MatrixExprT> const& me);

/**
 * \brief Compute the cumulative sum of the elements over each row of the
 *  given matrix  expression.
 * \tparam MatrixExprT The type of the matrix expression.
 * \param me The matrix expression whose elements are cumulative summed up over
 *  each row.
 * \return A vector containing the cumsum of the elements over each row of
 *  the given matrix expression.
 *
 * \author Marco Guazzone, <marco.guazzone@gmail.com>
 */
template <typename MatrixExprT>
matrix<typename matrix_traits<MatrixExprT>::value_type> cumsum_columns(matrix_expression<MatrixExprT> const& me);

/**
 * \brief Compute the cumulative sum of the elements of the given matrix
 *  expression along the given dimension tag.
 * \tparam MatrixExprT The type of the matrix expression.
 * \param me The matrix expression whose elements are cumulative summed up along
 *  the given dimension.
 * \return A vector containing the cumulative sum of the elements along the
 *  given dimension of the given matrix expression.
 *
 * \author Marco Guazzone, <marco.guazzone@gmail.com>
 */
template <typename TagT, typename MatrixExprT>
matrix<typename matrix_traits<MatrixExprT>::value_type> cumsum_by_tag(matrix_expression<MatrixExprT> const& me);

//@} Declarations

namespace detail {

//@{ Declarations

/**
 * \brief Auxiliary class for computing the cumsum of the elements over the given
 *  dimension for a container of the given category.
 * \tparam Dim The dimension number (starting from 1).
 * \tparam CategoryT The category type (e.g., vector_tag).
 */
template <std::size_t Dim, typename CategoryT>
struct cumsum_by_dim_impl;

/**
 * \brief Auxiliary class for computing the cumsum of the elements over the given
 *  dimension tag for a container of the given category.
 * \tparam TagT The dimension tag type (e.g., tag::major).
 * \tparam CategoryT The category type (e.g., vector_tag).
 * \tparam OrientationT The orientation category type (e.g., row_major_tag).
 */
template <typename TagT, typename CategoryT, typename OrientationT>
struct cumsum_by_tag_impl;

//@} Declarations

//@{ Definitions

template <>
struct cumsum_by_dim_impl<1, vector_tag>
{
    template <typename VectorExprT>
    BOOST_UBLAS_INLINE
    static vector<typename vector_traits<VectorExprT>::value_type> apply(vector_expression<VectorExprT> const& ve)
    {
        return cumsum(ve);
    }
};

template <>
struct cumsum_by_dim_impl<1, matrix_tag>
{
    template <typename MatrixExprT>
    BOOST_UBLAS_INLINE
    static matrix<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
    {
        return cumsum_rows(me);
    }
};

template <>
struct cumsum_by_dim_impl<2, matrix_tag>
{
    template <typename MatrixExprT>
    BOOST_UBLAS_INLINE
    static matrix<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
    {
        return cumsum_columns(me);
    }
};

template <>
struct cumsum_by_tag_impl<tag::major, matrix_tag, row_major_tag>
{
    template <typename MatrixExprT>
    BOOST_UBLAS_INLINE
    static matrix<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
    {
        return cumsum_rows(me);
    }
};

template <>
struct cumsum_by_tag_impl<tag::minor, matrix_tag, row_major_tag>
{
    template <typename MatrixExprT>
    BOOST_UBLAS_INLINE
    static matrix<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
    {
        return cumsum_columns(me);
    }
};

template <>
struct cumsum_by_tag_impl<tag::leading, matrix_tag, row_major_tag>
{
    template <typename MatrixExprT>
    BOOST_UBLAS_INLINE
    static matrix<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
    {
        return cumsum_columns(me);
    }
};

template <>
struct cumsum_by_tag_impl<tag::major, matrix_tag, column_major_tag>
{
    template <typename MatrixExprT>
    BOOST_UBLAS_INLINE
    static matrix<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
    {
        return cumsum_columns(me);
    }
};

template <>
struct cumsum_by_tag_impl<tag::minor, matrix_tag, column_major_tag>
{
    template <typename MatrixExprT>
    BOOST_UBLAS_INLINE
    static matrix<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
    {
        return cumsum_rows(me);
    }
};

template <>
struct cumsum_by_tag_impl<tag::leading, matrix_tag, column_major_tag>
{
    template <typename MatrixExprT>
    BOOST_UBLAS_INLINE
    static matrix<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
    {
        return cumsum_rows(me);
    }
};

template <typename TagT>
struct cumsum_by_tag_impl<TagT, matrix_tag, unknown_orientation_tag>: cumsum_by_tag_impl<TagT, matrix_tag, row_major_tag>
{
    // Empty
};

//@} Definitions

} // Namespace detail

//@{ Definitions

template <typename VectorExprT>
BOOST_UBLAS_INLINE
vector<typename vector_traits<VectorExprT>::value_type> cumsum(vector_expression<VectorExprT> const& ve)
{
    typedef typename vector_traits<VectorExprT>::size_type size_type;
    typedef vector<typename vector_traits<VectorExprT>::value_type> result_type;

    size_type n = size(ve);
    result_type s(ve);

    for (size_type i = 1; i < n; ++i)
    {
        s(i) += s(i-1);
    }

    return s;
}

template <typename MatrixExprT>
BOOST_UBLAS_INLINE
matrix<typename matrix_traits<MatrixExprT>::value_type> cumsum(matrix_expression<MatrixExprT> const& me)
{
    typedef typename matrix_traits<MatrixExprT>::size_type size_type;
    typedef matrix<typename matrix_traits<MatrixExprT>::value_type> result_type;

    size_type nr = num_rows(me);
    size_type nc = num_columns(me);

    result_type s(me);

    for (size_type c = 0; c < nc; ++c)
    {
        for (size_type r = 1; r < nr; ++r)
        {
            s(r,c) += s(r-1,c);
        }
    }

    return s;
}

template <typename MatrixExprT>
BOOST_UBLAS_INLINE
matrix<typename matrix_traits<MatrixExprT>::value_type> cumsum_rows(matrix_expression<MatrixExprT> const& me)
{
    return cumsum(me);
}

template <typename MatrixExprT>
BOOST_UBLAS_INLINE
matrix<typename matrix_traits<MatrixExprT>::value_type> cumsum_columns(matrix_expression<MatrixExprT> const& me)
{
    typedef typename matrix_traits<MatrixExprT>::size_type size_type;
    typedef matrix<typename matrix_traits<MatrixExprT>::value_type> result_type;

    size_type nr = num_rows(me);
    size_type nc = num_columns(me);

    result_type s(me);

    for (size_type r = 0; r < nr; ++r)
    {
        for (size_type c = 1; c < nc; ++c)
        {
            s(r,c) += s(r,c-1);
        }
    }

    return s;
}

template <size_t Dim, typename VectorExprT>
BOOST_UBLAS_INLINE
vector<typename vector_traits<VectorExprT>::value_type> cumsum(vector_expression<VectorExprT> const& ve)
{
    return detail::cumsum_by_dim_impl<Dim, vector_tag>::template apply(ve);
}

template <size_t Dim, typename MatrixExprT>
BOOST_UBLAS_INLINE
matrix<typename matrix_traits<MatrixExprT>::value_type> cumsum(matrix_expression<MatrixExprT> const& me)
{
    return detail::cumsum_by_dim_impl<Dim, matrix_tag>::template apply(me);
}

template <typename TagT, typename MatrixExprT>
//template <typename MatrixExprT, typename TagT>
BOOST_UBLAS_INLINE
matrix<typename matrix_traits<MatrixExprT>::value_type> cumsum_by_tag(matrix_expression<MatrixExprT> const& me)
{
    return detail::cumsum_by_tag_impl<TagT, matrix_tag, typename matrix_traits<MatrixExprT>::orientation_category>::template apply(me);
}

//@} Definitions

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_CUMSUM_HPP


//=======================================================================
// Copyright 2002 Indiana University.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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 <boost/graph/graph_as_tree.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/cstdlib.hpp>
#include <iostream>

class tree_printer
{
public:
    template < typename Node, typename Tree > void preorder(Node, Tree&)
    {
        std::cout << "(";
    }
    template < typename Node, typename Tree > void inorder(Node n, Tree& t)
    {
        std::cout << get(boost::vertex_name, t)[n];
    }
    template < typename Node, typename Tree > void postorder(Node, Tree&)
    {
        std::cout << ")";
    }
};

int main()
{
    using namespace boost;
    typedef adjacency_list< vecS, vecS, directedS,
        property< vertex_name_t, std::string > >
        graph_t;
    typedef graph_traits< graph_t >::vertex_descriptor vertex_t;

    graph_t g;

    vertex_t a = add_vertex(g), b = add_vertex(g), c = add_vertex(g);

    add_edge(a, b, g);
    add_edge(a, c, g);

    typedef property_map< graph_t, vertex_name_t >::type vertex_name_map_t;
    vertex_name_map_t name = get(vertex_name, g);
    name[a] = "A";
    name[b] = "B";
    name[c] = "C";

    typedef iterator_property_map< std::vector< vertex_t >::iterator,
        property_map< graph_t, vertex_index_t >::type >
        parent_map_t;
    std::vector< vertex_t > parent(num_vertices(g));
    typedef graph_as_tree< graph_t, parent_map_t > tree_t;
    tree_t t(
        g, a, make_iterator_property_map(parent.begin(), get(vertex_index, g)));

    tree_printer vis;
    traverse_tree(a, t, vis);

    return exit_success;
}


// Andrew Long
// Updated: Feb 13, 2017
// Doubly linked list acting on a 2D grid (1D representation)
// could probably extend to ND grids but for now this is easiest
// Requirements:
//      - Lookup of the top K values in the matrix
//      - Zero a column or row of the matrix
//      - Undo the last set of changes (lots of pushing and popping onto the stack, don't want pass by value)

// The GridDLL has the following properties:
// stores values in N1xN2 grid as a 1D array, linked list ordered by sorted values
// lazy deletion of matrix indices (invalidates the node and refreshes pointers around it)
// fast reinclusion of deleted nodes via a deleted item stack
// O(K) selection of topK values
// O(2*N2) removal of overlapping points
// O(N1+N2-2*i-1) to restore overlapped points from deletion stack

// Alternatives:
// (1) Vector of sorted pairs of <value, location(i,j)>
// initializiation is the same (requires sort)
// Lookup is O(K)
// Removal of overlap points is O(N1*N2)
// Undo is better managed via pass by value (requires O(N1*N2) copy)
//      - alternative is challenging, would need more bookkeeping and would be slower
//      - inserting into sorted list using the refresh_stack and a secondary copy of the matrix
//
// (2) Raw 1D form of a matrix
//  Initialization is O(1)
//  Lookup is O(N*k) (scan for top value through iteration, some kind of sorted buffer could also work)
//  Removal of overlap points is O(2*N2) (just zero out the indices in the matrix)
//  Undo could be done via a deletion stack O(N1+N2-2*i+1)
//      - needs a secondary copy of the matrix

#ifndef __GRID_DLL__
#define __GRID_DLL__

#include <vector>
#include <functional>
#include <utility>
#include <armadillo>
#include <cassert>

typedef std::pair<long,long> GridPoint;

class GridDLL
{
public:
	struct GridNode
	{
		GridNode(long _x=-1, long _y=-1, double _w=0, long p = -1, long n = -1, bool v = false)
			{	set(x,y,w,p,n,v);	}
		void set(long _x, long _y, double _w, long _p, long _n, bool _v = false)
		{
			x = _x;
			y = _y;
			w = _w;
			prev = _p;
			next = _n;
			valid = _v;
		}
		long x,y;
		double w;

		long prev;
		long next;
		bool valid;

		bool operator<(const GridNode& other) { return w < other.w; }
	};
protected:
	std::vector<GridNode> grid;
    std::vector<long> refresh_stack;
	long N1, N2, Nelem;
	long head,tail;
	long count;
private:
public:
	GridDLL()
	{
		reset();
	}

	GridDLL(const GridDLL& other)
	{
		grid.assign(other.grid.begin(), other.grid.end());

		N1 = other.N1;
		N2 = other.N2;
		Nelem = other.Nelem;
		count = other.count;
		head = other.head;
		tail = other.tail;
        refresh_stack.reserve(Nelem);
	}

	~GridDLL()
	{
		reset();
	}

	void reset()
	{
		grid.clear();
        refresh_stack.clear();
		N1 = 0;
		N2 = 0;
		count = 0;
		Nelem = 0;
		head = -1;
		tail = -1;
	}

	bool empty()
	{
		return count == 0;
	}

    long size()
    {
        return count;
    }

	bool consistent()
	{
		long c = 0;
		long n = head;
		while(n != -1)
		{
			++c;
			n = grid[n].next;
		}
		return c == count;
	}

	void buildFromMat(const arma::mat& mat)
	{
		N1 = (mat.n_rows);
		N2 = (mat.n_cols);
        Nelem = mat.n_elem;
        count = Nelem;

		assert(N1 > 1 && N2 > 1);

        arma::uvec sort_idx = arma::sort_index(mat, "descend");

        grid.resize(Nelem);
        refresh_stack.reserve(Nelem);

		long x,y;
		// construct head element [0]
		x = sort_idx[0]%N1;
		y = sort_idx[0]/N1;

		head = sort_idx[0];
		grid[sort_idx[0]].set(x, y, mat(x,y), -1, sort_idx[1], true);

		// construct [1..Nelem-1)
		for(int i = 1; i < Nelem-1; ++i)
		{
			x = sort_idx[i]%N1;
			y = sort_idx[i]/N1;

			grid[sort_idx[i]].set(x, y, mat(x,y), sort_idx[i-1], sort_idx[i+1], true);
		}

		// construct tail element [Nelem-1]
		x = sort_idx[Nelem-1]%N1;
		y = sort_idx[Nelem-1]/N1;
		tail = sort_idx[Nelem-1];
		grid[sort_idx[Nelem-1]].set(x, y, mat(x,y), sort_idx[Nelem-2],-1, true);
	}

    void printList(std::string str = "")
	{
        if(str != "")
            printf("%s\n",str.c_str());
		printf("[Head:%ld]\t[Tail:%ld]\t[Count:%ld]\n",head,tail,count);
		long cur_node = head;
		std::string tabs = "";
		while(cur_node != -1)
		{
			long idx = grid[cur_node].x + (grid[cur_node].y*N1);
			printf("%s[%ld -> %ld -> %ld]\n",tabs.c_str(),grid[cur_node].prev, idx, grid[cur_node].next);
			cur_node = grid[cur_node].next;
			tabs = tabs + "\t";
		}
		printf("\n");
	}
	void print(std::string str = "")
	{
		if(str != "")
			printf("%s\n",str.c_str());

		printf("[Head:(%ld,%ld)]\t[Tail:(%ld,%ld)]\t[Count:%ld]\n",head%N1,head/N1,tail%N1,tail/N1,count);
		long cur_node = head;
		while(cur_node != -1)
		{
			printf("[%ld,%ld (%ld)]:%lf\n",	grid[cur_node].x, grid[cur_node].y, cur_node,grid[cur_node].w);
			cur_node = grid[cur_node].next;
		}
	}

	// invalidate the current node and refresh pointers
    // NOTE: assumes the node is valid.
    // OPS: 4x assignment, 2x add/subtract, 3 booleans
	bool deleteNode(long idx)
	{
		if(count == 1)
		{
			head = -1;
			tail = -1;
		}
		else
		{
			if(idx == head)
			{
				head = grid[idx].next;
				grid[head].prev = -1;
			}
			else if(idx == tail)
			{
				tail = grid[idx].prev;
				grid[tail].next = -1;
			}
			else
			{
				long p = grid[idx].prev;
				long n = grid[idx].next;
				grid[p].next = n;
				grid[n].prev = p;
			}
		}
		--count;
		grid[idx].valid = false;
        refresh_stack.push_back(idx);
        return true;
	}
    void checkALL()
    {
        bool b = consistent();
        long c = count;
        printf("CheckALL: consistent:%d, count:%ld\n",b,c);
        printList();
        printf("\nRefreshStack: %ld",refresh_stack[0]);
        for(int i = 1; i < (int)refresh_stack.size(); ++i)   printf(",%ld",refresh_stack[i]);
        printf("\n");
        printf("StackValid: %d",grid[refresh_stack[0]].valid);
        for(int i = 1; i < (int)refresh_stack.size(); ++i)   printf(",%d",grid[refresh_stack[i]].valid);
        printf("\n");
        long d = 0;
        for(int i = 0; i < Nelem; ++i)
        {
            if(grid[i].valid)
                ++d;
        }
        if(d != c)
        {
            printf("Crap");
        }
    }
	void clearOverlaps(long x, long y)
	{
		for(int i = 0; i < N2; ++i)
		{
			long idx_x = i*N1+x;
			if(grid[idx_x].valid)
				deleteNode(idx_x);
		}
		for(int i = 0; i < N1; ++i)
		{
			long idx_y = y*N1+i;
			if(grid[idx_y].valid)
				deleteNode(idx_y);
		}
	}
    void relinkStack(long depth)
    {
        long num_to_pop = N1 + N2 - 1 - 2*depth;
        for(long i = 0; i < num_to_pop; ++i)
        {
            long restore = refresh_stack.back();

            // restore the node
            grid[restore].valid = true;

            // prev restoration
            if(grid[restore].next != -1)
                grid[grid[restore].next].prev = restore;
            if(head == grid[restore].next)
                head = restore;

            // next restoration
            if(grid[restore].prev != -1)
                grid[grid[restore].prev].next = restore;
            if(tail == grid[restore].prev)
                tail = restore;

            ++count;
            refresh_stack.pop_back();
        }
    }

	long topK(long K, std::vector<GridPoint>& points, long start = 0)
	{
		long nK = std::min(K, count);
		long cur_node = head;
		for(long i = 0; i < nK; ++i)
		{
            points[start+i].first = grid[cur_node].x;
            points[start+i].second = grid[cur_node].y;
			cur_node = grid[cur_node].next;
		}

		return nK;
	}
};

#endif


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/list/instance.hpp>
using namespace boost::hana;
using namespace literals;

//! [main]
BOOST_HANA_CONSTANT_ASSERT(
    sort_by(_>_, list(1_c, -2_c, 3_c, 0_c)) == list(3_c, 1_c, 0_c, -2_c)
);
//! [main]

int main() { }


// Copyright 2015 National ICT Australia Limited (NICTA)
//
// 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.

#define BOOST_TEST_MODULE sgt_tests

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

#include "../SgtCore/SgtCore.h"
#include "../SgtSim/SgtSim.h"
#include "../SgtSim/WeakOrder.h"

#include <cmath>
#include <ostream>
#include <fstream>
#include <set>
#include <vector>

using namespace arma;
using namespace Sgt;
using namespace std;
using namespace boost::unit_test;

struct Init
{
    Init()
    {
        unit_test_log.set_threshold_level(boost::unit_test::log_messages);
        BOOST_TEST_MESSAGE("\nTesting " << framework::current_test_case().p_name);
    }
    ~Init()
    {
        BOOST_TEST_MESSAGE("Finished " << framework::current_test_case().p_name << "\n");
    }
};

BOOST_FIXTURE_TEST_SUITE(tests, Init)

BOOST_AUTO_TEST_CASE (test_overhead_compare_carson_1)
{
    // For this test, we compare values for an overhead line examined in one of Kersting's papers:
    // docs/background/Kersting_Carson.pdf.
    Network netw;
    Parser<Network> p;
    p.parse("test_overhead_compare_carson_1.yaml", netw);

    auto oh = netw.branches()["line_1_2"].as<OverheadLine>();

    Mat<Complex> ZPrim = oh->ZPrim();
    Mat<Complex> ZPhase = oh->ZPhase();

    Complex cmp;
    cmp = {1.3369, 1.3331};
    double err00 = abs(ZPhase(0,0) - cmp) / abs(cmp);
    cmp = {0.2102, 0.5778};
    double err01 = abs(ZPhase(0, 1) - cmp) / abs(cmp);
    cmp = {0.2132, 0.5014};
    double err02 = abs(ZPhase(0, 2) - cmp) / abs(cmp);
    cmp = {1.3239, 1.3557};
    double err11 = abs(ZPhase(1, 1) - cmp) / abs(cmp);
    cmp = {0.2067, 0.4591};
    double err12 = abs(ZPhase(1, 2) - cmp) / abs(cmp);
    cmp = {1.3295, 1.3459};
    double err22 = abs(ZPhase(2, 2) - cmp) / abs(cmp);

    BOOST_CHECK(err00 < 0.001);
    BOOST_CHECK(err01 < 0.001);
    BOOST_CHECK(err02 < 0.001);
    BOOST_CHECK(err11 < 0.001);
    BOOST_CHECK(err12 < 0.001);
    BOOST_CHECK(err22 < 0.001);

    BOOST_CHECK(abs(ZPhase(0,1) - ZPhase(1,0)) < 1e-6);
    BOOST_CHECK(abs(ZPhase(0,2) - ZPhase(2,0)) < 1e-6);
    BOOST_CHECK(abs(ZPhase(1,2) - ZPhase(2,1)) < 1e-6);
}

BOOST_AUTO_TEST_CASE (test_overhead_compare_carson_2)
{
    // For this test, we compare values for an overhead line examined in one of Kersting's papers:
    // docs/background/Kersting_Carson.pdf.
    Network netw;
    Parser<Network> p;
    p.parse("test_overhead_compare_carson_2.yaml", netw);

    netw.solvePowerFlow();

    auto bus2 = netw.buses()["bus_2"];

    Complex cmp;
    cmp = polar(14.60660, -0.62 * pi / 180.0);
    double err0 = abs(bus2->V()(0) - cmp) / abs(cmp);
    cmp = polar(14.72669, -121.0 * pi / 180.0);
    double err1 = abs(bus2->V()(1) - cmp) / abs(cmp);
    cmp = polar(14.80137, 119.2 * pi / 180.0);
    double err2 = abs(bus2->V()(2) - cmp) / abs(cmp);

    BOOST_CHECK(err0 < 0.001);
    BOOST_CHECK(err1 < 0.001);
    BOOST_CHECK(err2 < 0.001);
}

BOOST_AUTO_TEST_CASE (test_underground_conc_compare_carson)
{
    // For this test, we compare values for an underground line examined in one of Kersting's papers:
    // docs/background/Kersting_Carson.pdf.
    Network netw;
    Parser<Network> p;
    p.parse("test_underground_conc_compare_carson.yaml", netw);

    netw.solvePowerFlow();

    auto ug = netw.branches()["line_1_2"].as<UndergroundLine>();
    Mat<Complex> ZPhase = ug->ZPhase() * 1609.344; // Convert to ohms per mile.
    Mat<Complex> ZPhaseKersting;
    ZPhaseKersting << Complex(0.7981, 0.4463) << Complex(0.3191, 0.0328) << Complex(0.2849, -0.0143) << endr
                   << Complex(0.3191, 0.0328) << Complex(0.7891, 0.4041) << Complex(0.3191, 0.0328) << endr
                   << Complex(0.2849, -0.0143) << Complex(0.3191, 0.0328) << Complex(0.7981, 0.4463) << endr;
    BOOST_TEST_MESSAGE(ZPhase);
    BOOST_TEST_MESSAGE(ZPhaseKersting);
    double err = norm(ZPhase - ZPhaseKersting, "inf");
    BOOST_TEST_MESSAGE("Err = " << err);
    BOOST_CHECK(err < 0.0005);
}

BOOST_AUTO_TEST_CASE (test_underground_tape_compare_carson)
{
    // For this test, we compare values for an underground line examined in one of Kersting's papers:
    // docs/background/Kersting_Carson.pdf.
    Network netw;
    Parser<Network> p;
    p.parse("test_underground_tape_compare_carson.yaml", netw);

    netw.solvePowerFlow();

    auto ug = netw.branches()["line_1_2"].as<UndergroundLine>();
    Mat<Complex> ZPrim = ug->ZPrim() * 1609.344; // Convert to ohms per mile.
    Mat<Complex> ZPhase = ug->ZPhase() * 1609.344; // Convert to ohms per mile.
    Complex ZPhaseKersting = Complex(1.3219, 0.6743);
    BOOST_TEST_MESSAGE(ZPhase(0));
    BOOST_TEST_MESSAGE(ZPhaseKersting);
    double err = norm(ZPhase - ZPhaseKersting, "inf");
    BOOST_TEST_MESSAGE("Err = " << err);
    BOOST_CHECK(err < 0.0006);
}

BOOST_AUTO_TEST_CASE (test_data_timeseries)
{
    auto addPoint = [](auto& series, const Time& t, const auto& v){series.addPoint(t, v);};

    vector<Time> times = {hours(1), hours(2), hours(3), hours(4)};
    Time t0 = times[0] - minutes(30);

    {
        StepwiseTimeSeries<Time, double> series(0.0);
        vector<double> vals{1.0, 2.0, 3.0, 4.0};
        for (int i = 0; i < times.size(); ++i) addPoint(series, times[i], vals[i]);

        vector<double> expected{0.0, 1.0, 1.0, 2.0, 2.0, 3.0, 3.0, 4.0, 0.0};
        Time t = t0;
        for (auto it = expected.begin(); it != expected.end(); ++it, t += minutes(30))
        {
            std::cout << series.value(t) << " " << *it << std::endl;
            BOOST_CHECK_CLOSE(series.value(t), *it, 1e-6);
        }
    }

    std::cout << std::endl;

    {
        LerpTimeSeries<Time, double> series(0.0);
        vector<double> vals{1.0, 2.0, 3.0, 4.0};
        for (int i = 0; i < times.size(); ++i) addPoint(series, times[i], vals[i]);

        vector<double> expected{0.0, 1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0, 0.0};
        Time t = t0;
        for (auto it = expected.begin(); it != expected.end(); ++it, t += minutes(30))
        {
            std::cout << series.value(t) << " " << *it << std::endl;
            BOOST_CHECK_CLOSE(series.value(t), *it, 1e-6);
        }
    }

    std::cout << std::endl;

    {
        StepwiseTimeSeries<Time, Col<Complex>> series({{0.0, 0.0}, {0.0, 0.0}});
        vector<Col<Complex>> vals{
            {{1.0, 10.0}, {100.0, 1000.0}},
            {{2.0, 20.0}, {200.0, 2000.0}},
            {{3.0, 30.0}, {300.0, 3000.0}},
            {{4.0, 40.0}, {400.0, 4000.0}}
        };
        for (int i = 0; i < times.size(); ++i) addPoint(series, times[i], vals[i]);

        vector<Col<Complex>> expected{
            {{0.0, 0.0}, {0.0, 0.0}},
            {{1.0, 10.0}, {100.0, 1000.0}},
            {{1.0, 10.0}, {100.0, 1000.0}},
            {{2.0, 20.0}, {200.0, 2000.0}},
            {{2.0, 20.0}, {200.0, 2000.0}},
            {{3.0, 30.0}, {300.0, 3000.0}},
            {{3.0, 30.0}, {300.0, 3000.0}},
            {{4.0, 40.0}, {400.0, 4000.0}},
            {{0.0, 0.0}, {0.0, 0.0}}
        };
        Time t = t0;
        for (auto it = expected.begin(); it != expected.end(); ++it, t += minutes(30))
        {
            std::cout << series.value(t) << " " << *it << std::endl;
            std::cout << (norm(series.value(t) - *it, 2)) << " " << *it << std::endl;
            BOOST_CHECK_SMALL(norm(series.value(t) - *it, 2), 1e-6);
        }
    }

    std::cout << std::endl;

    {
        LerpTimeSeries<Time, Col<Complex>> series({{0.0, 0.0}, {0.0, 0.0}});
        vector<Col<Complex>> vals{
            {{1.0, 10.0}, {100.0, 1000.0}},
            {{2.0, 20.0}, {200.0, 2000.0}},
            {{3.0, 30.0}, {300.0, 3000.0}},
            {{4.0, 40.0}, {400.0, 4000.0}}
        };
        for (int i = 0; i < times.size(); ++i) addPoint(series, times[i], vals[i]);

        vector<Col<Complex>> expected{
            {{0.0, 0.0}, {0.0, 0.0}},
            {{1.0, 10.0}, {100.0, 1000.0}},
            {{1.5, 15.0}, {150.0, 1500.0}},
            {{2.0, 20.0}, {200.0, 2000.0}},
            {{2.5, 25.0}, {250.0, 2500.0}},
            {{3.0, 30.0}, {300.0, 3000.0}},
            {{3.5, 35.0}, {350.0, 3500.0}},
            {{4.0, 40.0}, {400.0, 4000.0}},
            {{0.0, 0.0}, {0.0, 0.0}}
        };
        Time t = t0;
        for (auto it = expected.begin(); it != expected.end(); ++it, t += minutes(30))
        {
            std::cout << series.value(t) << " " << *it << std::endl;
            std::cout << (norm(series.value(t) - *it, 2)) << " " << *it << std::endl;
            BOOST_CHECK_SMALL(norm(series.value(t) - *it, 2), 1e-6);
        }
    }
}

BOOST_AUTO_TEST_CASE (test_function_timeseries)
{
    FunctionTimeSeries <Time, double> fts([] (Time td) {return 2 * dSeconds(td);});
    BOOST_CHECK(fts.value(seconds(-1)) == -2.0);
    BOOST_CHECK(fts.value(seconds(3)) == 6.0);
}

BOOST_AUTO_TEST_CASE (test_matpower)
{
    BOOST_TEST_MESSAGE("Starting matpower tests");
    using namespace Sgt;

    vector<string> cases =
    {
        "caseSLPQ",
        "caseSLPV",
        "caseSLPQPV",
        "transformer",
        "nesta_case3_lmbd",
        "case4gs",
        "nesta_case4_gs",
        "case5",
        "nesta_case5_pjm",
        "case6ww",
        "nesta_case6_c",
        "nesta_case6_ww",
        "case9",
        "case9Q",
        "case9target",
        "nesta_case9_wscc",
        "case14",
        "case14_shift",
        "nesta_case14_ieee",
        "case24_ieee_rts",
        "nesta_case29_edin",
        "case30",
        "case30Q",
        "case30_all",
        "case30pwl",
        "case_ieee30",
        "nesta_case30_as",
        "nesta_case30_fsr",
        "nesta_case30_ieee",
        "case39",
        "nesta_case39_epri",
        "case57",
        "nesta_case57_ieee",
        "nesta_case73_ieee_rts",
        "case89pegase",
        "nesta_case89_pegase",
        "case118",
        "nesta_case118_ieee",
        "nesta_case162_ieee_dtc",
        "nesta_case189_edin",
        "case300",
        "nesta_case300_ieee",
        "case1354pegase",
        "nesta_case1354_pegase",
        "nesta_case1394sop_eir",
        "nesta_case1397sp_eir",
        "nesta_case1460wp_eir",
        "nesta_case2224_edin",
        "case2383wp",
        "nesta_case2383wp_mp",
        "case2736sp",
        "nesta_case2736sp_mp",
        "case2737sop",
        "nesta_case2737sop_mp",
        "case2746wop",
        "case2746wp",
        "nesta_case2746wop_mp",
        "nesta_case2746wp_mp",
        "case2869pegase",
        "nesta_case2869_pegase",
        "case3012wp", // TODO This fails, probably because SGT doesn't enforce reactive limits. See e.g. bus 70.
        "nesta_case3012wp_mp",
        "case3120sp",
        "nesta_case3120sp_mp",
        "case3375wp", // Can't solve correctly, but then neither can Matpower...
        "nesta_case3375wp_mp", // See above, doesn't converge.
        "case9241pegase",
        "nesta_case9241_pegase"

    };

    set<string> polExclude = {
        "nesta_case1394sop_eir", // Flat start doesn't converge, neither does matpower.
        "nesta_case1460wp_eir", // Flat start doesn't converge, neither does matpower.
        "case3012wp", // Flat start doesn't converge, neither does matpower.
        "nesta_case3012wp_mp", // Flat start doesn't converge, neither does matpower.
        "case3375wp", // Flat start doesn't converge, neither does matpower.
        "nesta_case3375wp_mp" // Flat start doesn't converge, neither does matpower.
    };

    set<string> rectExclude = {
        "nesta_case300_ieee", // Doesn't converge.
        "case1354pegase", // Doesn't converge.
        "nesta_case1354_pegase", // Doesn't converge.
        "case2869pegase", // Doesn't converge.
        "nesta_case2869_pegase", // Doesn't converge.
        "case3375wp", // Doesn't converge, neither does matpower.
        "nesta_case3375wp_mp", // Doesn't converge, neither does matpower.
        "case9241pegase" // Doesn't converge.
    };

    map<string, pair<double, double>> times;
    for (auto c : cases) times[c] = {-1, -1};

    auto doTest = [&](const decltype(cases)& cs, const string& solverType, const set<string>& exclude)
    {
        for (auto c : cs)
        {
            if (exclude.find(c) != exclude.end())
            {
                continue;
            }

            BOOST_TEST_MESSAGE("Case " << c);

            string yamlStr = string("--- [{matpower : {input_file : matpower_test_cases/")
                + c + ".m, default_kV_base : 11}, power_flow_solver : " + solverType + "}]";
            Network nw(100.0);
            nw.setUseFlatStart(true);
            YAML::Node n = YAML::Load(yamlStr);
            Parser<Network> p;
            p.parse(n, nw);

            Stopwatch sw;
            sw.start();
            bool ok = nw.solvePowerFlow();
            sw.stop();

            double& tRef = (solverType == "nr_pol") ? times[c].first : times[c].second;
            if (!ok)
            {
                tRef = -1;
                BOOST_ERROR("Case " << c << " could not be solved.\n");
                continue;
            }

            tRef = sw.cpuSeconds();
            BOOST_TEST_MESSAGE("Solve time = " << sw.cpuSeconds());

            ifstream compareName(string("mp_compare/") + c + ".compare");

            int nBadV = 0;
            int nBadS = 0;
            for (auto bus : nw.buses())
            {
                double Vr, Vi, P, Q;
                compareName >> Vr >> Vi >> P >> Q;
                assert(!compareName.eof());

                if (!bus->isInService()) continue;

                Complex V = {Vr, Vi};
                Complex S = {P, Q};
                if (abs(V - bus->V()(0) / bus->VBase()) >= 1e-3)
                {
                    cout << "Bus V mismatch at bus " << bus->id() << ": "
                        << V << " : " << bus->V() << endl;
                    ++nBadV;
                }
                if (abs(S - bus->SGen()(0)) / nw.PBase() >= 1e-3)
                {
                    cout << "orkBus S mismatch at bus " << bus->id() << ": "
                        << S << " : " << bus->SGen() << endl;
                    ++nBadS;
                }
            }
            BOOST_CHECK_MESSAGE(nBadV == 0, c << ": V doesn't agree with Matpower at " << nBadV << " buses.\n");
            BOOST_CHECK_MESSAGE(nBadS == 0, c << ": S doesn't agree with Matpower at " << nBadS << " buses.\n");
        }
    };

    // doTest(cases, "nr_pol", polExclude);
    doTest(cases, "nr_rect", rectExclude);

    BOOST_TEST_MESSAGE("Times(pol, rect)");
    BOOST_TEST_MESSAGE("----------------");
    for (auto elem : times)
    {
        BOOST_TEST_MESSAGE(setw(32) << elem.first << " => (" << setw(16) << elem.second.first << ", " << setw(16)
                << elem.second.second << ")");
    }
}

BOOST_AUTO_TEST_CASE (test_const_I)
{
    using namespace Sgt;

    string yamlStr(
            "--- [{matpower : {input_file : matpower_test_cases/nesta_case189_edin.m, " "default_kV_base : 11}}]");
    Network nw(100.0);
    nw.setUseFlatStart(true);
    YAML::Node n = YAML::Load(yamlStr);
    Parser<Network> p;
    p.parse(n, nw);

    string bus1Id = "bus_174";
    auto bus1 = nw.buses()[bus1Id]; // No existing zip generation.
    assert(bus1->zips().size() == 0);

    string branchId = "branch_194_bus_173_bus_174";
    auto branch = nw.branches()[branchId].as<CommonBranch>();

    auto bus0 = branch->bus0();

    auto zip = make_shared<Zip>("zip_I_const", Phases{Phase::BAL});
    Complex Ic = polar(1.0, 15.0 * pi / 180.0) * 0.789;
    zip->setIConst({Ic});
    nw.addZip(zip, bus1Id);

    // for (string solverType : {"nr_pol", "nr_rect"})
    for (string solverType : {"nr_rect"})
    {
        BOOST_TEST_MESSAGE("Solver type = " << solverType);
        /*
        auto solver = solverType == "nr_pol"
            ? unique_ptr<PowerFlowSolverInterface>(new PowerFlowNrPolSolver)
            : unique_ptr<PowerFlowSolverInterface>(new PowerFlowNrRectSolver);
        */
        auto solver = unique_ptr<PowerFlowSolverInterface>(new PowerFlowNrRectSolver);
        nw.setSolver(move(solver));
        bool ok = nw.solvePowerFlow();
        BOOST_CHECK(ok == true);

        Complex V0 = bus0->V()(0);
        Complex V1 = bus1->V()(0);
        Complex S1 = bus1->SZip()(0);

        BOOST_CHECK(abs(S1 - conj(Ic) * abs(V1)) < 1e-6);

        Complex I1 = -(branch->Y() * Col<Complex>({V0, V1})).eval()(1); // Current entering bus 1 from bus 0.
        BOOST_CHECK(abs(Ic * V1 / abs(V1)  - I1) < 1e-6);
    }
}

BOOST_AUTO_TEST_CASE (test_weak_order)
{
    WoGraph g(6);
    g.link(3, 1);
    g.link(4, 1);
    g.link(0, 4);
    g.link(1, 0);
    g.link(1, 2);
    g.link(0, 5);
    g.link(5, 2);
    // Order should be 3, (0, 1, 4), 5, 2.

    g.weakOrder();

    BOOST_CHECK(g.nodes()[0]->index() == 3);
    BOOST_CHECK(g.nodes()[1]->index() == 0);
    BOOST_CHECK(g.nodes()[2]->index() == 1);
    BOOST_CHECK(g.nodes()[3]->index() == 4);
    BOOST_CHECK(g.nodes()[4]->index() == 5);
    BOOST_CHECK(g.nodes()[5]->index() == 2);
}

BOOST_AUTO_TEST_CASE (test_loops)
{
    Simulation sim;
    Parser<Simulation> p;
    p.parse("test_loops.yaml", sim);
    auto comps = sim.simComponents();
    BOOST_CHECK(comps.size() == 6);
    BOOST_CHECK(comps[0]->id() == "b");
    BOOST_CHECK(comps[1]->id() == "ba");
    BOOST_CHECK(comps[2]->id() == "bb");
    BOOST_CHECK(comps[3]->id() == "c");
    BOOST_CHECK(comps[4]->id() == "ca");
    BOOST_CHECK(comps[5]->id() == "cb");
}

BOOST_AUTO_TEST_CASE (test_properties)
{
    class Foo : public HasProperties
    {
        public:
            SGT_PROPS_INIT(Foo);

            int four() const {return 4;}
            SGT_PROP_GET(fourProp, four, int);
    };

    class Bar : public Foo
    {
        public:
            SGT_PROPS_INIT(Bar);
            SGT_PROPS_INHERIT(Foo);

            const int& x() const {return x_;}
            void setX(const int& x) {x_ = x;}
            SGT_PROP_GET_SET(xProp, x, const int&, setX, const int&);

        public:
            int x_{5};
    };

    Bar bar;
    auto& fourProp = bar.properties()["fourProp"];
    auto& xProp = bar.properties()["xProp"];
    BOOST_CHECK_EQUAL(fourProp.string(bar), "4");
    BOOST_CHECK_EQUAL(fourProp.getAs<int>(bar), 4);
    BOOST_CHECK_EQUAL(xProp.string(bar), "5");
    BOOST_CHECK_EQUAL(xProp.getAs<const int&>(bar), 5);
    xProp.setAs<const int&>(bar, 345);
    BOOST_CHECK_EQUAL(xProp.getAs<const int&>(bar), 345);
    xProp.setFromString(bar, "678");
    BOOST_CHECK_EQUAL(xProp.getAs<const int&>(bar), 678);
};

BOOST_AUTO_TEST_CASE (test_phases_A)
{
    Network netw;
    Parser<Network> p;
    p.parse("test_phases_A.yaml", netw);
    netw.solvePowerFlow();
    auto V0 = netw.buses()[0]->V();
    auto V1 = netw.buses()[1]->V();
    BOOST_CHECK_SMALL(abs(V0(0) - V1(1)), 1e-9);
    BOOST_CHECK_SMALL(abs(V0(1) - V1(2)), 1e-9);
    BOOST_CHECK_SMALL(abs(V0(2) - V1(0)), 1e-9);
}

BOOST_AUTO_TEST_CASE (test_phases_B)
{
    Network netw;
    Parser<Network> p;
    p.parse("test_phases_B.yaml", netw);
    netw.solvePowerFlow();
    auto V0 = netw.buses()[0]->V();
    auto V1 = netw.buses()[1]->V();
    BOOST_CHECK_SMALL(abs(V0(0) - V1(2)), 1e-9);
    BOOST_CHECK_SMALL(abs(V0(1) - V1(0)), 1e-9);
    BOOST_CHECK_SMALL(abs(V0(2) - V1(1)), 1e-9);
}

BOOST_AUTO_TEST_CASE (test_phases_C)
{
    Network netw;
    Parser<Network> p;
    p.parse("test_phases_C.yaml", netw);
    netw.solvePowerFlow();
    auto V0 = netw.buses()[0]->V();
    auto V1 = netw.buses()[1]->V();
    auto S0Gen = netw.buses()[0]->SGen();
    auto S1Zip = netw.buses()[1]->SZip();
    auto y = netw.branches()[0]->Y()(0, 0);
    BOOST_CHECK_SMALL(abs(V0(0) - V1(0)), 1e-9);
    BOOST_CHECK_SMALL(abs(S0Gen(0)), 1e-9);
    BOOST_CHECK_SMALL(abs(S0Gen(1)), 1e-9);
    auto delta = (V0(2) - V1(1));
    auto diff = S0Gen(2) - S1Zip(1, 1) - delta * conj(y) * conj(delta);
    BOOST_CHECK_SMALL(abs(diff), 1e-9);
}

BOOST_AUTO_TEST_CASE (test_vv_transformer)
{
    Network netw;
    Parser<Network> p;
    p.parse("test_vv_transformer.yaml", netw);
    netw.solvePowerFlow();

    auto V0 = netw.buses()[0]->V();
    auto V1 = netw.buses()[1]->V();
    auto V0ab = V0(0) - V0(1);
    auto V0bc = V0(1) - V0(2);
    auto V0ca = V0(2) - V0(0);
    auto V1ab = V1(0) - V1(1);
    auto V1bc = V1(1) - V1(2);
    auto V1ca = V1(2) - V1(0);
    cout << abs(V0(0)) << " " << abs(V0(1)) << " " << abs(V0(2)) << endl;
    cout << abs(V1(0)) << " " << abs(V1(1)) << " " << abs(V1(2)) << endl;
    cout << abs(V0ab) << " " << abs(V0bc) << " " << abs(V0ca) << endl;
    cout << abs(V1ab) << " " << abs(V1bc) << " " << abs(V1ca) << endl;
    cout << abs(V0ab/V1ab) << " " << abs(V0bc/V1bc) << " " << abs(V0ca/V1ca) << endl;
    BOOST_CHECK_CLOSE(abs(V0ab/V1ab), 0.95, 1e-6);
    BOOST_CHECK_CLOSE(abs(V0bc/V1bc), 1.05, 1e-6);
}

BOOST_AUTO_TEST_CASE (test_dgy_transformer)
{
    Network netw;
    Parser<Network> p;
    p.parse("test_dgy_transformer.yaml", netw);
    netw.solvePowerFlow();

    auto V0 = netw.buses()[0]->V();
    auto V1 = netw.buses()[1]->V();
    cout << abs(V0).eval() << " " << (arg(V0) * 180 / pi).eval() << endl;
    cout << abs(V1).eval() << " " << (arg(V1) * 180 / pi).eval() << endl;
    BOOST_CHECK_CLOSE(abs(V1[0]), 0.240 / 0.95, 1e-2);
    BOOST_CHECK_CLOSE(abs(V1[1]), 0.240 / 0.95, 1e-2);
    BOOST_CHECK_CLOSE(abs(V1[2]), 0.240 / 0.95, 1e-2);
    BOOST_CHECK_CLOSE(arg(V1[0]) * 180 / pi, 30, 1e-2);
    BOOST_CHECK_CLOSE(arg(V1[1]) * 180 / pi, -90, 1e-2);
    BOOST_CHECK_CLOSE(arg(V1[2]) * 180 / pi, 150, 1e-2);
}

BOOST_AUTO_TEST_CASE (test_single_phase_dgy_transformer)
{
    Network netw;
    Parser<Network> p;
    p.parse("test_single_phase_dgy_transformer.yaml", netw);
    netw.solvePowerFlow();

    auto V0 = netw.buses()[0]->V();
    auto V1 = netw.buses()[1]->V();
    cout << abs(V0).eval() << " " << (arg(V0) * 180 / pi).eval() << endl;
    cout << abs(V1).eval() << " " << (arg(V1) * 180 / pi).eval() << endl;
    BOOST_CHECK_CLOSE(abs(V1[0]), 0.240 / 1.05, 1e-2);
    BOOST_CHECK_CLOSE(arg(V1[0]) * 180 / pi, 30, 1e-2);
}

BOOST_AUTO_TEST_CASE (test_load_model_a)
{
    Network netw;
    Parser<Network> p;
    p.parse("test_load_model_a.yaml", netw);
    netw.solvePowerFlow();

    Complex y{0.1, -0.1};
    Complex s{0.1, 0.05};

    const auto& bus1 = *netw.buses()["bus_1"];
    const auto& bus2 = *netw.buses()["bus_2"];

    const Col<Complex>& V1 = bus1.V();
    const Col<Complex>& V2 = bus2.V();

    BOOST_CHECK_SMALL(norm(V1 - bus1.VNom()), 1e-9);

    {
        Complex IA = y * (V1(0) - V2(0));
        Complex IAB = conj(s/(V2(0) - V2(1)));
        Complex ICA = conj(s/(V2(2) - V2(0)));
        BOOST_CHECK_SMALL(abs(IA - IAB + ICA), 1e-9);
    }

    {
        Complex IB = y * (V1(1) - V2(1));
        Complex IBC = conj(s/(V2(1) - V2(2)));
        Complex IAB = conj(s/(V2(0) - V2(1)));
        BOOST_CHECK_SMALL(abs(IB - IBC + IAB), 1e-9);
    }

    {
        Complex IC = y * (V1(2) - V2(2));
        Complex ICA = conj(s/(V2(2) - V2(0)));
        Complex IBC = conj(s/(V2(1) - V2(2)));
        BOOST_CHECK_SMALL(abs(IC - ICA + IBC), 1e-9);
    }
}

BOOST_AUTO_TEST_CASE (test_load_model_b)
{
    Network netw;
    Parser<Network> p;
    p.parse("test_load_model_b.yaml", netw);
    netw.solvePowerFlow();

    Complex y{0.1, -0.1};
    Complex s{0.1, 0.05};

    const auto& bus1 = *netw.buses()["bus_1"];
    const auto& bus2 = *netw.buses()["bus_2"];

    const Col<Complex>& V1 = bus1.V();
    const Col<Complex>& V2 = bus2.V();

    BOOST_CHECK_SMALL(norm(V1 - bus1.VNom()), 1e-9);

    {
        Complex IA = y * (V1(0) - V2(0));
        Complex IAB = conj(s/(V2(0) - V2(1)));
        BOOST_CHECK_SMALL(abs(IA - IAB), 1e-9);
    }

    {
        Complex IB = y * (V1(1) - V2(1));
        Complex IAB = conj(s/(V2(0) - V2(1)));
        BOOST_CHECK_SMALL(abs(IB + IAB), 1e-9);
    }

    {
        Complex IC = y * (V1(2) - V2(2));
        BOOST_CHECK_SMALL(abs(IC), 1e-9);
    }
}

BOOST_AUTO_TEST_CASE (test_load_model_c)
{
    Network netw;
    Parser<Network> p;
    p.parse("test_load_model_c.yaml", netw);
    netw.solvePowerFlow();

    Complex y{0.1, -0.1};
    Complex i{0.1, 0.05};

    const auto& bus1 = *netw.buses()["bus_1"];
    const auto& bus2 = *netw.buses()["bus_2"];

    const Col<Complex>& V1 = bus1.V();
    const Col<Complex>& V2 = bus2.V();

    BOOST_CHECK_SMALL(norm(V1 - bus1.VNom()), 1e-8);

    {
        Complex IA = y * (V1(0) - V2(0));
        Complex IAB = i * (V2(0) - V2(1)) / abs(V2(0) - V2(1));
        BOOST_CHECK_SMALL(abs(IA - IAB), 1e-8);
    }

    {
        Complex IB = y * (V1(1) - V2(1));
        Complex IAB = i * (V2(0) - V2(1)) / abs(V2(0) - V2(1));
        BOOST_CHECK_SMALL(abs(IB + IAB), 1e-8);
    }

    {
        Complex IC = y * (V1(2) - V2(2));
        BOOST_CHECK_SMALL(abs(IC), 1e-8);
    }
}

BOOST_AUTO_TEST_CASE (test_include)
{
    Network netw;
    Parser<Network> p;
    p.parse("test_include_A.yaml", netw);
    cout << netw << endl;
    BOOST_CHECK(netw.buses().size() == 2);
    BOOST_CHECK(netw.branches().size() == 1);
    BOOST_CHECK(netw.gens().size() == 1);
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef UTILS_HPP
#define UTILS_HPP
/**
 * @file utils.hpp
 * @author Rafael Rey (rreyarc@upo.es)
 * @brief A set of utils used alongside the project
 * @version 0.1
 * @date 2021-06-29
 *
 * @copyright Copyright (c) 2021
 *
 */
#include <iostream>
#include <vector>
#include <set>
#include <map>
#include <variant>
#include <math.h>
#include <memory>
#include <Eigen/Dense>

#include <boost/multi_index_container.hpp>
#include <boost/multi_index/ordered_index.hpp>
#include <boost/multi_index/identity.hpp>
#include <boost/multi_index/member.hpp>
#include <boost/multi_index/hashed_index.hpp>
#include <boost/multi_index/key_extractors.hpp>

using namespace ::boost;
using namespace ::boost::multi_index;

namespace Planners
{
    namespace utils
    {
        class Vec3i;
        class Node;
        struct NodeComparator;

        using CoordinateList     = std::vector<Planners::utils::Vec3i>;
        using CoordinateListPtr  = std::shared_ptr<std::vector<Planners::utils::Vec3i>>;
        using DataVariant        = std::variant<std::string, Vec3i, CoordinateList, double, size_t, int, bool, unsigned int>;
        using PathData           = std::map<std::string, DataVariant>;

        //Compile time constants
        static constexpr int const dist_scale_factor_{100};
        //To use with costs
        static constexpr int const dist_scale_factor_reduced_{ dist_scale_factor_ / 100 };
        //Dont touch these ones (diagonal distances in 2D and 3D)
        //The static cast from floating point to integer returns the truncated value
        //i.e discards the decimal part
        static constexpr int const dd_2D_{static_cast<int>( dist_scale_factor_ * 1.41421356237 )}; //sqrt(2)
        static constexpr int const dd_3D_{static_cast<int>( dist_scale_factor_ * 1.73205080757 )}; //sqrt(3)

        /**
         * @brief
         *
         */
        struct gridCell
	    {
	    	float dist{0};
	    	float prob{0};
	    };
        /**
         * @brief
         *
         */
        struct gridData
        {
            unsigned int grid_size{0};
            unsigned int grid_size_x{0};
            unsigned int grid_size_y{0};
            unsigned int grid_size_z{0};
            unsigned int sensor_dev{0};
        };
        /**
         * @brief Overload ofstream operator << for std::vector<<CoordinateList>
         *
         * @tparam T
         * @param os
         * @param v
         * @return std::ostream&
         */
        template <class T>
        inline std::ostream& operator << (std::ostream& os, const std::vector<T>& v)
        {

            for (typename std::vector<T>::const_iterator ii = v.begin(); ii != v.end()-1; ++ii)
                os << *ii << ", ";

            os << *(v.end()-1);

            return os;
        }

        /**
         * @brief This is one of the main classes used internally by the algorithms.
         * It only stores a set of 3D discrete coordinates values
         * and has some operator overloaded implementations to easily work with objects
         */
        class Vec3i
        {
            public:
            int x, y, z;
            /**
             * @brief
             *
             * @param coordinates_
             * @return true
             * @return false
             */
            bool operator==(const Vec3i &coordinates_);
            /**
             * @brief
             *
             * @param coordinates_
             * @return true if all of the coordinates are greather or equal than the others
             * @return false if any of the coordinates are not greather or equal than the others
             */
            bool operator>=(const Vec3i &coordinates_);
            /**
             * @brief
             *
             * @param coordinates_
             * @return true if all the coordinates are <= of the corresponding ones
             * @return false if any of the coordinates are not <=
             */
            bool operator<=(const Vec3i &coordinates_);

            /**
             * @brief
             *
             * @param _divid
             * @return Planners::utils::Vec3i&
             */
            Planners::utils::Vec3i &operator/=(float _divid);

            /**
             * @brief
             *
             * @param right_
             * @return Vec3i
             */
            Vec3i operator+(const Vec3i &right_) const
            {
                return {this->x + right_.x, this->y + right_.y, this->z + right_.z};
            }

            /**
             * @brief
             *
             * @param right_
             * @return Vec3i
             */
            Vec3i operator-(const Vec3i &right_) const
            {
                return {this->x - right_.x, this->y - right_.y, this->z - right_.z};
            }

            /**
             * @brief
             *
             * @param _mult
             * @return Vec3i
             */
            inline Vec3i operator*(int &_mult) const
            {
                return {this->x * _mult, this->y * _mult, this->z * _mult };
            }

            Eigen::Vector3d toEigen() const{
                return Eigen::Vector3d(x, y, y);
            }

        };
        /**
         * @brief
         *
         * @param _mult
         * @param _vec
         * @return Planners::utils::Vec3i
         */
        inline Planners::utils::Vec3i operator*(const int &_mult, const Planners::utils::Vec3i &_vec){

            return { _vec.x * _mult, _vec.y * _mult, _vec.z * _mult};
        }
        /**
         * @brief
         *
         * @param os
         * @param dt
         * @return std::ostream&
         */
        inline std::ostream& operator<<(std::ostream& os, const Vec3i& dt)
        {
            os << "[" << dt.x << ", " << dt.y << ", " << dt.z << "]";
            return os;
        }
        /**
         * @brief This object store the main information used by the algorithms
         *
         */
        class Node
        {
            public:
            Node *parent{nullptr};

            Planners::utils::Vec3i coordinates;

            unsigned int G{0}, H{0}, C{0};

            unsigned int gplush{0};
            unsigned int world_index{0};

            double cost{0};

            bool occuppied{false};
            bool isInOpenList{false};
            bool isInClosedList{false};

            Node(Planners::utils::Vec3i coord_, Node *parent_ = nullptr);
            Node();
            unsigned int getScore();
            unsigned int getScoreWithSafetyCost();

        };

        struct IndexByCost {};
        struct IndexByWorldPosition {};

        using MagicalMultiSet = boost::multi_index_container<
          Node*, // the data type stored
          boost::multi_index::indexed_by< // list of indexes
            boost::multi_index::hashed_unique<  //hashed index over 'l'
              boost::multi_index::tag<IndexByWorldPosition>, // give that index a name
              boost::multi_index::member<Node, unsigned int, &Node::world_index> // what will be the index's key
            >,
            boost::multi_index::ordered_non_unique<  //ordered index over 'i1'
              boost::multi_index::tag<IndexByCost>, // give that index a name
              boost::multi_index::member<Node, unsigned int, &Node::gplush> // what will be the index's key
            >
          >
        >;

        typedef MagicalMultiSet::index<IndexByWorldPosition>::type node_by_position;
        typedef MagicalMultiSet::index<IndexByCost>::type          node_by_cost;

    }
}

#endif


#include "modules/neuralNetwork/layer/pooling/pooling.hpp"
#include "modules/neuralNetwork/neuralNetwork.hpp"

#ifdef _KORALI_USE_CUDNN
  #include "auxiliar/cudaUtils.hpp"
#endif

#ifdef _KORALI_USE_ONEDNN
  #include "auxiliar/dnnUtils.hpp"
using namespace dnnl;
#endif

#include <Eigen/Dense>
using namespace Eigen;

namespace korali
{
namespace neuralNetwork
{
namespace layer
{
;

void Pooling::initialize()
{
  // Checking Layer size
  if (_outputChannels == 0) KORALI_LOG_ERROR("Node count for layer (%lu) should be larger than zero.\n", _index);

  // Checking position
  if (_index == 0) KORALI_LOG_ERROR("Pooling layers cannot be the starting layer of the NN\n");
  if (_index == _nn->_layers.size() - 1) KORALI_LOG_ERROR("Pooling layers cannot be the last layer of the NN\n");

  // Precalculating values for the pooling operation
  N = _batchSize;
  IH = _imageHeight;
  IW = _imageWidth;
  KH = _kernelHeight;
  KW = _kernelWidth;

  SV = _verticalStride;
  SH = _horizontalStride;
  PT = _paddingTop;
  PL = _paddingLeft;
  PB = _paddingBottom;
  PR = _paddingRight;

  // Check for non zeros
  if (IH <= 0) KORALI_LOG_ERROR("Image height must be larger than zero for pooling layer.\n");
  if (IW <= 0) KORALI_LOG_ERROR("Image width must be larger than zero for pooling layer.\n");
  if (KH <= 0) KORALI_LOG_ERROR("Kernel height must be larger than zero for pooling layer.\n");
  if (KW <= 0) KORALI_LOG_ERROR("Kernel width must be larger than zero for pooling layer.\n");
  if (SV <= 0) KORALI_LOG_ERROR("Vertical stride must be larger than zero for pooling layer.\n");
  if (SH <= 0) KORALI_LOG_ERROR("Horizontal stride must be larger than zero for pooling layer.\n");

  // Several sanity checks
  if (KH > IH) KORALI_LOG_ERROR("Kernel height cannot be larger than input image height.\n");
  if (KW > IW) KORALI_LOG_ERROR("Kernel height cannot be larger than input image height.\n");
  if (PR + PL > IW) KORALI_LOG_ERROR("L+R Paddings cannot exceed the width of the input image.\n");
  if (PT + PB > IH) KORALI_LOG_ERROR("T+B Paddings cannot exceed the height of the input image.\n");

  // Check whether the output channels of the previous layer is divided by the height and width
  if (_prevLayer->_outputChannels % (IH * IW) > 0) KORALI_LOG_ERROR("Previous layer contains a number of channels (%lu) not divisible by the pooling 2D HxW setup (%lux%lu).\n", _prevLayer->_outputChannels, IH, IW);
  IC = _prevLayer->_outputChannels / (IH * IW);

  // Deriving output height and width
  OH = std::floor((IH - (KH - (PR + PL))) / SH) + 1;
  OW = std::floor((IW - (KW - (PT + PB))) / SV) + 1;

  // Check whether the output channels of the previous layer is divided by the height and width
  if (_outputChannels % (OH * OW) > 0) KORALI_LOG_ERROR("Pooling layer contains a number of output channels (%lu) not divisible by the output image size (%lux%lu) given kernel (%lux%lu) size and padding/stride configuration.\n", _outputChannels, OH, OW, KH, KW);
  OC = _outputChannels / (OH * OW);
}

void Pooling::createForwardPipeline()
{
  // Calling base layer function
  Layer::createForwardPipeline();

  if (_nn->_engine == "Korali") KORALI_LOG_ERROR("Pooling Layers still not supported in Korali's NN backend. Use OneDNN.\n");
  if (_nn->_engine == "CuDNN") KORALI_LOG_ERROR("Pooling Layers still not supported in CuDNNbackend. Use OneDNN.\n");

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // Creating memory descriptor mappings for input memory
    _srcMemDesc = memory::desc({N, IC, IH, IW}, memory::data_type::f32, memory::format_tag::nchw);
    _dstMemDesc = memory::desc({N, OC, OH, OW}, memory::data_type::f32, memory::format_tag::nchw);

    // Creating padding dims
    memory::dims ST = {SV, SH};  // Horizontal Vertical
    memory::dims PTL = {PT, PL}; // Top Left
    memory::dims PBR = {PB, PR}; // Bottom Right

    // Creating work memory
    memory::dims kernelDims = {KH, KW};

    // Determining algorithm
    dnnl::algorithm algorithmType;
    if (_function == "Max") algorithmType = dnnl::algorithm::pooling_max;
    if (_function == "Inclusive Average") algorithmType = dnnl::algorithm::pooling_avg_include_padding;
    if (_function == "Exclusive Average") algorithmType = dnnl::algorithm::pooling_avg_exclude_padding;

    // We create the pooling operation
    auto pooling_d = pooling_forward::desc(_propKind, algorithmType, _srcMemDesc, _dstMemDesc, ST, kernelDims, PTL, PBR);

    // Create inner product primitive descriptor.
    dnnl::primitive_attr poolingPrimitiveAttributes;
    _forwardPoolingPrimitiveDesc = pooling_forward::primitive_desc(pooling_d, poolingPrimitiveAttributes, _nn->_dnnlEngine);

    // Create pooling workspace memory
    _workspaceMem.resize(_nn->_timestepCount);
    for (size_t t = 0; t < _nn->_timestepCount; t++)
      _workspaceMem[t] = memory(_forwardPoolingPrimitiveDesc.workspace_desc(), _nn->_dnnlEngine);

    // Create the weights+bias primitive.
    _forwardPoolingPrimitive = pooling_forward(_forwardPoolingPrimitiveDesc);
  }
#endif
}

void Pooling::createBackwardPipeline()
{
  //  Initializing memory objects and primitives for BACKWARD propagation

  // Calling base layer function
  Layer::createBackwardPipeline();

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // Creating memory descriptor mappings for input memory
    _srcMemDesc = memory::desc({N, IC, IH, IW}, memory::data_type::f32, memory::format_tag::nchw);
    _dstMemDesc = memory::desc({N, OC, OH, OW}, memory::data_type::f32, memory::format_tag::nchw);

    // Creating padding dims
    memory::dims ST = {SV, SH};  // Horizontal Vertical
    memory::dims PTL = {PT, PL}; // Top Left
    memory::dims PBR = {PB, PR}; // Bottom Right

    // Creating work memory
    memory::dims kernelDims = {KH, KW};

    // Determining algorithm
    dnnl::algorithm algorithmType;
    if (_function == "Max") algorithmType = dnnl::algorithm::pooling_max;
    if (_function == "Inclusive Average") algorithmType = dnnl::algorithm::pooling_avg_include_padding;
    if (_function == "Exclusive Average") algorithmType = dnnl::algorithm::pooling_avg_exclude_padding;

    auto backwardDataDesc = pooling_backward::desc(
      algorithmType,
      _srcMemDesc,
      _dstMemDesc,
      ST,
      kernelDims,
      PTL,
      PBR);

    // Create the primitive.
    auto backwardDataPrimitiveDesc = pooling_backward::primitive_desc(backwardDataDesc, _nn->_dnnlEngine, _forwardPoolingPrimitiveDesc);
    _backwardDataPrimitive = pooling_backward(backwardDataPrimitiveDesc);
  }
#endif
}

void Pooling::forwardData(const size_t t)
{
#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // Arguments to the inner product operation
    std::unordered_map<int, dnnl::memory> forwardPoolingArgs;
    forwardPoolingArgs[DNNL_ARG_SRC] = _prevLayer->_outputMem[t];
    forwardPoolingArgs[DNNL_ARG_DST] = _outputMem[t];
    forwardPoolingArgs[DNNL_ARG_WORKSPACE] = _workspaceMem[t];
    _forwardPoolingPrimitive.execute(_nn->_dnnlStream, forwardPoolingArgs);
  }
#endif
}

void Pooling::backwardData(const size_t t)
{
  if (_nn->_mode == "Inference")
    KORALI_LOG_ERROR("Requesting Layer backward data propagation but NN was configured for inference only.\n");

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    _backwardDataArgs[DNNL_ARG_DIFF_DST] = _outputGradientMem[t];             // Input
    _backwardDataArgs[DNNL_ARG_DIFF_SRC] = _prevLayer->_outputGradientMem[t]; // Output
    _backwardDataArgs[DNNL_ARG_WORKSPACE] = _workspaceMem[t];
    _backwardDataPrimitive.execute(_nn->_dnnlStream, _backwardDataArgs);
  }
#endif
}

void Pooling::setConfiguration(knlohmann::json& js)
{
 if (isDefined(js, "Results"))  eraseValue(js, "Results");

 if (isDefined(js, "Function"))
 {
 try { _function = js["Function"].get<std::string>();
} catch (const std::exception& e)
 { KORALI_LOG_ERROR(" + Object: [ pooling ] \n + Key:    ['Function']\n%s", e.what()); }
{
 bool validOption = false;
 if (_function == "Max") validOption = true;
 if (_function == "Inclusive Average") validOption = true;
 if (_function == "Exclusive Average") validOption = true;
 if (validOption == false) KORALI_LOG_ERROR(" + Unrecognized value (%s) provided for mandatory setting: ['Function'] required by pooling.\n", _function.c_str());
}
   eraseValue(js, "Function");
 }
  else   KORALI_LOG_ERROR(" + No value provided for mandatory setting: ['Function'] required by pooling.\n");

 if (isDefined(js, "Image Height"))
 {
 try { _imageHeight = js["Image Height"].get<ssize_t>();
} catch (const std::exception& e)
 { KORALI_LOG_ERROR(" + Object: [ pooling ] \n + Key:    ['Image Height']\n%s", e.what()); }
   eraseValue(js, "Image Height");
 }
  else   KORALI_LOG_ERROR(" + No value provided for mandatory setting: ['Image Height'] required by pooling.\n");

 if (isDefined(js, "Image Width"))
 {
 try { _imageWidth = js["Image Width"].get<ssize_t>();
} catch (const std::exception& e)
 { KORALI_LOG_ERROR(" + Object: [ pooling ] \n + Key:    ['Image Width']\n%s", e.what()); }
   eraseValue(js, "Image Width");
 }
  else   KORALI_LOG_ERROR(" + No value provided for mandatory setting: ['Image Width'] required by pooling.\n");

 if (isDefined(js, "Kernel Height"))
 {
 try { _kernelHeight = js["Kernel Height"].get<ssize_t>();
} catch (const std::exception& e)
 { KORALI_LOG_ERROR(" + Object: [ pooling ] \n + Key:    ['Kernel Height']\n%s", e.what()); }
   eraseValue(js, "Kernel Height");
 }
  else   KORALI_LOG_ERROR(" + No value provided for mandatory setting: ['Kernel Height'] required by pooling.\n");

 if (isDefined(js, "Kernel Width"))
 {
 try { _kernelWidth = js["Kernel Width"].get<ssize_t>();
} catch (const std::exception& e)
 { KORALI_LOG_ERROR(" + Object: [ pooling ] \n + Key:    ['Kernel Width']\n%s", e.what()); }
   eraseValue(js, "Kernel Width");
 }
  else   KORALI_LOG_ERROR(" + No value provided for mandatory setting: ['Kernel Width'] required by pooling.\n");

 if (isDefined(js, "Vertical Stride"))
 {
 try { _verticalStride = js["Vertical Stride"].get<ssize_t>();
} catch (const std::exception& e)
 { KORALI_LOG_ERROR(" + Object: [ pooling ] \n + Key:    ['Vertical Stride']\n%s", e.what()); }
   eraseValue(js, "Vertical Stride");
 }
  else   KORALI_LOG_ERROR(" + No value provided for mandatory setting: ['Vertical Stride'] required by pooling.\n");

 if (isDefined(js, "Horizontal Stride"))
 {
 try { _horizontalStride = js["Horizontal Stride"].get<ssize_t>();
} catch (const std::exception& e)
 { KORALI_LOG_ERROR(" + Object: [ pooling ] \n + Key:    ['Horizontal Stride']\n%s", e.what()); }
   eraseValue(js, "Horizontal Stride");
 }
  else   KORALI_LOG_ERROR(" + No value provided for mandatory setting: ['Horizontal Stride'] required by pooling.\n");

 if (isDefined(js, "Padding Left"))
 {
 try { _paddingLeft = js["Padding Left"].get<ssize_t>();
} catch (const std::exception& e)
 { KORALI_LOG_ERROR(" + Object: [ pooling ] \n + Key:    ['Padding Left']\n%s", e.what()); }
   eraseValue(js, "Padding Left");
 }
  else   KORALI_LOG_ERROR(" + No value provided for mandatory setting: ['Padding Left'] required by pooling.\n");

 if (isDefined(js, "Padding Right"))
 {
 try { _paddingRight = js["Padding Right"].get<ssize_t>();
} catch (const std::exception& e)
 { KORALI_LOG_ERROR(" + Object: [ pooling ] \n + Key:    ['Padding Right']\n%s", e.what()); }
   eraseValue(js, "Padding Right");
 }
  else   KORALI_LOG_ERROR(" + No value provided for mandatory setting: ['Padding Right'] required by pooling.\n");

 if (isDefined(js, "Padding Top"))
 {
 try { _paddingTop = js["Padding Top"].get<ssize_t>();
} catch (const std::exception& e)
 { KORALI_LOG_ERROR(" + Object: [ pooling ] \n + Key:    ['Padding Top']\n%s", e.what()); }
   eraseValue(js, "Padding Top");
 }
  else   KORALI_LOG_ERROR(" + No value provided for mandatory setting: ['Padding Top'] required by pooling.\n");

 if (isDefined(js, "Padding Bottom"))
 {
 try { _paddingBottom = js["Padding Bottom"].get<ssize_t>();
} catch (const std::exception& e)
 { KORALI_LOG_ERROR(" + Object: [ pooling ] \n + Key:    ['Padding Bottom']\n%s", e.what()); }
   eraseValue(js, "Padding Bottom");
 }
  else   KORALI_LOG_ERROR(" + No value provided for mandatory setting: ['Padding Bottom'] required by pooling.\n");

 Layer::setConfiguration(js);
 _type = "layer/pooling";
 if(isDefined(js, "Type")) eraseValue(js, "Type");
 if(isEmpty(js) == false) KORALI_LOG_ERROR(" + Unrecognized settings for Korali module: pooling: \n%s\n", js.dump(2).c_str());
}

void Pooling::getConfiguration(knlohmann::json& js)
{

 js["Type"] = _type;
   js["Function"] = _function;
   js["Image Height"] = _imageHeight;
   js["Image Width"] = _imageWidth;
   js["Kernel Height"] = _kernelHeight;
   js["Kernel Width"] = _kernelWidth;
   js["Vertical Stride"] = _verticalStride;
   js["Horizontal Stride"] = _horizontalStride;
   js["Padding Left"] = _paddingLeft;
   js["Padding Right"] = _paddingRight;
   js["Padding Top"] = _paddingTop;
   js["Padding Bottom"] = _paddingBottom;
 Layer::getConfiguration(js);
}

void Pooling::applyModuleDefaults(knlohmann::json& js)
{

 std::string defaultString = "{}";
 knlohmann::json defaultJs = knlohmann::json::parse(defaultString);
 mergeJson(js, defaultJs);
 Layer::applyModuleDefaults(js);
}

void Pooling::applyVariableDefaults()
{

 Layer::applyVariableDefaults();
}

;

} //layer
} //neuralNetwork
} //korali
;


#include <boost/math/special_functions/ulp.hpp>


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "Framework/TestingFramework.hpp"

#include <array>
#include <boost/optional.hpp>
#include <cmath>
#include <memory>
#include <pup.h>

#include "DataStructures/DataVector.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "Domain/CoordinateMaps/BulgedCube.hpp"
#include "Framework/TestHelpers.hpp"
#include "Helpers/Domain/CoordinateMaps/TestMapHelpers.hpp"
#include "NumericalAlgorithms/Spectral/Spectral.hpp"
#include "Utilities/TypeTraits.hpp"

namespace domain {
namespace {
void test_bulged_cube_fail() {
  INFO("Bulged cube fail");
  const CoordinateMaps::BulgedCube map(2.0 * sqrt(3.0), 0.5, false);

  // The corners of the mapped cube have coordinates +/- 2 in each
  // dimension.  If we inverse-map points that are sufficiently
  // outside of the mapped cube, then the inverse map will fail.
  const std::array<double, 3> test_mapped_point1{{3.0, 3.0, 3.0}};
  const std::array<double, 3> test_mapped_point2{{2.0, 2.0, 2.01}};
  const std::array<double, 3> test_mapped_point3{{4.0, 0.0, 0.0}};

  // These points are outside the mapped BulgedCube. So inverse should either
  // return the correct inverse (which happens to be computable for
  // these points) or it should return boost::none.
  const std::array<double, 3> test_mapped_point4{{2.0, 2.0, 2.0}};
  const std::array<double, 3> test_mapped_point5{{3.0, 0.0, 0.0}};

  CHECK_FALSE(static_cast<bool>(map.inverse(test_mapped_point1)));
  CHECK_FALSE(static_cast<bool>(map.inverse(test_mapped_point2)));
  CHECK_FALSE(static_cast<bool>(map.inverse(test_mapped_point3)));
  if (map.inverse(test_mapped_point4)) {
    CHECK_ITERABLE_APPROX(map(map.inverse(test_mapped_point4).get()),
                          test_mapped_point4);
  }
  if (map.inverse(test_mapped_point5)) {
    CHECK_ITERABLE_APPROX(map(map.inverse(test_mapped_point5).get()),
                          test_mapped_point5);
  }
}

void test_bulged_cube(bool with_equiangular_map) {
  INFO("Bulged cube");
  const std::array<double, 3> lower_corner{{-1.0, -1.0, -1.0}};
  const std::array<double, 3> upper_corner{{1.0, 1.0, 1.0}};
  const CoordinateMaps::BulgedCube map(2.0 * sqrt(3.0), 0.5,
                                       with_equiangular_map);

  CHECK(map(lower_corner)[0] == approx(-2.0));
  CHECK(map(lower_corner)[1] == approx(-2.0));
  CHECK(map(lower_corner)[2] == approx(-2.0));
  CHECK(map(upper_corner)[0] == approx(2.0));
  CHECK(map(upper_corner)[1] == approx(2.0));
  CHECK(map(upper_corner)[2] == approx(2.0));

  const std::array<double, 3> test_point1{{-1.0, 0.25, 0.0}};
  const std::array<double, 3> test_point2{{1.0, 1.0, -0.5}};
  const std::array<double, 3> test_point3{{0.7, -0.2, 0.4}};
  const std::array<double, 3> test_point4{{0.0, 0.0, 0.0}};
  const std::array<DataVector, 3> test_points{
      {DataVector{-1.0, 1.0, 0.7, 0.0}, DataVector{0.25, 1.0, -0.2, 0.0},
       DataVector{0.0, -0.5, 0.4, 0.0}}};
  const DataVector& collocation_pts =
      Spectral::collocation_points<Spectral::Basis::Legendre,
                                   Spectral::Quadrature::GaussLobatto>(7);
  const std::array<DataVector, 3> test_points2{
      {collocation_pts, collocation_pts, collocation_pts}};

  test_jacobian(map, test_point1);
  test_jacobian(map, test_point2);
  test_jacobian(map, test_point3);
  test_jacobian(map, test_point4);

  test_inv_jacobian(map, test_point1);
  test_inv_jacobian(map, test_point2);
  test_inv_jacobian(map, test_point3);
  test_inv_jacobian(map, test_point4);

  test_coordinate_map_implementation<CoordinateMaps::BulgedCube>(map);

  CHECK(serialize_and_deserialize(map) == map);
  CHECK_FALSE(serialize_and_deserialize(map) != map);

  test_coordinate_map_argument_types(map, test_point1);

  test_inverse_map(map, test_point1);
  test_inverse_map(map, test_point2);
  test_inverse_map(map, test_point3);
  test_inverse_map(map, test_point4);
}
}  // namespace

SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.BulgedCube", "[Domain][Unit]") {
  const CoordinateMaps::BulgedCube map(sqrt(3.0), 0, false);
  const std::array<double, 3> lower_corner{{-1.0, -1.0, -1.0}};
  const std::array<double, 3> upper_corner{{1.0, 1.0, 1.0}};
  const std::array<double, 3> test_point1{{-1.0, 0.25, 0.0}};
  const std::array<double, 3> test_point2{{1.0, 1.0, -0.5}};
  const std::array<double, 3> test_point3{{0.7, -0.2, 0.4}};

  CHECK_ITERABLE_APPROX(map(lower_corner), lower_corner);
  CHECK_ITERABLE_APPROX(map(upper_corner), upper_corner);
  CHECK_ITERABLE_APPROX(map(test_point1), test_point1);
  CHECK_ITERABLE_APPROX(map(test_point2), test_point2);
  CHECK_ITERABLE_APPROX(map(test_point3), test_point3);

  test_jacobian(map, test_point1);
  test_jacobian(map, test_point2);
  test_jacobian(map, test_point3);

  test_inv_jacobian(map, test_point1);
  test_inv_jacobian(map, test_point2);
  test_inv_jacobian(map, test_point3);

  test_coordinate_map_implementation<CoordinateMaps::BulgedCube>(map);

  CHECK(serialize_and_deserialize(map) == map);
  CHECK_FALSE(serialize_and_deserialize(map) != map);

  test_coordinate_map_argument_types(map, test_point1);
  test_bulged_cube(true);   // Equiangular
  test_bulged_cube(false);  // Equidistant
  test_bulged_cube_fail();

  check_if_map_is_identity(CoordinateMaps::BulgedCube{sqrt(3.0), 0, false});
}
}  // namespace domain


/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\
|  Phycas: Python software for phylogenetic analysis                          |
|  Copyright (C) 2006 Mark T. Holder, Paul O. Lewis and David L. Swofford     |
|                                                                             |
|  This program is free software; you can redistribute it and/or modify       |
|  it under the terms of the GNU General Public License as published by       |
|  the Free Software Foundation; either version 2 of the License, or          |
|  (at your option) any later version.                                        |
|                                                                             |
|  This program is distributed in the hope that it will be useful,            |
|  but WITHOUT ANY WARRANTY; without even the implied warranty of             |
|  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              |
|  GNU General Public License for more details.                               |
|                                                                             |
|  You should have received a copy of the GNU General Public License along    |
|  with this program; if not, write to the Free Software Foundation, Inc.,    |
|  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.                |
\~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

#if ! defined(TREE_SCALER_MOVE_HPP)
#define TREE_SCALER_MOVE_HPP

#include <vector>									// for std::vector
#include <boost/shared_ptr.hpp>						// for boost::shared_ptr
#include <boost/weak_ptr.hpp>						// for boost::weak_ptr
//#include <boost/enable_shared_from_this.hpp>		// for boost::enable_shared_from_this
#include "mcmc_updater.hpp"		// for base class MCMCUpdater

namespace phycas
{

class MCMCChainManager;
typedef boost::weak_ptr<MCMCChainManager>			ChainManagerWkPtr;

/*----------------------------------------------------------------------------------------------------------------------
|	Encapsulates a whole-tree-scaling move. Each update scales all edge lengths by a common factor. Useful for moving
|   quickly to an appropriate scale for the tree. Depending on the LargetSimonMove alone sometimes requires a long
|   burn-in period due to the fact that each LargetSimonMove update only affects three edges.
|>
*/
class TreeScalerMove : public MCMCUpdater
	{
	public:
						TreeScalerMove();
						virtual ~TreeScalerMove()
							{
							//std::cerr << "TreeScalerMove dying..." << std::endl;
							}

        void            proposeNewState();

		// These are virtual functions in the MCMCUpdater base class
        virtual void    setTuningParameter(double x);
		virtual bool	update();
		virtual void	revert();
		virtual void	accept();
		virtual double	getLnHastingsRatio() const;
        bool            computesTreeLengthPrior() const;
		virtual bool    computesTopologyPrior() const {return true;}

        // recomputes working prior for purpose of this move only (not used to recompute joint working prior)
		double			recalcWorkingPriorForMove(bool using_tree_length_prior, bool using_vartopol_prior) const;

    private:

        double          lambda;         /**< the tuning parameter used for this move */
        unsigned        n;              /**< the number of edges in the tree */
        double          m;              /**< the preproposal tree length */
        double          mstar;          /**< the proposed new tree length */
        double          forward_scaler; /**< the factor by which the original edge lengths should be multiplied to create proposed new edge lengths */
        double          reverse_scaler; /**< the factor by which the new edge lengths should be multiplied to return them to their original lengths*/
	};

} // namespace phycas

#endif


//
// Created by jachu on 05.02.18.
//

#ifndef PLANELOC_EKFPLANE_HPP
#define PLANELOC_EKFPLANE_HPP

#include <boost/serialization/access.hpp>

#include <Eigen/Eigen>

class EKFPlane {
public:
    EKFPlane();

    EKFPlane(const Eigen::Quaterniond &xq,
             const Eigen::Matrix4d &Pq);

    EKFPlane(const Eigen::Quaterniond &xq, int npts);

    void init(const Eigen::Quaterniond &xq,
              const Eigen::Matrix4d &Pq);

    void init(const Eigen::Quaterniond &xq, int npts);

    void update(const Eigen::Quaterniond &zq,
                const Eigen::Matrix4d &Rq);

    void update(const Eigen::Quaterniond &zq,
                const Eigen::Matrix3d &R);

    void update(const Eigen::Quaterniond &zq,
                int znpts);

//    void transform(const Eigen::Matrix4d T);

    double distance(const Eigen::Quaterniond &xcq) const;

    static void compPlaneEqAndCovar(const Eigen::MatrixXd &pts,
                                    Eigen::Quaterniond &q,
                                    Eigen::Matrix4d &R);

    const Eigen::Quaterniond &getX() const;

    const Eigen::Matrix3d &getP() const;

private:

    static Eigen::MatrixXd jacob_dom_dq(const Eigen::Quaterniond &q);

    static Eigen::MatrixXd jacob_dqn_dq(const Eigen::Quaterniond &q);

    Eigen::Quaterniond x;

    Eigen::Matrix3d P;

    int npts;

    friend class boost::serialization::access;

    template<class Archive>
    void serialize(Archive & ar, const unsigned int version)
    {
        ar & x;
        ar & P;
        ar & npts;
    }
};

#endif //PLANELOC_EKFPLANE_HPP