/*
// Copyright 2018 University of Liege
//
// 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.
//
// Authors:
// - Adrien Crovato
*/

//// Interpolation from center to vertices
// Interpolate linearly surface singularities from adjacent panel centers to panel vertices. If panel is on an edge of a
// network, linear extrapolation is used instead
//
// Inputs:
// - idG: current panel global index
// - idC: current panel chordwise index
// - idS: current panel spanwise index
// - bPan: body panels (structure)
//
// Output:
// - vis: Matrix of interpolate singularities (row = vertex number; col 0 = doublet, col 1 = source)

#include <iostream>
#include <Eigen/Dense>
#include "interp_ctv.h"
#include <interp.h>

#define NDIM 3
#define NV 4

using namespace std;
using namespace Eigen;

MatrixXd interp_ctv(int idG, int idC, int idS, Network &bPan) {

    //// Initialization
    // Temporary
    MatrixXd v; // vertices coordinates
    v.resize(NV, NDIM);
    MatrixXi c; // centers (corner of interpolation)
    c.resize(NV, 4);
    MatrixXd vis; // interpolated singularities on vertices
    vis.resize(NV, 2);

    // Store vertices coordinates
    v.row(0) = bPan.v0.row(idG);
    v.row(1) = bPan.v1.row(idG);
    v.row(2) = bPan.v2.row(idG);
    v.row(3) = bPan.v3.row(idG);

    //// Compute panel center indices
    // If panel is on the last row/column, linear extrapolation is used
    if (idS == 0) {
        if (idC == 0) {
            c(0,0) = idG;
            c(0,1) = idG + 1;
            c(0,2) = idG + 1 + bPan.nC_;
            c(0,3) = idG + bPan.nC_;
            c(1,0) = idG;
            c(1,1) = idG + 1;
            c(1,2) = idG + 1 + bPan.nC_;
            c(1,3) = idG + bPan.nC_;
            c(2,0) = idG;
            c(2,1) = idG + 1;
            c(2,2) = idG + 1 + bPan.nC_;
            c(2,3) = idG + bPan.nC_;
            c(3,0) = idG;
            c(3,1) = idG + 1;
            c(3,2) = idG + 1 + bPan.nC_;
            c(3,3) = idG + bPan.nC_;
        }
        else if (idC == bPan.nC_ - 1) {
            c(0,0) = idG - 1;
            c(0,1) = idG;
            c(0,2) = idG + bPan.nC_;
            c(0,3) = idG - 1 + bPan.nC_;
            c(1,0) = idG - 1;
            c(1,1) = idG;
            c(1,2) = idG + bPan.nC_;
            c(1,3) = idG - 1 + bPan.nC_;
            c(2,0) = idG - 1;
            c(2,1) = idG;
            c(2,2) = idG + bPan.nC_;
            c(2,3) = idG - 1 + bPan.nC_;
            c(3,0) = idG - 1;
            c(3,1) = idG;
            c(3,2) = idG + bPan.nC_;
            c(3,3) = idG - 1 + bPan.nC_;
        }
        else {
            c(0,0) = idG - 1;
            c(0,1) = idG;
            c(0,2) = idG + bPan.nC_;
            c(0,3) = idG - 1 + bPan.nC_;
            c(1,0) = idG;
            c(1,1) = idG + 1;
            c(1,2) = idG + 1 + bPan.nC_;
            c(1,3) = idG + bPan.nC_;
            c(2,0) = idG;
            c(2,1) = idG + 1;
            c(2,2) = idG + 1 + bPan.nC_;
            c(2,3) = idG + bPan.nC_;
            c(3,0) = idG - 1;
            c(3,1) = idG;
            c(3,2) = idG + bPan.nC_;
            c(3,3) = idG - 1 + bPan.nC_;
        }
    }
    else if (idS == bPan.nS_ - 1) {
        if (idC == 0) {
            c(0,0) = idG - bPan.nC_;
            c(0,1) = idG - bPan.nC_ + 1;
            c(0,2) = idG + 1;
            c(0,3) = idG;
            c(1,0) = idG - bPan.nC_;
            c(1,1) = idG - bPan.nC_ + 1;
            c(1,2) = idG + 1;
            c(1,3) = idG;
            c(2,0) = idG - bPan.nC_;
            c(2,1) = idG - bPan.nC_ + 1;
            c(2,2) = idG + 1;
            c(2,3) = idG;
            c(3,0) = idG - bPan.nC_;
            c(3,1) = idG - bPan.nC_ + 1;
            c(3,2) = idG + 1;
            c(3,3) = idG;
        }
        else if (idC == bPan.nC_ - 1) {
            c(0,0) = idG - 1 - bPan.nC_;
            c(0,1) = idG - bPan.nC_;
            c(0,2) = idG;
            c(0,3) = idG - 1;
            c(1,0) = idG - 1 - bPan.nC_;
            c(1,1) = idG - bPan.nC_;
            c(1,2) = idG;
            c(1,3) = idG - 1;
            c(2,0) = idG - 1 - bPan.nC_;
            c(2,1) = idG - bPan.nC_;
            c(2,2) = idG;
            c(2,3) = idG - 1;
            c(3,0) = idG - 1 - bPan.nC_;
            c(3,1) = idG - bPan.nC_;
            c(3,2) = idG;
            c(3,3) = idG - 1;
        }
        else {
            c(0,0) = idG - 1 - bPan.nC_;
            c(0,1) = idG - bPan.nC_;
            c(0,2) = idG;
            c(0,3) = idG - 1;
            c(1,0) = idG - bPan.nC_;
            c(1,1) = idG - bPan.nC_ + 1;
            c(1,2) = idG + 1;
            c(1,3) = idG;
            c(2,0) = idG - bPan.nC_;
            c(2,1) = idG - bPan.nC_ + 1;
            c(2,2) = idG + 1;
            c(2,3) = idG;
            c(3,0) = idG - 1 - bPan.nC_;
            c(3,1) = idG - bPan.nC_;
            c(3,2) = idG;
            c(3,3) = idG - 1;
        }
    }
    else {
        c(0,0) = idG - 1 - bPan.nC_;
        c(0,1) = idG - bPan.nC_;
        c(0,2) = idG;
        c(0,3) = idG - 1;
        c(1,0) = idG - bPan.nC_;
        c(1,1) = idG - bPan.nC_ + 1;
        c(1,2) = idG + 1;
        c(1,3) = idG;
        c(2,0) = idG;
        c(2,1) = idG + 1;
        c(2,2) = idG + 1 + bPan.nC_;
        c(2,3) = idG + bPan.nC_;
        c(3,0) = idG - 1;
        c(3,1) = idG;
        c(3,2) = idG + bPan.nC_;
        c(3,3) = idG - 1 + bPan.nC_;
    }

    //// Interpolate each vertex
    for (int l = 0; l < NV; l++) {
        vis(l,0) = interp(bPan.CG(c(l,0),0), bPan.CG(c(l,0),1), bPan.CG(c(l,0),2),
                          bPan.CG(c(l,1),0), bPan.CG(c(l,1),1), bPan.CG(c(l,1),2),
                          bPan.CG(c(l,2),0), bPan.CG(c(l,2),1), bPan.CG(c(l,2),2),
                          bPan.CG(c(l,3),0), bPan.CG(c(l,3),1), bPan.CG(c(l,3),2),
                          bPan.mu(c(l,0)), bPan.mu(c(l,1)), bPan.mu(c(l,2)), bPan.mu(c(l,3)),
                          v(l,0), v(l,1), v(l,2));

        vis(l,1) = interp(bPan.CG(c(l,0),0), bPan.CG(c(l,0),1), bPan.CG(c(l,0),2),
                          bPan.CG(c(l,1),0), bPan.CG(c(l,1),1), bPan.CG(c(l,1),2),
                          bPan.CG(c(l,2),0), bPan.CG(c(l,2),1), bPan.CG(c(l,2),2),
                          bPan.CG(c(l,3),0), bPan.CG(c(l,3),1), bPan.CG(c(l,3),2),
                          bPan.tau(c(l,0)), bPan.tau(c(l,1)), bPan.tau(c(l,2)), bPan.tau(c(l,3)),
                          v(l,0), v(l,1), v(l,2));
    }
    return vis;
}


/**
 * @file tests/image_load_test.cpp
 * @author Mehul Kumar Nirala
 *
 * Tests for loading and saving images.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */

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

using namespace mlpack;
using namespace mlpack::data;
using namespace std;

#ifdef HAS_STB // Compile this only if stb is present.

BOOST_AUTO_TEST_SUITE(ImageLoadTest);

/**
 * Test if an image with an unsupported extension throws an expected
 * exception.
 */
BOOST_AUTO_TEST_CASE(LoadInvalidExtensionFile)
{
  arma::Mat<unsigned char> matrix;
  data::ImageInfo info;
  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(data::Load("invalidExtendion.p4ng", matrix, info,
      true),  std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Test that the image is loaded correctly into the matrix using the API.
 */
BOOST_AUTO_TEST_CASE(LoadImageAPITest)
{
  arma::Mat<unsigned char> matrix;
  data::ImageInfo info;
  BOOST_REQUIRE(data::Load("test_image.png", matrix, info, false) == true);
  // width * height * channels.
  BOOST_REQUIRE_EQUAL(matrix.n_rows, 50 * 50 * 3);
  BOOST_REQUIRE_EQUAL(info.Height(), 50);
  BOOST_REQUIRE_EQUAL(info.Width(), 50);
  BOOST_REQUIRE_EQUAL(info.Channels(), 3);
  BOOST_REQUIRE_EQUAL(matrix.n_cols, 1);
}

/**
 * Test if the image is saved correctly using API.
 */
BOOST_AUTO_TEST_CASE(SaveImageAPITest)
{
  data::ImageInfo info(5, 5, 3, 90);

  arma::Mat<unsigned char> im1;
  size_t dimension = info.Width() * info.Height() * info.Channels();
  im1 = arma::randi<arma::Mat<unsigned char>>(dimension, 1);
  BOOST_REQUIRE(data::Save("APITest.bmp", im1, info, false) == true);

  arma::Mat<unsigned char> im2;
  BOOST_REQUIRE(data::Load("APITest.bmp", im2, info, false) == true);

  BOOST_REQUIRE_EQUAL(im1.n_cols, im2.n_cols);
  BOOST_REQUIRE_EQUAL(im1.n_rows, im2.n_rows);
  for (size_t i = 0; i < im1.n_elem; ++i)
    BOOST_REQUIRE_EQUAL(im1[i], im2[i]);
  remove("APITest.bmp");
}

/**
 * Test if an image with a wrong dimesion throws an expected
 * exception while saving.
 */
BOOST_AUTO_TEST_CASE(SaveImageWrongInfo)
{
  data::ImageInfo info(5, 5, 3, 90);

  arma::Mat<unsigned char> im1;
  size_t dimension = info.Width() * info.Height() * info.Channels();
  im1 = arma::randi<arma::Mat<unsigned char>>(24 * 25 * 7, 1);
  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(data::Save("APITest.bmp", im1, info, false),
      std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Test that the image is loaded correctly into the matrix using the API
 * for vectors.
 */
BOOST_AUTO_TEST_CASE(LoadVectorImageAPITest)
{
  arma::Mat<unsigned char> matrix;
  data::ImageInfo info;
  std::vector<std::string> files = {"test_image.png", "test_image.png"};
  BOOST_REQUIRE(data::Load(files, matrix, info, false) == true);
  // width * height * channels.
  BOOST_REQUIRE_EQUAL(matrix.n_rows, 50 * 50 * 3);
  BOOST_REQUIRE_EQUAL(info.Height(), 50);
  BOOST_REQUIRE_EQUAL(info.Width(), 50);
  BOOST_REQUIRE_EQUAL(info.Channels(), 3);
  BOOST_REQUIRE_EQUAL(matrix.n_cols, 2);
}

/**
 * Test if the image is saved correctly using API for arma mat.
 */
BOOST_AUTO_TEST_CASE(SaveImageMatAPITest)
{
  data::ImageInfo info(5, 5, 3);

  arma::Mat<unsigned char> im1;
  size_t dimension = info.Width() * info.Height() * info.Channels();
  im1 = arma::randi<arma::Mat<unsigned char>>(dimension, 1);
  arma::mat input = arma::conv_to<arma::mat>::from(im1);
  BOOST_REQUIRE(Save("APITest.bmp", input, info, false) == true);

  arma::mat output;
  BOOST_REQUIRE(Load("APITest.bmp", output, info, false) == true);

  BOOST_REQUIRE_EQUAL(input.n_cols, output.n_cols);
  BOOST_REQUIRE_EQUAL(input.n_rows, output.n_rows);
  for (size_t i = 0; i < input.n_elem; ++i)
    BOOST_REQUIRE_CLOSE(input[i], output[i], 1e-5);
  remove("APITest.bmp");
}

/**
 * Serialization test for the ImageInfo class.
 */
BOOST_AUTO_TEST_CASE(ImageInfoSerialization)
{
  data::ImageInfo info(5, 5, 3, 90);
  data::ImageInfo xmlInfo, textInfo, binaryInfo;

  SerializeObjectAll(info, xmlInfo, textInfo, binaryInfo);

  BOOST_REQUIRE_EQUAL(info.Width(), xmlInfo.Width());
  BOOST_REQUIRE_EQUAL(info.Height(), xmlInfo.Height());
  BOOST_REQUIRE_EQUAL(info.Channels(), xmlInfo.Channels());
  BOOST_REQUIRE_EQUAL(info.Quality(), xmlInfo.Quality());
  BOOST_REQUIRE_EQUAL(info.Width(), textInfo.Width());
  BOOST_REQUIRE_EQUAL(info.Height(), textInfo.Height());
  BOOST_REQUIRE_EQUAL(info.Channels(), textInfo.Channels());
  BOOST_REQUIRE_EQUAL(info.Quality(), textInfo.Quality());
  BOOST_REQUIRE_EQUAL(info.Width(), binaryInfo.Width());
  BOOST_REQUIRE_EQUAL(info.Height(), binaryInfo.Height());
  BOOST_REQUIRE_EQUAL(info.Channels(), binaryInfo.Channels());
  BOOST_REQUIRE_EQUAL(info.Quality(), binaryInfo.Quality());
}

BOOST_AUTO_TEST_SUITE_END();

#endif // HAS_STB.


/*
 * File:   main.cpp
 * Author: malone
 *
 * Created on August 6, 2012, 9:05 PM
 */

#include <string>
#include <iostream>
#include <ostream>
#include <vector>
#include <cstdlib>

#include <boost/program_options.hpp>
#include <boost/timer/timer.hpp>
#include <boost/asio.hpp>
#include <boost/thread.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

#include "urlearning/base/node.h"
#include "urlearning/base/typedefs.h"
#include "urlearning/score_cache/score_cache.h"

#include "urlearning/priority_queue/priority_queue.h"
#include "urlearning/fileio/hugin_structure_writer.h"

#include "urlearning/score_cache/best_score_calculator.h"
#include "urlearning/score_cache/best_score_creator.h"

#include "urlearning/heuristic/heuristic.h"
#include "urlearning/heuristic/heuristic_creator.h"
#include "urlearning/base/skeleton.hpp"

#include "urlearning/scoring_function/bdeu_scoring_function.h"
#include "urlearning/scoring_function/bic_scoring_function.h"
#include "urlearning/scoring_function/fnml_scoring_function.h"
#include "urlearning/scoring_function/lasso_entropy_scoring_function.h"
#include "urlearning/scoring_function/adaptive_lasso_entropy.h"
//#define RUN_GOBNILP

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <sys/stat.h>
#include <sys/types.h>

void getFileCreationTime(char *path) {
    struct stat attr;
    stat(path, &attr);
    printf("%s, last modified time: %s", path, ctime(&attr.st_mtime));
}

namespace po = boost::program_options;

/**
 * A timer to keep track of how long the algorithm has been running.
 */
boost::asio::io_service io;

/**
 * A variable to check if the user-specified time limit has expired.
 */
bool outOfTime;
/**
 * The file containing the data.
 */
std::string inputFile;
std::string sf = "adaptive"; // scoring function
/**
 * The path to the score cache file.
 */
std::string scoreFile;
double lambda = 0.5; // for lasso
bool adaptive = true;
int which = 2;
/**
 * The data structure to use to calculate best parent set scores.
 * "tree", "list", "bitwise"
 */
std::string bestScoreCalculator;

/**
 * The type of heuristic to use.
 */
std::string heuristicType;

/**
 * The argument for creating the pattern database.
 */
std::string heuristicArgument;

/**
 * The file to write the learned network.
 */
std::string netFile;
std::ofstream network_file;

/**
 * The ancestor-only variables for the search.
 * See UAI '14.
 */
std::string ancestorsArgument;       // csv list
varset ancestors;

/**
 * The variables for this search.
 */
std::string sccArgument;      //csv list
varset scc;

/**
 * The number of nodes expanded during the search.
 */
int nodesExpanded;

/**
 * the skeleton file, Ni added
 */
std::string skeletonFile;

/**
 * the scoring function to use in post-processing
 */
std::string scoring_function;

/**
 * The maximum running time for the algorithm.
 */
int runningTime;

/**
 * Handler when out of time.
 */
void timeout(const boost::system::error_code& /*e*/) {
    printf("Out of time\n");
    outOfTime = true;
}

std::vector<varset> reconstructSolution(Node *goal, std::vector<bestscorecalculators::BestScoreCalculator*> &spgs, NodeMap &closedList, const varset & scc, std::vector<int> & total_ordering) {
    std::vector<varset> optimalParents;
    for (int i = 0; i < spgs.size(); i++) {
        optimalParents.push_back(VARSET(spgs.size()));
    }

    VARSET_COPY(goal->getSubnetwork(), remainingVariables);
    Node *current = goal;
    float score = 0;
    int count = cardinality(scc);   //Ni added
    printf("reconstructSolution, before loop, parents %x, count %d\n", (unsigned int) remainingVariables, count);
    for (int i = 0; i < count; i++) {
        int leaf = current->getLeaf();
        total_ordering[count-1-i] = leaf;
        score += spgs[leaf]->getScore(remainingVariables);
        varset parents = spgs[leaf]->getParents();
        optimalParents[count-1-i] = parents;
	float the_score = spgs[leaf]->getScore(parents);

        printf("reconstructSolution, iteration %d, leaf %d, the_score %f, score %f, parents %x\n", i, leaf, the_score, score,(unsigned int)parents);
        VARSET_CLEAR(remainingVariables, leaf);

        current = closedList[remainingVariables];
    }

    return optimalParents;
}

int post_astar_process_count = 0;
void post_astar_processing(
  std::vector<bestscorecalculators::BestScoreCalculator*> & spgs
, int variableCount
, std::vector<int> & total_ordering
, std::vector<varset> & optimalParents
)
{
	post_astar_process_count++;
        float final_score = 0.0;
        for(int i=0; i < variableCount; i++)
        {
                int var_idx = total_ordering[i];
                final_score += spgs[var_idx]->getScore(optimalParents[i]);
        }
    printf("Finished setting parents for network, post_astar_process_count %d, count %d, total ordering: "
          , post_astar_process_count, variableCount);
    for(int i=0; i < variableCount; i++)
    {
        printf("%d ", total_ordering[i]);
    }
    printf("\n");

    // Ni added
    network_file << "NumVars " << variableCount << std::endl;
    for(int v=0; v < variableCount; v++)
    {
        const int var_idx = total_ordering[v];
        const varset & parents = optimalParents[v];
        printf("Variable # %d, parent bit set %lx, parents are (index starts from 1) ", var_idx + 1, parents);
        network_file << "Var " << total_ordering[v] + 1
                             << ", parents"
                                         ;

        for(int i=0; i < variableCount; i++)
        {
            if(VARSET_GET(parents,i))
            {
                printf("  %d", i+1);
                network_file << ", " << (i+1);
            }
        }
        printf("\n");
        network_file << std::endl;
    }
}

//Ni added
void run_astar_on_one_scc( int variableCount
                         , std::vector<bestscorecalculators::BestScoreCalculator*> & spgs
                         , heuristics::Heuristic* heuristic
                         , scoring::ScoreCache & cache
                         , const varset & scc        // the original scc from Matlone
                         , const varset & ancestors  // ancestors
                         , const varset & the_scc    // our scc from skeleton
                         , int scc_idx
                         , datastructures::Skeleton & skeleton
                         )
{

    NodeMap generatedNodes;
    init_map(generatedNodes);

    PriorityQueue openList;
    // find one bit = 1, ideally from beginning
    int first_set_bit = VARSET_FIND_NEXT_SET(the_scc, 0);

    byte leaf(first_set_bit);   //original leaf =0, Ni modified
    Node *root = new Node(0.0f, 0.0f, ancestors, leaf);
    openList.push(root);

#ifdef DEBUG
    printf("The start is '%s'\n", varsetToString(ancestors).c_str());

    bool c;
    float lb = heuristic->h(ancestors, c);

    printf("The lb is %.2f\n", lb);
#endif

    Node *goal = NULL;
    VARSET_NEW(allVariables, variableCount);
    VARSET_SET_VALUE(allVariables, ancestors);
    allVariables = VARSET_OR(allVariables, the_scc);
    printf("allVariables equals the_scc = %d, scc#%d\n", allVariables == the_scc, scc_idx);
    printf("allVariables = %s\n", varsetToString(allVariables).c_str());
    printf("the_scc      = %s\n", varsetToString(the_scc).c_str());
    printf("original_scc = %s\n", varsetToString(scc).c_str());
#ifdef DEBUG
    printf("The goal is '%s'\n", varsetToString(allVariables).c_str());
#endif

    float upperBound = std::numeric_limits<float>::max();
    bool complete = false;

    printf("Beginning search\n");
    nodesExpanded = 0;
    VARSET_NEW(zero_varset, variableCount);
    while (openList.size() > 0 && !outOfTime) {
        //printf("top of while\n");
        Node *u = openList.pop();
        //printf("Expanding: '%s', g: '%.2f'\n", varsetToString(u->getSubnetwork()).c_str(), u->getG());
        nodesExpanded++;

        //        if (nodesExpanded % 100 == 0) {
        //            std::cout << ".";
        //            std::cout.flush();
        //        }

        varset variables = u->getSubnetwork();

        // check if it is the goal
        if (variables == allVariables) {
            printf("Found goal node, total cost = %f\n, leaf = %d\n", u->getG(), u->getLeaf());
            goal = u;
            break;
        }

        // TODO: this is mostly broken
        if (u->getF() > upperBound) {
            printf("Stop loop due to too high cost, f = %f, g = %f\n", u->getF(), u->getG());
            break;
        }

        // note that it is in the closed list
        u->setPqPos(-2);

        // expand
        // Ni added, more efficiently, calculate set of neighbors from variables, then exclude variables,
        // get the candidate list, to be improved later
        for (byte leaf = 0; leaf < variableCount; leaf++) {
            // make sure this variable was not already present
            if (VARSET_GET(variables, leaf)) continue;

            // also make sure this is one of the variables we care about
            if (!VARSET_GET(the_scc, leaf)) continue;
            //Ni added the following if-statement
            if(skeleton.good() and not VARSET_EQUAL(variables, zero_varset))//excluding the empty set so that no skeleton checking at startup
            {
                const varset & neighbors = skeleton.get_neighbors(leaf);
                if(VARSET_EQUAL(VARSET_AND(variables, neighbors), zero_varset))
                {
                    //printf("Skipping leaf %d because it is not in neighbors %16lx\n", leaf, neighbors);
                    continue;
                }
            }
            // get the new variable set
            VARSET_COPY(variables, newVariables);
            VARSET_SET(newVariables, leaf);

            Node *succ = generatedNodes[newVariables];

            // check if this is the first time we have generated this node
            if (succ == NULL) {
                // get the cost along this path

                //printf("About to check for using leaf '%d', newVariables: '%s'\n", leaf, varsetToString(newVariables).c_str());
                float leaf_score = spgs[leaf]->getScore(newVariables);
                float g = u->getG() + leaf_score; //spgs[leaf]->getScore(newVariables);
                //printf("I have g\n");
                // calculate the heuristic estimate
                complete = false;
                float h = heuristic->h(newVariables, complete);
                //printf("I have h: %.2f\n", h);

#ifdef RUN_GOBNILP
                std::string filename = "exclude_" + TO_STRING(newVariables) + ".pss";
                time_t t;
                double seconds;

                t = time(NULL);
                int count = cache.writeExclude(filename, newVariables);
                seconds = difftime(time(NULL), t);

                // check whether to execute or not
                if (count < 10000 && variableCount > 25) {

                    // now, call gobnilp on the subproblem
                    FILE *fpipe;
                    char buffer[1048576];
                    char cmd[4096];
                    int i;

                    t = time(NULL);
                    snprintf(cmd, sizeof (cmd), "gobnilp %s", filename.c_str());

                    if (0 == (fpipe = (FILE*) popen(cmd, "r"))) {
                        perror("popen() failed.");
                        exit(1);
                    }

                    while ((i = fread(buffer, sizeof (char), sizeof buffer, fpipe))) {

                        /* We found some solutions, so split them up and add the cutting planes. */

                        // make sure to clean up the buffer so it behaves as a string
                        buffer[i] = 0;

                        //printf("buffer: '%s'\n", buffer);

                        char *pch;
                        pch = strtok(buffer, "\n");

                        bool optimal = false;

                        while (pch != NULL) {

                            // check if this is a "Dual Bound         : -4.62524687000000e+02" line

                            // I believe this is the only line in the output which starts with a 'D'
                            // this is really brittle, though
                            if (pch[0] == 'D') {
                                std::vector<std::string> tokens;
                                boost::split(tokens, pch, boost::is_any_of(" "), boost::token_compress_on);
                                float score = -1 * atof(tokens[3].c_str());
                                printf("Node: %s, g: %f, (%s) h(gobnilp): %f, f(gobnilp): %f, h(pd): %f, f(pd): %f\n",
                                        varsetToString(newVariables).c_str(), g, (optimal ? "optimal" : "bound"), score, (score + g), h, (g + h));
                                h = score;
                            }

                            // also, check if this line contains the word "optimal"
                            // if so, then this is actually the globally optimal solution
                            if (strstr(pch, "optimal") != NULL) {
                                optimal = true;
                            }

                            pch = strtok(NULL, "\n");
                        }
                    }
                    pclose(fpipe);

                    remove(filename.c_str());

                    seconds = difftime(time(NULL), t);
                    printf("Time to solve and parse ip: %f(s)\n", seconds);
                }

#endif

#ifdef CHECK_COMPLETE
                if (complete) {
                    float score = g + h;
                    if (score < upperBound) {
                        upperBound = score;
                        printf("new upperBound: %f, nodes expanded: %d, open list size: %d\n", upperBound, nodesExpanded, openList.size());
                    }
                    continue;
                }
#endif

                // update all the values
                succ = new Node(g, h, newVariables, leaf);

                //printf("I have created succ\n");

                // add it to the open list
                openList.push(succ);

                //printf("pushed succ\n");

                // and to the list of generated nodes
                generatedNodes[newVariables] = succ;
                //printf("added succ to generatedNodes\n");
                continue;
            }

            // assume the heuristic is consistent
            // so check if it was in the closed list
            if (succ->getPqPos() == -2) {
                continue;
            }
            // so we have generated a node in the open list
            // see if the new path is better
            float g = u->getG() + spgs[leaf]->getScore(variables);
            if (g < succ->getG()) {
                // the update the information
                succ->setLeaf(leaf);
                succ->setG(g);

                // and the priority queue
//                if (succ->getPqPos() == -2)
//                {
//                	succ->setPqPos(0);
//                	openList.push(succ);
//                }
//                else
                openList.update(succ);
            }
        }
    }

    printf("Nodes expanded: %d, open list size: %d\n", nodesExpanded, openList.size());

    heuristic->printStatistics();

    if (goal != NULL) {
        //printf("Found solution: %f\n", goal->getF());
        printf("Found solution: %f, scc # %d, netFile.length %lu\n", goal->getG(), scc_idx, netFile.length());

        if (netFile.length() > 0) {
	    network_file.open(netFile.c_str(), std::ios_base::trunc);
            printf("Opened network file %s\n", netFile.c_str());
            datastructures::BayesianNetwork *network = cache.getNetwork();
            network->fixCardinalities();
            std::vector<int> total_ordering(variableCount);
            std::vector<varset> optimalParents = reconstructSolution(goal, spgs, generatedNodes, the_scc, total_ordering);
            printf("Finished reconstructSolution\n");
            network->setParents(optimalParents);
            printf("Setting uniform probability\n");
            //network->setUniformProbabilities();
            // post processing
            printf("Start post-processing\n");
            scoring::ScoringFunction *scoringFunction;
            if (sf.compare("lasso") == 0 or sf.compare("entropy") == 0){
                printf("Creating Entropy/LassoFunction with input file %s\n", inputFile.c_str());
                scoringFunction = new scoring::LassoEntropyScoringFunction(*network, which, inputFile, lambda, NULL, false);
            } else if (sf.compare("adaptive_lasso") == 0 or sf.compare("adaptive") == 0){
                printf("Creating Adaptive Entropy/LassoFunction with input file %s\n", inputFile.c_str());
                scoringFunction = new scoring::AdaptiveLassoEntropyScoringFunction(*network, which, inputFile, lambda, NULL, adaptive, false);
            }
            // run post_processing
            scoringFunction->post_processing(total_ordering, optimalParents);
	    post_astar_processing(spgs,variableCount, total_ordering, optimalParents); // just print network file
	    network_file.close();

            printf("Finished setting parents for network, count %d, total ordering: ", variableCount);
            for(int i=0; i < variableCount; i++)
            {
                printf("%d ", total_ordering[i]);
            }
            printf("\n");
// Ni commented  out
//           fileio::HuginStructureWriter writer;
//           writer.write(network, netFile);
            // Ni added
            std::vector<uint64_t> vpar(variableCount, 0L);
            for(int v=0; v < variableCount; v++)
            {
                const varset & parents = optimalParents[v];
                vpar[total_ordering[v]] = parents;
                printf("Variable # %d, parent bit set %lx, parents are (index starts from 0) ", total_ordering[v], parents);
                for(int i=0; i < variableCount; i++)
                {
                    if(VARSET_GET(parents,i))
                        printf("  %d", i);
                }
                printf("\n");
            }
            std::string netFile2 = netFile + ".csv";
            std::ofstream network_file2(netFile2.c_str(), std::ios_base::trunc);
            for(int v=0; v<variableCount; v++)
            {
            	const varset parents = vpar[v];
            	char buf[256];
            	int length = 0;
            	for(int i=0; i<variableCount; i++)
            	{
            		length += snprintf(buf+length, 4, "%d,", (VARSET_GET(parents, i) ? 1 : 0));
            	}
            	buf[length-1]='\n';
            	buf[length] = '\0';
            	network_file2 << buf;
            }
            network_file2.close();
        }
    } else {
        Node *u = openList.pop();
        printf("No solution found.\n");
        if(u)
          printf("Lower bound: %f\n", u->getF());
    }

    for (auto pair = generatedNodes.begin(); pair != generatedNodes.end(); pair++) {
        delete (pair->second);
    }

}// run_astar_on_one_scc

void astar() {
    printf("URLearning, A*\n");
    printf("Dataset: '%s'\n", scoreFile.c_str());
    printf("Net file: '%s'\n", netFile.c_str());
    printf("Best score calculator: '%s'\n", bestScoreCalculator.c_str());
    printf("Heuristic type: '%s'\n", heuristicType.c_str());
    printf("Heuristic argument: '%s'\n", heuristicArgument.c_str());
    printf("Ancestors: '%s'\n", ancestorsArgument.c_str());
    printf("SCC: '%s'\n", sccArgument.c_str());

    boost::timer::auto_cpu_timer act;

    printf("Reading score cache.\n");
    scoring::ScoreCache cache;
    cache.read(scoreFile);
    printf("Done reading score cache. Verifying \n");
    int variableCount = cache.getVariableCount();
    printf("Variable count is %d\n",variableCount);

    const int num = variableCount;
    varset optimal_parents[num];
    //VARSET_NEW(vs0, num);
    //VARSET_NEW(vs1, num);
    //VARSET_NEW(vs2, num); VARSET_SET(vs2, 0);
    //VARSET_NEW(vs3, num); VARSET_SET(vs2, 0); VARSET_SET(vs2, 1);
    //VARSET_NEW(vs4, num); VARSET_SET(vs4, 2); VARSET_SET(vs4, 3);

    //optimal_parents[0] = vs0;
    //optimal_parents[1] = vs1;
    //optimal_parents[2] = vs2;
    //optimal_parents[3] = vs3;
    //optimal_parents[4] = vs4;
    for(int variable=0; variable<num; variable++)
    {
        //Ni added, print out score
        printf("Independent and dependent scores, variable # %d, ind_score %f, depend_score %f\n"
              , variable, cache.getScore(variable, 0)
              , cache.getScore(variable, optimal_parents[variable]) );
    }

    // parse the ancestor and scc variables
    VARSET_NEW(ancestors,variableCount);
    VARSET_NEW(scc,variableCount);

    // if no scc was specified, assume we just want to learn everything
    if (sccArgument == "") {
        VARSET_SET_ALL(scc, variableCount);
    } else {
        setFromCsv(scc, sccArgument);
        setFromCsv(ancestors, ancestorsArgument);
    }

    act.start();
    printf("Creating BestScore calculators.\n");
    std::vector<bestscorecalculators::BestScoreCalculator*> spgs = bestscorecalculators::create(bestScoreCalculator, cache);
    act.stop();
    act.report();

    act.start();
    printf("Creating heuristic.\n");
    heuristics::Heuristic *heuristic = heuristics::createWithAncestors(heuristicType, heuristicArgument, spgs, ancestors, scc);

#ifdef DEBUG
    heuristic->print();
#endif

    act.stop();
    act.report();

    datastructures::Skeleton skeleton; // Ni added
	if(skeletonFile.find(".arc") + 4 == skeletonFile.size())
		skeleton.read_arc_list_file(skeletonFile, variableCount);
	else
	    skeleton.read_matrix_file(skeletonFile); // Ni added
    if(not skeleton.good())
        skeleton.set_variable_count(variableCount);
    printf("Checking skeleton, good = %d\n", skeleton.good());

    const std::vector<varset> & scc_list = skeleton.get_scc();//Ni added
    const int num_scc = scc_list.size();    //Ni added
    printf("num of sccs = %d\n", num_scc);
    act.start();
    for(int i=0; i < num_scc; i++)  //Ni added
    {
        run_astar_on_one_scc(variableCount, spgs, heuristic, cache, scc, ancestors, scc_list[i], i, skeleton);
    }
    for (auto spg = spgs.begin(); spg != spgs.end(); spg++) {
        delete *spg;
    }
    delete heuristic;
    act.stop();
    act.report();
    io.stop();
}//astar

int main(int argc, char** argv) {

	getFileCreationTime(argv[0]); // print the binary executable creation time

    boost::timer::auto_cpu_timer act;
    srand(time(NULL));

    std::string description = std::string("Learn an optimal Bayesian network using A*.  Example usage: ") + argv[0] + " iris.pss";
    po::options_description desc(description);

    desc.add_options()
            ("scoreFile", po::value<std::string > (&scoreFile)->required(), "The file containing the local scores in pss format. First positional argument.")
            ("skeleton,k", po::value<std::string > (&skeletonFile), "The file containing the edges of a skeleton")
            ("scoring_function,f", po::value<std::string>(&sf), "The scoring function to use in post-processing")
            ("raw_inputFile,i", po::value<std::string>(&inputFile), "The raw data file to use in scoring and post-processing")
            ("lambda,l", po::value<double> (&lambda), "The lambda in Lasso.")
            ("adaptive", "Use adaptive Lasso")
            ("scoreType,w", po::value<int> (&which)->default_value(1), "which score, 0 for lasso, 1 for entropy1, 2 for entropy2")
            ("bestScore,b", po::value<std::string > (&bestScoreCalculator)->default_value("list"), bestscorecalculators::bestScoreCalculatorString.c_str())
            ("heuristic,e", po::value<std::string > (&heuristicType)->default_value("static"), heuristics::heuristicTypeString.c_str())
            ("argument,a", po::value<std::string > (&heuristicArgument)->default_value("2"), heuristics::heuristicArgumentString.c_str())
            ("pc_{i-1},p", po::value<std::string > (&ancestorsArgument)->default_value(""), "Variables which can only be used as ancestors. They will not be added in the search. CSV-list of variable indices. See UAI '14.")
            ("scc_i,s", po::value<std::string > (&sccArgument)->default_value(""), "Variables which will be added in the search. CSV-list of variable indices. Leave blank to add all variables. See UAI '14.")
            ("runningTime,r", po::value<int> (&runningTime)->default_value(0), "The maximum running time for the algorithm.  0 means no running time.")
            ("netFile,n", po::value<std::string > (&netFile)->default_value(""), "The file to which the learned network is written.  Leave blank to not create the file.")
            ("help,h", "Show this help message.")
            ;

    po::positional_options_description positionalOptions;
    positionalOptions.add("scoreFile", 1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc)
            .positional(positionalOptions).run(),
            vm);

    if (vm.count("help") || argc == 1) {
        std::cout << desc;
        return 0;
    }
    if(vm.count("adaptive"))
    {
        adaptive = true;
        std::cout << "Will use adaptive Lasso\n";
    }
    po::notify(vm);
    outOfTime = false;

    boost::to_lower(bestScoreCalculator);

    boost::asio::deadline_timer t(io);
    if (runningTime > 0) {
        printf("Maximum running time: %d\n", runningTime);
        t.expires_from_now(boost::posix_time::seconds(runningTime));
        t.async_wait(timeout);
        boost::thread workerThread(astar);
        io.run();
        workerThread.join();
    } else {
        astar();
    }

    return 0;
}


#include <svgpp/parser/length.hpp>
#include <svgpp/traits/length_units.hpp>

#include <gtest/gtest.h>
#include <boost/assign/list_of.hpp>
#include <boost/range/any_range.hpp>

namespace svgpp { namespace tag { namespace length_units {

#define ON_UNIT(unit) \
  bool operator== (unit const &, unit const &) { return true; } \
  std::ostream & operator<<(std::ostream & os, unit) \
  { os << "<" << #unit << ">"; return os; }

ON_UNIT(in)
ON_UNIT(cm)
ON_UNIT(mm)
ON_UNIT(pt)
ON_UNIT(pc)
ON_UNIT(em)
ON_UNIT(ex)
ON_UNIT(px)
ON_UNIT(percent)
ON_UNIT(none)

}}}

namespace
{

using namespace boost::assign;
using namespace svgpp::tag::length_units;

/*template<class UnitsTag>
struct units_wrapper
{
  units_wrapper() {}
  units_wrapper(units_wrapper const &) {}
  units_wrapper & operator= (units_wrapper const &) { return *this; }
  bool operator== (units_wrapper const &) const { return true; }
};*/

typedef boost::variant<
  in,
  cm,
  mm,
  pt,
  pc,
  em,
  ex,
  px,
  percent,
  none
> length_units;

typedef std::pair<double, length_units> length_t;

struct length_factory_t
{
  typedef double number_type;
  typedef length_t length_type;

  length_factory_t() {}

  template<class UnitsTag>
  length_type create_length(double val, UnitsTag tag) const
  {
    return length_t(val, tag);
  }

  length_type create_length(double val, percent tag, svgpp::tag::length_dimension::any const &) const
  {
    return length_t(val, tag);
  }
};

struct length_policy
{
  typedef length_factory_t const length_factory_type;

  template<class Context>
  static length_factory_type & length_factory(Context const & context)
  {
    static const length_factory_t instance;
    return instance;
  }
};

typedef std::vector<length_t> lengths_t;
typedef std::pair<const char *, lengths_t> valid_case_t;

valid_case_t valid_tests[] = {
  valid_case_t("1 2", list_of(length_t(1, none()))(length_t(2, none())).convert_to_container<lengths_t>()),
  valid_case_t("4em 5.0ex", list_of(length_t(4, em()))(length_t(5, ex())).convert_to_container<lengths_t>() ),
  valid_case_t("1e-9px, 2e+10pt", list_of(length_t(1e-9, px()))(length_t(2e10, pt())).convert_to_container<lengths_t>() ),
  valid_case_t(".59px , 21.0cm", list_of(length_t(.59, px()))(length_t(21, cm())).convert_to_container<lengths_t>() ),
  valid_case_t("-56px -77pt", list_of(length_t(-56, px()))(length_t(-77, pt())).convert_to_container<lengths_t>() ),
};

const char * invalid_tests[] = {
  "1 2 ",
  " 1 2",
  "1,,2",
  "1 ,,2",
  "1, ,2",
  "1, 2,",
  ",1, 2",
  "1.1.2",
  "1-2",
  "1 px 2",
  "1,px 2",
  "1 px,2",
  "1px.4",
};

class list_of_lengths_valid : public ::testing::TestWithParam<valid_case_t>
{};

class list_of_lengths_invalid : public ::testing::TestWithParam<const char *>
{};

struct test_context
{
#if 0
  typedef boost::any_range<length_t, boost::single_pass_traversal_tag, length_t const &, std::ptrdiff_t> Range;
  void set(svgpp::tag::attribute::by, Range const & range)
#else
  template<class Range>
  void set(svgpp::tag::attribute::by, Range const & range)
#endif
  {
    for(typename boost::range_iterator<Range>::type it = boost::begin(range), end = boost::end(range);
      it != end; ++it)
      values_.push_back(*it);
  }

  std::vector<length_t> values_;
};

}

TEST_P(list_of_lengths_valid, t1)
{
  test_context context;
  svgpp::value_parser<
    svgpp::tag::type::list_of<svgpp::tag::type::length>,
    svgpp::length_policy<length_policy>
  >::parse(svgpp::tag::attribute::by(),
    context, std::string(GetParam().first), svgpp::tag::source::attribute());
  EXPECT_TRUE(context.values_ == GetParam().second);
}

INSTANTIATE_TEST_CASE_P(value_parser,
                        list_of_lengths_valid,
                        ::testing::ValuesIn(valid_tests));

TEST_P(list_of_lengths_invalid, t1)
{
}

INSTANTIATE_TEST_CASE_P(value_parser,
                        list_of_lengths_invalid,
                        ::testing::ValuesIn(invalid_tests));


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include <boost/test/unit_test.hpp>
#include <bitset>
#include "iputils.hh"

using namespace boost;

BOOST_AUTO_TEST_SUITE(nmtree)

BOOST_AUTO_TEST_CASE(test_basic) {
  NetmaskTree<int> nmt;
  nmt.insert(Netmask("130.161.252.0/24")).second=0;
  nmt.insert(Netmask("130.161.0.0/16")).second=1;
  nmt.insert(Netmask("130.0.0.0/8")).second=2;

  BOOST_CHECK_EQUAL(nmt.lookup(ComboAddress("213.244.168.210")), (void*)0);
  auto found=nmt.lookup(ComboAddress("130.161.252.29"));
  BOOST_CHECK(found);
  BOOST_CHECK_EQUAL(found->second, 0);
  found=nmt.lookup(ComboAddress("130.161.180.1"));
  BOOST_CHECK(found);
  BOOST_CHECK_EQUAL(found->second, 1);

  BOOST_CHECK_EQUAL(nmt.lookup(ComboAddress("130.255.255.255"))->second, 2);
  BOOST_CHECK_EQUAL(nmt.lookup(ComboAddress("130.161.252.255"))->second, 0);
  BOOST_CHECK_EQUAL(nmt.lookup(ComboAddress("130.161.253.255"))->second, 1);

  found=nmt.lookup(ComboAddress("130.145.180.1"));
  BOOST_CHECK(found);
  BOOST_CHECK_EQUAL(found->second, 2);

  nmt.clear();
  BOOST_CHECK(!nmt.lookup(ComboAddress("130.161.180.1")));

  nmt.insert(Netmask("::1")).second=1;
  nmt.insert(Netmask("::/0")).second=0;
  nmt.insert(Netmask("fe80::/16")).second=2;
  BOOST_CHECK_EQUAL(nmt.lookup(ComboAddress("130.161.253.255")), (void*)0);
  BOOST_CHECK_EQUAL(nmt.lookup(ComboAddress("::2"))->second, 0);
  BOOST_CHECK_EQUAL(nmt.lookup(ComboAddress("::ffff"))->second, 0);
  BOOST_CHECK_EQUAL(nmt.lookup(ComboAddress("::1"))->second, 1);
  BOOST_CHECK_EQUAL(nmt.lookup(ComboAddress("fe80::1"))->second, 2);
}

BOOST_AUTO_TEST_CASE(test_single) {
  NetmaskTree<bool> nmt;
  nmt.insert(Netmask("127.0.0.0/8")).second=1;
  BOOST_CHECK_EQUAL(nmt.lookup(ComboAddress("127.0.0.1"))->second, 1);
}

BOOST_AUTO_TEST_CASE(test_scale) {
  string start="192.168.";
  NetmaskTree<int> works;
  for(int i=0; i < 256; ++i) {
    for(int j=0; j < 256; ++j) {
      works.insert(Netmask(start+std::to_string(i)+"."+std::to_string(j))).second=i*j;
    }
  }

  for(int i=0; i < 256; ++i) {
    for(int j=0; j < 256; ++j) {
      BOOST_CHECK_EQUAL(works.lookup(ComboAddress(start+std::to_string(i)+"."+std::to_string(j)))->second, i*j);
    }
  }

  start="130.161.";
  for(int i=0; i < 256; ++i) {
    for(int j=0; j < 256; ++j) {
      BOOST_CHECK_EQUAL(works.lookup(ComboAddress(start+std::to_string(i)+"."+std::to_string(j))), (void*)0);
    }
  }

  start="2000:123:";
  for(int i=0; i < 256; ++i) {
    for(int j=0; j < 256; ++j) {
      works.insert(Netmask(start+std::to_string(i)+":"+std::to_string(j)+"::/64")).second=i*j;
    }
  }

  for(int i=0; i < 256; ++i) {
    for(int j=0; j < 256; ++j) {
      BOOST_CHECK_EQUAL(works.lookup(ComboAddress(start+std::to_string(i)+":"+std::to_string(j)+"::"+std::to_string(i)+":"+std::to_string(j)))->second, i*j);
    }
  }

  start="2001:123:";
  for(int i=0; i < 256; ++i) {
    for(int j=0; j < 256; ++j) {
      BOOST_CHECK_EQUAL(works.lookup(ComboAddress(start+std::to_string(i)+":"+std::to_string(j)+"::"+std::to_string(i)+":"+std::to_string(j))), (void*)0);
    }
  }
}

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         Copyright 2015 J.T. Lapreste
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_CORE_FUNCTIONS_TWOOFIVE_HPP_INCLUDED
#define NT2_CORE_FUNCTIONS_TWOOFIVE_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <nt2/include/constants/twoofive.hpp>
#include <nt2/sdk/meta/generative_hierarchy.hpp>
#include <nt2/core/container/dsl/generative.hpp>
#include <nt2/core/functions/common/generative.hpp>

#include <nt2/sdk/parameters.hpp>
#include <boost/preprocessor/arithmetic/inc.hpp>
#include <boost/preprocessor/repetition/repeat_from_to.hpp>

namespace nt2
{
  #define M0(z,n,t)                                    \
  NT2_FUNCTION_IMPLEMENTATION(nt2::tag::Twoofive,twoofive, n) \
  /**/

  BOOST_PP_REPEAT_FROM_TO(1,BOOST_PP_INC(BOOST_PP_INC(NT2_MAX_DIMENSIONS)),M0,~)

  #undef M0
}

namespace nt2 { namespace ext
{
  /// INTERNAL ONLY
  template<typename Domain, typename Expr, int N>
  struct  value_type<tag::Twoofive,Domain,N,Expr>
        : meta::generative_value<Expr>
  {};

  /// INTERNAL ONLY
  template<typename Domain, typename Expr, int N>
  struct  size_of<tag::Twoofive,Domain,N,Expr>
        : meta::generative_size<Expr>
  {};
} }
#endif


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2018 John Maddock
//  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_MATH_HYPERGEOMETRIC_1F1_CF_HPP
#define BOOST_MATH_HYPERGEOMETRIC_1F1_CF_HPP

#include <boost/math/tools/fraction.hpp>

//
// Evaluation of 1F1 by continued fraction
// see http://functions.wolfram.com/HypergeometricFunctions/Hypergeometric1F1/10/0002/
//
// This is not terribly useful, as like the series we're adding a something to 1,
// so only really useful when we know that the result will be > 1.
//

  namespace boost { namespace math { namespace detail {

     template <class T>
     struct hypergeometric_1F1_cf_func
     {
        typedef std::pair<T, T>  result_type;
        hypergeometric_1F1_cf_func(T a_, T b_, T z_) : a(a_), b(b_), z(z_), k(0) {}
        std::pair<T, T> operator()()
        {
           ++k;
           return std::make_pair(-(((a + k) * z) / ((k + 1) * (b + k))), 1 + ((a + k) * z) / ((k + 1) * (b + k)));
        }
        T a, b, z;
        unsigned k;
     };

     template <class T, class Policy>
     T hypergeometric_1F1_cf(const T& a, const T& b, const T& z, const Policy& pol, const char* function)
     {
        hypergeometric_1F1_cf_func<T> func(a, b, z);
        boost::uintmax_t max_iter = boost::math::policies::get_max_series_iterations<Policy>();
        T result = boost::math::tools::continued_fraction_a(func, boost::math::policies::get_epsilon<T, Policy>(), max_iter);
        boost::math::policies::check_series_iterations<T>(function, max_iter, pol);
        return 1 + a * z / (b * (1 + result));
     }

  } } } // namespaces

#endif // BOOST_MATH_HYPERGEOMETRIC_1F1_BESSEL_HPP


#include <MuonGun/Floodlight.h>
#include <MuonGun/I3MuonGun.h>
#include <MuonGun/EnergyDependentSurfaceInjector.h>
#include <MuonGun/Cylinder.h>
#include <dataclasses/I3Position.h>
#include <dataclasses/I3Direction.h>
#include <dataclasses/physics/I3Particle.h>
#include <dataclasses/physics/I3MCTreeUtils.h>

#include <boost/foreach.hpp>

namespace I3MuonGun {

template <typename Archive>
void
Floodlight::serialize(Archive &ar, unsigned version)
{
	if (version > 1)
		log_fatal_stream("Version "<<version<<" is from the future");

	ar & make_nvp("Generator", base_object<Generator>(*this));
	ar & make_nvp("Surface", surface_);
	ar & make_nvp("EnergySpectrum", energyGenerator_);
	if (version == 0) {
		zenith_range_ = std::make_pair(-1., 1.);
	} else {
		ar & make_nvp("CosZenithRange", zenith_range_);
	}
	log_acceptance_ = std::log(surface_->GetAcceptance(zenith_range_.first, zenith_range_.second));

}

Floodlight::Floodlight(SamplingSurfacePtr surface, boost::shared_ptr<OffsetPowerLaw> energyGenerator,
    double cosMin, double cosMax) : surface_(surface), energyGenerator_(energyGenerator),
    zenith_range_(cosMin, cosMax)
{
	if (!surface_)
		surface_ = boost::make_shared<Cylinder>(1000, 600, I3Position(31.25, 19.64, 0));
	if (!energyGenerator_)
		energyGenerator_ = boost::make_shared<OffsetPowerLaw>(1, 0., 5e2, 1e7);
	i3_assert(zenith_range_.second > zenith_range_.first);
	i3_assert(std::abs(zenith_range_.first) <= 1);
	i3_assert(std::abs(zenith_range_.second) <= 1);
	log_acceptance_ = std::log(surface_->GetAcceptance(zenith_range_.first, zenith_range_.second));
	i3_assert(std::isfinite(log_acceptance_));
}

GenerationProbabilityPtr
Floodlight::Clone() const
{
	return GenerationProbabilityPtr(new Floodlight(*this));
}

bool
Floodlight::IsCompatible(GenerationProbabilityConstPtr o) const
{
	boost::shared_ptr<const Floodlight> other = boost::dynamic_pointer_cast<const Floodlight>(o);
	if (!other)
		return false;
	else
		return (*surface_ == *(other->surface_)
		    && zenith_range_ == other->zenith_range_
		    && *energyGenerator_ == *(other->energyGenerator_));
}

void
Floodlight::Generate(I3RandomService &rng, I3MCTree &tree, BundleConfiguration &bundle __attribute__((unused))) const
{
	I3Direction dir;
	I3Position pos;
	surface_->SampleImpactRay(pos, dir, rng, zenith_range_.first, zenith_range_.second);

	I3Particle primary;
	primary.SetDir(dir);
	primary.SetPos(pos);
	primary.SetTime(0);
	primary.SetEnergy(energyGenerator_->Generate(rng));

	I3Particle muon = primary.Clone();
	muon.SetType(I3Particle::MuMinus);
	muon.SetLocationType(I3Particle::InIce);
	muon.SetShape(I3Particle::Null);

	I3MCTreeUtils::AddPrimary(tree, primary);
	I3MCTreeUtils::AppendChild(tree, primary, muon);
}

double
Floodlight::GetLogGenerationProbability(const I3Particle &axis, const BundleConfiguration &bundle) const
{
	std::pair<double, double> steps = surface_->GetIntersection(axis.GetPos(), axis.GetDir());
	// Bail if the axis doesn't intersect the surface, or there's more than 1 muon.
	double ct = std::cos(axis.GetDir().GetZenith());
	if (!std::isfinite(steps.first) || bundle.size() != 1
	    || ct < zenith_range_.first
	    || ct > zenith_range_.second)
		return -std::numeric_limits<double>::infinity();

	return energyGenerator_->GetLog(bundle.front().energy) - log_acceptance_;
}

}

I3_SERIALIZABLE(I3MuonGun::Floodlight);


#include <map>
#include <exception>
#include <string>
#include <cstdint>
#include <iostream>
#include <math.h>
#include <fc/exception/exception.hpp>
/*
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/rational.hpp>

#include <sg14/fixed_point>
#include "fixed.hpp"
*/

#include <fc/time.hpp>
#include <fc/log/logger.hpp>

//#include "bfp/lib/posit.h"

using namespace std;

typedef long double real_type;
typedef double token_type;

/*
struct margin_position {
   account_name owner;
   uint64_t     exchange_id;
   asset        lent;
   asset        collateral;
   uint64_t     open_time;

   uint64_t     primary_key()const{ return owner; }
   uint256_t    by_owner_ex_lent_collateral()const {

   }

   real_type    by_call_price()const {
      return collateral.amount / real_type(lent.amount);
   }
};
*/

template<typename Real>
Real Abs(Real Nbr)
{
 if( Nbr >= 0 )
  return Nbr;
 else
  return -Nbr;
}

template<typename Real>
Real sqrt_safe( const Real Nbr)
{
  return sqrt(Nbr);
// cout << " " << Nbr << "\n";;
 Real Number = Nbr / Real(2.0);
 const Real Tolerance = Real(double(1.0e-12));
 //cout << "tol: " << Tolerance << "\n";

 Real Sq;
 Real Er;
 do {
  auto tmp = Nbr / Number;
  tmp += Number;
  tmp /= real_type(2.0);
  if( Number == tmp ) break;
  Number = tmp;
  Sq = Number * Number;
  Er = Abs(Sq - Nbr);
// wdump((Er.getDouble())(1.0e-8)(Tolerance.getDouble()));
//  wdump(((Er - Tolerance).getDouble()));
 }while( Er >= Tolerance );

 return Number;
}

typedef __uint128_t uint128_t;
typedef string  account_name;
typedef string  symbol_type;

static const symbol_type exchange_symbol = "EXC";

struct asset {
   token_type amount;
   symbol_type symbol;
};

struct margin_key {
   symbol_type lent;
   symbol_type collat;
};

struct margin {
   asset       lent;
   symbol_type collateral_symbol;
   real_type   least_collateralized_rate;
};

struct user_margin {
   asset lent;
   asset collateral;

   real_type call_price()const {
      return collateral.amount / real_type(lent.amount);
   }
};

struct exchange_state;
struct connector {
   asset      balance;
   real_type  weight = 0.5;
   token_type total_lent; /// lent from maker to users
   token_type total_borrowed; /// borrowed from users to maker
   token_type total_available_to_lend; /// amount available to borrow
   token_type interest_pool; /// total interest earned but not claimed,
                             /// each user can claim user_lent

   void  borrow( exchange_state& ex, const asset& amount_to_borrow );
   asset convert_to_exchange( exchange_state& ex, const asset& input );
   asset convert_from_exchange( exchange_state& ex, const asset& input );
};

struct balance_key {
   account_name owner;
   symbol_type  symbol;

   friend bool operator < ( const balance_key& a, const balance_key& b ) {
      return std::tie( a.owner, a.symbol ) < std::tie( b.owner, b.symbol );
   }
   friend bool operator == ( const balance_key& a, const balance_key& b ) {
      return std::tie( a.owner, a.symbol ) == std::tie( b.owner, b.symbol );
   }
};

real_type fee = 1;//.9995;

int64_t maxtrade = 20000ll;

struct exchange_state {
   token_type  supply;
   symbol_type symbol = exchange_symbol;

   connector  base;
   connector  quote;

   void transfer( account_name user, asset q ) {
      output[balance_key{user,q.symbol}] += q.amount;
   }
   map<balance_key, token_type> output;
   vector<margin>               margins;
};

/*
void  connector::borrow( exchange_state& ex, account_name user,
                         asset amount_to_borrow,
                         asset collateral,
                         user_margin& marg ) {
   FC_ASSERT( amount_to_borrow.amount < balance.amount, "attempt to borrow too much" );
   lent.amount += amount_to_borrow.amount;
   balance.amount -= amount_to_borrow.amount;
   ex.transfer( user, amount_to_borrow );

   marg.collateral.amount += collateral.amount;
   marg.lent.amount += amount_to_borrow.amount;
   auto p = marg.price();

   if( collateral.symbol == ex.symbol ) {
      if( p > ex_margin.least_collateralized_rate )
         ex_margin.least_collateralized_rate = p;
   }
   else if( collateral.symbol == peer_margin.collateral.symbol ) {
      if( p > peer_margin.least_collateralized_rate )
         peer_margin.least_collateralized_rate = p;
   }
}
*/

asset connector::convert_to_exchange( exchange_state& ex, const asset& input ) {

   real_type R(ex.supply);
   real_type S(balance.amount+input.amount);
   real_type F(weight);
   real_type T(input.amount);
   real_type ONE(1.0);

   auto E = R * (ONE - std::pow( ONE + T / S, F) );

   //auto real_issued = real_type(ex.supply) * (sqrt_safe( 1.0 + (real_type(input.amount) / (balance.amount+input.amount))) - 1.0);
   //auto real_issued = real_type(ex.supply) * (std::pow( 1.0 + (real_type(input.amount) / (balance.amount+input.amount)), weight) - real_type(1.0));
   //auto real_issued = R * (std::pow( ONE + (T / S), F) - ONE);

   //wdump((double(E))(double(real_issued)));
   token_type issued = -E; //real_issued;

   ex.supply      += issued;
   balance.amount += input.amount;

   return asset{ issued, exchange_symbol };
}

asset connector::convert_from_exchange( exchange_state& ex, const asset& input ) {

   real_type R(ex.supply - input.amount);
   real_type S(balance.amount);
   real_type F(weight);
   real_type E(input.amount);
   real_type ONE(1.0);

   real_type T = S * (std::pow( ONE + E/R, ONE/F) - ONE);

   /*
   real_type base = real_type(1.0) + ( real_type(input.amount) / real_type(ex.supply-input.amount));
   auto out = (balance.amount * ( std::pow(base,1.0/weight) - real_type(1.0) ));
   */
   auto out = T;

//   edump((double(out-T))(double(out))(double(T)));

   ex.supply -= input.amount;
   balance.amount -= token_type(out);
   return asset{ token_type(out), balance.symbol };
}

void besio_assert( bool test, const string& msg ) {
   if( !test ) throw std::runtime_error( msg );
}

void print_state( const exchange_state& e );

/**
 *  Given the current state, calculate the new state
 */
exchange_state convert( const exchange_state& current,
                        account_name user,
                        asset        input,
                        asset        min_output,
                        asset*       out = nullptr) {

  besio_assert( min_output.symbol != input.symbol, "cannot convert" );

  exchange_state result(current);

  asset initial_output = input;

  if( input.symbol != exchange_symbol ) {
     if( input.symbol == result.base.balance.symbol ) {
        initial_output = result.base.convert_to_exchange( result, input );
     }
     else if( input.symbol == result.quote.balance.symbol ) {
        initial_output = result.quote.convert_to_exchange( result, input );
     }
     else besio_assert( false, "invalid symbol" );
  } else {
     if( min_output.symbol == result.base.balance.symbol ) {
        initial_output = result.base.convert_from_exchange( result, initial_output );
     }
     else if( min_output.symbol == result.quote.balance.symbol ) {
        initial_output= result.quote.convert_from_exchange( result, initial_output );
     }
     else besio_assert( false, "invalid symbol" );
  }

  asset final_output = initial_output;

//  std::cerr << "\n\nconvert " << input.amount << " "<< input.symbol << "  =>  " << final_output.amount << " " << final_output.symbol << "  final: " << min_output.symbol << " \n";

  result.output[ balance_key{user,final_output.symbol} ] += final_output.amount;
  result.output[ balance_key{user,input.symbol} ] -= input.amount;

  if( min_output.symbol != final_output.symbol ) {
    return convert( result, user, final_output, min_output, out );
  }

  if( out ) *out = final_output;
  return result;
}

/*  VALIDATE MARGIN ALGORITHM
 *
 *  Given an initial condition, verify that all margin positions can be filled.
 *
 *  Assume 3 assets, B, Q, and X and the notation LENT-COLLAT we get the following
 *  pairs:
 *
 *  B-X
 *  B-A
 *  A-X
 *  A-B
 *  X-A
 *  X-B
 *
 *  We assume that pairs of the same lent-type have to be simultainously filled,
 *  as filling one could make it impossible to fill the other.
 *
 *
void validate_margin( exchange_state& e ) {
   for( const auto& pos : e.margins ) {
      token_type min_collat = pos.lent.amount * pos.least_collateralized_rate;
      asset received;
      e = convert( e, "user", asset{ min_collat, pos.first.collat }, pos.lent, &received );
      FC_ASSERT( received > pos.lent.amount, "insufficient collateral" );

      received.amount -= pos.lent.amount;
      e = convert( e, "user", received, asset{ token_type(0), pos.collateral_symbol} );
   }
}
*/

/**
 *  A user has Collateral C and wishes to borrow B, so we give user B
 *  provided that C is enough to buy B back after removing it from market and
 *  that no margin calls would be triggered.
 */
exchange_state borrow( const exchange_state& current, account_name user,
                       asset amount_to_borrow,
                       asset collateral_provided ) {
    FC_ASSERT( amount_to_borrow.symbol != collateral_provided.symbol );

    /// lookup the margin position for user
    ///   update user's margin position
    ///   update least collateralized margin position on state
    ///   remove amount_to_borrow from exchange
    ///   lock collateral for user
    /// simulate complete margin calls
    return exchange_state();
}

exchange_state cover( const exchange_state& current, account_name user,
                      asset amount_to_cover, asset collateral_to_cover_with )
{
   /// lookup existing position for user/debt/collat
   /// verify collat > collateral_to_cover_with
   /// sell collateral_to_cover_with for debt on market
   /// reduce debt by proceeds
   /// add proceeds to connector
   //     - if borrowed from user, reduce borrowed from user
   /// calculate new call price and update least collateralized position
   /// simulate complete margin calls
   return exchange_state();
}

exchange_state lend( const exchange_state& current, account_name lender,
                     asset asset_to_lend ) {
   /// add to pool of funds available for lending and buy SHARES in
   ///  interest pool at current rate.
   return exchange_state();
}

exchange_state unlend( const exchange_state& current, account_name lender,
                     asset asset_to_lend ) {
    /// sell shares in interest pool at current rate
    /// this is permitable so long as total borrowed from users remains less than
    /// total available to lend. Otherwise, margin is called on the least
    /// collateralized position.
   return exchange_state();
}

void print_state( const exchange_state& e ) {
   std::cerr << "\n-----------------------------\n";
   std::cerr << "supply: " <<  e.supply  << "\n";
   std::cerr << "base: " <<  e.base.balance.amount << " " << e.base.balance.symbol << "\n";
   std::cerr << "quote: " <<  e.quote.balance.amount << " " << e.quote.balance.symbol << "\n";

   for( const auto& item : e.output ) {
      cerr << item.first.owner << "  " << item.second << " " << item.first.symbol << "\n";
   }
   std::cerr << "\n-----------------------------\n";
}

int main( int argc, char** argv ) {
 //  std::cerr << "root: " << double(root.numerator())/root.denominator() << "\n";

   exchange_state state;
   state.supply = 100000000000ll;
   //state.base.weight  = state.total_weight / 2.;
   state.base.balance.amount = 100000000;
   state.base.balance.symbol = "USD";
   state.base.weight = .49;
   //state.quote.weight = state.total_weight / 2.;
   state.quote.balance.amount = state.base.balance.amount;
   state.quote.balance.symbol = "BTC";
   state.quote.weight = .51;

   print_state( state );

   //state = convert( state, "dan", asset{ 100, "USD"}, asset{ 0, "BTC" } );

   auto start = fc::time_point::now();
   for( uint32_t i = 0; i < 10000; ++i ) {
     if( rand() % 2 == 0 )
        state = convert( state, "dan", asset{ token_type(uint32_t(rand())%maxtrade), "USD"}, asset{ 0, "BTC" } );
     else
        state = convert( state, "dan", asset{ token_type(uint32_t(rand())%maxtrade), "BTC"}, asset{ 0, "USD" } );
   }
   for( const auto& item : state.output ) {
      if( item.second > 0 ) {
         if( item.first.symbol == "USD" )
           state = convert( state, "dan", asset{ item.second, item.first.symbol}, asset{ 0, "BTC" } );
         else
           state = convert( state, "dan", asset{ item.second, item.first.symbol}, asset{ 0, "USD" } );
        break;
      }
   }
   print_state( state );

   auto end = fc::time_point::now();
   wdump((end-start));
   /*
   auto new_state = convert( state, "dan", asset{ 100, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 100, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 100, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 100, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 100, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 100, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 100, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 100, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 100, "USD"}, asset{ 0, "BTC" } );

        new_state = convert( new_state, "dan", asset{ 100, "BTC"}, asset{ 0, "USD" } );
        new_state = convert( new_state, "dan", asset{ 100, "BTC"}, asset{ 0, "USD" } );
        new_state = convert( new_state, "dan", asset{ 100, "BTC"}, asset{ 0, "USD" } );
        new_state = convert( new_state, "dan", asset{ 100, "BTC"}, asset{ 0, "USD" } );
        new_state = convert( new_state, "dan", asset{ 100, "BTC"}, asset{ 0, "USD" } );
        new_state = convert( new_state, "dan", asset{ 100, "BTC"}, asset{ 0, "USD" } );
        new_state = convert( new_state, "dan", asset{ 100, "BTC"}, asset{ 0, "USD" } );
        new_state = convert( new_state, "dan", asset{ 92.5-0.08-.53, "BTC"}, asset{ 0, "USD" } );
        new_state = convert( new_state, "dan", asset{ 100, "BTC"}, asset{ 0, "USD" } );
        */

        //new_state = convert( new_state, "dan", asset{ 442+487-733+280+349+4.493+62.9, "BTC"}, asset{ 0, "USD" } );
   /*
   auto new_state = convert( state, "dan", asset{ 500, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 500, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 442+487, "BTC"}, asset{ 0, "USD" } );
        */
        /*
        new_state = convert( new_state, "dan", asset{ 487, "BTC"}, asset{ 0, "USD" } );
        new_state = convert( new_state, "dan", asset{ 442, "BTC"}, asset{ 0, "USD" } );
        */
        //new_state = convert( new_state, "dan", asset{ 526, "BTC"}, asset{ 0, "USD" } );
        //new_state = convert( new_state, "dan", asset{ 558, "BTC"}, asset{ 0, "USD" } );
        //new_state = convert( new_state, "dan", asset{ 1746, "BTC"}, asset{ 0, "USD" } );
        /*
        new_state = convert( new_state, "dan", asset{ 526, "BTC"}, asset{ 0, "USD" } );
        new_state = convert( new_state, "dan", asset{ 500, "USD"}, asset{ 0, "EXC" } );
        new_state = convert( new_state, "dan", asset{ 500, "BTC"}, asset{ 0, "EXC" } );
        new_state = convert( new_state, "dan", asset{ 10, "EXC"}, asset{ 0, "USD" } );
        new_state = convert( new_state, "dan", asset{ 10, "EXC"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 500, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 500, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 500, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 500, "USD"}, asset{ 0, "BTC" } );
        new_state = convert( new_state, "dan", asset{ 2613, "BTC"}, asset{ 0, "USD" } );
        */

   /*
   auto new_state = convert( state, "dan", asset{ 10, "EXC"}, asset{ 0, "USD" } );

   print_state( new_state );

   new_state = convert( state, "dan", asset{ 10, "EXC"}, asset{ 0, "BTC" } );
   print_state( new_state );
   new_state = convert( new_state, "dan", asset{ 10, "EXC"}, asset{ 0, "USD" } );
   print_state( new_state );

   //new_state = convert( new_state, "dan", asset{ 52, "USD"}, asset{ 0, "EXC" } );
   */

   return 0;
}

#if 0

0. if( margin_fault )
     Convert Least Collateral
     if( margin fault ))
        defer

if( margin_fault ) assert( false, "busy calling" );

1. Fill Incoming Order
2. Check Counter Order
3. if( margin fault )
   Defer Trx to finish margin call

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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_GEOMETRY_ALGORITHM_REMOVE_WITHIN_DISTANCE_HPP
#define BOOST_GEOMETRY_ALGORITHM_REMOVE_WITHIN_DISTANCE_HPP

#include <algorithm>

#include <boost/range.hpp>

#include <boost/geometry/algorithms/distance.hpp>
#include <boost/geometry/algorithms/detail/point_on_border.hpp>

#include <boost/geometry/core/interior_rings.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace buffer
{

template<typename Geometry, typename GeometryInput, typename T>
struct remove_false_ring_predicate
{
private :
    typedef typename point_type<Geometry>::type point_type;

    GeometryInput const& m_input;
    T m_distance;

public :
    remove_false_ring_predicate(GeometryInput const& input,
            T const& distance)
        : m_input(input)
        , m_distance(distance)
    {
    }

    inline bool operator()(Geometry const& geometry)
    {
        point_type point, point_input;
        if (geometry::point_on_border(point, geometry, false)
            && geometry::point_on_border(point_input, m_input, false))
        {
            if (m_distance > T())
            {
                // If the input is within the output, it is acceptable
                if (geometry::within(point_input, geometry))
                {
                    return false;
                }

                // Check the distance to the input geometry
                // (for a polygon, this is: the distance from OUTSIDE
                // to the border
                T d = geometry::distance(point, m_input);
                if (d < m_distance)
                {
                    return true;
                }
            }
        }
        return false;
    }
};

template<typename Polygon, typename Geometry>
struct polygon_remove_false_rings
{
    template <typename T>
    static inline void apply(Polygon& polygon, Geometry const& input, T const& distance)
    {
        interior_rings(polygon).erase
            (
                std::remove_if
                    (
                        boost::begin(interior_rings(polygon)),
                        boost::end(interior_rings(polygon)),
                        remove_false_ring_predicate
                            <
                                typename ring_type<Polygon>::type,
                                Geometry,
                                T
                            >(input, distance)
                    ),
                boost::end(interior_rings(polygon))
            );

    }
};

template<typename Collection, typename Geometry, typename T>
inline void collection_remove_within_distance(Collection& dissolved,
            Geometry const& input,
            T const& distance)
{
    typedef typename boost::range_value<Collection>::type polygon_type;
    // 1: remove all polygons which are false positive (positive,
    //    but too close to the input geometry
    dissolved.erase
        (
            std::remove_if(boost::begin(dissolved), boost::end(dissolved),
                remove_false_ring_predicate
                    <
                        polygon_type,
                        Geometry,
                        T
                    >(input, distance)),
            boost::end(dissolved)
        );

    // 2: within all polygons, remove false negative interior rings
    for (typename boost::range_iterator<Collection>::type
        it = boost::begin(dissolved);
        it != boost::end(dissolved);
        ++it)
    {
        polygon_remove_false_rings
            <
                typename boost::range_value<Collection>::type,
                Geometry
            >::apply(*it, input, distance);
    }
}

}} // namespace detail::buffer

#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHM_REMOVE_WITHIN_DISTANCE_HPP


#pragma once
#include <math.h>       /* log */
#include <boost/uuid/uuid.hpp>
#include "UncertainIntervals/IntervalAnalysis.hpp"
#include <autoppl/expression/model/glue_node.hpp>
#include <map>
#include <set>
#include <iostream>
#include <utility>

using boost::uuids::uuid;

namespace globals {

std::map<uuid, std::set<uuid>> dependencies;
std::map<uuid, DeterministicInterval> intervals;
std::map<uuid, DeterministicInterval> single_likelihood_intervals; //max value of a SINGLE likelihood
std::map<uuid, DeterministicInterval> likelihood_intervals;	//interval bounding the SUM of all log likelihoods over ALL data samples
std::map<uuid, DeterministicInterval> biggest_intervals;

/*
void printDependencies(){
	for(auto elem : dependencies)
	{
	   std::cout << elem.first << " "   << "\n"; // << elem.second
	}
}*/

void printIntervals(){
	for(auto elem : intervals)
	{
	   std::cout << elem.first << " ["  << elem.second.lower << "," << elem.second.upper << "]" << "\n";
	}
}

void printSingleLikelihoodIntervals(){
	for(auto elem : single_likelihood_intervals)
	{
	   std::cout << elem.first << " ["  << elem.second.lower << "," << elem.second.upper << "]" << "\n";
	}
}

void printLikelihoodIntervals(){
	for(auto elem : likelihood_intervals)
	{
	   std::cout << elem.first << " ["  << elem.second.lower << "," << elem.second.upper << "]" << "\n";
	}
}

void printBiggestIntervals(){
	for(auto elem : biggest_intervals)
	{
	   std::cout << elem.first << " ["  << elem.second.lower << "," << elem.second.upper << "]" << "\n";
	}
}

} //end of globals::

namespace analysis {

//for a given number returns the number of integer bits needed to represent it
int numIntBits(double x){
	return std::max(1.,std::ceil(std::log2(x)));
}

int getValueTSize(){
	DeterministicInterval max_size_interval(0.,0.);

	for(auto elem : globals::intervals)
	{
	   //max_size_interval.print();
	   max_size_interval = union_(max_size_interval,elem.second);
	}

	//max_size_interval.print();
	double biggest = std::max(std::abs(max_size_interval.lower),std::abs(max_size_interval.upper));
	return numIntBits(biggest);
}

int getDistValueTSize(){
	DeterministicInterval max_size_interval(0.,0.);

	for(auto elem : globals::likelihood_intervals)
	{
	   max_size_interval = add(max_size_interval,elem.second);	//since we have to sum all log-likelihoods together
	}

	for(auto elem2 : globals::biggest_intervals)
	{
	   max_size_interval = union_(max_size_interval,elem2.second);	//since we union with the biggest interval we could have seen during intermediate computation steps
	}

	double biggest = std::max(std::abs(max_size_interval.lower),std::abs(max_size_interval.upper));

	return numIntBits(biggest);
}

int getSingleLikelihoodDistValueTSize(){
	DeterministicInterval max_size_interval(0.,0.);

	for(auto elem2 : globals::single_likelihood_intervals)
	{
	   max_size_interval = union_(max_size_interval,elem2.second);	//since we union with the biggest interval we could have seen during intermediate computation steps
	}
	max_size_interval.print();

	double biggest = std::max(std::abs(max_size_interval.lower),std::abs(max_size_interval.upper));

	return numIntBits(biggest);
}

template <class LHSNodeType, class RHSNodeType>
std::pair<int,int> getBothSizes(ppl::expr::GlueNode<LHSNodeType, RHSNodeType>& model){
	model.getInterval();
	model.getLikelihoodInterval();
	int value_t = getValueTSize();
	int dist_value_t = getDistValueTSize();
	int single_likelihood_dist_value_t = getSingleLikelihoodDistValueTSize();

	std::cout << "Value_t needed: " << value_t << std::endl;
	std::cout << "Dist_Value_t needed: " << dist_value_t << std::endl;
	std::cout << "Single Likelihood Dist_Value_t needed (for Rem's optimization): " << single_likelihood_dist_value_t << std::endl;
	return std::pair<int,int>({value_t,dist_value_t});
}

}


#include "vikit/cameras/camera_factory.h"

#include <Eigen/Geometry>
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#include <yaml-cpp/yaml.h>
#pragma diagnostic pop
#include <vikit/cameras.h>

#include "vikit/cameras/atan_distortion.h"
#include "vikit/cameras/camera_geometry.h"
#include "vikit/cameras/equidistant_distortion.h"
#include "vikit/cameras/no_distortion.h"
#include "vikit/cameras/pinhole_projection.h"
#include "vikit/cameras/radial_tangential_distortion.h"

namespace vk {
namespace cameras {
namespace factory {

CameraGeometryBase::Ptr makePinholeCamera(
    const Eigen::VectorXd& intrinsics, uint32_t width, uint32_t height)
{
  CameraGeometryBase::Ptr cam = std::make_shared<PinholeGeometry>(
        width, height,
        PinholeProjection<NoDistortion>(
          intrinsics, NoDistortion()));
  return cam;
}

CameraGeometryBase::Ptr loadFromYAML(
    const std::string& filename,
    const std::string& cam_name)
{
  YAML::Node data = YAML::LoadFile(filename);
  if(!data)
  {
    std::cerr << "Could not load camera from file: " << filename << std::endl;
    return nullptr;
  }

  // The YAML File may contain multiple cameras, specify which one to load
  data = data[cam_name];
  if(!data)
  {
    std::cerr << "Camera with name '"<< cam_name << "' does not exist in "
              << "file: " << filename << std::endl;
    return nullptr;
  }

  // load imu camera transformation
  Transformation T_body_cam; // identity by default
  if(data["T_body_cam"].IsDefined())
  {
    T_body_cam = Transformation(
        Eigen::Quaterniond(
            data["T_body_cam"]["qw"].as<double>(),
            data["T_body_cam"]["qx"].as<double>(),
            data["T_body_cam"]["qy"].as<double>(),
            data["T_body_cam"]["qz"].as<double>()),
        Eigen::Vector3d(
            data["T_body_cam"]["tx"].as<double>(),
            data["T_body_cam"]["ty"].as<double>(),
            data["T_body_cam"]["tz"].as<double>()));
  }
  else if(data["T_cam_body"].IsDefined())
  {
    T_body_cam = Transformation(
        Eigen::Quaterniond(
            data["T_cam_body"]["qw"].as<double>(),
            data["T_cam_body"]["qx"].as<double>(),
            data["T_cam_body"]["qy"].as<double>(),
            data["T_cam_body"]["qz"].as<double>()),
        Eigen::Vector3d(
            data["T_cam_body"]["tx"].as<double>(),
            data["T_cam_body"]["ty"].as<double>(),
            data["T_cam_body"]["tz"].as<double>())).inverse();
  }

  // load camera
  CameraGeometryBase::Ptr cam;
  std::string cam_model = data["cam_model"].as<std::string>();
  if(cam_model == "PinholeRadialTangential")
  {
    double d0 = data["cam_d0"].IsDefined() ? data["cam_d0"].as<double>() : 0.0;
    double d1 = data["cam_d1"].IsDefined() ? data["cam_d1"].as<double>() : 0.0;
    double d2 = data["cam_d2"].IsDefined() ? data["cam_d2"].as<double>() : 0.0;
    double d3 = data["cam_d3"].IsDefined() ? data["cam_d3"].as<double>() : 0.0;
    if(d0 == 0 && d1 == 0 && d2 == 0 && d3 == 0)
    {
      cam_model = "PinholeNoDistortion";
    }
    else
    {
      cam.reset(new PinholeRadTanGeometry(
                  data["cam_width"].as<int>(),
                  data["cam_height"].as<int>(),
                  PinholeProjection<RadialTangentialDistortion>(
                    data["cam_fx"].as<double>(),
                    data["cam_fy"].as<double>(),
                    data["cam_cx"].as<double>(),
                    data["cam_cy"].as<double>(),
                    RadialTangentialDistortion(d0, d1, d2, d3))));
    }
  }
  if(cam_model == "PinholeNoDistortion")
  {
    cam.reset(new PinholeGeometry(
                data["cam_width"].as<int>(),
                data["cam_height"].as<int>(),
                PinholeProjection<NoDistortion>(
                  data["cam_fx"].as<double>(),
                  data["cam_fy"].as<double>(),
                  data["cam_cx"].as<double>(),
                  data["cam_cy"].as<double>(),
                  NoDistortion())));
  }
  else if(cam_model == "PinholeEquidistant")
  {
    cam.reset(new PinholeEquidistantGeometry(
                data["cam_width"].as<int>(),
                data["cam_height"].as<int>(),
                PinholeProjection<EquidistantDistortion>(
                  data["cam_fx"].as<double>(),
                  data["cam_fy"].as<double>(),
                  data["cam_cx"].as<double>(),
                  data["cam_cy"].as<double>(),
                  EquidistantDistortion(
                    data["cam_d0"].as<double>(),
                    data["cam_d1"].as<double>(),
                    data["cam_d2"].as<double>(),
                    data["cam_d3"].as<double>()))));
  }
  else if(cam_model == "PinholeAtan")
  {
    cam.reset(new PinholeAtanGeometry(
                data["cam_width"].as<int>(),
                data["cam_height"].as<int>(),
                PinholeProjection<AtanDistortion>(
                  data["cam_fx"].as<double>(),
                  data["cam_fy"].as<double>(),
                  data["cam_cx"].as<double>(),
                  data["cam_cy"].as<double>(),
                  AtanDistortion(
                    data["cam_d0"].as<double>()))));
  }
  else if(cam_model == "Pinhole")
  {
    std::cerr << "ERROR: Camera Factory: Select between 'PinholeNoDistortion' "
              << "and 'PinholeRadialTangential'." << std::endl;
  }
  else if(cam_model == "OCam")
  {
    std::cerr << "ERROR: Camera Factory: OCam model not yet implemented."
              << std::endl;
  }
  else if(cam_model != "PinholeRadialTangential")
  {
    std::cerr << "ERROR: Camera Factory: Camera model '"<< cam_model
              << "' doesn't exist." << std::endl;
  }

  cam->setLabel(cam_name);

  return cam;
}

} // namespace factory
} // namespace cameras
} // namespace vk


#include <stdio.h>
#include <rrt_planners/RandomNumbers.h>
#include <boost/random/lagged_fibonacci.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread/once.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/math/constants/constants.hpp>

/// @cond IGNORE
namespace
{
/// We use a different random number generator for the seeds of the
/// other random generators. The root seed is from the number of
/// nano-seconds in the current time, or given by the user.
class RNGSeedGenerator
{
public:
  RNGSeedGenerator()
    : someSeedsGenerated_(false)
    , firstSeed_((boost::uint32_t)(boost::posix_time::microsec_clock::universal_time() -
                                   boost::posix_time::ptime(boost::date_time::min_date_time))
                     .total_microseconds())
    , sGen_(firstSeed_)
    , sDist_(1, 1000000000)
    , s_(sGen_, sDist_)
  {
  }

  boost::uint32_t firstSeed()
  {
    boost::mutex::scoped_lock slock(rngMutex_);
    return firstSeed_;
  }

  void setSeed(boost::uint32_t seed)
  {
    boost::mutex::scoped_lock slock(rngMutex_);
    if (seed > 0)
    {
      if (someSeedsGenerated_)
      {
        printf("ERROR. Random number generation already started. Changing seed now will not lead to deterministic sampling.");
      }
      else
      {
        // In this case, since no seeds have been generated yet, so we remember this seed
        // as the first one.
        firstSeed_ = seed;
      }
    }
    else
    {
      if (someSeedsGenerated_)
      {
        printf("WARN. Random generator seed cannot be 0. Ignoring seed.");
        return;
      }
      else
      {
        printf("WARN. Random generator seed cannot be 0. Using 1 instead.");
        seed = 1;
      }
    }
    sGen_.seed(seed);
  }

  boost::uint32_t nextSeed()
  {
    boost::mutex::scoped_lock slock(rngMutex_);
    someSeedsGenerated_ = true;
    return s_();
  }

private:
  bool someSeedsGenerated_;
  boost::uint32_t firstSeed_;
  boost::mutex rngMutex_;
  boost::lagged_fibonacci607 sGen_;
  boost::uniform_int<> sDist_;
  boost::variate_generator<boost::lagged_fibonacci607&, boost::uniform_int<> > s_;
};

static boost::once_flag g_once = BOOST_ONCE_INIT;
static boost::scoped_ptr<RNGSeedGenerator> g_RNGSeedGenerator;

void initRNGSeedGenerator()
{
  g_RNGSeedGenerator.reset(new RNGSeedGenerator());
}

RNGSeedGenerator& getRNGSeedGenerator()
{
  boost::call_once(&initRNGSeedGenerator, g_once);
  return *g_RNGSeedGenerator;
}
}  // namespace
/// @endcond

boost::uint32_t RNG::getSeed()
{
  return getRNGSeedGenerator().firstSeed();
}

void RNG::setSeed(boost::uint32_t seed)
{
  getRNGSeedGenerator().setSeed(seed);
}

RNG::RNG()
  : generator_(getRNGSeedGenerator().nextSeed())
  , uniDist_(0, 1)
  , normalDist_(0, 1)
  , uni_(generator_, uniDist_)
  , normal_(generator_, normalDist_)
{
}

double RNG::halfNormalReal(double r_min, double r_max, double focus)
{
  assert(r_min <= r_max);

  const double mean = r_max - r_min;
  double v = gaussian(mean, mean / focus);

  if (v > mean)
    v = 2.0 * mean - v;
  double r = v >= 0.0 ? v + r_min : r_min;
  return r > r_max ? r_max : r;
}

int RNG::halfNormalInt(int r_min, int r_max, double focus)
{
  int r = (int)floor(halfNormalReal((double)r_min, (double)(r_max) + 1.0, focus));
  return (r > r_max) ? r_max : r;
}

// From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III,
//       pg. 124-132
void RNG::quaternion(double value[4])
{
  double x0 = uni_();
  double r1 = sqrt(1.0 - x0), r2 = sqrt(x0);
  double t1 = 2.0 * boost::math::constants::pi<double>() * uni_(),
         t2 = 2.0 * boost::math::constants::pi<double>() * uni_();
  double c1 = cos(t1), s1 = sin(t1);
  double c2 = cos(t2), s2 = sin(t2);
  value[0] = s1 * r1;
  value[1] = c1 * r1;
  value[2] = s2 * r2;
  value[3] = c2 * r2;
}

// From Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning, by James
// Kuffner, ICRA 2004
void RNG::eulerRPY(double value[3])
{
  value[0] = boost::math::constants::pi<double>() * (-2.0 * uni_() + 1.0);
  value[1] = acos(1.0 - 2.0 * uni_()) - boost::math::constants::pi<double>() / 2.0;
  value[2] = boost::math::constants::pi<double>() * (-2.0 * uni_() + 1.0);
}