/* Copyright (C) 2011 Ion Torrent Systems, Inc. All Rights Reserved */

#include <armadillo>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <cinttypes>

#include "ByteSwapUtils.h"
#include "LSRowImageProcessor.h"

#define N_CORRECTION_GROUPS 8
#define SKIP_ROWS_AT_END    10

#define PINNED_HIGH_LIMIT   16373
#define PINNED_LOW_LIMIT    10

#define LSROWIMAGE_MAGIC_VALUE    0xFF115E3A

using namespace arma;
// generate an electrical cross-talk correction from the lsrow image
// if the file pointed to by lsimg_path does not exist, the method returns NULL
// If lsimg_path does exist, then a correction is generated and a pointer to
// a ChannelXTCorrection object with all the relevant information is returned
ChannelXTCorrection *LSRowImageProcessor::GenerateCorrection(const char *lsimg_path)
{
    uint32_t magic_value;
    uint32_t file_version;
    uint32_t rows = 0;
    uint32_t cols = 0;
    int nread;

    // the lsrowimage file contains:
    // uint32 magic value (0xFF115E3A)
    // uint32 file version number (0+)
    // uint32 rows
    // uint32 columns
    // uint16[rows*columns] high-speed and low-speed reference data, in row major order
    //  ..with the first row in the image being a high-speed collected row
    //    and the next row in the image being the low-speed reference for the previous high-speed row data

    FILE *lsrowfile;

    lsrowfile = fopen(lsimg_path,"rb");

    // if we have trouble opening the file, just return NULL
    if(lsrowfile == NULL)
        return NULL;

    nread = fread(&magic_value,sizeof(int32_t),1,lsrowfile);
    if(nread != 1)
    {
        printf("Ivalid lsrowfile detected\n");
        fclose(lsrowfile);
        return NULL;
    }
    magic_value = BYTE_SWAP_4(magic_value);

    if(magic_value != LSROWIMAGE_MAGIC_VALUE)
    {
        printf("Ivalid lsrowfile detected\n");
        fclose(lsrowfile);
        return NULL;
    }

    nread = fread(&file_version,sizeof(int32_t),1,lsrowfile);
    if(nread != 1)
    {
        printf("Ivalid lsrowfile detected\n");
        fclose(lsrowfile);
        return NULL;
    }
    file_version = BYTE_SWAP_4(file_version);

    if(file_version != 0)
    {
        printf("Unsupported lsrowimage file version\n");
        fclose(lsrowfile);
        return NULL;
    }

    nread = fread(&rows,sizeof(int32_t),1,lsrowfile);
    if(nread != 1)
    {
        printf("Ivalid lsrowfile detected\n");
        fclose(lsrowfile);
        return NULL;
    }
    rows = BYTE_SWAP_4(rows);

    nread = fread(&cols,sizeof(int32_t),1,lsrowfile);
    if(nread != 1)
    {
        printf("Ivalid lsrowfile detected\n");
        fclose(lsrowfile);
        return NULL;
    }
    cols = BYTE_SWAP_4(cols);

    int tot_pts = rows*cols;

    printf("reading lsrowfile with %d rows and %d columns\n",rows,cols);
    uint16_t *img = new uint16_t[tot_pts];

    nread = fread(img,sizeof(uint16_t),tot_pts,lsrowfile);
    if(nread != tot_pts)
    {
        printf("Ivalid lsrowfile detected\n");
        delete [] img;
        fclose(lsrowfile);
        return NULL;
    }

    // byte swap the image
    for(int i=0;i < tot_pts;i++)
        img[i] = BYTE_SWAP_2(img[i]);

    // this version of the code generates a correction for columns-modulo eight. (it generates a correction
    // for columns that belong to one of eight different groups, where membership is determined from
    // (column % 8))
    ChannelXTCorrection *xtptr = new ChannelXTCorrection();
    float *pvects = xtptr->AllocateVectorStorage(N_CORRECTION_GROUPS,nLen);
    float **vect_ptrs = xtptr->AllocateVectorPointerStorage(N_CORRECTION_GROUPS);
    xtptr->SetVectorIndicies(indicies,nLen);

    bool correction_valid = true;

    for(int i=0;i < N_CORRECTION_GROUPS;i++)
    {
        vect_ptrs[i] = pvects+nLen*i;
        if (GenerateGroupCorrection(i,vect_ptrs[i],rows,cols,img) == false)
        {
            correction_valid = false;
            break;
        }
    }

    delete [] img;
    fclose(lsrowfile);

    if(!correction_valid)
    {
        printf("Unable to compute valid correction\n");
        delete xtptr;
        return NULL;
    }

    return xtptr;
}

// generates a set of correction coefficients for a group of columns
// this basically solves the matrix equation Ax=B for x, where A are the measured high-speed
// pixel values and B are the low-speed measured pixels values.  The special image collected on the PGM
// contains pairs of high-speed and low-speed rows for the same pixels that can be used to populate
// these matricies
//
// As written, A is a nLen column matrix, with many rows in it (one for each example pixel we extract from the data)
// and B is an nLen row vector, again with one row per example pixel we extract from the data.
// When the equation is solved, both sides are multiplied by A-tranpose, which turns the left side into a square
// nLen by nLen matrix, and the rhs into an nLen row vector.  In order to facilitate ease of adding example pixels to
// the equations sequentially, A and B are not computed, and instead lhs = (A-transpose x A) and rhs = (A-transpose x B)
// are computed directly from the low-level data, and the solution is determined from lhs x = rhs
bool LSRowImageProcessor::GenerateGroupCorrection(int group_num,float *vect_output,int rows,int cols,uint16_t *img)
{
    double lhs[nLen*nLen];
    double rhs[nLen];

    memset(lhs,0,sizeof(lhs));
    memset(rhs,0,sizeof(rhs));

    // the image contains some invalid rows at the end that should be skipped
    int row_limit = rows - SKIP_ROWS_AT_END;

    int mcnt = 0;

    for(int column = group_num;column < cols;column += N_CORRECTION_GROUPS)
    {
        // make sure we have enough space on the left and right-hand sides to use this particular column
        if (((column + indicies[0]) < 0) || ((column + indicies[nLen-1]) >= cols))
            continue;

        // every other row is the start of a pair of rows, one high-speed and one reference
        for(int row = 0;row < row_limit;row += 2)
        {
            double amat[nLen];
            bool skip = false;

            uint16_t *hsrow = &img[row*cols];
            uint16_t *lsrow = &img[(row+1)*cols];

            // get the data points that are to be added to the matrix, making sure to filter out entries
            // that might reference pinned values
            for(int i=0;i < nLen;i++)
            {
                uint16_t temp = hsrow[indicies[i]+column];

                if((temp < PINNED_LOW_LIMIT) || (temp > PINNED_HIGH_LIMIT))
                {
                    skip = true;
                    break;
                }
                amat[i] = (double)temp;

                temp = lsrow[indicies[i]+column];
                if((temp < PINNED_LOW_LIMIT) || (temp > PINNED_HIGH_LIMIT))
                {
                    skip = true;
                    break;
                }
            }

            // if everything checks out, add this data into the matrix equation
            if(!skip)
            {
                mcnt++;
                AccumulateMatrixData(lhs,rhs,amat,(double)(lsrow[column]));
            }
        }
    }

    Mat<double> lhs_matrix(nLen,nLen);
    Col<double> rhs_vector(nLen);
    Col<double> coeffs(nLen);
    bool result_ok = true;

    for(int col=0;col < nLen;col++)
        for(int row=0;row <= col;row++)
        {
            lhs_matrix(row,col) = lhs[row*nLen+col];
            lhs_matrix(col,row) = lhs[row*nLen+col];
        }

    for(int row=0;row < nLen;row++)
        rhs_vector(row) = rhs[row];

    try {
      //LaSpdMatFactorize(lhs_matrix,lhs_matrix_fact);
      //LaLinearSolve(lhs_matrix_fact,coeffs,rhs_vector);
      coeffs = solve(lhs_matrix,rhs_vector);
    }
    catch (std::runtime_error& le) {
        result_ok = false;
        coeffs.zeros(nLen);
    }

    // make sure derived coefficients are valid
    for(int row=0;row < nLen;row++)
        if(std::isnan(coeffs(row)))
        {
            result_ok = false;
            break;
        }
        else
            vect_output[row] = coeffs(row);

    printf("group %d correction coefficients: ",group_num);
    for(int row=0;row < nLen;row++)
        printf("%11.8lf ",coeffs(row));

    printf("\n");

    return(result_ok);
}

// Adds one example pixel's data into the lhs and rhs matrix and vector
void LSRowImageProcessor::AccumulateMatrixData(double *lhs,double *rhs,double *amat,double bval)
{
    for(int col=0;col < nLen;col++)
        for(int row=0;row <= col;row++)
            lhs[row*nLen+col] += amat[row]*amat[col];

    for(int row=0;row < nLen;row++)
        rhs[row] += amat[row]*bval;
}


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

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

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

/*
 * @file    testDecisionTreeFactor.cpp
 * @brief   unit tests for DiscreteConditional
 * @author  Duy-Nguyen Ta
 * @date Feb 14, 2011
 */

#include <boost/assign/std/map.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/make_shared.hpp>
using namespace boost::assign;

#include <CppUnitLite/TestHarness.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteConditional.h>

using namespace std;
using namespace gtsam;

/* ************************************************************************* */
TEST( DiscreteConditional, constructors)
{
  DiscreteKey X(0, 2), Y(2, 3), Z(1, 2); // watch ordering !

  DiscreteConditional::shared_ptr expected1 = //
      boost::make_shared<DiscreteConditional>(X | Y = "1/1 2/3 1/4");
  EXPECT(expected1);
  EXPECT_LONGS_EQUAL(0, *(expected1->beginFrontals()));
  EXPECT_LONGS_EQUAL(2, *(expected1->beginParents()));
  EXPECT(expected1->endParents() == expected1->end());
  EXPECT(expected1->endFrontals() == expected1->beginParents());

  DecisionTreeFactor f1(X & Y, "0.5 0.4 0.2 0.5 0.6 0.8");
  DiscreteConditional actual1(1, f1);
  EXPECT(assert_equal(*expected1, actual1, 1e-9));

  DecisionTreeFactor f2(X & Y & Z,
      "0.2 0.5 0.3 0.6 0.4 0.7 0.25 0.55 0.35 0.65 0.45 0.75");
  DiscreteConditional actual2(1, f2);
  EXPECT(assert_equal(f2 / *f2.sum(1), *actual2.toFactor(), 1e-9));
}

/* ************************************************************************* */
TEST(DiscreteConditional, constructors_alt_interface) {
  DiscreteKey X(0, 2), Y(2, 3), Z(1, 2);  // watch ordering !

  Signature::Table table;
  Signature::Row r1, r2, r3;
  r1 += 1.0, 1.0;
  r2 += 2.0, 3.0;
  r3 += 1.0, 4.0;
  table += r1, r2, r3;
  auto actual1 = boost::make_shared<DiscreteConditional>(X | Y = table);
  EXPECT(actual1);
  DecisionTreeFactor f1(X & Y, "0.5 0.4 0.2 0.5 0.6 0.8");
  DiscreteConditional expected1(1, f1);
  EXPECT(assert_equal(expected1, *actual1, 1e-9));

  DecisionTreeFactor f2(
      X & Y & Z, "0.2 0.5 0.3 0.6 0.4 0.7 0.25 0.55 0.35 0.65 0.45 0.75");
  DiscreteConditional actual2(1, f2);
  EXPECT(assert_equal(f2 / *f2.sum(1), *actual2.toFactor(), 1e-9));
}

/* ************************************************************************* */
TEST(DiscreteConditional, constructors2) {
  // Declare keys and ordering
  DiscreteKey C(0, 2), B(1, 2);
  DecisionTreeFactor actual(C & B, "0.8 0.75 0.2 0.25");
  Signature signature((C | B) = "4/1 3/1");
  DiscreteConditional expected(signature);
  DecisionTreeFactor::shared_ptr expectedFactor = expected.toFactor();
  EXPECT(assert_equal(*expectedFactor, actual));
}

/* ************************************************************************* */
TEST(DiscreteConditional, constructors3) {
  // Declare keys and ordering
  DiscreteKey C(0, 2), B(1, 2), A(2, 2);
  DecisionTreeFactor actual(C & B & A, "0.8 0.5 0.5 0.2 0.2 0.5 0.5 0.8");
  Signature signature((C | B, A) = "4/1 1/1 1/1 1/4");
  DiscreteConditional expected(signature);
  DecisionTreeFactor::shared_ptr expectedFactor = expected.toFactor();
  EXPECT(assert_equal(*expectedFactor, actual));
}

/* ************************************************************************* */
TEST(DiscreteConditional, Combine) {
  DiscreteKey A(0, 2), B(1, 2);
  vector<DiscreteConditional::shared_ptr> c;
  c.push_back(boost::make_shared<DiscreteConditional>(A | B = "1/2 2/1"));
  c.push_back(boost::make_shared<DiscreteConditional>(B % "1/2"));
  DecisionTreeFactor factor(A & B, "0.111111 0.444444 0.222222 0.222222");
  DiscreteConditional actual(2, factor);
  auto expected = DiscreteConditional::Combine(c.begin(), c.end());
  EXPECT(assert_equal(*expected, actual, 1e-5));
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


//=======================================================================
// Copyright 2012
// Authors: David Doria
//
// 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_concepts.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/serialization/utility.hpp>

#include <boost/graph/directed_graph.hpp> // A subclass to provide reasonable arguments to adjacency_list for a typical directed graph

#include <boost/graph/graph_traits.hpp>

using namespace boost;

int main(int,char*[])
{
  // directed_graph is a subclass of adjacency_list which gives you object oriented access to functions
  // like add_vertex and add_edge, which makes the code easier to understand. However, it hard codes many
  // of the template parameters, so it is much less flexible.

  typedef boost::property<boost::edge_weight_t, double> EdgeWeightProperty;
  typedef boost::directed_graph<boost::no_property, EdgeWeightProperty> Graph;
  Graph g;
  boost::graph_traits<Graph>::vertex_descriptor v0 = g.add_vertex();
  boost::graph_traits<Graph>::vertex_descriptor v1 = g.add_vertex();

    EdgeWeightProperty ew = 3.1;
  g.add_edge(v0, v1, ew);

  write_graphviz(std::cout, g, make_label_writer(get(&Edge::weight, g)));

  return 0;
}


// ======================================================================
/*!
 * \file
 * \brief Regression tests for class NFmiAreaFactory
 */
// ======================================================================

#include "NFmiGdalArea.h"

#include <boost/lexical_cast.hpp>
#include <regression/tframe.h>
#include <stdexcept>
#include <string>

std::string tostr(const NFmiPoint& p)
{
  std::ostringstream out;
  out << std::fixed << "(" << p.X() << "," << p.Y() << ")";
  return out.str();
}

//! Protection against conflicts with global functions
namespace NFmiAreaFactoryTest
{
// ----------------------------------------------------------------------

void create()
{
  {
    // WGS84
    NFmiGdalArea area("FMI", "EPSG:4326", NFmiPoint(20, 60), NFmiPoint(40, 70));
  }

  {
    // YKJ
    NFmiGdalArea area("FMI", "EPSG:2393", NFmiPoint(20, 60), NFmiPoint(40, 70));
  }

  TEST_PASSED();
}

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

void areastr()
{
  NFmiGdalArea area("FMI", "EPSG:2393", NFmiPoint(20, 60), NFmiPoint(40, 70));

  std::string expected = "EPSG:2393";

  std::string result = area.AreaStr();
  if (result != expected) TEST_FAILED("Expected " + expected + ", got " + result + " instead");

  TEST_PASSED();
}

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

void wkt()
{
  NFmiGdalArea area("FMI", "EPSG:2393", NFmiPoint(20, 60), NFmiPoint(40, 70));

  std::string expected =
      "PROJCS[\"KKJ / Finland Uniform Coordinate "
      "System\",GEOGCS[\"KKJ\",DATUM[\"Kartastokoordinaattijarjestelma_1966\",SPHEROID["
      "\"International "
      "1924\",6378388,297,AUTHORITY[\"EPSG\",\"7022\"]],TOWGS84[-96.062,-82.428,-121.753,4.801,0."
      "345,-1.376,1.496],AUTHORITY[\"EPSG\",\"6123\"]],PRIMEM[\"Greenwich\",0,AUTHORITY[\"EPSG\","
      "\"8901\"]],UNIT[\"degree\",0.0174532925199433,AUTHORITY[\"EPSG\",\"9122\"]],AUTHORITY["
      "\"EPSG\",\"4123\"]],PROJECTION[\"Transverse_Mercator\"],PARAMETER[\"latitude_of_origin\",0],"
      "PARAMETER[\"central_meridian\",27],PARAMETER[\"scale_factor\",1],PARAMETER[\"false_"
      "easting\",3500000],PARAMETER[\"false_northing\",0],UNIT[\"metre\",1,AUTHORITY[\"EPSG\","
      "\"9001\"]],AUTHORITY[\"EPSG\",\"2393\"]]";

  std::string result = area.WKT();
  if (result != expected) TEST_FAILED("Expected " + expected + ", got " + result + " instead");

  TEST_PASSED();
}

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

void tolatlon()
{
  // YKJ
  NFmiGdalArea area("FMI", "EPSG:2393", NFmiPoint(20, 60), NFmiPoint(40, 70));

  {
    NFmiPoint xy = NFmiPoint(1, 0);
    NFmiPoint expect = NFmiPoint(40, 70);
    NFmiPoint latlon = area.ToLatLon(xy);
    if (expect.Distance(latlon) > 1)  // degrees
      TEST_FAILED("Failed to project " + tostr(xy) + " to " + tostr(expect) + ", got " +
                  tostr(latlon) + " instead");
  }

  {
    NFmiPoint xy = NFmiPoint(0, 1);
    NFmiPoint expect = NFmiPoint(20, 60);
    NFmiPoint latlon = area.ToLatLon(xy);
    if (expect.Distance(latlon) > 1)  // degrees
      TEST_FAILED("Failed to project " + tostr(xy) + " to " + tostr(expect) + ", got " +
                  tostr(latlon) + " instead");
  }

  TEST_PASSED();
}

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

void toxy()
{
  // YKJ
  NFmiGdalArea area("FMI", "EPSG:2393", NFmiPoint(20, 60), NFmiPoint(40, 70));

  {
    NFmiPoint expect = NFmiPoint(1, 0);
    NFmiPoint latlon = NFmiPoint(40, 70);
    NFmiPoint xy = area.ToXY(latlon);
    if (expect.Distance(xy) > 0.01)  // xy-units
      TEST_FAILED("Failed to project " + tostr(latlon) + " to " + tostr(expect) + ", got " +
                  tostr(xy) + " instead");
  }

  {
    NFmiPoint expect = NFmiPoint(0, 1);
    NFmiPoint latlon = NFmiPoint(20, 60);
    NFmiPoint xy = area.ToXY(latlon);
    if (expect.Distance(xy) > 0.01)  // xy-units
      TEST_FAILED("Failed to project " + tostr(latlon) + " to " + tostr(expect) + ", got " +
                  tostr(xy) + " instead");
  }

  TEST_PASSED();
}

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

void latlontoworldxy()
{
  // YKJ
  NFmiGdalArea area("FMI", "EPSG:2393", NFmiPoint(20, 60), NFmiPoint(40, 70));

  NFmiPoint expect = NFmiPoint(3388405, 6655915);
  NFmiPoint xy = area.LatLonToWorldXY(NFmiPoint(25, 60));
  if (expect.Distance(xy) > 1)  // meters
    TEST_FAILED("Failed to project 25,60 to " + tostr(expect) + ", got " + tostr(xy) + " instead");

  TEST_PASSED();
}

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

void worldxytolatlon()
{
  // YKJ
  NFmiGdalArea area("FMI", "EPSG:2393", NFmiPoint(20, 60), NFmiPoint(40, 70));

  NFmiPoint wxy = NFmiPoint(3388888, 6655555);
  NFmiPoint expect = NFmiPoint(25, 60);
  NFmiPoint xy = area.WorldXYToLatLon(wxy);
  if (expect.Distance(xy) > 0.01)  // degrees
    TEST_FAILED("Failed to project " + tostr(wxy) + " to " + tostr(expect) + ", got " + tostr(xy) +
                " instead");

  TEST_PASSED();
}

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

void worldxyheight()
{
  {
    // YKJ
    NFmiGdalArea area("FMI", "EPSG:2393", NFmiPoint(20, 60), NFmiPoint(40, 70));
    double h = std::round(area.WorldXYHeight() / 1000.0);
    if (h != 1147)
      TEST_FAILED("YKJ height should be 1147 km, got " + boost::lexical_cast<std::string>(h));
  }
  {
    NFmiGdalArea area("FMI", "WGS84", NFmiPoint(20, 60), NFmiPoint(40, 70));
    double h = std::round(area.WorldXYHeight() / 1000.0);
    if (h != 1112)
      TEST_FAILED("WGS84 height should be 1112, got " + boost::lexical_cast<std::string>(h));
  }

  TEST_PASSED();
}

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

void worldxywidth()
{
  {
    // YKJ
    NFmiGdalArea area("FMI", "EPSG:2393", NFmiPoint(20, 60), NFmiPoint(40, 70));
    int h = std::round(area.WorldXYWidth() / 1000.0);
    if (h != 883)
      TEST_FAILED("YKJ width should be 883, got " + boost::lexical_cast<std::string>(h));
  }
  {
    NFmiGdalArea area("FMI", "WGS84", NFmiPoint(20, 60), NFmiPoint(40, 70));
    int h = std::round(area.WorldXYWidth() / 1000.0);
    if (h != 940)
      TEST_FAILED("WGS84 width should be 940, got " + boost::lexical_cast<std::string>(h));
  }

  TEST_PASSED();
}

// ----------------------------------------------------------------------
/*!
 * The actual test suite
 */
// ----------------------------------------------------------------------

class tests : public tframe::tests
{
  virtual const char* error_message_prefix() const { return "\n\t"; }
  void test(void)
  {
    TEST(create);
    TEST(areastr);
    TEST(wkt);
    TEST(tolatlon);
    TEST(toxy);
    TEST(latlontoworldxy);
    TEST(worldxytolatlon);
    TEST(worldxyheight);
    TEST(worldxywidth);
  }
};

}  // namespace NFmiGdalAreaTest

//! The main program
int main(void)
{
  using namespace std;
  cout << endl << "NFmiGdalArea tester" << endl << "===================" << endl;
  NFmiAreaFactoryTest::tests t;
  return t.run();
}

// ======================================================================


/// \file
/// Maintainer: Felice Serena
///
///

#include "label_clustering.h"

#include <boost/log/trivial.hpp>

namespace MouseTrack {

std::vector<Cluster> LabelClustering::
operator()(const PointCloud &cloud) const {
  BOOST_LOG_TRIVIAL(trace) << "LabelClustering: dims: " << cloud.labelsDim();
  int clusterCount = cloud.labelsDim() + 1;
  std::vector<Cluster> clusters(clusterCount);
  for (size_t i = 0; i < cloud.size(); ++i) {
    const auto p = cloud[i];
    const auto &labels = p.labels();
    int maxI, maxJ;
    Precision max = labels.maxCoeff(&maxI, &maxJ);
    if (max > _rejectionThreshold) {
      clusters[maxI].points().push_back(i);
    } else {
      // rubbish is collected in the last cluster
      clusters.back().points().push_back(i);
    }
  }
  return clusters;
}

} // namespace MouseTrack


#include <boost/date_time/posix_time/posix_time.hpp>
#include <iostream>

using namespace boost::posix_time;

int main()
{
  time_duration td{16, 30, 0};
  std::cout << td.hours() << '\n';
  std::cout << td.minutes() << '\n';
  std::cout << td.seconds() << '\n';
  std::cout << td.total_seconds() << '\n';
}


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

#ifndef LNCENTRALITY_ALGOS_HPP
#define LNCENTRALITY_ALGOS_HPP

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/graph/betweenness_centrality.hpp>

#include <fstream>
#include <numeric>

#include "readgraph.hpp"
#include "support.hpp"

using namespace boost::accumulators;

namespace lncentrality {

// Given a graph g, computes the centrality stats for the graph and
// stores them in outdir with stem as part of the filename.
//
// Callers can therefore specify the full filename to save the
// outputs.
//
// Stats computed:
//
// 1. Betweeness centrality for all nodes in the graph. Then stored in
// stem.centrality.csv.
//
// 2. Central point dominance of the graph. Stored in cpd.csv.
void get_centrality(LitGraph &g, string &stem, string &outdir) {
  std::ofstream out;
  std::vector<double> centrality(num_vertices(g));
  graph_traits<LitGraph>::vertex_iterator i, end;

  std::vector<centrality_s> centrality_vector(num_vertices(g));

  brandes_betweenness_centrality(
      g, centrality_map(make_iterator_property_map(
                            centrality.begin(), get(vertex_index, g), double()))
             .vertex_index_map(get(vertex_index, g)));

  relative_betweenness_centrality(
      g, make_iterator_property_map(centrality.begin(), get(vertex_index, g),
                                    double()));

  for (boost::tie(i, end) = vertices(g); i != end; ++i) {
    centrality_s c = {g[*i].name, g[*i].pub_key, centrality[*i]};
    centrality_vector.push_back(c);
    // std::cout << g[*i].name << " :: "
    //           << g[*i].pub_key << " :: "
    //           << centrality[*i] << endl;
  }

  // sort centrality vector
  std::sort(centrality_vector.begin(), centrality_vector.end(),
            compare_by_centrality<centrality_s>());

  out.open(outdir + "/" + stem + ".centrality.csv",
           std::fstream::out | std::fstream::trunc);

  out << "alias,centrality,pub_key" << std::endl;
  for (std::vector<centrality_s>::iterator centrality_iter =
           centrality_vector.begin();
       centrality_iter != centrality_vector.begin() + 50; centrality_iter++) {
    out << "\"" << centrality_iter->name << "\""
        << "," << centrality_iter->centrality << "," << centrality_iter->pub_key
        << std::endl;
  }
  out.close();

  double dominance = central_point_dominance(
      g, make_iterator_property_map(centrality.begin(), get(vertex_index, g),
                                    double()));
  out.open(outdir + "/cpd.csv", std::fstream::out | std::fstream::app);
  out << stem << "," << dominance << std::endl;
  out.close();
}

// Sorts inplace, the list of articulation points, art_points.
//
// Stores the sorted list in stem.all_ap.csv
void sort_and_print_art_points(std::vector<Vertex> &art_points, LitGraph &g,
                               string &stem, string &outdir) {
  std::ofstream out;
  out.open(outdir + "/" + stem + ".all_ap.csv",
           std::fstream::out | std::fstream::trunc);

  out << "alias,degree,pub_key" << std::endl;
  std::sort(art_points.begin(), art_points.end(),
            compare_by_degree<LitGraph>(g));
  // print top art points
  std::for_each(art_points.begin(), art_points.end(), [&out, &g](Vertex &v) {
    out << "\"" << g[v].name << "\""
        << "," << degree(v, g) << "," << g[v].pub_key << std::endl;
  });

  out.close();
}

// For a list of articulation points, art_points, already identified
// for the LitGraph g, find only those articulation points that
// connects components of size greater than one.
//
// Stores the interesting articulation points in stem.key_ap.csv.
//
// Also stores for each run, the time, number of biconnected components
// and the number of articulation points in stem.key_ap_stats.csv
void find_non_singleton_components(std::vector<Vertex> &art_points, LitGraph &g,
                                   string &stem, string &outdir) {

  // make a map component id to component size
  std::map<int, int> component_sizes;
  std::vector<Vertex>::iterator i;
  std::ofstream out;

  graph_traits<LitGraph>::edge_iterator ei, ei_end;
  graph_traits<LitGraph>::out_edge_iterator out_i, out_end;

  typedef graph_traits<LitGraph>::edge_descriptor Edge;
  std::vector<Edge> bridges;
  std::set<int> component_ids;
  std::map<int, set<int>> interesting_art_points;
  int num_interesting;

  // find the size of components by iterating over all edges.
  for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) {
    component_sizes[g[*ei].component]++;
  }

  // iterate over all edges again and select those edges where
  // component size on both vertices are greater than 1. This and the
  // previous loop can probably be optimised.
  for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) {
    if (component_sizes[g[*ei].component] == 1) {
      bridges.push_back(*ei);
    }
  }

  // iterate over all art points
  for (i = art_points.begin(); i != art_points.end(); i++) {
    // get all the components for all the edges of the art points
    component_ids.clear();
    for (boost::tie(out_i, out_end) = out_edges(*i, g); out_i != out_end;
         ++out_i) {
      component_ids.insert(g[*out_i].component);
    }
    // find the number of components the art point is connecting
    num_interesting =
        std::accumulate(component_ids.begin(), component_ids.end(), 0,
                        [&component_sizes](int a, int cid) {
                          if (component_sizes[cid] > 5) {
                            a = a + 1;
                          }
                          return a;
                        });
    // change the min size of components to consider "interesting"
    if (num_interesting > 1) {
      interesting_art_points[*i] = component_ids;
    }
  }

  // date, #components, #interesting art points. No header output as
  // this is appended to file.
  out.open(outdir + "/key_ap_stats.csv", std::fstream::out | std::fstream::app);
  out << stem << "," << component_sizes.size() << ","
      << interesting_art_points.size() << std::endl;
  out.close();

  out.open(outdir + "/" + stem + ".key_ap.csv",
           std::fstream::out | std::fstream::trunc);

  // Key Articulation Points. Header included.
  out << "alias,num components,degree,pub_key" << std::endl;
  for (std::map<int, set<int>>::iterator x = interesting_art_points.begin();
       x != interesting_art_points.end(); x++) {
    out << "\"" << g[x->first].name << "\""
        << "," << (x->second).size() << "," << degree(x->first, g) << ","
        << g[x->first].pub_key << endl;
  }
  out.close();
}

// For a given graph, g, find stats about articulation points and
// store to file. The filename is based on outdir and stem.
//
// Stats computed:
//
// 1. Number of biconnected components. Stored in all_ap_stats.csv
//
// 2. Number of components with more than one node. Stored in
// key_ap_stats.csv.in
//
// 3. Identify all nodes that are articulation points in the
// graph. Stored in stem-all_ap.csv
void get_articulation_points(LitGraph &g, string &stem, string &outdir) {
  std::ofstream out;
  std::vector<Vertex> art_points;
  std::pair<std::size_t, std::back_insert_iterator<std::vector<Vertex>>>
      results = biconnected_components(g, get(&LitEdge::component, g),
                                       std::back_inserter(art_points));

  // date, #components, #interesting art points
  out.open(outdir + "/all_ap_stats.csv", std::fstream::out | std::fstream::app);
  out << stem << "," << results.first << "," << art_points.size() << std::endl;
  out.close();

  find_non_singleton_components(art_points, g, stem, outdir);
  sort_and_print_art_points(art_points, g, stem, outdir);
}

} // namespace lncentrality

#endif


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file find_test.cpp
/// \brief Test for find functions.

#define BOOST_TEST_MODULE find_test
#include <boost/test/included/unit_test.hpp>

#include "mcrl2/data/detail/print_utility.h"
#include "mcrl2/process/find.h"
#include "mcrl2/process/parse.h"

using namespace mcrl2;
using namespace mcrl2::process;

std::string DATA_DECL =
  "glob                   \n"
  "  m: Nat;              \n"
  "                       \n"
  "act                    \n"
  "  a: Nat;              \n"
  ;

std::string PROC_DECL =
  "proc P(n:Nat);         \n"
  ;

inline
data::variable nat(const std::string& name)
{
  return data::variable(core::identifier_string(name), data::sort_nat::nat());
}

inline
data::variable pos(const std::string& name)
{
  return data::variable(core::identifier_string(name), data::sort_pos::pos());
}

inline
data::variable bool_(const std::string& name)
{
  return data::variable(core::identifier_string(name), data::sort_bool::bool_());
}

void test_find()
{
  process_expression x = parse_process_expression("a(m).P(0)", DATA_DECL, PROC_DECL);

  //--- find_all_variables ---//
  data::variable m = nat("m");
  std::set<data::variable> v = process::find_all_variables(x);
  BOOST_CHECK(v.find(m) != v.end());

  //--- find_sort_expressions ---//
  std::set<data::sort_expression> e = process::find_sort_expressions(x);
  BOOST_CHECK(std::find(e.begin(), e.end(), data::sort_nat::nat()) != e.end());
  BOOST_CHECK(std::find(e.begin(), e.end(), data::sort_pos::pos()) == e.end());

}

void test_free_variables()
{
  data::variable b = bool_("b");
  data::data_expression_list d = { b };
  process_identifier P(core::identifier_string("P"), { b });
  process_instance pi(P, d);

  std::set<data::variable> free_variables = process::find_free_variables(pi);
  std::cout << "free variables: " << core::detail::print_set(free_variables) << std::endl;
  BOOST_CHECK(free_variables.find(b) != free_variables.end());
}

BOOST_AUTO_TEST_CASE(test_main)
{
  test_find();
  test_free_variables();
}


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

#include "Size.hpp"

#include <stdexcept>
#include <iostream>
#include <sstream>
#include <iomanip>

#include <boost/io/ios_state.hpp>
#include <boost/regex.hpp>

namespace enyx {
namespace net_tester {

namespace {

Size
parse_size(const std::string & s)
{
    boost::regex regex_iec(R"((\d+)\s*([KMGTPE]i)?(B|bit))");
    boost::regex regex_si(R"((\d+)\s*([kKMGTPE])?(B|b|bit))");
    boost::smatch m;

    Size::UnitSystem unit_system;
    if (boost::regex_match(s, m, regex_iec))
        unit_system = Size::IEC;
    else if (boost::regex_match(s, m, regex_si))
        unit_system = Size::SI;
    else
    {
        std::ostringstream error;
        error << "size '" << s
              << "' doesn't match either \\d+\\s*([KMGTPE]i)?(B|bit)"
                 " nor \\d+\\s*([kKMGTPE])?(B|b|bit)";
        throw std::runtime_error(error.str());
    }

    const uint64_t factor = unit_system == Size::SI ? 1000 : 1024;

    uint64_t size = std::atoll(m.str(1).c_str());

    if (m[2].matched)
        switch (m.str(2)[0])
        {
            case 'E':
                size *= factor;
            case 'P':
                size *= factor;
            case 'T':
                size *= factor;
            case 'G':
                size *= factor;
            case 'M':
                size *= factor;
            case 'K':
            case 'k':
                size *= factor;
        }

    if (m.str(3) == "bit" || m.str(3) == "b")
        size /= 8;

    return Size{size, unit_system};
}

const char * units_iec[] = { "bit", "Kibit", "Mibit",
                             "Gibit", "Tibit", "Pibit",
                             "Eibit" };

const char * units_si[] = { "bit", "kbit", "Mbit",
                            "Gbit", "Tbit", "Pbit",
                            "Ebit" };

#define UNITS_COUNT ((sizeof(units_iec)) / sizeof(units_iec[0]))

} // anonymous namespace

std::istream &
operator>>(std::istream & in, Size & size)
{
    std::istream::sentry sentry(in);

    if (sentry)
    {
        std::string s;
        in >> s;

        size = parse_size(s);
    }

    return in;
}

std::ostream &
operator<<(std::ostream & out, const Size & size)
{
    std::ostream::sentry sentry(out);
    if (sentry)
    {
        const Size::UnitSystem unit_system = size.get_unit_system();
        const uint64_t factor = unit_system == Size::SI ? 1000 : 1024;
        const char ** units = unit_system == Size::SI ? units_si : units_iec;

        long double value = size * 8;
        uint64_t i = 0;
        for (i = 0; i != UNITS_COUNT - 1 && value / factor >= 1.; ++i)
            value /= factor;

        boost::io::ios_flags_saver s(out);
        out << std::fixed << std::setprecision(1) << value << units[i]
            << "(" << size * 8 << units[0] << ")";
    }

    return out;
}

} // namespace net_tester
} // namespace enyx


/*
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE LinearEquationTest
*/

#include <boost/test/unit_test.hpp>

#include "XYCoord.h"

BOOST_AUTO_TEST_CASE( XYCoordTest )
{
    XYCoord simple(1.0,2.0);

    BOOST_CHECK( simple.x() == 1.0);
    BOOST_CHECK( simple.y() == 2.0);
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2012-2016 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef NMIII_APCG_HH
#define NMIII_APCG_HH

#include <numeric>
#include <string>

#include <boost/circular_buffer.hpp>
#include <boost/timer/timer.hpp>

#include "dune/common/timer.hh"
#include "dune/istl/istlexception.hh"
#include "dune/istl/operators.hh"
#include "dune/istl/preconditioners.hh"
#include "dune/istl/scalarproducts.hh"
#include "dune/istl/solvers.hh"

#include "linalg/symmetricOperators.hh"
#include "utilities/detailed_exception.hh"
#include "utilities/geometric_sequence.hh"
#include "utilities/scalar.hh"

namespace Kaskade
{
  /**
   * \ingroup iterative
   * \brief Whether a preconditioner needs zero initialized result vector or not.
   *
   * For a couple of known preconditioners, this function returns the actual information.
   * For unknown preconditioners, it errs on the conservative side, i.e. reports true.
   *
   * The function is moderately expensive, hence don't use it inside of loops.
   */
  template <class Domain, class Range>
  bool requiresInitializedInput(Dune::Preconditioner<Domain,Range> const* p)
  {
    if (dynamic_cast<Dune::Richardson<Domain,Range> const*>(p))
      return false;
    if (auto sp = dynamic_cast<SymmetricPreconditioner<Domain,Range> const*>(p))
      return sp->requiresInitializedInput();
    return true;
  }

  /**
   * \ingroup iterative
   * \brief Interface for IterateType::PCG termination criterion policy classes
   * \tparam R a floating point type for real numbers
   */
  template <class R>
  class PCGTerminationCriterion
  {
  public:
    /**
     * \brief real field type
     */
    typedef R Real;

    /**
     * \brief re-initializes the termination criterion for a new IterateType::CG run
     */
    virtual void clear() = 0;

    /**
     * \brief set requested tolerance
     *
     * \param tol the requested tolerance (nonnegative)
     */
    virtual PCGTerminationCriterion<R>& tolerance(Real tol) = 0;

    /**
     * \brief supplies energy of step to the termination criterion
     * \param gamma2 the energy of the step
     */
    virtual void step(Real gamma2) = 0;

    /**
     * \brief supplies the preconditioned residual to the termination criterion
     * \param sigma the preconditioned residual norm
     */
    virtual void residual(Real sigma) = 0;

    /**
     * \brief termination decision
     * \return true if the iteration has reached the required accuracy
     */
    virtual operator bool() const = 0;
  };

  /**
   * \ingroup iterative
   * \brief TerminationCriterion based on an absolute energy error estimate
   *
   * This termination criterion assumes the error reduction occurs in two phases: First a smoothing phase where
   * \f$ \epsilon_k^2 \approx a^2/(1+\sqrt{k})^2 + b^2 \f$, and a linear convergence phase with \f$ \epsilon_k \approx q^k \epsilon_0 \f$.
   * The termination criterion tries to fit the model parameters \f$ a, b, q \f$ and to decide where the crossover of both
   * phases occurs. Based on this decision, the energy error \f$ \epsilon_k \f$ is estimated as \f$ [\epsilon_k] \f$.
   *
   * For greater robustnes, the actual error estimate used is \f[ [\epsilon_{k-l}]^2 := [\epsilon_k]^2 + \sum_{i=k-l}^{k-1} \gamma_i^2. \f] with
   * \f$ \gamma_i = \|u_{i+1}-u_i\|_A \f$.The lookahead value \f$ l \f$ can be set using \ref lookahead.
   *
   * \tparam R the real field type. Currently instantiated for float and double in file apcg.cpp.
   */
  template <class R>
  class PCGEnergyErrorTerminationCriterion: public PCGTerminationCriterion<R> {
  public:
    typedef R Real;

    /**
     * \brief constructor
     *
     * The pcg iteration is terminated as soon as either \f$ [\epsilon] \le \mathrm{atol} \f$ or
     * the number of iterations exceeds the limit maxit.
     *
     * \param atol the absolute error tolerance for termination
     * \param maxit the maximum number of iterations
     */
    PCGEnergyErrorTerminationCriterion(Real atol, int maxit);

    /**
     * \brief re-initializes the termination criterion for a new IterateType::CG run
     */
    virtual void clear();

    /**
     * \brief set requested absolute tolerance
     *
     * \param atol the requested tolerance (nonnegative)
     */
    virtual PCGEnergyErrorTerminationCriterion<Real>& tolerance(Real atol);

    /**
     * \brief set requested lookahead value
     *
     * \param lah the requested lookahead (nonnegative)
     *
     * The default value is 6.
     */
     PCGEnergyErrorTerminationCriterion<Real>& lookahead(int lah);

    /**
     * \brief supplies energy of step to the termination criterion
     * \param gamma2 the energy \f$ \gamma^2 = \| u_{k+1}-u_k \|_A^2 \f$ of the step
     */
    virtual void step(Real gamma2);

    /**
     * \brief supplies the preconditioned residual to the termination criterion
     * \param sigma the preconditioned residual norm
     */
    virtual void residual(Real sigma) {}

    /**
     * \brief termination decision
     * \return true if the iteration has reached the required accuracy
     */
    virtual operator bool() const;

    /**
     * \brief returns the estimated absolute energy error
     */
    Real error() const;

    std::vector<Real> const& gamma2() const {return gammas2; }

  private:
    Real tol;
    int maxit;
    // squared gammas
    std::vector<Real> gammas2;
    int lookah;
  };

 /**
  * \ingroup iterative
  * \brief preconditioned conjugate gradient method
  *
  * This implements a preconditioned IterateType::CG iteration for an operator \f$ A: X\to X^* \f$, preconditioned by a
  * preconditioner \f$ B^{-1}: X^* \to X \f$. The termination is policy-based.
  *
  * The implementation follows Deuflhard/Weiser, Section 5.3.3.
  *
  * \tparam X the type of vectors from the primal space
  * \tparam Xstar the type of vectors from the dual space
  *
  * \see PCGEnergyErrorTerminationCriterion
  */
  template<class X, class Xstar>
  class Pcg : public Dune::InverseOperator<X,Xstar> {
  public:
    /// \brief The domain type of the operator to be inverted.
    typedef X domain_type;
    /// \brief The range type of the operator to be inverted.
    typedef Xstar range_type;
    /// \brief The field type of the operator to be inverted.
    typedef typename X::field_type field_type;

    /**
     * \brief the real field type corresponding to field_type
     */
    typedef typename ScalarTraits<field_type>::Real Real;

    /**
     * \brief Set up conjugate gradient solver.
     *
     * \param op the operator
     * \param prec the preconditioner
     * \param terminate the termination criterion. The object has to exist during the lifetime of the pcg object as it is referenced.
     * \param verbose controls the verbosity of logging to std::cout. 0 means no output at all (default). Values in 0,1,2 are valid.
     */
    Pcg(SymmetricLinearOperator<X,Xstar>& op_, SymmetricPreconditioner<X,Xstar>& prec_,
        PCGTerminationCriterion<Real>& terminate_, int verbose_=0) :
      proxyOp(op_,zeroDp), op(op_),
      proxyPrec(prec_,zeroDp), prec(prec_),
      terminate(terminate_), verbose(verbose_), requiresInit(requiresInitializedInput(&prec))
    {
// Do we need this in Kaskade7?
//         dune_static_assert( static_cast<int>(L::category) == static_cast<int>(P::category),
//                             "L and P must have the same category!");
//         dune_static_assert( static_cast<int>(L::category) == static_cast<int>(SolverCategory::sequential),
//                             "L must be sequential!");
    }

    /**
     * \brief Set up conjugate gradient solver.
     *
     * \param op the operator
     * \param prec the preconditioner
     * \param dp the dual pairing with respect to which the operator is symmetric
     * \param terminate the termination criterion. The object has to exist during the lifetime of the pcg object as it is referenced.
     * \param verbose controls the verbosity of logging to std::cout. 0 means no output at all (default). Values in 0,1,2 are valid.
     */
    Pcg(Dune::LinearOperator<X,Xstar>& op_, Dune::Preconditioner<X,Xstar>& prec_, DualPairing<X,Xstar> const& dp_,
        PCGTerminationCriterion<Real>& terminate_, int verbose_=0) :
      proxyOp(op_,dp_), op(proxyOp),
      proxyPrec(prec_,dp_), prec(proxyPrec),
      terminate(terminate_), verbose(verbose_), requiresInit(requiresInitializedInput(&prec))
    {    }

    /**
     * \brief Apply inverse operator by performing a number of IterateType::PCG iterations.
     *
     * \param u the initial value (starting iterate)
     * \param b the right hand side (which will not be modified)
     */
    virtual void apply (X& u, Xstar& b, Dune::InverseOperatorResult& res) {
      res.clear();                // clear solver statistics
      terminate.clear();          // clear termination criterion
      Dune::Timer watch;          // start a timer

      boost::timer::cpu_timer mvTimer;
      mvTimer.stop();
      boost::timer::cpu_timer apTimer;
      apTimer.stop();
      boost::timer::cpu_timer prTimer;
      prTimer.stop();

      Xstar r(b);
      op.applyscaleadd(-1,u,r);  // r = b-Au

      // Keeping rq and q in unique_ptr allows efficient computation of q = rq + beta*q below by swapping the pointers.
      // More elegant would be a swap method directly on X, but this is hard to implement as there is no swap either
      // for boost::fusion vectors nor for Dune::BlockVectors. Even more elegant would be a loop fusion expression
      // framework for BLAS level 1 operations.
      std::unique_ptr<X> rq(new X(u)); *rq = 0;
      prec.apply(*rq,r); // rq = B^{-1} r

      std::unique_ptr<X> q(new X(*rq));

      X Aq(b);

      // some local variables
      field_type alpha,beta,sigma,gamma;
      sigma = op.dp(*rq,r); // preconditioned residual norm squard
      terminate.residual(ScalarTraits<field_type>::real(sigma));
      double const sigma0 = std::abs(sigma);

      // Check for trivial case
      if (sigma0 == 0)
        return;

      if (verbose>0) {            // printing
        std::cout << "=== Kaskade7 IterateType::PCG" << std::endl;
        if (verbose>1) {
          this->printHeader(std::cout);
          this->printOutput(std::cout,0.0,sigma0);
        }
      }

      // the loop
      int i=1;
      for ( ; true; i++ ) {
        // minimize in given search direction p
        mvTimer.resume();
        field_type qAq = op.applyDp(*q,Aq);             // compute Aq = A*q and qAq = <q,A*q>
if (std::isnan(qAq)) std:: cerr << "qAq is nan\n";
        mvTimer.stop();

	if (std::abs(qAq) < 1e-28*sigma)
	  break; // oops.

        if (qAq <= 0)
          throw NonpositiveMatrixException("encountered nonpositive energy product " + std::to_string(qAq) + " vs sigma " + std::to_string(sigma) + " in IterateType::PCG.",__FILE__,__LINE__);

        alpha = sigma/qAq;
        apTimer.resume();
        u.axpy(alpha,*q);
        apTimer.stop();
        gamma = sigma*alpha;
	terminate.step(ScalarTraits<field_type>::real(gamma));
	resDebug.push_back(std::sqrt(sigma));

        // convergence test
        if (terminate)
	  break;

        apTimer.resume();
        r.axpy(-alpha,Aq); // r = r - alpha*A*q
        apTimer.stop();

        prTimer.resume();
        if (requiresInit)
          *rq = 0;
        double sigmaNew = prec.applyDp(*rq,r); // compute rq = B^{-1}r and <rq,r>
        prTimer.stop();

        // determine new search direction
	terminate.residual(ScalarTraits<field_type>::real(sigmaNew));
        if (verbose>1)             // print
          this->printOutput(std::cout,static_cast<double>(i),std::abs(sigmaNew),std::abs(sigma));

	// convergence check to prevent division by zero if we obtained an exact solution
	// (which may happen for low dimensional systems)
	if (std::abs(sigmaNew) < 1e-28*sigma0)
	  break;

        beta = sigmaNew/sigma;
        sigma = sigmaNew;
        apTimer.resume();
        rq->axpy(beta,*q); // compute q = rq + beta*q
        std::swap(rq,q);
        apTimer.stop();
      }

      if (verbose>0)                // printing for non verbose
        this->printOutput(std::cout,static_cast<double>(i),std::abs(sigma));

      res.iterations = i;               // fill statistics
      res.reduction = std::sqrt(std::abs(sigma)/sigma0);
      res.conv_rate  = pow(res.reduction,1.0/i);
      res.elapsed = watch.elapsed();

      if (verbose>0)                 // final print
      {
        std::cout << "=== rate=" << res.conv_rate
                  << ", T=" << res.elapsed
                  << ", TIT=" << res.elapsed/i
                  << ", IT=" << i << std::endl;

        std::cout << "Matrix-Vector time: " << mvTimer.format()
                  << "vector ops        : " << apTimer.format()
                  << "preconditioner    : " << prTimer.format() << '\n';
      }
  }

    /**
     * \brief Apply inverse operator with given tolerance.
     *
     * This method is equivalent to setting first the tolerance in the termination criterion, then calling apply().
     */
  virtual void apply (X& x, X& b, double tol, Dune::InverseOperatorResult& res) {
    terminate.tolerance(tol);
    (*this).apply(x,b,res);
  }

  private:
    // The solver may be constructed with either symmetric operator/preconditioner interface provided,
    // or with Dune interfaces provided. The implementation relies on the symmetric interface
    // in order to exploit performance gains with simultaneous evaluation of matrix-vector products
    // and dual pairings.
    // Therefore we hold "working" references to symmetric operator/preconditioners. Those reference
    // either the provided ones, or wrapper objects relaying the evaluation to provided Dune interfaces.
    // For the second case, we hold the (small) wrapper objects ourselves. In the first case, those
    // wrapper objects have to be initialized correctly (even though they are never accessed). For this
    // we need a dual pairing, hence the dummy zeroDp.
    ZeroDualPairing<X,Xstar>                zeroDp;  // just a dummy

    SymmetricLinearOperatorWrapper<X,Xstar> proxyOp; // proxy in case a Dune::LinearOperator is provided
    SymmetricLinearOperator<X,Xstar>& op;  // working reference to operator

    SymmetricPreconditionerWrapper<X,Xstar>  proxyPrec; // proxy in case a Dune::Preconditioner is provided
    SymmetricPreconditioner<X,Xstar>&        prec;      // working reference to preconditioner

    PCGTerminationCriterion<Real>& terminate;
    int verbose;
    bool requiresInit;

    std::vector<double> resDebug;
  };
} // namespace Kaskade
#endif


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

#include <boost/preprocessor/control/if.hpp>
#include <boost/preprocessor/repetition/enum_params.hpp>
#include <iostream>

#include "terms.hpp"
#include "macros.hpp"

namespace cta {
namespace detail {

template <typename number, int var>
std::ostream& operator<<(std::ostream& o, const var_type<number, var>&) {
	return o << "x" << var;
}

template <typename number, int var, int index>
std::ostream& operator<<(std::ostream& o, const dvar_type<number, var, index>&) {
	return o << "x" << var << "(" << index << ")";
}

template <typename number, typename T, int pow>
std::ostream& operator<<(std::ostream&, const static_power<number, T, pow>&);
#define CTA_FORWARD(type, n)                                                                       \
	template <CTA_TEMPLATE_DECL_ARGS(n)>                                                           \
	std::ostream& operator<<(std::ostream&, const type<CTA_TEMPLATE_USE_ARGS(n)>&);
#define CTA_VISIT_TYPE(type, n, ...) CTA_FORWARD(type, n)
#include "types.hpp"
#undef CTA_FORWARD

template <typename number, typename T, int pow>
std::ostream& operator<<(std::ostream& o, const static_power<number, T, pow>& v) {
	return o << "pow(" << v.a0 << ", " << pow << ")";
}

#define CTA_VISIT_TYPE(type, n, kind, op, ...) kind(type, op, n)
#define CTA_RET(type, op, n)                                                                       \
	template <CTA_TEMPLATE_DECL_ARGS(n)>                                                           \
	std::ostream& operator<<(std::ostream& o, const type<CTA_TEMPLATE_USE_ARGS(n)>& t) {           \
		(void) t;                                                                                \
		return o << #op;                                                                           \
	}
#define CTA_FUN_ARG(_, n, __) BOOST_PP_IF(n, << ", " <<, << ) t.a##n
#define CTA_FUN(type, fun, n)                                                                      \
	template <CTA_TEMPLATE_DECL_ARGS(n)>                                                           \
	std::ostream& operator<<(std::ostream& o, const type<CTA_TEMPLATE_USE_ARGS(n)>& t) {           \
		(void) t;                                                                                \
		return o << #fun << "(" BOOST_PP_REPEAT(n, CTA_FUN_ARG, _) << ")";                         \
	}
#define CTA_PREFIX(type, op, n)                                                                    \
	template <CTA_TEMPLATE_DECL_ARGS(n)>                                                           \
	std::ostream& operator<<(std::ostream& o, const type<CTA_TEMPLATE_USE_ARGS(n)>& t) {           \
		return o << "(" << #op << t.a0 << ")";                                                     \
	}
#define CTA_INFIX(type, op, n)                                                                     \
	template <CTA_TEMPLATE_DECL_ARGS(n)>                                                           \
	std::ostream& operator<<(std::ostream& o, const type<CTA_TEMPLATE_USE_ARGS(n)>& t) {           \
		return o << "(" << t.a0 << #op << t.a1 << ")";                                             \
	}

#include "types.hpp"
#undef CTA_FUN_ARG
} // namespace detail
} // namespace cta

#endif


/*******************************************************************************
 * reservoir/ams_select_multi.hpp
 *
 * Approximate selection from sorted sequences (in this case, mostly B-trees)
 *
 * Copyright (C) 2019 Lorenz Hübschle-Schneider <lorenz@4z2.de>
 *
 * All rights reserved. Published under the GNU General Public License 3
 ******************************************************************************/

#pragma once
#ifndef RESERVOIR_AMS_SELECT_MULTI_HEADER
#define RESERVOIR_AMS_SELECT_MULTI_HEADER

#include <reservoir/aggregate.hpp>
#include <reservoir/logger.hpp>
#include <reservoir/select_helpers.hpp>
#include <reservoir/timer.hpp>
#include <reservoir/util.hpp>

#include <tlx/die/core.hpp>
#include <tlx/math/aggregate.hpp>

#include <boost/mpi.hpp>

#include <algorithm>
#include <functional>
#include <limits>
#include <ostream>
#include <random>
#include <tuple>
#include <utility>
#include <vector>

namespace mpi = boost::mpi;

namespace reservoir {

template <typename Seq, int d = 16>
class ams_select_multi {
public:
    static constexpr const char short_name[] = "[amm]";

    // this is too bothersome to do as constexpr
    static const std::string name() {
        return "ams-multi-" + std::to_string(d);
    }

    using Iterator = typename Seq::const_iterator;
    using Cmp = typename Seq::key_compare;
    using Key = typename Seq::key_type;
    using Elem = typename Seq::value_type;

    // pair of iterator and local rank
    using Result = std::pair<Iterator, ssize_t>;

    // Upper/lower bound iterator and an index
    struct Bound {
        ssize_t ub_pos, lb_pos;
        Iterator ub_it, lb_it;

        Bound() = default;
        Bound(const std::tuple<ssize_t, ssize_t, Iterator, Iterator> &tup) {
            std::tie(ub_pos, lb_pos, ub_it, lb_it) = tup;
        }
    };

    static constexpr bool debug = false;
    static constexpr bool check = false;
    static constexpr bool time = true;

    // Construct a selector using a given communicator and seed.  The seed must
    // be different on every PE (member of the communicator)!
    ams_select_multi(mpi::communicator &comm, size_t seed)
        : comm_(comm), rng_(seed) {
        // divide stats counts by d
        stats_.norm_factor = d;
    }

    // wrapper for kmin = kmax
    Result operator()(Seq &seq, size_t k) {
        return operator()(seq, k, k);
    }

    Result operator()(const Seq &seq, const size_t kmin, const size_t kmax) {
        timer total_timer;

        sLOGR << "Selecting between" << kmin << "and" << kmax << "with"
              << comm_.size() << "PEs";
        if (kmin > kmax || kmax == 0) {
            sLOGR << "aborting: kmin =" << kmin << "kmax =" << kmax;
            return std::make_pair(seq.begin(), 0);
        }
        if constexpr (check) {
            seq.verify();
            size_t size = seq.size();
            mpi::all_reduce(comm_, mpi::inplace(size), std::plus<>());
            sLOGR << "Checking size: want at least" << kmin << "have" << size;
            tlx_die_unless(kmin <= size);
        }

        // calculate global size
        size_t size = seq.size();
        mpi::all_reduce(comm_, mpi::inplace(size), std::plus<>());
        LOGR << "global size: " << size;
        // clang-format off
        tlx_die_verbose_unless(kmin <= size,
                               "Cannot select " << kmin << " to " << kmax
                               << " smallest out of " << size << " items; have "
                               << seq.size() << " at PE " << comm_.rank());
        // clang-format on

        // allocate space for pivots
        pivots_.resize(d);
        bounds_.resize(d);
        gbounds_.resize(2 * d);

        auto res = select(seq, kmin, kmax, 0, seq.size(), size);

        if constexpr (check) {
            size_t result_size = res.second;
            spLOG << "result size contribution:" << result_size;
            mpi::all_reduce(comm_, mpi::inplace(result_size), std::plus<>());
            sLOGR << "Result size:" << result_size << "kmin:" << kmin
                  << "kmax:" << kmax;
            tlx_die_unless(kmin <= result_size && kmax >= result_size);
        }

        if constexpr (debug) {
            comm_.barrier();
            auto it = seq.begin();
            std::vector<Elem> vec;
            while (it != res.first) {
                vec.emplace_back(*it++);
            }
            size_t size = vec.size();
            pLOG << "local result " << size << " elements";
            // double-check output size
            mpi::all_reduce(comm_, mpi::inplace(size), std::plus<>());
            tlx_die_unless(size >= kmin && size <= kmax);
        }

        stats_.record_total(total_timer.get());
        stats_.reset_level();
        pLOGC(time && debug) << "stats: " << stats_;
        return res;
    }

    _detail::select_stats<time> &get_stats() {
        return stats_;
    }

protected:
    Result select(const Seq &seq, const ssize_t kmin, const ssize_t kmax,
                  const ssize_t min_idx, const ssize_t max_idx,
                  const ssize_t global_size) {
        stats_.next_level(); // debug timings
        if (comm_.rank() == 0)
            stats_.record_size(global_size);
        timer_.reset();

        tlx_die_verbose_unless(max_idx >= min_idx,
                               "Expected max_idx >= min_idx, got max_idx = "
                                   << max_idx << " min_idx = " << min_idx);
        tlx_die_unless(kmin <= kmax && kmin <= global_size);

        const ssize_t local_size = max_idx - min_idx;
        spLOG << "kmin =" << kmin << "kmax =" << kmax
              << "global_size =" << global_size << "local range:" << min_idx
              << max_idx << "size" << local_size;

        Iterator min_it = seq.find_rank(min_idx), max_it = seq.find_rank(max_idx);

        if (kmin == 1 || kmax == 1) {
            Key pivot = std::numeric_limits<Key>::max();
            if (local_size > 0) {
                pivot = get_key(min_it);
            }
            pLOG << "Aborting at level " << stats_.level << " with kmin = " << kmin
                 << " kmax = " << kmax << " local minimum: " << pivot;
            mpi::all_reduce(comm_, mpi::inplace(pivot), mpi::minimum<Key>());

            auto [ub_pos, ub_it] = seq.rank_of_upper_bound(pivot);
            // slightly cheaty, normally we subtract min_idx from ub_pos
            if (static_cast<ssize_t>(ub_pos) < min_idx) {
                ub_pos = min_idx;
                ub_it = min_it;
            }
            pLOG << "pivot = " << pivot << " pos " << ub_pos;

            stats_.record(timer_.get());
            return std::make_pair(ub_it, ub_pos);
        }

        if (kmin < global_size - kmax) {
            stats_.kcase.add(0);
            double p =
                1.0 - std::pow((kmin - 1.0) / kmax, 1.0 / (kmax - kmin + 1));
            sLOGR << "Case 1, p =" << p << "base" << (kmin - 1.0) / kmax
                  << "exponent" << 1.0 / (kmax - kmin + 1);
            tlx_die_unless(0 <= p && p <= 1);

            std::geometric_distribution<ssize_t> pidx_dist(p);
            for (int i = 0; i < d; i++) {
                ssize_t pivot_idx = pidx_dist(rng_);
                tlx_die_unless(pivot_idx >= 0);

                if (pivot_idx < local_size) {
                    pivots_[i] = get_key(seq.find_rank(min_idx + pivot_idx));
                } else {
                    pivots_[i] = std::numeric_limits<Key>::max();
                    stats_.pidx_oob++;
                }
                spLOG << "chose pivot index" << pivot_idx << "value" << pivots_[i]
                      << "for local size" << local_size << "pivot" << i;
            }
            // use smallest of local pivots as global pivots
            mpi::all_reduce(comm_, mpi::inplace(pivots_.data()), d,
                            mpi::minimum<Key>());
        } else {
            stats_.kcase.add(1);
            double p =
                1.0 - std::pow((global_size - kmax) / (global_size - kmin + 1.0),
                               1.0 / (kmax - kmin + 1));
            sLOGR << "Case 2, p =" << p << "base"
                  << (global_size - kmax) / (global_size - kmin + 1.0)
                  << "exponent" << 1.0 / (kmax - kmin + 1);
            tlx_die_unless(0 <= p && p <= 1);

            std::geometric_distribution<ssize_t> pidx_dist(p);
            for (int i = 0; i < d; i++) {
                ssize_t pivot_idx = pidx_dist(rng_);
                tlx_die_unless(pivot_idx >= 0);

                if (pivot_idx < local_size) {
                    pivots_[i] = get_key(seq.find_rank(max_idx - pivot_idx - 1));
                } else {
                    pivots_[i] = std::numeric_limits<Key>::min();
                    stats_.pidx_oob++;
                }
                spLOG << "chose pivot index" << pivot_idx << "value" << pivots_[i]
                      << "for local size" << local_size << "pivot" << i;
            }

            // use largest of local pivots as global pivots
            mpi::all_reduce(comm_, mpi::inplace(pivots_.data()), d,
                            mpi::maximum<Key>());
        }
        LOGR << "pivot values = " << pivots_;

        for (int i = 0; i < d; i++) {
            bounds_[i] = _detail::get_bounds<false>(
                seq, stats_, pivots_[i], min_idx, max_idx, min_it, max_it,
                comm_, short_name, debug);
            gbounds_[ubidx(i)] = bounds_[i].ub_pos;
            gbounds_[lbidx(i)] = bounds_[i].lb_pos;
        }

        mpi::all_reduce(comm_, mpi::inplace(gbounds_.data()), 2 * d, std::plus<>());

        sLOGR << "global_size =" << global_size << "want" << kmin << "to"
              << kmax << "got bounds (ub,lb)" << gbounds_;

        // dummy
        int best_ub_idx = -1, best_lb_idx = -1;
        ssize_t best_ub_diff = std::numeric_limits<ssize_t>::max(),
                best_lb_diff = best_ub_diff;

        for (int i = 0; i < d; i++) {
            ssize_t global_ub = gbounds_[ubidx(i)], global_lb = gbounds_[lbidx(i)];
            if (global_ub >= kmin && global_lb <= kmax) {
                // we're good, just figure out the duplicates
                sLOGR << "Success! Bound" << i << "is perfect:" << global_lb
                      << "ub" << global_ub << "kmin" << kmin << "kmax" << kmax;
                if (global_lb < kmin) {
                    // Discard all items equal to the pivot
                    // XXX TODO sure we don't need find_eq_pos for this?
                    stats_.record(timer_.get());
                    return std::make_pair(bounds_[i].ub_it,
                                          min_idx + bounds_[i].ub_pos);
                }

                Result result = _detail::find_eq_pos(
                    global_ub, bounds_[i].ub_pos, bounds_[i].ub_it, global_lb,
                    bounds_[i].lb_pos, bounds_[i].lb_it, min_idx,
                    kmin - global_lb, comm_, debug, short_name);

                stats_.record(timer_.get());
                return result;
            }

            if (global_ub < kmin) {
                ssize_t diff = kmin - global_ub;
                if (diff < best_ub_diff) {
                    sLOGR << "Pair" << i << "improves UB:" << global_ub << "<"
                          << kmin << "diff" << diff << "<" << best_ub_diff;
                    best_ub_diff = diff;
                    best_ub_idx = i;
                }
            }

            if (global_lb > kmax) {
                ssize_t diff = global_lb - kmax;
                if (diff < best_lb_diff) {
                    sLOGR << "Pair" << i << "improves LB:" << global_lb << ">"
                          << kmax << "diff" << diff << "<" << best_lb_diff;
                    best_lb_diff = diff;
                    best_lb_idx = i;
                }
            }
        }

        sLOGR << "Narrowed it to within" << best_ub_diff << "of kmin with pivot"
              << best_ub_idx << "and" << best_lb_diff << "of kmax with"
              << best_lb_idx;

        ssize_t new_min_idx = min_idx, new_max_idx = max_idx, new_kmin = kmin,
                new_kmax = kmax, new_global_size = global_size;
        if (best_ub_idx >= 0) {
            new_min_idx += bounds_[best_ub_idx].ub_pos;
            ssize_t global_ub = gbounds_[ubidx(best_ub_idx)];
            new_kmin -= global_ub;
            new_kmax -= global_ub;
            new_global_size -= global_ub;
        }
        if (best_lb_idx >= 0) {
            new_max_idx = min_idx + bounds_[best_lb_idx].lb_pos;
            ssize_t global_lb = gbounds_[lbidx(best_lb_idx)];
            new_global_size -= (global_size - global_lb);
        }
        tlx_die_unless(new_global_size > 0);
        tlx_die_unless(new_global_size <= global_size);

        sLOGR << "New kmin:" << new_kmin << "kmax:" << new_kmax
              << "size:" << new_global_size;

        // Record magnitude of size change in statistics
        if (new_global_size == global_size)
            stats_.size_unchanged++;
        else if ((global_size - new_global_size) * 50 <= global_size ||
                 (global_size - new_global_size) <= 5)
            stats_.tinychange++;

        stats_.record(timer_.get());
        return select(seq, new_kmin, new_kmax, new_min_idx, new_max_idx,
                      new_global_size);
    }

    constexpr Key get_key(const Iterator &it) {
        if constexpr (std::is_same_v<Key, Elem>) {
            return *it;
        } else {
            return it->first;
        }
    }

    constexpr ssize_t ubidx(int i) {
        return 2 * i;
    }
    constexpr ssize_t lbidx(int i) {
        return 2 * i + 1;
    }

    mpi::communicator &comm_;
    std::mt19937_64 rng_;
    std::vector<Key> pivots_;
    std::vector<Bound> bounds_;
    std::vector<ssize_t> gbounds_;
    mutable _detail::select_stats<time> stats_;
    mutable timer timer_;
};

} // namespace reservoir

#endif // RESERVOIR_AMS_SELECT_MULTI_HEADER


// Copyright (C) 2017 Minhyuk Sung <mhsung@cs.stanford.edu>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
//

#include "LibiglMeshT.h"

#include <sstream>
#include <Eigen/Geometry>
#include <igl/embree/reorient_facets_raycast.h>
#include <igl/per_face_normals.h>
#include <igl/per_vertex_normals.h>
#include <igl/read_triangle_mesh.h>
#include <igl/readPLY.h>
#include <igl/write_triangle_mesh.h>
#include <igl/writePLY.h>
#include <modules/merge_meshes.h>
#include <utils/filesystem/path.h>
#include <utils/google_tools.h>
#include <utils/utils.h>
#include <utils/SparseICP/ICP.h>

// Define input variables.
DEFINE_string(mesh, "", "mesh file.");
DEFINE_string(face_labels, "", "face label file.");
DEFINE_string(point_cloud, "", "point cloud file.");
DEFINE_string(point_labels, "", "point label file.");
DEFINE_string(point_values, "", "point value file.");
DEFINE_string(meshes, "", "a list of mesh files (separated by comma).");
DEFINE_string(mesh_labels, "", "a list of mesh labels (separated by comma).");
DEFINE_double(azimuth_deg, 0.0, "azimuth (degree). "
    "ignored if 'modelview_matrix' is set");
DEFINE_double(elevation_deg, 0.0, "elevation (degree). "
    "ignored if 'modelview_matrix' is set");
DEFINE_double(theta_deg, 0.0, "theta (degree). "
    "ignored if 'modelview_matrix' is set");
DEFINE_string(projection_matrix, "", "projection matrix file.");
DEFINE_string(modelview_matrix, "", "modelview matrix file.");
DEFINE_string(bbox, "", "bounding box file.");
DEFINE_string(snapshot, "", "snapshot file.");
DEFINE_string(out_mesh, "", "output mesh file.");
DEFINE_string(out_face_labels, "", "output face label file.");
DEFINE_string(out_point_cloud, "", "output point cloud file.");
DEFINE_string(out_point_labels, "", "output point label file.");
DEFINE_string(out_projection_matrix, "", "output projection matrix file.");
DEFINE_string(out_modelview_matrix, "", "output modelview matrix file.");
DEFINE_bool(reorient_faces, false, "reorient mesh faces.");

LibiglMeshT::LibiglMeshT()
  : renderer_(nullptr),
    mesh_name_(""),
    V_(MatrixXd(0, 3)),
    F_(MatrixXi(0, 3)),
    VC_(MatrixXf(0, 3)),
    FC_(MatrixXf(0, 3)),
    VL_(VectorXi(0, 3)),
    FL_(VectorXi(0, 3)),
    VN_(MatrixXd(0, 3)),
    FN_(MatrixXd(0, 3)),
    P_(MatrixXd(0, 3)),
    PC_(MatrixXf(0, 3)),
    PL_(VectorXi(0, 3)),
    PN_(MatrixXd(0, 3)),
    bb_min_(Vector3d::Zero()),
    bb_max_(Vector3d::Zero()),
    center_(Vector3d::Zero()),
    radius_(1.0) {
}

LibiglMeshT::LibiglMeshT(LibiglMeshRendererT* _renderer)
  : renderer_(_renderer),
    mesh_name_(""),
    V_(MatrixXd(0, 3)),
    F_(MatrixXi(0, 3)),
    VC_(MatrixXf(0, 3)),
    FC_(MatrixXf(0, 3)),
    VL_(VectorXi(0, 3)),
    FL_(VectorXi(0, 3)),
    VN_(MatrixXd(0, 3)),
    FN_(MatrixXd(0, 3)),
    P_(MatrixXd(0, 3)),
    PC_(MatrixXf(0, 3)),
    PL_(VectorXi(0, 3)),
    PN_(MatrixXd(0, 3)),
    bb_min_(Vector3d::Zero()),
    bb_max_(Vector3d::Zero()),
    center_(Vector3d::Zero()),
    radius_(1.0) {
}

bool LibiglMeshT::read_mesh(const std::string& _filename) {
  if (!igl::read_triangle_mesh(_filename, V_, F_)) {
    LOG(WARNING) << "Can't read the file: '" << _filename << "'.";
    return false;
  }
  mesh_name_ = filesystem::path(_filename).filename();

  update_bounding_box();

  if (renderer_ == nullptr) {
    LOG(WARNING) << "Renderer is not set.";
  } else {
    renderer_->set_mesh(V_, F_);
    renderer_->set_scene_pos(center_.cast<float>(), (float) radius_);
  }

  LOG(INFO) << "Read " << V_.rows() << " vertices and " << F_.rows()
            << " faces.";

  if (FLAGS_reorient_faces) {
    LOG(INFO) << "Reorient mesh faces...";
    Eigen::MatrixXi new_F;
    Eigen::VectorXi I;
    igl::embree::reorient_facets_raycast(V_, F_, new_F, I);
    F_.swap(new_F);
    LOG(INFO) << "Done.";
  }

  return true;
}

/*
bool LibiglMeshT::read_meshes(const std::string& _filenames) {
  const std::vector<std::string> filenames = Utils::split_string(_filenames);

  MatrixXd all_V = MatrixXd(0, 3);
  MatrixXi all_F = MatrixXi(0, 3);
  double offset = 0.0;

  for (const auto filename : filenames) {
    if (!read_mesh(filename)) return false;

    // Centerize.
    V_ = V_.rowwise() - center_.transpose();

    offset -= (1.5 * radius_);

    // Translate.
    V_ = V_.rowwise() + (offset * RowVector3d::UnitX());

    // Add the mesh.
    igl::add_mesh(all_V, all_F, V_, F_);

    // Add offset.
    offset -= (1.5 * radius_);
  }

  V_ = all_V;
  F_ = all_F;
  update_bounding_box();

  // Centerize.
  V_ = V_.rowwise() - center_.transpose();

  if (renderer_ == nullptr) {
    LOG(WARNING) << "Renderer is not set.";
  } else {
    renderer_->set_mesh(V_, F_);
    renderer_->set_scene_pos(center_.cast<float>(), (float) radius_);
  }

  return true;
}
*/

bool LibiglMeshT::read_meshes(const std::string& _filenames) {
  const std::vector<std::string> filenames = Utils::split_string(_filenames);
  const int n_meshes = filenames.size();

  MatrixXd all_V = MatrixXd(0, 3);
  MatrixXi all_F = MatrixXi(0, 3);

  std::vector<int> n_faces;
  int n_total_faces = 0;

  for (const auto filename : filenames) {
    if (!read_mesh(filename)) return false;

    n_faces.push_back(F_.rows());
    n_total_faces += F_.rows();

    // Add the mesh.
    igl::add_mesh(all_V, all_F, V_, F_);
  }

  V_ = all_V;
  F_ = all_F;
  update_bounding_box();

  if (renderer_ == nullptr) {
    LOG(WARNING) << "Renderer is not set.";
  } else {
    renderer_->set_mesh(V_, F_);
    renderer_->set_scene_pos(center_.cast<float>(), (float) radius_);
  }

  // Parse labels if given.
  if (FLAGS_mesh_labels != "") {
    const auto label_strs = Utils::split_string(FLAGS_mesh_labels);
    if (label_strs.size() != n_meshes) {
      LOG(ERROR) << "Numbers of meshes and labels do not match.";
    }
    std::vector<int> labels(n_meshes);
    for (int i = 0; i < n_meshes; ++i) labels[i] = std::stoi(label_strs[i]);

    FL_ = VectorXi(n_total_faces);
    int fid = 0;
    for (int i = 0; i < n_meshes; ++i) {
      for (int j = 0; j < n_faces[i]; ++j) {
        FL_[fid++] = labels[i];
      }
    }

    // Set face colors.
    set_face_label_colors();
  }

  return true;
}

bool LibiglMeshT::read_face_labels(const std::string& _filename) {
  FL_ = VectorXi(n_faces());
  if (!Utils::read_eigen_matrix_from_file(_filename, &FL_)) {
    return false;
  }

  // Set face colors.
  set_face_label_colors();
  return true;
}

bool LibiglMeshT::read_point_cloud(const std::string& _filename) {
  filesystem::path path(_filename);
  const std::string ext = path.extension();
  if (ext == "obj" || ext == "off" || ext == "ply") {
    MatrixXi temp_F;
    if (!igl::read_triangle_mesh(_filename, P_, temp_F)) {
      LOG(WARNING) << "Can't read the file: '" << _filename << "'.";
      return false;
    }
  } else {
    MatrixXd X;

    if (!Utils::read_eigen_matrix_from_file(_filename, &X, ' ')) {
      LOG(WARNING) << "Can't read the file: '" << _filename << "'";
      return false;
    }

    if (X.cols() == 7) {
      // px, py, pz, nx, ny, nz, label
      P_ = MatrixXd(X.rows(), 3);
      P_ = X.leftCols(3);

      PN_ = MatrixXd(X.rows(), 3);
      PN_ = X.middleCols(3, 3);

      PL_ = VectorXi(X.rows());
      PL_ = X.rightCols(1).cast<int>();
    } else if (X.cols() == 6) {
      // px, py, pz, nx, ny, nz
      P_ = MatrixXd(X.rows(), 3);
      P_ = X.leftCols(3);

      PN_ = MatrixXd(X.rows(), 3);
      PN_ = X.rightCols(3);
    } else if (X.cols() == 3) {
      // px, py, pz
      P_ = MatrixXd(X.rows(), 3);
      P_ = X;
    } else {
      LOG(WARNING) << "Wrong file format: '" << _filename << "'.";
      return false;
    }
  }

  update_bounding_box();

  if (renderer_ == nullptr) {
    LOG(WARNING) << "Renderer is not set.";
  } else {
    renderer_->set_point_cloud(P_);
    renderer_->set_scene_pos(center_.cast<float>(), (float) radius_);
  }

  if (P_.rows() == PN_.rows() && P_.rows() == PL_.rows()) {
    LOG(INFO) << "Read " << P_.rows() << " points with normals and labels.";
  } else if (P_.rows() == PN_.rows()) {
    LOG(INFO) << "Read " << P_.rows() << " points with normals.";
  } else {
    LOG(INFO) << "Read " << P_.rows() << " points.";
  }

  return true;
}

bool LibiglMeshT::write_point_cloud(const std::string& _filename) {
  // FIXME:
  // Consider other formats.
  MatrixXi temp_F;
  MatrixXd temp_UV;

	if (P_.rows() == PN_.rows()) {
		if (!igl::writePLY(_filename, P_, temp_F, PN_, temp_UV)) {
			LOG(WARNING) << "Can't write the file: '" << _filename << "'.";
			return false;
		}
		LOG(INFO) << "Wrote " << P_.rows() << " points with normals.";
	}
	else{
		if (!igl::writePLY(_filename, P_, temp_F)) {
			LOG(WARNING) << "Can't write the file: '" << _filename << "'.";
			return false;
		}
		LOG(INFO) << "Wrote " << P_.rows() << " points.";
	}

  return true;
}

bool LibiglMeshT::write_face_labels(const std::string& _filename) {
  if (!Utils::write_eigen_matrix_to_file(_filename, FL_)) {
    return false;
  }
  return true;
}

void LibiglMeshT::set_face_label_colors() {
  if (FL_.rows() != F_.rows()) {
    LOG(WARNING) << "Number of face labels does not match number of faces.";
    return;
  }

  FC_ = MatrixXf(n_faces(), 3);
  for (int fid = 0; fid < n_faces(); ++fid) {
    Vector3f color;
    Utils::random_label_rgb_color(FL_(fid), &color);
    FC_.row(fid) = color.transpose();
  }

  if (renderer_ == nullptr) {
    LOG(WARNING) << "Renderer is not set.";
  } else {
    renderer_->set_face_colors(FC_);
  }
}

bool LibiglMeshT::read_point_labels(const std::string& _filename) {
  if (!Utils::read_eigen_matrix_from_file(_filename, &PL_)) {
    return false;
  }

  // Set point colors.
  set_point_label_colors();
  return true;
}

bool LibiglMeshT::write_point_labels(const std::string& _filename) {
  if (!Utils::write_eigen_matrix_to_file(_filename, PL_)) {
    return false;
  }
  return true;
}

bool LibiglMeshT::read_point_values(const std::string& _filename) {
  VectorXf PV;
  if (!Utils::read_eigen_matrix_from_file(_filename, &PV)) {
    return false;
  }

  if (PV.size() != P_.rows()) {
    LOG(WARNING) << "Number of point values does not match number of points.";
    return false;
  }

  PC_ = compute_color_map(PV);

  if (renderer_ == nullptr) {
    LOG(WARNING) << "Renderer is not set";
  } else {
    renderer_->set_point_colors(PC_);
  }

  return true;
}

void LibiglMeshT::set_point_label_colors() {
  if (PL_.rows() != P_.rows()) {
    LOG(WARNING) << "Number of point labels does not match number of points.";
    return;
  }

  PC_ = MatrixXf(n_points(), 3);
  for (int pid = 0; pid < n_points(); ++pid) {
    Vector3f color;
    Utils::random_label_rgb_color(PL_(pid), &color);
    PC_.row(pid) = color.transpose();
  }

  if (renderer_ == nullptr) {
    LOG(WARNING) << "Renderer is not set.";
  } else {
    renderer_->set_point_colors(PC_);
  }
}

MatrixXf LibiglMeshT::compute_color_map(const VectorXf& _values) {
  const int n_values = _values.size();
  MatrixXf colors = MatrixXf(n_values, 3);
  colors.setZero();

  const float vmin = _values.minCoeff();
  const float vmax = _values.maxCoeff();
  const float dv = vmax - vmin;

  if (dv > 1.0e-8) {
    for (int pid = 0; pid < n_values; ++pid) {
      Vector3f color = Vector3f::Ones();
      const float v = _values[pid];

      // https://stackoverflow.com/questions/7706339/grayscale-to-red-green-blue-matlab-jet-color-scale
      if (v < (vmin + 0.25f * dv)) {
        color[0] = 0.0f;
        color[1] = 4.0f * (v - vmin) / dv;
      } else if (v < (vmin + 0.5f * dv)) {
        color[0] = 0.0f;
        color[2] = 1.0f + 4.0f * (vmin + 0.25f * dv - v) / dv;
      } else if (v < (vmin + 0.75f * dv)) {
        color[0] = 4.0f * (v - vmin - 0.5f * dv) / dv;
        color[2] = 0.0f;
      } else {
        color[1] = 1.0f + 4.0f * (vmin + 0.75f * dv - v) / dv;
        color[2] = 0.0f;
      }

      colors.row(pid) = color.transpose();
    }
  }

  return colors;
}

void LibiglMeshT::update_bounding_box(const MatrixXd& _P) {
  bb_min_ = _P.colwise().minCoeff();
  bb_max_ = _P.colwise().maxCoeff();
  center_ = 0.5 * (bb_min_ + bb_max_);
  radius_ = 0.5 * (bb_max_ - bb_min_).norm();
}

void LibiglMeshT::update_bounding_box() {
  if (V_.rows() == 0 && P_.rows() == 0) {
    return;
  }

  for (int i = 0; i < 3; ++i) {
    bb_min_[i] = +std::numeric_limits<double>::max();
    bb_max_[i] = -std::numeric_limits<double>::max();
  }

  if (V_.rows() > 0) {
    const auto V_bb_min = V_.colwise().minCoeff();
    const auto V_bb_max = V_.colwise().maxCoeff();
    for (int i = 0; i < 3; ++i) {
      bb_min_[i] = std::min(bb_min_[i], V_bb_min[i]);
      bb_max_[i] = std::max(bb_max_[i], V_bb_max[i]);
    }
  }
  else if (P_.rows() > 0) {
    const auto P_bb_min = P_.colwise().minCoeff();
    const auto P_bb_max = P_.colwise().maxCoeff();
    for (int i = 0; i < 3; ++i) {
      bb_min_[i] = std::min(bb_min_[i], P_bb_min[i]);
      bb_max_[i] = std::max(bb_max_[i], P_bb_max[i]);
    }
  }

  center_ = 0.5 * (bb_min_ + bb_max_);
  radius_ = 0.5 * (bb_max_ - bb_min_).norm();
}

bool LibiglMeshT::write_bounding_box(const std::string& _filename) {
  const double bbox_diagonal = (bb_max_ - bb_min_).norm();
  Eigen::VectorXd bb_info(4);
  bb_info << center_, bbox_diagonal;
  if (!Utils::write_eigen_matrix_to_file(_filename, bb_info.transpose())) {
    return false;
  }
  return true;
}

void LibiglMeshT::pre_processing() {
  if (FLAGS_mesh != "") {
    if (!read_mesh(FLAGS_mesh)) {
      return;
    }
  }

  if (FLAGS_meshes != "") {
    if (!read_meshes(FLAGS_meshes)) {
      return;
    }
  }

  if (FLAGS_face_labels != "") {
    if (!read_face_labels(FLAGS_face_labels)) {
      return;
    }
  }

  if (FLAGS_point_cloud != "") {
    if (!read_point_cloud(FLAGS_point_cloud)) {
      return;
    }
  }

  if (FLAGS_point_labels != "") {
    if (!read_point_labels(FLAGS_point_labels)) {
      return;
    }
  }

  if (FLAGS_point_values != "") {
    if (!read_point_values(FLAGS_point_values)) {
      return;
    }
  }
}

void LibiglMeshT::post_processing() {
  if (renderer_ == nullptr) {
    LOG(WARNING) << "Renderer is not set.";
  } else {
    renderer_->set_camera_params(
        Vector3f(FLAGS_azimuth_deg, FLAGS_elevation_deg, FLAGS_theta_deg),
        center_.cast<float>(), (float) radius_);
  }

  if (FLAGS_projection_matrix != "") {
    if (renderer_ == nullptr) {
      LOG(WARNING) << "Renderer is not set.";
    } else if (!renderer_->read_projection(FLAGS_projection_matrix)) {
      return;
    }
  }

  if (FLAGS_modelview_matrix != "") {
    if (renderer_ == nullptr) {
      LOG(WARNING) << "Renderer is not set.";
    } else if (!renderer_->read_modelview(FLAGS_modelview_matrix)) {
      return;
    }
  }

  if (FLAGS_bbox != "") {
    if (!write_bounding_box(FLAGS_bbox)) {
      return;
    }
  }

  if (FLAGS_out_mesh != "") {
    igl::write_triangle_mesh(FLAGS_out_mesh, V_, F_);
  }

  if (FLAGS_out_face_labels != "") {
    if (!write_face_labels(FLAGS_out_face_labels)) {
      return;
    }
  }

  if (FLAGS_out_point_cloud != "") {
    if (!write_point_cloud(FLAGS_out_point_cloud)) {
      return;
    }
  }

  if (FLAGS_out_point_labels != "") {
    if (!write_point_labels(FLAGS_out_point_labels)) {
      return;
    }
  }

  if (FLAGS_out_projection_matrix != "") {
    if (renderer_ == nullptr) {
      LOG(WARNING) << "Renderer is not set";
    } else if (!renderer_->write_projection(FLAGS_out_projection_matrix)) {
      return;
    }
  }

  if (FLAGS_out_modelview_matrix != "") {
    if (renderer_ == nullptr) {
      LOG(WARNING) << "Renderer is not set";
    } else if (!renderer_->write_modelview(FLAGS_out_modelview_matrix)) {
      return;
    }
  }

  // Compute normals.
  if (F_.rows() > 0) igl::per_face_normals(V_, F_, FN_);
  if (F_.rows() > 0 && V_.rows() > 0) igl::per_vertex_normals(V_, F_, FN_, VN_);

  if (renderer_ == nullptr) {
    LOG(WARNING) << "Renderer is not set.";
  } else {
#ifdef USE_OSMESA
    if (FLAGS_snapshot != "") {
      renderer_->snapshot(FLAGS_snapshot);
    }
#else
    renderer_->run_loop();
#endif
  }
}

void LibiglMeshT::parse_arguments_and_run() {
  pre_processing();
  processing();
  post_processing();
}


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern feature constant_attribute
#include <boost/test/unit_test.hpp>
#include "fern/language/feature/core/array_value.h"
#include "fern/language/feature/core/constant_attribute.h"

namespace fl = fern::language;

BOOST_AUTO_TEST_CASE(int_)
{
    // An integer value is stored.
    using Value = int;
    using Attribute = fl::ConstantAttribute<Value>;

    Value value = 5;

    Attribute attribute;
    attribute.set(value);
    BOOST_CHECK_EQUAL(attribute.values().value(), value);
}

BOOST_AUTO_TEST_CASE(array_per_box)
{
    // A pointer to a 2D array is stored.
    using Value = fl::ArrayValue<int, 2>;
    using ValuePtr = std::shared_ptr<fl::ArrayValue<int, 2>>;
    using Attribute = fl::ConstantAttribute<ValuePtr>;

    size_t const nr_rows = 3;
    size_t const nr_cols = 2;
    ValuePtr grid = std::make_shared<Value>(fern::extents[nr_rows][nr_cols]);
    (*grid)[0][0] = -2;
    (*grid)[0][1] = -1;
    (*grid)[1][0] = 0;
    (*grid)[1][1] = 999;
    (*grid)[2][0] = 1;
    (*grid)[2][1] = 2;

    Attribute attribute;
    attribute.set(grid);
    // TODO BOOST_CHECK(*attribute.values().value() == *grid);
}


//  (C) Copyright John Maddock 2005.
//  Use, modification and distribution are 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)

// Verify that the headers can be included, even if their contents
// aren't neccessarily usable by broken compilers.

#ifdef TEST_STD_HEADERS

#include <memory>
#include <array>
#include <complex>
#include <functional>
#include <random>
#include <regex>
#include <tuple>
#include <utility>
#include <type_traits>

#else

#include <boost/tr1/array.hpp>
#include <boost/tr1/complex.hpp>
#include <boost/tr1/functional.hpp>
#include <boost/tr1/memory.hpp>
#include <boost/tr1/random.hpp>
#include <boost/tr1/regex.hpp>
#include <boost/tr1/tuple.hpp>
#include <boost/tr1/type_traits.hpp>
#include <boost/tr1/utility.hpp>

#endif


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2018-2020, LAAS-CNRS, New York University, Max Planck Gesellschaft
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#define BOOST_TEST_NO_MAIN
#define BOOST_TEST_ALTERNATIVE_INIT_API

#include <iterator>
#include <Eigen/Dense>
#include <pinocchio/fwd.hpp>
#include <boost/test/included/unit_test.hpp>
#include <boost/bind.hpp>
#include "crocoddyl/core/diff-action-base.hpp"
#include "crocoddyl/core/actions/diff-lqr.hpp"
#include "crocoddyl/core/numdiff/diff-action.hpp"

using namespace boost::unit_test;

struct TestTypes {
  enum Type { DifferentialActionModelLQR, NbTestTypes };
  static std::vector<Type> init_all() {
    std::vector<Type> v;
    v.clear();
    for (int i = 0; i < NbTestTypes; ++i) {
      v.push_back((Type)i);
    }
    return v;
  }
  static const std::vector<Type> all;
};
const std::vector<TestTypes::Type> TestTypes::all(TestTypes::init_all());

class DifferentialActionModelFactory {
 public:
  DifferentialActionModelFactory(TestTypes::Type type) {
    // build the DifferentialActionModelLQR
    nq_ = 40;
    nu_ = 40;
    driftfree_ = true;
    num_diff_modifier_ = 1e4;
    test_type_ = type;

    switch (test_type_) {
      case TestTypes::DifferentialActionModelLQR:
        diff_action_model_ = boost::make_shared<crocoddyl::DifferentialActionModelLQR>(nq_, nu_, driftfree_);
        break;
      default:
        throw std::runtime_error(__FILE__ ": Wrong TestTypes::Type given");
        break;
    }
  }

  ~DifferentialActionModelFactory() {}

  boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> get_diff_action_model() { return diff_action_model_; }

  double get_num_diff_modifier() { return num_diff_modifier_; }

 private:
  std::size_t nq_;
  std::size_t nu_;
  double num_diff_modifier_;
  bool driftfree_;
  TestTypes::Type test_type_;
  boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> diff_action_model_;
};

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

void test_construct_data(TestTypes::Type test_type) {
  // create the model
  DifferentialActionModelFactory factory(test_type);
  const boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract>& model = factory.get_diff_action_model();

  // create the corresponding data object
  const boost::shared_ptr<crocoddyl::DifferentialActionDataAbstract>& data = model->createData();
}

void test_calc_returns_state(TestTypes::Type test_type) {
  // create the model
  DifferentialActionModelFactory factory(test_type);
  const boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract>& model = factory.get_diff_action_model();

  // create the corresponding data object
  const boost::shared_ptr<crocoddyl::DifferentialActionDataAbstract>& data = model->createData();

  // Generating random state and control vectors
  const Eigen::VectorXd& x = model->get_state()->rand();
  const Eigen::VectorXd& u = Eigen::VectorXd::Random(model->get_nu());

  // Getting the state dimension from calc() call
  model->calc(data, x, u);

  BOOST_CHECK(static_cast<std::size_t>(data->xout.size()) == model->get_state()->get_nv());
}

void test_calc_returns_a_cost(TestTypes::Type test_type) {
  // create the model
  DifferentialActionModelFactory factory(test_type);
  const boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract>& model = factory.get_diff_action_model();

  // create the corresponding data object and set the cost to nan
  const boost::shared_ptr<crocoddyl::DifferentialActionDataAbstract>& data = model->createData();
  data->cost = nan("");

  // Getting the cost value computed by calc()
  const Eigen::VectorXd& x = model->get_state()->rand();
  const Eigen::VectorXd& u = Eigen::VectorXd::Random(model->get_nu());
  model->calc(data, x, u);

  // Checking that calc returns a cost value
  BOOST_CHECK(!std::isnan(data->cost));
}

void test_partial_derivatives_against_numdiff(TestTypes::Type test_type) {
  // create the model
  DifferentialActionModelFactory factory(test_type);
  const boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract>& model = factory.get_diff_action_model();

  // create the corresponding data object and set the cost to nan
  const boost::shared_ptr<crocoddyl::DifferentialActionDataAbstract>& data = model->createData();

  crocoddyl::DifferentialActionModelNumDiff model_num_diff(model);
  const boost::shared_ptr<crocoddyl::DifferentialActionDataAbstract>& data_num_diff = model_num_diff.createData();

  // Generating random values for the state and control
  const Eigen::VectorXd& x = model->get_state()->rand();
  const Eigen::VectorXd& u = Eigen::VectorXd::Random(model->get_nu());

  // Computing the action derivatives
  model->calcDiff(data, x, u);
  model_num_diff.calcDiff(data_num_diff, x, u);

  // Checking the partial derivatives against NumDiff
  double tol = factory.get_num_diff_modifier() * model_num_diff.get_disturbance();
  BOOST_CHECK((data->Fx - data_num_diff->Fx).isMuchSmallerThan(1.0, tol));
  BOOST_CHECK((data->Fu - data_num_diff->Fu).isMuchSmallerThan(1.0, tol));
  BOOST_CHECK((data->Lx - data_num_diff->Lx).isMuchSmallerThan(1.0, tol));
  BOOST_CHECK((data->Lu - data_num_diff->Lu).isMuchSmallerThan(1.0, tol));
  if (model_num_diff.get_with_gauss_approx()) {
    BOOST_CHECK((data->Lxx - data_num_diff->Lxx).isMuchSmallerThan(1.0, tol));
    BOOST_CHECK((data->Lxu - data_num_diff->Lxu).isMuchSmallerThan(1.0, tol));
    BOOST_CHECK((data->Luu - data_num_diff->Luu).isMuchSmallerThan(1.0, tol));
  } else {
    BOOST_CHECK((data_num_diff->Lxx).isMuchSmallerThan(1.0, tol));
    BOOST_CHECK((data_num_diff->Lxu).isMuchSmallerThan(1.0, tol));
    BOOST_CHECK((data_num_diff->Luu).isMuchSmallerThan(1.0, tol));
  }
}

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

void register_action_model_unit_tests(TestTypes::Type test_type) {
  framework::master_test_suite().add(BOOST_TEST_CASE(boost::bind(&test_construct_data, test_type)));
  framework::master_test_suite().add(BOOST_TEST_CASE(boost::bind(&test_calc_returns_state, test_type)));
  framework::master_test_suite().add(BOOST_TEST_CASE(boost::bind(&test_calc_returns_a_cost, test_type)));
  framework::master_test_suite().add(
      BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff, test_type)));
}

bool init_function() {
  for (size_t i = 0; i < TestTypes::all.size(); ++i) {
    register_action_model_unit_tests(TestTypes::all[i]);
  }
  return true;
}

int main(int argc, char** argv) { return ::boost::unit_test::unit_test_main(&init_function, argc, argv); }