/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Burden, R.L., Faires, J.D. Numerical Analysis, 7th Edition, Books/Cole, 2001.
 *
 */

#define BOOST_TEST_MAIN

#include <cmath>
#include <limits>
#include <map>

#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Mathematics/Statistics/simpleLinearRegression.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_simple_linear_regression )

//! Test if simple linear regression method computes fit correctly.
BOOST_AUTO_TEST_CASE( testSimpleLinearRegressionBF )
{
    // Test 1: Test implementation of simple linear regression method against benchmark data from
    //         pg. 487, example 1 of (Burden and Faires, 2001). Standard deviations were benchmarked
    //         using MATLAB's lscov() function.

    // Benchmark data.
    std::map< double, double > benchmarkInputData;
    benchmarkInputData[ 1.0 ] = 1.3;
    benchmarkInputData[ 2.0 ] = 3.5;
    benchmarkInputData[ 3.0 ] = 4.2;
    benchmarkInputData[ 4.0 ] = 5.0;
    benchmarkInputData[ 5.0 ] = 7.0;
    benchmarkInputData[ 6.0 ] = 8.8;
    benchmarkInputData[ 7.0 ] = 10.1;
    benchmarkInputData[ 8.0 ] = 12.5;
    benchmarkInputData[ 9.0 ] = 13.0;
    benchmarkInputData[ 10.0 ] = 15.6;

    // Expected coefficients of linear fit.
    const double expectedCoefficientOfConstantTerm = -0.359999999999999999;
    const double expectedCoefficientOfLinearTerm = 1.5381818181818181818;

    // Expected standard deviations of fit coefficients.
    const double expectedStandardDeviationOfCoefficientOfConstantTerm = 0.369832066721825;
    const double expectedStandardDeviationOfCoefficientOfLinearTerm = 0.059603834439483;

    // Expected chi-squared value.
    const double expectedChiSquared = 2.344727272727272;

    // Declare simple linear regression object and set input data.
    statistics::SimpleLinearRegression simpleLinearRegression( benchmarkInputData );

    // Compute linear fit.
    simpleLinearRegression.computeFit( );

    // Check that computed coefficient of constant term matches expected value.
    BOOST_CHECK_CLOSE_FRACTION( expectedCoefficientOfConstantTerm,
                                simpleLinearRegression.getCoefficientOfConstantTerm( ),
                                1.0e-14 );

    // Check that computed coefficient of linear term matches expected value.
    BOOST_CHECK_CLOSE_FRACTION( expectedCoefficientOfLinearTerm,
                                simpleLinearRegression.getCoefficientOfLinearTerm( ),
                                1.0e-15 );

    // Compute linear fit errors.
    simpleLinearRegression.computeFitErrors( );

    // Check that computed standard deviation of coefficient of constant term matches expected
    // value.
    BOOST_CHECK_CLOSE_FRACTION( expectedStandardDeviationOfCoefficientOfConstantTerm,
                                simpleLinearRegression
                                .getStandardDeviationOfCoefficientOfConstantTerm( ),
                                1.0e-13 );

    // Check that computed standard deviation of coefficient of linear term matches expected
    // value.
    BOOST_CHECK_CLOSE_FRACTION( expectedStandardDeviationOfCoefficientOfLinearTerm,
                                simpleLinearRegression
                                .getStandardDeviationOfCoefficientOfLinearTerm( ),
                                1.0e-13 );

    // Check that computed chi-squared fit matches expected value.
    BOOST_CHECK_CLOSE_FRACTION( expectedChiSquared,
                                simpleLinearRegression.getChiSquared( ),
                                1.0e-15 );
}

BOOST_AUTO_TEST_CASE( testSimpleLinearRegressionHorizontal )
{
    // Test 2: Test implementation of simple linear regression method in case of sample points
    // coinciding with the x-axis.

    std::map< double, double > benchmarkInputData;
    benchmarkInputData[ 1.0 ] = 0.0;
    benchmarkInputData[ 2.0 ] = 0.0;
    benchmarkInputData[ 3.0 ] = 0.0;
    benchmarkInputData[ 4.0 ] = 0.0;
    benchmarkInputData[ 5.0 ] = 0.0;
    benchmarkInputData[ 6.0 ] = 0.0;
    benchmarkInputData[ 7.0 ] = 0.0;
    benchmarkInputData[ 8.0 ] = 0.0;
    benchmarkInputData[ 9.0 ] = 0.0;
    benchmarkInputData[ 10.0 ] = 0.0;

    // Declare simple linear regression object and set input data.
    statistics::SimpleLinearRegression simpleLinearRegression( benchmarkInputData );

    // Compute linear fit.
    simpleLinearRegression.computeFit( );

    // Check that computed coefficient of constant term is zero.
    BOOST_CHECK_SMALL( simpleLinearRegression.getCoefficientOfConstantTerm( ),
                       std::numeric_limits< double >::min( ) );

    // Check that computed coefficient of linear term matches is zero.
    BOOST_CHECK_SMALL( simpleLinearRegression.getCoefficientOfLinearTerm( ),
                       std::numeric_limits< double >::min( ) );

    // Compute linear fit errors.
    simpleLinearRegression.computeFitErrors( );

    // Check that computed standard deviation of coefficient of constant term is zero.
    BOOST_CHECK_SMALL( simpleLinearRegression.getStandardDeviationOfCoefficientOfConstantTerm( ),
                       std::numeric_limits< double >::min( ) );

    // Check that computed standard deviation of coefficient of linear term is zero.
    BOOST_CHECK_SMALL( simpleLinearRegression.getStandardDeviationOfCoefficientOfLinearTerm( ),
                       std::numeric_limits< double >::min( ) );

    // Check that computed chi-squared fit is zero.
    BOOST_CHECK_SMALL( simpleLinearRegression.getChiSquared( ),
                       std::numeric_limits< double >::min( ) );
}

BOOST_AUTO_TEST_CASE( testSimpleLinearRegressionVertical )
{
    // Test 3: Test implementation of simple linear regression method in case of sample points
    // coinciding with the y-axis.

    std::map< double, double > benchmarkInputData;
    benchmarkInputData[ 0.0 ] = 1.3;
    benchmarkInputData[ 0.0 ] = 3.5;
    benchmarkInputData[ 0.0 ] = 4.2;
    benchmarkInputData[ 0.0 ] = 5.0;
    benchmarkInputData[ 0.0 ] = 7.0;
    benchmarkInputData[ 0.0 ] = 8.8;
    benchmarkInputData[ 0.0 ] = 10.1;
    benchmarkInputData[ 0.0 ] = 12.5;
    benchmarkInputData[ 0.0 ] = 13.0;
    benchmarkInputData[ 0.0 ] = 15.6;

    // Declare simple linear regression object and set input data.
    statistics::SimpleLinearRegression simpleLinearRegression( benchmarkInputData );

    // Compute linear fit.
    simpleLinearRegression.computeFit( );

    // Check that computed coefficient of constant term matches expected value.
    BOOST_CHECK( boost::math::isnan( simpleLinearRegression.getCoefficientOfConstantTerm( ) ) );

    // Check that computed coefficient of linear term matches expected value.
    BOOST_CHECK( boost::math::isnan( simpleLinearRegression.getCoefficientOfLinearTerm( ) ) );

    // Compute linear fit errors.
    simpleLinearRegression.computeFitErrors( );

    // Check that computed standard deviation of coefficient of constant term matches expected
    // value.
    BOOST_CHECK( boost::math::isnan( simpleLinearRegression
                             .getStandardDeviationOfCoefficientOfConstantTerm( ) ) );

    // Check that computed standard deviation of coefficient of linear term matches expected
    // value.
    BOOST_CHECK( boost::math::isnan(simpleLinearRegression
                            .getStandardDeviationOfCoefficientOfLinearTerm( ) ) );

    // Check that computed chi-squared fit matches expected value.
    BOOST_CHECK( boost::math::isnan(simpleLinearRegression.getChiSquared( ) ) );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#include <catch2/catch.hpp>

#include <Eigen/Dense>

#include <cannon/ml/rls.hpp>
#include <cannon/log/registry.hpp>
#include <cannon/math/nearly_equal.hpp>

using namespace Eigen;

using namespace cannon::ml;
using namespace cannon::log;
using namespace cannon::math;

TEST_CASE("RLS", "[ml]") {
  // Basic correctness
  RLSFilter f(1, 1);

  VectorXd xs = VectorXd::LinSpaced(10, 0.0, 1.0);
  VectorXd ys = 5.0 * xs;

  log_info("xs is", xs, "ys is", ys);

  for (int i = 0; i < 10; i++) {
    VectorXd tmp_x(1);
    tmp_x << xs[i];
    VectorXd tmp_y(1);
    tmp_y << ys[i];
    f.process_datum(tmp_x, tmp_y);
  }

  for (int i = 0; i < 10; i++) {
    VectorXd tmp_x(1);
    tmp_x << xs[i];
    VectorXd tmp_y(1);
    tmp_y << ys[i];
    log_info("Predicted", f.predict(tmp_x), "for actual", tmp_y);
    REQUIRE((f.predict(tmp_x) - tmp_y).norm() < 0.01);
  }

  f.reset();
  MatrixXd t;
  VectorXd i;
  std::tie(t, i) = f.get_identified_mats();

  REQUIRE(t(0,0) == 0.0);
  REQUIRE(i[0] == 0.0);
}


//
//  ShamirGenerator.cpp
//  edgeRuntime
//
//  Created by Abdelrahaman Aly on 20/11/13.
//  Copyright (c) 2013 Abdelrahaman Aly. All rights reserved.
//

//Generic Headers
#include <iostream>

//Library Headers
#include <NTL/ZZ_p.h>
#include <NTL/ZZ.h>

//Custom Headers
#include "List.h"
#include "Constants.h"
#include "StandardPlayer.h"
#include "StandardShare.h"
#include "ShamirGenerator.h"

using namespace NTL;
namespace ShareGenerators
{
    //Method implementation of ShamirGenerator

    //Constructor
    ShareGenerators::ShamirGenerator::ShamirGenerator(Players::StandardPlayer * player, long p)
    {
        //Initialize the Random number generator's seed using the system clocks
        srand((unsigned)time(0));
        //Initialize variables
        player_=player;
        this->p_=p;
    };

    //Destructor
    ShareGenerators::ShamirGenerator::~ShamirGenerator(){

    };

    //generate the shares
    int ShamirGenerator::generateShares(long secret, int players, Utils::List<Shares::StandardShare> *list)
    {
        //Gets the slope for the equation randomically
        ZZ_p slope= conv<ZZ_p>(rand()% this->p_-1);
        //TODO: The process to obtain the shares should be based on the T-1 paradigm.
        // hence the construction of the shares will depend on the number of players. there is a formula to do that to build polinomials of n deegree.
        for (ZZ i=conv<ZZ>(0); i< players;i++)
        {
            //creates the share TODO: take the process to the constructor
            Shares::StandardShare * aux =new Shares::StandardShare();

            aux->setPlayerId(player_->getPlayer());
            aux->setValue(conv<long>(slope*(conv<ZZ_p>(i)+ conv<ZZ_p>(1)) +conv<ZZ_p>(secret) ));

            list->add(aux);

        }

        return 1;

    };
    //generates the multiplication Regenerations
     int ShamirGenerator::multiplicationRegeneration(Shares::StandardShare * share, int players,Utils::List<Shares::StandardShare> * list)
    {
        //TODO: Add modulus operation that is why is here
        //TODO: modify this to make it authomatic in the number of players maybe slower though
        ZZ_p result=3*(conv<ZZ_p>(list->get(0)->getValue())) -3*conv<ZZ_p>((list->get(1)->getValue()))+conv<ZZ_p>((list->get(2)->getValue()));
        long value= conv<long>(result);
        share->setValue(value);
        return  1;
    };

}


#include <iostream>
#include <Eigen/Dense>
#include "testing_functions.h"

using namespace std;
using namespace Eigen;

// This function checks the equality of two doubles to make sure they are within some epsilon
// It will output a 0 or a 1.
// 0 means not equal
// 1 means equal
bool isEqualDouble(double a, double b, double epsilon) {   // Works as intended. 2021/11/14
    return abs(a - b) <= epsilon;
} // Tested. Works correctly. 2021/11/14

// This function checks the equality of two 1D arrays of doubles to make sure they are within some epsilon
// It will output a same size array of booleans
// 0 means not equal
// 1 means equal
Array<bool,Dynamic,1> isEqualArray1D(ArrayXd &a, ArrayXd &b, double epsilon) {
    // Get the size of a
    // This function assumes a and b have the same number of elements
    int N = a.size();

    // Initialize boolean array
    Array<bool,Dynamic,1> testEquality(N);

    // Iterate through entire array
    // Use isEqualDouble function to determine if each individual element is equal in the double sense
    for (int i = 0; i < N; i++){
        testEquality(i) = isEqualDouble( a(i), b(i), epsilon );
    }
    return testEquality;
} // Tested. Works correctly. 2021/11/16

// This function checks the equality of two 2D arrays of doubles to make sure they are within some epsilon
// It will output a same size array of booleans
// 0 means not equal
// 1 means equal
Array<bool,Dynamic,Dynamic> isEqualArray2D(ArrayXXd &a, ArrayXXd &b, double epsilon) {
    // Get the size of a
    // This function assumes a and b have the same number of elements
    int NR = a.rows();
    int NC = a.cols();

    // Initialize boolean array
    Array<bool,Dynamic,Dynamic> testEquality(NR,NC);

    // Iterate through entire array
    // Use isEqualDouble function to determine if each individual element is equal in the double sense
    for (int i = 0; i < NR; i++){
        for (int j = 0; j < NC; j++){
            testEquality(i,j) = isEqualDouble( a(i,j), b(i,j), epsilon );
        }
    }
    return testEquality;
} // Tested. Works correctly. 2021/11/16

// This function checks the equality of two 1D arrays of 1D arrays of doubles to make sure they are within some epsilon
// It will output a same size array of arrays of booleans
// 0 means not equal
// 1 means equal
Array<Array<bool,Dynamic,1>, 3, 1> isEqualArray1D_of_Array1D(Array<ArrayXd, 3, 1> &a, Array<ArrayXd, 3, 1> &b, double epsilon) {
    // This function assumes a and b have the same number of elements for each internal array

    // Initialize the boolean array of arrays
    Array<Array<bool,Dynamic,1>, 3, 1> testEquality;

    // We know that these arrays have a size of 3.
    // Iterate through top array
    for (int i = 0; i < 3; i++) {
        // Can use the isEqualArray1D function for the internal arrays
        testEquality(i) = isEqualArray1D(a(i), b(i), epsilon);
    }
    return testEquality;
} // Tested. Works correctly. 2021/11/21


/**
 * @file
 * @brief NPDE homework ConvBLFMatrixProvider
 * @author Ralf Hiptmair
 * @date May 2021
 * @copyright Developed at SAM, ETH Zurich
 */

#include "../convblfmatrixprovider.h"

#include <gtest/gtest.h>

#include <Eigen/Core>
#include <iostream>

namespace cblfdemo::test {

TEST(ConvBLFMatrixProvider, Test) {
  /* Macros available in the Google test framework:
     EXPECT_EQ(x,y), EXPECT_NE(x,y), EXPECT_LT(x,y), EXPECT_LE(x,y),
     EXPECT_GT(x,y), EXPECT_GE(x,y) EXPECT_STREQ(x,y), EXPECT_STRNE(x,y) -> for
     C-strings only ! EXPECT_NEAR(x,y,abs_tol) All testing macros can output a
     message by a trailing << ....
   */
  // Obtain a purely triangular mesh from the collection of LehrFEM++'s
  // built-in meshes
  std::shared_ptr<lf::mesh::Mesh> mesh_p{
      lf::mesh::test_utils::GenerateHybrid2DTestMesh(3)};
  // vectors for testing
  const Eigen::Vector2d a(1.0, 2.0);
  const Eigen::Vector2d b(3.0, 2.0);
  // Run test computation
  const double itg = cblfdemo::testCDBLF(mesh_p, a, b);
  EXPECT_NEAR(itg, 63, 1E-6);
}
}  // namespace cblfdemo::test


//
//  Meshrectangular.hpp
//  hybrid_fem_bie
//
//  Created by Max on 2/5/18.
//
//

#ifndef Meshrectangular_hpp
#define Meshrectangular_hpp

#include <stdio.h>
#include <Eigen/Eigen>
#include "share_header.hpp"
using namespace Eigen;

void Meshrectangular(double x_min, double x_max, double y_min, double y_max, double dx ,double dy ,int nx, int ny , MatrixXd &Node, MatrixXi_rm &Element);

#endif /* Meshrectangular_hpp */


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file normalize_test.cpp
/// \brief Test for normalization functions.

#include <functional>
#include <iostream>
#include <boost/test/minimal.hpp>
#include "mcrl2/utilities/detail/test_operation.h"
#include "mcrl2/lps/specification.h"
#include "mcrl2/lps/linearise.h"
#include "mcrl2/modal_formula/parse.h"
#include "mcrl2/pbes/pbes.h"
#include "mcrl2/pbes/lps2pbes.h"
#include "mcrl2/pbes/normalize.h"
#include "mcrl2/pbes/parse.h"
#include "mcrl2/pbes/rewriter.h"
#include "mcrl2/pbes/detail/normalize_and_or.h"

using namespace mcrl2;
using namespace mcrl2::pbes_system;

void test_normalize1()
{
  pbes_expression x = propositional_variable_instantiation("x:X");
  pbes_expression y = propositional_variable_instantiation("y:Y");
  pbes_expression z = propositional_variable_instantiation("z:Z");
  pbes_expression f;
  pbes_expression f1;
  pbes_expression f2;

  f = not_(x);
  f = not_(f); // N.B. not_(not_(x)) does not work!
  f1 = pbes_system::normalize(f);
  f2 = x;
  std::cout << "f  = " << f  << std::endl;
  std::cout << "f1 = " << f1 << std::endl;
  std::cout << "f2 = " << f2 << std::endl;
  BOOST_CHECK(f1 == f2);

  f = imp(not_(x), y);
  f1 = pbes_system::normalize(f);
  f2 = or_(x, y);
  std::cout << "f  = " << f  << std::endl;
  std::cout << "f1 = " << f1 << std::endl;
  std::cout << "f2 = " << f2 << std::endl;
  BOOST_CHECK(f1 == f2);

  f  = not_(and_(not_(x), not_(y)));
  f1 = pbes_system::normalize(f);
  f2 = or_(x, y);
  std::cout << "f  = " << f << std::endl;
  std::cout << "f1 = " << f1 << std::endl;
  std::cout << "f2 = " << f2 << std::endl;
  BOOST_CHECK(f1 == f2);

  f  = imp(and_(not_(x), not_(y)), z);
  f1 = pbes_system::normalize(f);
  f2 = or_(or_(x, y), z);
  std::cout << "f  = " << f << std::endl;
  std::cout << "f1 = " << f1 << std::endl;
  std::cout << "f2 = " << f2 << std::endl;
  BOOST_CHECK(f1 == f2);

  x = data::variable("x", data::sort_bool::bool_());
  y = data::variable("y", data::sort_bool::bool_());
  z = data::variable("z", data::sort_bool::bool_());
  const data::data_expression& x1 = atermpp::down_cast<data::data_expression>(x);
  const data::data_expression& y1 = atermpp::down_cast<data::data_expression>(y);

  f  = not_(x);
  f1 = pbes_system::normalize(f);
  f2 = data::sort_bool::not_(x1);
  std::cout << "f  = " << f << std::endl;
  std::cout << "f1 = " << f1 << std::endl;
  std::cout << "f2 = " << f2 << std::endl;
  BOOST_CHECK(f1 == f2);

  f  = imp(and_(x, y), z);
  f1 = pbes_system::normalize(f);
  f2 = or_(or_(data::sort_bool::not_(x1), data::sort_bool::not_(y1)), z);
  std::cout << "f  = " << f << std::endl;
  std::cout << "f1 = " << f1 << std::endl;
  std::cout << "f2 = " << f2 << std::endl;
  BOOST_CHECK(f1 == f2);

  pbes_expression T = true_();
  pbes_expression F = false_();
  x = pbes_expression(atermpp::aterm_appl(core::detail::function_symbol_PBESImp(), T, F));
  y = pbes_system::normalize(x);
  std::cout << "x = " << x << std::endl;
  std::cout << "y = " << y << std::endl;

  data::variable_list ab = { data::variable("s", data::basic_sort("S")) };
  x = propositional_variable_instantiation("x:X");
  y = and_(x, imp(pbes_expression(atermpp::aterm_appl(core::detail::function_symbol_PBESAnd(), false_(), false_())), false_()));
  z = pbes_system::normalize(y);
  std::cout << "y = " << y << std::endl;
  std::cout << "z = " << z << std::endl;
}

void test_normalize2()
{
  // test case from Aad Mathijssen, 2/11/2008
  lps::specification spec=remove_stochastic_operators(lps::linearise("init tau + tau;"));
  state_formulas::state_formula formula  = state_formulas::parse_state_formula("nu X. [true]X", spec);
  bool timed = false;
  pbes_system::pbes p = pbes_system::lps2pbes(spec, formula, timed);
  pbes_system::normalize(p);
}

void test_normalize3()
{
  // test case from Aad Mathijssen, 1-4-2008
  lps::specification spec=remove_stochastic_operators(lps::linearise(
                              "proc P = tau.P;\n"
                              "init P;        \n"));
  state_formulas::state_formula formula = state_formulas::parse_state_formula("![true*]<true>true", spec);
  bool timed = false;
  pbes_system::pbes p = pbes_system::lps2pbes(spec, formula, timed);
  pbes_system::normalize(p);
}

const std::string VARIABLE_SPECIFICATION =
  "datavar         \n"
  "  b:  Bool;     \n"
  "  b1: Bool;     \n"
  "  b2: Bool;     \n"
  "  b3: Bool;     \n"
  "                \n"
  "  n:  Nat;      \n"
  "  n1: Nat;      \n"
  "  n2: Nat;      \n"
  "  n3: Nat;      \n"
  "                \n"
  "  p:  Pos;      \n"
  "  p1: Pos;      \n"
  "  p2: Pos;      \n"
  "  p3: Pos;      \n"
  "                \n"
  "predvar         \n"
  "  X;            \n"
  "  Y: Nat;       \n"
  "  Z: Bool, Pos; \n"
  ;

inline
pbes_system::pbes_expression expr(const std::string& text)
{
  return pbes_system::parse_pbes_expression(text, VARIABLE_SPECIFICATION);
}

inline
pbes_expression parse(const std::string& expr)
{
  std::string var_decl =
    "datavar    \n"
    "  m: Nat;  \n"
    "  n: Nat;  \n"
    "           \n"
    "predvar    \n"
    "  X;       \n"
    "  Y;       \n"
    "  Z;       \n"
    ;

  std::string data_spec = "";
  return pbes_system::parse_pbes_expression(expr, var_decl, data_spec);
}

inline
pbes_expression norm(const pbes_expression& x)
{
  return pbes_system::detail::normalize_and_or(x);
}

void test_normalize_and_or_equality(const std::string& expr1, const std::string& expr2)
{
  BOOST_CHECK(utilities::detail::test_operation(
    expr1,
    expr2,
    parse,
    std::equal_to<pbes_expression>(),
    norm,
    "normalize_and_or",
    norm,
    "normalize_and_or"
  ));
}

void test_normalize_and_or()
{
  test_normalize_and_or_equality("X && Y", "Y && X");
  test_normalize_and_or_equality("X && X && Y", "X && Y && X");
  test_normalize_and_or_equality("X && X && Y", "Y && X && X");
}

int test_main(int argc, char** argv)
{
  test_normalize1();
  test_normalize2();
  test_normalize3();
  test_normalize_and_or();

  return 0;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015.
//  Copyright Nikhar Agrawal 2015.
//  Copyright Paul Bristow 2015.
//  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
//!\brief Tests round-trip for fixed_point negatable with 999 decimal digits.

#define BOOST_TEST_MODULE test_negatable_round_trip_digits10_999
#define BOOST_LIB_DIAGNOSTIC

#include <algorithm>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <string>

#include <boost/cstdint.hpp>
#include <boost/fixed_point/fixed_point.hpp>
#include <boost/generator_iterator.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/random.hpp>
#include <boost/test/included/unit_test.hpp>

namespace local
{
  // Define a binary fixed-point type with 999 decimal digits of precision.
  typedef
  boost::fixed_point::negatable<0,
                                -3320,
                                boost::fixed_point::round::nearest_even>
  fixed_point_type;

  BOOST_CONSTEXPR_OR_CONST std::string::size_type digits10_string_length =
    ((long(std::numeric_limits<fixed_point_type>::digits) - 1L) * 301L) / 1000L;

  bool round_trip(const fixed_point_type& x);
}

bool local::round_trip(const local::fixed_point_type& x)
{
  using local::fixed_point_type;

  std::stringstream ss1;

  ss1 << std::setprecision(std::numeric_limits<fixed_point_type>::digits10)
      << std::fixed
      << x;

  std::stringstream ss2(ss1.str());

  fixed_point_type y;
  ss2 >> y;

  const bool b(x == y);

  return b;
}

BOOST_AUTO_TEST_CASE(test_negatable_round_trip_digits10_999)
{
  using local::fixed_point_type;

  typedef fixed_point_type::float_type floating_point_type;

  typedef boost::mt19937 random_generator_type;

  const boost::uniform_int<boost::uint_fast8_t> uniform_bit_range(UINT8_C(0), UINT8_C(1));

  boost::variate_generator<random_generator_type,
                           boost::uniform_int<boost::uint_fast8_t>>
  radom_bit_maker(random_generator_type(), uniform_bit_range);

  boost::uint_fast32_t count = UINT32_C(0);

  BOOST_CONSTEXPR_OR_CONST boost::uint_fast32_t number_of_test_cases = UINT16_C(2000);

  bool b = true;

  // Test random values with 999 decimal digits of precision.
  for( ; ((count < number_of_test_cases) && b); ++count)
  {
    typedef
      boost::fixed_point::detail::integer_type_helper<fixed_point_type::all_bits>::exact_unsigned_type
    unsigned_integral_type;

    unsigned_integral_type u(0);

    for(int i = 0; i < int((-fixed_point_type::resolution - 2)); ++i)
    {
      u |= unsigned_integral_type(radom_bit_maker()) << i;
    }

    std::stringstream ss1;

    ss1 << u;

    std::string str(ss1.str());

    str.insert(std::string::size_type(0U),
               local::digits10_string_length - (std::min)(local::digits10_string_length, str.length()),
               char('0'));

    str.insert(std::string::size_type(0U), "0.");

    const fixed_point_type x(boost::lexical_cast<floating_point_type>(str));

    const bool next_test_result = local::round_trip(x);

    b = (b && next_test_result);
  }

  BOOST_CHECK_EQUAL(count, number_of_test_cases);

  BOOST_CHECK_EQUAL(b, true);
}


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>

TEST(MathFunctions, abs) {
  using stan::math::abs;

  double y = 2.0;
  EXPECT_FLOAT_EQ(2.0, abs(y));

  y = 128745.72;
  EXPECT_FLOAT_EQ(128745.72, abs(y));

  y = -y;
  EXPECT_FLOAT_EQ(128745.72, abs(y));

  y = -1.3;
  EXPECT_FLOAT_EQ(1.3, abs(y));

  int z = 10; // promoted to double by abs(double)
  EXPECT_FLOAT_EQ(10.0, abs(z));

}

TEST(MathFunctions, abs2){
  double yy=0;
  yy=0;
  EXPECT_FLOAT_EQ(0, stan::math::abs(yy));
}

TEST(MathFunctions, abs_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::abs(nan));
}


#include <Eigen/Dense>
#include <math.h>
#include <optimization.h>

namespace al = alglib;

class LieMPC {

  public:
  Eigen::Matrix<double, 18, 1> x; // State vector
  Eigen::Matrix<double, 6, 1> y;  // Output vector
  Eigen::Vector4d u;              // Control input vector
  Eigen::Vector4d u_lin;          // Linearization control input
  Eigen::VectorXd Y;              // Target trajectory

  private:
  // MPC parameters
  int _nx = 12; // Number of states
  int _ny = 6;  // Number of outputs
  int _nu = 4;  // Number of control inputs
  int _np;      // Prediction steps
  double _ts;   // Time step (s)

  // Linear system Matrices
  Eigen::Matrix<double, 12, 12> _Ac = Eigen::Matrix<double, 12, 12>::Zero(); // Continuous state matrix
  Eigen::Matrix<double, 12, 4> _Bc = Eigen::Matrix<double, 12, 4>::Zero();   // Continuous input matrix
  Eigen::Matrix<double, 12, 4> _Bcg;                                         // Input matrix with direct allocation
  Eigen::Matrix<double, 12, 1> _f;                                           // Constant offset matrix
  Eigen::Matrix<double, 12, 1> _f_t;                                         // Time-variant offset matrix
  Eigen::Matrix<double, 12, 12> _A;                                          // Discrete state matrix
  Eigen::Matrix<double, 12, 4> _B;                                           // Discrete input matrix
  Eigen::Matrix<double, 6, 12> _C = Eigen::Matrix<double, 6, 12>::Zero();    // Discrete output matrix

  // Misc matrix definitions
  Eigen::Matrix<double, 4, 4> _Gamma;               // Control allocation matrix
  Eigen::Matrix3d _I = Eigen::Matrix3d::Identity(); // 3x3 identity matrix
  Eigen::Vector3d _1_3 = Eigen::Vector3d(0, 0, 1);  // Z-axis vector
  Eigen::Matrix3d _J = Eigen::Matrix3d::Zero();     // Inertia matrix (kg.m^2)
  Eigen::Matrix3d _J_inv;                           // Inverse of inertia matrix

  // Partial state matrices
  Eigen::Matrix3d _A_rv = _I;
  Eigen::Matrix3d _A_vp;
  Eigen::Matrix3d _A_pp;
  Eigen::Matrix3d _A_pw = _I;
  Eigen::Matrix3d _A_ww;

  // Partial input matrices
  Eigen::Vector3d _B_vt;
  Eigen::Matrix3d _B_wm;

  // Partial offset matrices
  Eigen::Vector3d _f_r;
  Eigen::Vector3d _f_v;
  Eigen::Vector3d _f_p;
  Eigen::Vector3d _f_p_t;
  Eigen::Vector3d _f_w;

  // MPC Matrices
  Eigen::MatrixXd _H;
  Eigen::VectorXd _F;
  Eigen::MatrixXd _H_f;
  Eigen::VectorXd _F_f;
  Eigen::MatrixXd _G;
  Eigen::MatrixXd _C_bar;
  Eigen::VectorXd _dY;
  Eigen::VectorXd _U_lin;

  // Optimization variables
  al::real_1d_array _U;             // Control input prediction
  al::real_1d_array _U0;            // Initial control input
  al::real_1d_array _Uc;            // Intermediate control input sequence
  al::real_2d_array _MPC_H;         // Quadratic program H matrix
  al::real_1d_array _MPC_F;         // Quadratic program F matrix
  Eigen::MatrixXd _MPC_H_Eig;       // Eigen Quadratic program H matrix
  Eigen::MatrixXd _MPC_H_Eig_Sym;   // Eigen Symmetric Quadratic program H matrix
  Eigen::VectorXd _MPC_F_Eig;       // Eigen Quadratic program F matrixi
  Eigen::Matrix<double, 6, 6> _q;   // Output weighting matrix
  Eigen::Matrix<double, 4, 4> _r;   // Control input weighting matrix
  Eigen::Matrix<double, 6, 6> _q_f; // Terminal output weighting matrix
  Eigen::MatrixXd _Q;               // General output weighting matrix
  Eigen::MatrixXd _R;               // General input weighting matrix
  al::minqpstate _qpstate;          // State for Quadratic Program
  al::minqpreport _qpreport;        // Report for QP solver

  // Constraint handling
  double _t_input;           // Time for thrust constraint
  al::real_1d_array _u_bndl; // Lower bound on thrusts
  al::real_1d_array _u_bndh; // Upper bound on thrusts
  double _T_scaled;
  double _time;

  // Constants
  double _m = 1.0;       // Mass (kg)
  double _g = 9.81;      // Gravity (m/s)
  double _j_xx = 0.0122; // x-axis principle moment
  double _j_yy = 0.0126; // y-axis principle moment
  double _j_zz = 0.0239; // z-axis principle moment
  double _k = 1.8;       // Propeller yaw moment ratio
  double _c = 0.166;     // Propeller thrust to moment
  double _T_max = 7.0;   // Maximum thrust (N)
  double _t_ramp = 0.2;  // Thrust ramp time (s)

  // Calculation Variables;
  Eigen::Matrix3d _Rotm;
  Eigen::Vector3d _gamma;
  Eigen::Vector4d _u_tm;
  double _phi;
  double _sphi;
  Eigen::Matrix<double, 12, 12> _E;
  Eigen::Matrix<double, 12, 12> _Ek;
  Eigen::Matrix<double, 12, 12> _Ak;
  Eigen::Matrix<double, 12, 4> _ABk;
  Eigen::Matrix<double, 12, 1> _Afk;
  Eigen::Matrix<double, 6, 6> _A_dare;
  Eigen::Matrix<double, 6, 4> _B_dare;
  Eigen::Matrix<double, 6, 6> _Ad;
  Eigen::Matrix<double, 6, 6> _Adk;
  Eigen::Matrix<double, 6, 6> _Gd;
  Eigen::Matrix<double, 6, 6> _Gdk;
  Eigen::Matrix<double, 6, 6> _Hd;
  Eigen::Matrix<double, 6, 6> _Hdk = Eigen::Matrix<double, 6, 6>::Identity();
  Eigen::Matrix<double, 6, 6> _W;
  Eigen::Matrix<double, 6, 6> _V1;
  Eigen::Matrix<double, 6, 6> _V2;

  public:
  // Default constructor
  LieMPC()
  {
    // Set default parameter values
    _np = 8;
    _ts = 0.04;
    _J(0, 0) = _j_xx;
    _J(1, 1) = _j_yy;
    _J(2, 2) = _j_zz;
    _J_inv = _J.inverse();
    _B_wm = _J_inv;

    //Build control allocation matrix;
    _Gamma << 1, 1, 1, 1,
        -_c, _c, _c, -_c,
        _c, -_c, _c, -_c,
        _k, _k, -_k, -_k;

    // Set default cost weights
    Eigen::Matrix<double, 6, 1> y_weight;
    y_weight << 1.e-2, 1.e-2, 1.e-2, 100., 100., 100.;
    _q = y_weight.asDiagonal();

    Eigen::Matrix<double, 4, 1> u_weight;
    u_weight << 1.e-7, 1.e-7, 1.e-7, 1.e-7;
    _r = u_weight.asDiagonal();

    // Assign constant state matrix parts
    _Ac.block(0, 3, 3, 3) = _A_rv;
    _Ac.block(6, 9, 3, 3) = _A_pw;
    _Bc.block(9, 1, 3, 3) = _B_wm;

    // Assign output matrix parts;
    _C.block(0, 0, 3, 3) = _I;
    _C.block(3, 6, 3, 3) = _I;

    // Set matrix sizes
    Y.resize(12 * _np);
    _H.resize(_nx * _np, _nu * _np);
    _F.resize(_nx * _np);
    _G.resize(_nx * _np, _nx);
    _C_bar.resize(_ny * _np, _nx * _np);
    _Q.resize(_ny * _np, _ny * _np);
    _R.resize(_nu * _np, _nu * _np);
    _H_f.resize(_nx, _nu * _np);
    _F_f.resize(_nx);
    _dY.resize(_ny * _np);
    _U_lin.resize(_nu * _np);
    _MPC_H_Eig.resize(_np * _nu, _np * _nu);
    _MPC_H_Eig_Sym.resize(_np * _nu, _np * _nu);
    _MPC_F_Eig.resize(_np * _nu);
    _U.setlength(_np * _nu);
    _U0.setlength(_np * _nu);
    _Uc.setlength(_np * _nu);
    _u_bndl.setlength(_np * _nu);
    _u_bndh.setlength(_np * _nu);

    // Initialize matrix values
    _H = Eigen::MatrixXd::Zero(_nx * _np, _nu * _np);
    _H_f = Eigen::MatrixXd::Zero(_nx, _nu * _np);
    _G = Eigen::MatrixXd::Zero(_nx * _np, _nx);
    _C_bar = Eigen::MatrixXd::Zero(_ny * _np, _nx * _np);
    _Q = Eigen::MatrixXd::Zero(_ny * _np, _ny * _np);
    _R = Eigen::MatrixXd::Zero(_nu * _np, _nu * _np);

    // Initialize QP Solver
    al::minqpcreate(_nu * _np, _qpstate);
    for (int i = 0; i < _np * _nu; i++) {
      _U0(i) = _m * _g / 4;
    }
    al::minqpsetscale(_qpstate, _U0);
    al::minqpsetalgoquickqp(_qpstate, 1e-10, 1e-10, 1e-10, 100, true);
  };

  // Constructor used to specify prediction horizon, timestep, and weights
  LieMPC(int n_p, double timestep, Eigen::MatrixXd y_weight, Eigen::MatrixXd u_weight)
  {
    // Fetch paramter values
    _np = n_p;
    _ts = timestep;
    _q = y_weight;
    _r = u_weight;
    _J(0, 0) = _j_xx;
    _J(1, 1) = _j_yy;
    _J(2, 2) = _j_zz;
    _J_inv = _J.inverse();
    _B_wm = _J_inv;

    //Build control allocation matrix;
    _Gamma << 1, 1, 1, 1,
        -_c, _c, _c, -_c,
        _c, -_c, _c, -_c,
        _k, _k, -_k, -_k;

    // Assign constant state matrix parts
    _Ac.block(0, 3, 3, 3) = _A_rv;
    _Ac.block(6, 9, 3, 3) = _A_pw;
    _Bc.block(9, 1, 3, 3) = _B_wm;

    // Assign output matrix parts;
    _C.block(0, 0, 3, 3) = _I;
    _C.block(3, 6, 3, 3) = _I;

    // Set matrix sizeis
    Y.resize(12 * _np);
    _H.resize(_nx * _np, _nu * _np);
    _F.resize(_nx * _np);
    _G.resize(_nx * _np, _nx);
    _C_bar.resize(_ny * _np, _nx * _np);
    _Q.resize(_ny * _np, _ny * _np);
    _R.resize(_nu * _np, _nu * _np);
    _H_f.resize(_nx, _nu * _np);
    _F_f.resize(_nx);
    _dY.resize(_ny * _np);
    _U_lin.resize(_nu * _np);
    _MPC_H_Eig.resize(_np * _nu, _np * _nu);
    _MPC_H_Eig.resize(_np * _nu, _np * _nu);
    _MPC_F_Eig.resize(_np * _nu);
    _U.setlength(_np * _nu);
    _u_bndl.setlength(_np * _nu);
    _u_bndh.setlength(_np * _nu);

    // Initialize matrix values
    _H = Eigen::MatrixXd::Zero(_nx * _np, _nu * _np);
    _H_f = Eigen::MatrixXd::Zero(_nx, _nu * _np);
    _G = Eigen::MatrixXd::Zero(_nx * _np, _nx);
    _C_bar = Eigen::MatrixXd::Zero(_ny * _np, _nx * _np);
    _Q = Eigen::MatrixXd::Zero(_ny * _np, _ny * _np);
    _R = Eigen::MatrixXd::Zero(_nu * _np, _nu * _np);
  };

  void linearize()
  {
    // Build rotation matrix
    _Rotm = Eigen::Map<Eigen::Matrix3d>((x.segment(6, 9)).data()).transpose();

    // Calculate forces and moments;
    _u_tm = _Gamma * u_lin;

    // Build state matrix components
    _A_vp.noalias() = (1. / _m) * _Rotm * _cross_op(_1_3 * _u_tm(0));
    _A_pp.noalias() = -_cross_op(x.segment(15, 3));
    _A_ww.noalias() = _J_inv * (_cross_op(_J * x.segment(15, 3)) - (_cross_op(x.segment(15, 3)) * _J));

    // Build input matrix components
    _B_vt.noalias() = -(1. / _m) * _Rotm * _1_3;

    // Build offset matrix componenets
    _f_r = -x.segment(3, 3);
    _f_v.noalias() = (1. / _m) * _Rotm * _1_3 * _u_tm(0) - _1_3 * _g;
    _f_p = -x.segment(15, 3);
    _gamma.noalias() = (-_cross_op(x.segment(15, 3)) * _J * x.segment(15, 3) + _u_tm.segment(1, 3));
    _f_p_t.noalias() = -_J_inv * _gamma;
    _f_w = _f_p_t;

    // Build matrices
    _Ac.block(3, 6, 3, 3) = _A_vp;
    _Ac.block(6, 6, 3, 3) = _A_pp;
    _Ac.block(9, 9, 3, 3) = _A_ww;

    _Bc.block(3, 0, 3, 1) = _B_vt;
    _Bcg = _Bc * _Gamma;

    _f.segment(0, 3) = _f_r;
    _f.segment(3, 3) = _f_v;
    _f.segment(6, 3) = _f_p;
    _f.segment(9, 3) = _f_w;

    _f_t.segment(6, 3) = _f_p_t;
  };

  void discretize()
  {
    // Obtain discrete A and B matrices using a ZOH discretization
    _Ek = Eigen::Matrix<double, 12, 12>::Identity();
    _E = _Ek;

    for (int i = 1; i < 16; i++) {
      _Ek = _Ek * (_Ac * _ts) / (i + 1);
      _E += _Ek;
    }

    _A = Eigen::Matrix<double, 12, 12>::Identity() + _E * _Ac * _ts;
    _B = _E * _Bcg * _ts;

    // Solve for terminal cost from discrete-time algebraic Ricatti equation
    _q_f = _solve_dare(_C * _A * _C.transpose(), _C * _B, _q, _r);
  };

  void build_mpc()
  {
    // Build prediction matrices for MPC
    _Ak = Eigen::MatrixXd::Identity(_nx, _nx);
    _ABk = _B;
    _Afk = _ts * (_f + (_ts / 2.) * _f_t);
    _F = Eigen::VectorXd::Zero(_nx * _np);
    _F_f = Eigen::VectorXd::Zero(_nx);

    for (int i = 0; i < _np; i++) {

      if (i != 0) {
        _Ak *= _A;
        _ABk.noalias() = _Ak * _B;
        _Afk.noalias() = _Ak * _ts * (_f + i * (_ts / 2) * _f_t);
      }

      _C_bar.block(i * _ny, i * _nx, _ny, _nx) = _C;
      _Q.block(i * _ny, i * _ny, _ny, _ny) = _q;
      _R.block(i * _nu, i * _nu, _nu, _nu) = _r;
      // _G.block(i * _nx, 0, _nx, _nx) = _Ak;

      _dY.segment(i * _ny, 3) = x.segment(0, 3) - Y.segment(i * 12, 3);
      _dY.segment(i * _ny + 3, 3) = _vee_SO3(_Rotm.transpose() * Eigen::Map<Eigen::Matrix3d>((Y.segment(i * 12 + 3, 9)).data()));

      _U_lin.segment(i * _nu, _nu) = u_lin;

      for (int j = 0; j + i < _np - 1; j++) {
        _F.segment(_nx * (_np - 1 - j), _nx) += _Afk;
        _H.block((j + i + 1) * _nx, j * _nu, _nx, _nu) = _ABk;
      }

      _H_f.block(0, (_np - i - 1) * _nu, _nx, _nu) = _ABk;
      _F_f += _Afk;
    }

    _MPC_H_Eig.noalias() = (_C_bar * _H).transpose() * _Q * (_C_bar * _H) + (_C * _H_f).transpose() * _q_f * (_C * _H_f) + _R;
    _MPC_H_Eig_Sym = _MPC_H_Eig + _MPC_H_Eig.transpose();
    _MPC_F_Eig.noalias() = 2. * (_C_bar * _F - _dY).transpose() * _Q * (_C_bar * _H) + 2. * (_C * _F_f - 2. * _dY.segment((_np - 1) * _ny, _ny)).transpose() * _q_f * (_C * _H_f) - _U_lin.transpose() * _R;
  };

  void solve()
  {
    // Solve QP to obtain optimal control inputs
    for (int i = 0; i < _np; i++) {
      for (int j = 0; j < _nu; j++) {
        _U0(i * _nu + j) = u_lin(j);
      }
    }

    _eigen_to_al(_MPC_H_Eig_Sym, &_MPC_H);
    _eigen_to_al(_MPC_F_Eig, &_MPC_F);
    al::minqpsetquadraticterm(_qpstate, _MPC_H);
    al::minqpsetlinearterm(_qpstate, _MPC_F);
    _init_constraints();
    _U = _U0;

    for (int i = 0; i < 5; i++) {
      al::minqpsetbc(_qpstate, _u_bndl, _u_bndh);
      al::minqpsetstartingpoint(_qpstate, _U);
      al::minqpoptimize(_qpstate);
      al::minqpresults(_qpstate, _U, _qpreport);
      for (int j = 0; j < (_np * _nu); j++) {
        _Uc(j) = _U0(j) - _U(j);
      }
      _build_constraints();
      if (_check_bounds(_U, _u_bndl, _u_bndh)) {
        break;
      }
    }

    u(0) = u_lin(0) - _U(0);
    u(1) = u_lin(1) - _U(1);
    u(2) = u_lin(2) - _U(2);
    u(3) = u_lin(3) - _U(3);
  };

  private:
  Eigen::Matrix3d _cross_op(Eigen::Vector3d _v)
  {
    // Calculate cross operator matrix from vector
    Eigen::MatrixXd _M = Eigen::MatrixXd::Zero(3, 3);
    _M(0, 1) = -_v(2);
    _M(0, 2) = _v(1);
    _M(1, 2) = -_v(0);
    _M(1, 0) = _v(2);
    _M(2, 0) = -_v(1);
    _M(2, 1) = _v(0);
    return _M;
  };

  Eigen::Vector3d _vee_SO3(Eigen::Matrix3d _rotMatrix)
  {
    _phi = acos((_rotMatrix.trace() - 1) / 2);
    if (_phi == 0.0) {
      return (Eigen::VectorXd(3) << 0,0,0).finished();
    } else {
    _sphi = _phi / sin(_phi);
    return (Eigen::VectorXd(3) << _sphi * (_rotMatrix(1, 2) - _rotMatrix(2, 1)) / 2., _sphi * (-_rotMatrix(0, 2) + _rotMatrix(2, 0)) / 2., _sphi * (_rotMatrix(0, 1) - _rotMatrix(1, 0)) / 2.).finished();
    }
  };

  double _constraint(double _init_time, int _timesteps)
  {
    // Calculate thrust limits after number of timesteps
    _time = _init_time + _timesteps * _ts;
    if (_time > 0.) {
      return _T_max / (1 + std::exp(-12. / _t_ramp * (_time - (_t_ramp / 2.))));
    } else if (_time < 0.) {
      return -_T_max / (1 + std::exp(12. / _t_ramp * (_time + (_t_ramp / 2.))));
    } else {
      return 0.;
    }
  };

  double _inv_constraint(double _thrust)
  {
    // Calculate time associated with thrust value on constraint curve
    _T_scaled = _thrust / _T_max;

    if (std::abs(_T_scaled) > 0.98) {
      _T_scaled = ((_thrust > 0.) - (_thrust < 0.)) * 0.98;
    }

    if (_thrust > 0.) {
      return (std::log(_T_scaled / (1. - _T_scaled)) * _t_ramp / 12.) + _t_ramp / 2.;
    } else if (_thrust < 0.) {
      return -(std::log(-_T_scaled / (1. - _T_scaled)) * _t_ramp / 12.) - _t_ramp / 2.;
    } else {
      return 0.;
    }
  };

  Eigen::Matrix<double, 6, 6> _solve_dare(Eigen::Matrix<double, 6, 6> _Adare, Eigen::Matrix<double, 6, 4> _Bdare, Eigen::Matrix<double, 6, 6> _Qdare, Eigen::Matrix<double, 4, 4> _Rdare)
  {
    // Solve discrete-time algebraic Ricatti equation
    _Ad = _Adare;
    _Gd.noalias() = _Bdare * (_Rdare.householderQr().solve(_Bdare.transpose()));
    _Hd = _Qdare;

    while ((_Hd - _Hdk).squaredNorm() > 1e-10 * _Hd.squaredNorm()) {
      _Adk = _Ad;
      _Gdk = _Gd;
      _Hdk = _Hd;

      _W = Eigen::Matrix<double, 6, 6>::Identity() + _Gdk * _Hdk;
      _V1 = _W.householderQr().solve(_Adk);
      _V2 = (_W.householderQr().solve(_Gdk.transpose())).transpose();

      _Ad.noalias() = _Adk * _V1;
      _Gd.noalias() = _Gdk + _Adk * _V2 * _Adk.transpose();
      _Hd.noalias() = _Hdk + _V1.transpose() * _Hdk * _Adk;
    }
    return _Hd;
  };

  void _eigen_to_al(Eigen::MatrixXd emat, al::real_2d_array* almat)
  {
    almat->setlength(emat.rows(), emat.cols());
    for (int i = 0; i < emat.rows(); i++) {
      for (int j = 0; j < emat.cols(); j++) {
        almat->operator()(i, j) = emat(i, j);
      }
    }
  };

  void _eigen_to_al(Eigen::VectorXd emat, al::real_1d_array* almat)
  {
    almat->setlength(emat.rows());
    for (int i = 0; i < emat.rows(); i++) {
      almat->operator()(i) = emat(i);
    }
  };

  bool _check_bounds(al::real_1d_array _U_ctrl, al::real_1d_array _U_low, al::real_1d_array _U_upp)
  {
    for (int i = 0; i < _np; i++) {
      if (_U_ctrl(i) < _U_low(i) || _U_ctrl(i) > _U_upp(i)) {
        return 0;
      }
    }
    return 1;
  };

  void _init_constraints()
  {
    // Build constraint matrices for MPC
    for (int i = 0; i < 4; i++) {
      _t_input = _inv_constraint(_U0(i));
     for (int j = 1; j <= _np; j++) {
        _u_bndh(i + _nu * (j - 1)) = _U0(i + _nu * (j - 1)) - _constraint(_t_input, -j);
        _u_bndl(i + _nu * (j - 1)) = _U0(i + _nu * (j - 1)) - _constraint(_t_input, j);
      }
    }
  };

  void _build_constraints()
  {
    // Build constraint matrices for MPC
    for (int i = 0; i < 4; i++) {
      _t_input = _inv_constraint(_U0(i));
      _u_bndh(i) = _U0(i) - _constraint(_t_input, -1);
      _u_bndl(i) = _U0(i) - _constraint(_t_input, 1);
      for (int j = 2; j <= _np; j++) {
        _t_input = _inv_constraint(_Uc(i + _nu * (j - 2)));
        _u_bndh(i + _nu * (j - 1)) = _U0(i + _nu * (j - 1)) - _constraint(_t_input, -1);
        _u_bndl(i + _nu * (j - 1)) = _U0(i + _nu * (j - 1)) - _constraint(_t_input, 1);
      }
    }
  };
};


#include <geometry.h>
#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

#include <vector>

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(closest_point_test)
{
  typedef tiny::MathTypes<float> MT;
  typedef MT::vector3_type       V;

  {
    geometry::Line<V> const L = geometry::Line<V>( V::make(0,0,0), V::make(1,0,0));  // x-axis line

    V const q = geometry::closest_point_on_line( V::make(10,4,5), L );

    BOOST_CHECK_CLOSE( q(0), 10.0f, 0.01f );
    BOOST_CHECK_CLOSE( q(1), 0.0f, 0.01f );
    BOOST_CHECK_CLOSE( q(2), 0.0f, 0.01f );
  }
  {
    geometry::Plane<V> const P = geometry::make_plane( V::make(1,0,0), 0);  // y-z plane

    V const q = geometry::closest_point_on_plane( V::make(10,4,5), P );

    BOOST_CHECK_CLOSE( q(0), 0.0f, 0.01f );
    BOOST_CHECK_CLOSE( q(1), 4.0f, 0.01f );
    BOOST_CHECK_CLOSE( q(2), 5.0f, 0.01f );
  }
  {
    geometry::Line<V> const L1 = geometry::Line<V>( V::make(0,0,1), V::make(0,1,0));  // y-axis line
    geometry::Line<V> const L2 = geometry::Line<V>( V::make(0,0,0), V::make(1,0,0));  // x-axis line

    V p1, p2;

    geometry::closest_points_line_line( L1, L2, p1, p2 );

    BOOST_CHECK_CLOSE( p1(0), 0.0f, 0.01f );
    BOOST_CHECK_CLOSE( p1(1), 0.0f, 0.01f );
    BOOST_CHECK_CLOSE( p1(2), 1.0f, 0.01f );

    BOOST_CHECK_CLOSE( p2(0), 0.0f, 0.01f );
    BOOST_CHECK_CLOSE( p2(1), 0.0f, 0.01f );
    BOOST_CHECK_CLOSE( p2(2), 0.0f, 0.01f );

  }
}

BOOST_AUTO_TEST_SUITE_END();


// Boost.Signals library
//
// Copyright (C) 2001 Doug Gregor (gregod@cs.rpi.edu)
//
// Permission to copy, use, sell and distribute this software is granted
// provided this copyright notice appears in all copies.
// Permission to modify the code and to distribute modified code is granted
// provided this copyright notice appears in all copies, and a notice
// that the code was modified is included with the copyright notice.
//
// This software is provided "as is" without express or implied warranty,
// and with no claim as to its suitability for any purpose.

// For more information, see http://www.boost.org

#include <algorithm>
#include <iostream>
#include <boost/signals/signal2.hpp>

template<typename T>
struct maximum {
  typedef T result_type;

  template<typename InputIterator>
  T operator()(InputIterator first, InputIterator last) const
  {
    if (first == last)
      throw std::runtime_error("Cannot compute maximum of zero elements!");
    return *std::max_element(first, last);
  }
};

int main()
{
  boost::signal2<int, int, int, maximum<int> > sig_max;
  sig_max.connect(std::plus<int>());
  sig_max.connect(std::multiplies<int>());
  sig_max.connect(std::minus<int>());
  sig_max.connect(std::divides<int>());

  std::cout << sig_max(5, 3) << std::endl; // prints 15

  return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EXPONENTIAL_FUNCTIONS_SIMD_COMMON_POW2_HPP_INCLUDED
#define NT2_EXPONENTIAL_FUNCTIONS_SIMD_COMMON_POW2_HPP_INCLUDED
#include <nt2/exponential/functions/pow2.hpp>
#include <nt2/include/functions/simd/ldexp.hpp>
#include <nt2/include/functions/simd/toint.hpp>
#include <nt2/include/functions/simd/twopower.hpp>
#include <nt2/include/functions/simd/is_nan.hpp>
#include <nt2/include/functions/simd/is_inf.hpp>
#include <nt2/include/functions/simd/if_allbits_else.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/is_inf.hpp>
#include <nt2/include/constants/zero.hpp>
#include <boost/dispatch/meta/as_floating.hpp>

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::pow2_, tag::cpu_
                            , (A0)(A1)(X)
                            , ((simd_< floating_<A0>,X >))
                              ((simd_< integer_<A1>,X >))
                            )
  {
    typedef A0 result_type;

    NT2_FUNCTOR_CALL(2)
    {
      return nt2::ldexp(a0, a1);
    }
  };
} }

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::pow2_, tag::cpu_
                            , (A0)(X)
                            , ((simd_< floating_<A0>,X >))
                              ((simd_< floating_<A0>,X >))
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL_REPEAT(2)
    {
      return nt2::ldexp(a0, nt2::toint(a1));
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::pow2_, tag::cpu_
                            , (A0)(X)
                            , ((simd_< integer_<A0>,X >))
                              ((simd_< integer_<A0>,X >))
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL_REPEAT(2)
    {
      return nt2::ldexp(a0, a1);
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::pow2_, tag::cpu_
                            , (A0)(X)
                            , ((simd_< integer_<A0>,X >))
                            )
  {

    typedef typename  boost::dispatch::meta::as_floating<A0>::type result_type;

    NT2_FUNCTOR_CALL_REPEAT(1)
    {
      return nt2::ldexp(One<A0>(), a0);
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::pow2_, tag::cpu_
                            , (A0)(X)
                            , ((simd_< floating_<A0>,X >))
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL_REPEAT(1)
    {
      return nt2::if_allbits_else(is_nan(a0),
                                  nt2::if_else(is_inf(a0),
                                               if_else(is_gtz(a0), a0, Zero<result_type>()),
                                               nt2::ldexp(One<A0>(), nt2::toint(a0))
                                               )
                                  );
    }
  };

} }

#endif


/*
 * Copyright (c) 2016 Abhishek Agrawal (abhishek.agrawal@protonmail.com)
 * Distributed under the MIT License.
 * See accompanying file LICENSE.md or copy at http://opensource.org/licenses/MIT
 */

#include <iostream>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>
#include <fstream>
#include <exception>
#include <cstdlib>
#include <execinfo.h>

#include <boost/exception/info.hpp>

#include <gsl/gsl_multiroots.h>
#include <gsl/gsl_vector.h>

#include <libsgp4/DateTime.h>
#include <libsgp4/Eci.h>
#include <libsgp4/Globals.h>
#include <libsgp4/SGP4.h>
#include <libsgp4/Tle.h>

#include <Astro/astro.hpp>
#include <SML/sml.hpp>
#include <SML/constants.hpp>
#include <SML/basicFunctions.hpp>

#include <Atom/printFunctions.hpp>

#include <Astro/orbitalElementConversions.hpp>
#include <Atom/convertCartesianStateToTwoLineElements.hpp>

#include "/media/abhishek/work/TU delft/INTERNSHIP at DINAMICA/work/github/pykep/src/core_functions/par2ic.h"

#include "CppProject/TleGen.hpp"

namespace TleGen{

	typedef double Real;
	typedef std::vector< Real > Vector6;
	typedef std::vector< Real > Vector3;
	typedef std::vector< Real > Vector2;
	typedef std::vector < std::vector < Real > > Vector2D;

	void TleGen( Vector6 randKepElem, std::string& SolverStatus, int& IterationCount )
	{
		// grav. parameter 'mu' of earth
    	const double muEarth = kMU*( pow( 10, 9 ) ); // unit m^3/s^2
	    // generate sets of cartesian elements corresponding to each of the psuedo random orbital element set.
	    // The conversion from keplerian elements to the cartesian elements is done using the PyKep library from ESA.
	    Vector3 CartPos( 3 ); // empty vector to store position coordinates
	    Vector3 CartVel( 3 ); // empty vector to store velocity components
	    // Real rangeMag = 0; // magnitude range
	    // Real velocityMag = 0; // velocity magnitude
	    kep_toolbox::par2ic( randKepElem, muEarth, CartPos, CartVel );

	    // verification of the conversion process using Ron Noomen's lecture notes from TUDelft
	    // Document ID ae4878.basics.v4-16.pdf
	    // Vector6 testKep = { 6787746.891, 0.000731104, sml::convertDegreesToRadians( 51.68714486 ),
	    //     sml::convertDegreesToRadians( 127.5486706 ), sml::convertDegreesToRadians( 74.21987137 ), sml::convertDegreesToRadians( 24.08317766 ) };
	    // Vector3 testPos( 3 );
	    // Vector3 testVel( 3 );
	    // kep_toolbox::par2ic( testKep, muEarth, testPos, testVel );
	    // std::cout << testPos[ 0 ] << std::endl;
	    // std::cout << testPos[ 1 ] << std::endl;
	    // std::cout << testPos[ 2 ] << std::endl;
	    // std::cout << testVel[ 0 ] << std::endl;
	    // std::cout << testVel[ 1 ] << std::endl;
	    // std::cout << testVel[ 2 ] << std::endl;

	    // convert the cartesian elements to the corresponding TLE format using the ATOM toolbox
	    Vector6 cartesianState( 6 );
	    // cartesianState[ 0 ] = -7.1e3;
	    // cartesianState[ 1 ] = 2.7e3;
	    // cartesianState[ 2 ] = 1.3e3;
	    // cartesianState[ 3 ] = -2.5;
	    // cartesianState[ 4 ] = -5.5;
	    // cartesianState[ 5 ] = 5.5;
	    Tle convertedTle;
	    Tle referenceTle = Tle(); // empty TLE for reference
	    // std::cout << referenceTle << std::endl << std::endl;
	    const Real absTol = 1.0e-10; // absolute tolerance
	    const Real relTol = 1.0e-5; // relative tolerance
	    const int maxItr = 100; // maximum allowed iterations per conversion run

	    // important note, the atom function converting cartesian to TLEs takes in values in km and km/s.
	    cartesianState[ 0 ] = CartPos[ 0 ]/1000;
	    cartesianState[ 1 ] = CartPos[ 1 ]/1000;
	    cartesianState[ 2 ] = CartPos[ 2 ]/1000;
	    cartesianState[ 3 ] = CartVel[ 0 ]/1000;
	    cartesianState[ 4 ] = CartVel[ 1 ]/1000;
	    cartesianState[ 5 ] = CartVel[ 2 ]/1000;
		convertedTle = atom::convertCartesianStateToTwoLineElements< Real, Vector6 >( cartesianState, DateTime( ), SolverStatus,
	    	IterationCount, referenceTle, kMU, kXKMPER, absTol, relTol, maxItr );
	}
}


#include <algorithm>
#include <iostream>
#include <iterator>
#include <string>
#include <vector>
#include <map>
#include <valarray>
#include <boost/algorithm/string.hpp>

using namespace std;

map<string, valarray<int>> DIRECTIONS = {
    // x, y, z
    // Hex cubic labeling
    {"e", {1, -1, 0}},
    {"w", {-1, 1, 0}},
    {"se", {1, 0, -1}},
    {"ne", {0, -1, 1}},
    {"sw", {0, 1, -1}},
    {"nw", {-1, 0, 1}},
};

string hash_loc(valarray<int> loc){
  return (to_string(loc[0]) + ","+
          to_string(loc[1]) + ","+
          to_string(loc[2]));
}

valarray<int> unhash_loc(string hloc) {
  vector<string> split_str;
  boost::split(split_str, hloc, boost::is_any_of(","));
  valarray<int> out = {
      stoi(split_str[0]),
      stoi(split_str[1]),
      stoi(split_str[2]),
  };
  return out;
}

int count_black(map<string, int> tile_flips){
  int num_black = 0;
  for (auto [sloc, flips] : tile_flips) {
    num_black += flips % 2;
  }
  return num_black;
}

int main() {

  map<string, int> tile_flips; // 1 if black

  for (string line; getline(cin, line);){
    // cout << line << endl;
    vector<string> directions;
    int curr_loc=0;
    while (curr_loc < line.size()){
      if ((line[curr_loc] == 'n') ||
          (line[curr_loc] == 's')) {
        directions.push_back(line.substr(curr_loc, 2));
        curr_loc += 2;
      } else{
        directions.push_back(line.substr(curr_loc, 1));
        curr_loc += 1;
      }
    }
    valarray<int> loc = {0,0,0}; // Reference tile
    for (string dir : directions){
      loc += DIRECTIONS[dir];
    }
    tile_flips[hash_loc(loc)] += 1;
  }

  cout << "Part 1: " << count_black(tile_flips) << endl;
  cout << endl;

  // Part 2
  int num_days = 100;
  for (int i=1; i<=num_days; i++){
    auto start_flips = tile_flips;
    // Map of white tiles to num black adjacent
    // Only gets evaluated for tiles with atleast 1 black neighbour
    map<string,int> blackn_count;
    // Look over all known tiles;
    for (auto [hloc, flips] : start_flips){
      bool black = flips % 2;
      valarray<int> loc = unhash_loc(hloc);
      if (black){
        // If a black tile, count adjacent black tiles
        int blackn = 0;
        for (auto [dir, mdir] : DIRECTIONS){
          string nhash = hash_loc(loc + mdir);
          bool is_blackn = (start_flips[nhash] % 2);
          blackn += is_blackn;
          // If neighbour is white, increment
          // the number of black adjacents that
          // that white tile has.
          if (not is_blackn){
            blackn_count[nhash] += 1;
          }
        }
        // Flip if matches rule for black tile
        if ((blackn == 0) || (blackn > 2)){
          tile_flips[hloc]++;
        }
      }
    }

    // Find white tiles with 2 black adjacent.
    for (auto [hloc, nblackn] : blackn_count){
      if (nblackn == 2){
        tile_flips[hloc]++;
      }
    }
    cout << "Day " << i << ": " << count_black(tile_flips) << endl;
  }
  cout << endl;
  cout << "Part 2: " << count_black(tile_flips) << endl;
}


/* test_lognormal_distribution.cpp
 *
 * Copyright Steven Watanabe 2011
 * 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)
 *
 * $Id$
 *
 */

#include <boost/random/lognormal_distribution.hpp>
#include <limits>

#define BOOST_RANDOM_DISTRIBUTION boost::random::lognormal_distribution<>
#define BOOST_RANDOM_ARG1 m
#define BOOST_RANDOM_ARG2 s
#define BOOST_RANDOM_ARG1_DEFAULT 0.0
#define BOOST_RANDOM_ARG2_DEFAULT 1.0
#define BOOST_RANDOM_ARG1_VALUE 7.5
#define BOOST_RANDOM_ARG2_VALUE 0.25

#define BOOST_RANDOM_DIST0_MIN 0.0
#define BOOST_RANDOM_DIST0_MAX (std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST1_MIN 0.0
#define BOOST_RANDOM_DIST1_MAX (std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST2_MIN 0.0
#define BOOST_RANDOM_DIST2_MAX (std::numeric_limits<double>::infinity)()

#define BOOST_RANDOM_TEST1_PARAMS (-100.0)
#define BOOST_RANDOM_TEST1_MAX 1

#define BOOST_RANDOM_TEST2_PARAMS (100.0)
#define BOOST_RANDOM_TEST2_MIN 1

#include "test_distribution.ipp"


//Author: Dr. Shantanu Shahane
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>
#include <float.h>
#include <string.h>
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <unsupported/Eigen/SparseExtra>
#include <Eigen/SparseLU>
#include <Eigen/OrderingMethods>
#include <Eigen/Core>
#include "_hypre_utilities.h"
#include "HYPRE_krylov.h"
#include "HYPRE.h"
#include "HYPRE_parcsr_ls.h"
#include "general_functions.hpp"
#include "class.hpp"

IMPLICIT_SCALAR_TRANSPORT_SOLVER::IMPLICIT_SCALAR_TRANSPORT_SOLVER(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, vector<bool> &dirichlet_flag1, int precond_freq_it1, double unsteady_coeff1, double conv_coeff1, double diff_coeff1, bool solver_log_flag1)
{
    dirichlet_flag = dirichlet_flag1, solver_log_flag = solver_log_flag1;
    unsteady_coeff = unsteady_coeff1, conv_coeff = conv_coeff1, diff_coeff = diff_coeff1;
    precond_freq_it = precond_freq_it1;

    zero_vector = Eigen::VectorXd::Zero(points.nv);
    bc_full_neumann = true;
    for (int iv = 0; iv < points.nv; iv++)
        if (points.boundary_flag[iv] && dirichlet_flag[iv])
        { //boundary point found with dirichlet BC
            bc_full_neumann = false;
            break;
        }
    if (bc_full_neumann)
    {
        printf("\n\nERROR from IMPLICIT_SCALAR_TRANSPORT_SOLVER::IMPLICIT_SCALAR_TRANSPORT_SOLVER Setting  full Neumann BC is not permitted\n\n");
        throw bad_exception();
    }
    source = zero_vector, phi_old_old = zero_vector;
}

void IMPLICIT_SCALAR_TRANSPORT_SOLVER::set_matrix(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new)
{
    matrix.resize(0, 0);
    vector<Eigen::Triplet<double>> triplet;
    int ivnb, dim = parameters.dimension;
    double value, unsteady_factor;
    if (it == 0)
        unsteady_factor = 1.0; //BDF1: implicit Euler
    else
        unsteady_factor = bdf2_alpha_1;
    for (int iv = 0; iv < points.nv; iv++)
    {
        if (points.boundary_flag[iv])
        {
            if (dirichlet_flag[iv])
                triplet.push_back(Eigen::Triplet<double>(iv, iv, 1.0));
            else
            {
                for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
                {
                    ivnb = cloud.nb_points_col[i1];
                    value = points.normal[dim * iv] * cloud.grad_x_coeff[i1] + points.normal[dim * iv + 1] * cloud.grad_y_coeff[i1];
                    triplet.push_back(Eigen::Triplet<double>(iv, ivnb, value));
                }
            }
        }
        else
        {
            for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
            {
                ivnb = cloud.nb_points_col[i1];
                value = diff_coeff * cloud.laplacian_coeff[i1];                    //diffusion
                value = value + (u_new[iv] * conv_coeff * cloud.grad_x_coeff[i1]); //convection
                value = value + (v_new[iv] * conv_coeff * cloud.grad_y_coeff[i1]); //convection
                if (ivnb == iv)
                    value = value + (unsteady_factor * unsteady_coeff); //diagonal term
                triplet.push_back(Eigen::Triplet<double>(iv, ivnb, value));
            }
        }
    }
    matrix.resize(points.nv, points.nv);
    matrix.setFromTriplets(triplet.begin(), triplet.end());
    matrix.makeCompressed();
    triplet.clear();
}

void IMPLICIT_SCALAR_TRANSPORT_SOLVER::modify_matrix(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new)
{
    int ivnb, dim = parameters.dimension, index;
    double value, unsteady_factor;
    if (it == 0)
        unsteady_factor = 1.0; //BDF1: implicit Euler
    else
        unsteady_factor = bdf2_alpha_1;
    for (int iv = 0; iv < points.nv; iv++)
        if (!points.boundary_flag[iv])
        { //coefficients of boundary points never updated for velocities
            for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
            {
                ivnb = cloud.nb_points_col[i1];
                value = diff_coeff * cloud.laplacian_coeff[i1];                    //diffusion
                value = value + (u_new[iv] * conv_coeff * cloud.grad_x_coeff[i1]); //convection
                value = value + (v_new[iv] * conv_coeff * cloud.grad_y_coeff[i1]); //convection
                if (ivnb == iv)
                    value = value + (unsteady_factor * unsteady_coeff); //diagonal term
                index = nb_points_col_matrix[i1];
                matrix.valuePtr()[index] = value;
            }
        }
}

void IMPLICIT_SCALAR_TRANSPORT_SOLVER::calc_nb_points_col_matrix(POINTS &points, CLOUD &cloud, PARAMETERS &parameters)
{
    int ivnb, index;
    nb_points_col_matrix.clear();
    for (int i1 = 0; i1 < cloud.nb_points_col.size(); i1++) //initialize to -1
        nb_points_col_matrix.push_back(-1);
    for (int iv = 0; iv < points.nv; iv++)
        if (!points.boundary_flag[iv])
        { //coefficients of boundary points never updated for velocities; nb_points_col_matrix ahas value of [-1] at bounday points
            for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
            {
                index = -1;
                ivnb = cloud.nb_points_col[i1];
                for (int i2 = matrix.outerIndexPtr()[iv]; i2 < matrix.outerIndexPtr()[iv + 1]; i2++)
                    if (matrix.innerIndexPtr()[i2] == ivnb)
                    {
                        index = i2;
                        break;
                    }
                if (index < 0)
                {
                    cout << "\n\nError from SEMI_IMPLICIT_SPLIT_SOLVER::calc_nb_points_col_matrix in matrix_u, unable to find ivnb: " << ivnb << " for iv: " << iv << ", points.boundary_flag[iv]: " << points.boundary_flag[iv] << "\n\n";
                    throw bad_exception();
                }
                nb_points_col_matrix[i1] = index;
            }
        }
}

void IMPLICIT_SCALAR_TRANSPORT_SOLVER::single_timestep_2d(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &phi_new, Eigen::VectorXd &phi_old, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, int it1)
{
    it = it1;
    if (it % precond_freq_it == 0 || it == 0 || it == 1)
    {
        set_matrix(points, cloud, parameters, u_new, v_new);
        solver_eigen.setTolerance(parameters.solver_tolerance); //default is machine precision (https://eigen.tuxfamily.org/dox/classEigen_1_1IterativeSolverBase.html#ac160a444af8998f93da9aa30e858470d)
        solver_eigen.setMaxIterations(parameters.n_iter);       //default is twice number of columns (https://eigen.tuxfamily.org/dox/classEigen_1_1IterativeSolverBase.html#af83de7a7d31d9d4bd1fef6222b07335b)
        solver_eigen.preconditioner().setDroptol(parameters.precond_droptol);
        solver_eigen.compute(matrix);
        if (it == 0)
            calc_nb_points_col_matrix(points, cloud, parameters);
    }
    modify_matrix(points, cloud, parameters, u_new, v_new);

    if (it == 0) //BDF1: implicit Euler
        source = (unsteady_coeff * phi_old);
    else //BDF2
        source = -((bdf2_alpha_2 * unsteady_coeff) * phi_old) - ((bdf2_alpha_3 * unsteady_coeff) * phi_old_old);

    for (int iv = 0; iv < points.nv; iv++)
        if (points.boundary_flag[iv])
        { //retain boundary condition from "_old"
            if (dirichlet_flag[iv])
                source[iv] = phi_old[iv];
            else
                source[iv] = 0.0;
        }
    phi_new = solver_eigen.solveWithGuess(source, phi_old);
    if (solver_log_flag)
    {
        double absolute_residual = (matrix * phi_new - source).norm();
        double relative_residual = absolute_residual / source.norm();
        parameters.rel_res_log.push_back(relative_residual);
        parameters.abs_res_log.push_back(absolute_residual);
        parameters.n_iter_actual.push_back(solver_eigen.iterations());
    }
    phi_old_old = phi_old;
}


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [inverse_trigonometric]
#include <boost/simd/trigonometric.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/function/enumerate.hpp>
#include <iostream>

namespace bs =  boost::simd;
using pack_ft =  bs::pack <float, 8>;

int main()
{
  pack_ft p = bs::enumerate<pack_ft>(-1.0f, 0.25f);
  std::cout << " p =  " << p << std::endl
            << " -> bs::acos(p) =   " << bs::acos(p)   << std::endl
            << " -> bs::acospi(p) = " << bs::acospi(p) << std::endl
            << " -> bs::acosd(p) =  " << bs::acosd(p)  << std::endl;
  return 0;
}
//! [inverse_trigonometric]


/*
 * baseframe.cc
 * Copyright (C) 2017 roman <roman.podolski@tum.de>
 *
 * Distributed under terms of the MIT license.
 */

#include "planner/baseframe.h"
#include <gflags/gflags.h>
#include <boost/math/interpolators/cubic_b_spline.hpp>
#include <boost/math/quadrature/trapezoidal.hpp>
#include <boost/math/tools/minima.hpp>
#include <boost/math/tools/roots.hpp>
#include <fstream>
#include <iostream>
#include <map>
#include <utility>
#include <vector>
#include "planner/map.h"

namespace planner {

using boost::math::quadrature::trapezoidal;
using boost::math::cubic_b_spline;
using boost::math::tools::bisect;
using boost::math::tools::eps_tolerance;
using boost::math::tools::newton_raphson_iterate;

Baseframe::Baseframe(const Map &map) {
  const auto vx = map.get_waypoints_x();
  const auto vy = map.get_waypoints_y();
  init(vx, vy);
}

Baseframe::Baseframe(const std::vector<std::pair<double, double>> &wp) {
  std::vector<double> vx, vy;
  for (const auto &p : wp) {
    vx.push_back(p.first);
    vy.push_back(p.second);
  }
  init(vx, vy);
}

Baseframe::Baseframe(const std::vector<point> &wp) {
  std::vector<double> vx, vy;
  for (const auto &p : wp) {
    vx.push_back(p.x());
    vy.push_back(p.y());
  }
  init(vx, vy);
}

void Baseframe::init(const std::vector<double> vx,
                     const std::vector<double> vy) {
  CHECK_EQ(vx.size(), vy.size());
  _N = vx.size() - 1;
  _x = cubic_b_spline<double>(vx.begin(), vx.end(), 0, 1.0);
  _y = cubic_b_spline<double>(vy.begin(), vy.end(), 0, 1.0);
}

point Baseframe::Q(const double t) const {
  if (t < 0 || t > _N)
    throw std::domain_error("t = " + std::to_string(t) +
                            " not within the domain [0,N]");
  return point(_x(t), _y(t));
}

point Baseframe::Q_prime(const double t) const {
  if (t < 0 || t > _N)
    throw std::domain_error("t = " + std::to_string(t) +
                            " not within the domain [0,N]");
  return point(_x.prime(t), _y.prime(t));
}

point Baseframe::Q_pprime(const double t) const {
  const double ddt_x =
      derivative(t, 0.001, [this](const double t) { return _x.prime(t); });
  const double ddt_y =
      derivative(t, 0.001, [this](const double t) { return _y.prime(t); });
  return point(ddt_x, ddt_y);
}

double Baseframe::theta_Q(const double t) const {
  auto p = Q_prime(t);
  // return atan2(p.y(), p.x());
  return atan(p.y() / p.x());
}

static std::map<double, double> _A_lut{{0, 0}};
static std::vector<double> _segment_lut;
double Baseframe::A(const double t) const {
  if (t < 0 || t > _N)
    throw std::domain_error("t = " + std::to_string(t) +
                            " not within the domain [0,N]");
  if (_A_lut.find(t) != _A_lut.end()) {
    return _A_lut[t];
  }

  double result = 0.0;
  if (t == 0) return result;

  auto f = [this](double t) {
    return sqrt(pow(_x.prime(t), 2) + pow(_y.prime(t), 2));
  };

  // The 8 and 1e-5 are a little borderline, but help performance.
  size_t max_refinements = 8;
  result = trapezoidal(f, 0.0, t, 1e-5, max_refinements);

  _A_lut[t] = result;

  return result;
}

static std::map<double, double> _inv_A_lut{{0, 0}};
double Baseframe::inv_A(const double s) const {
  if (s < 0 || s > length())
    throw std::domain_error("s = " + std::to_string(s) +
                            " not within the domain [0,L]");

  if (_segment_lut.empty()) {
    for (int t = 0; t < _N; ++t) {
      _segment_lut.push_back(A(t));
    }
  }

  if (_inv_A_lut.find(s) != _inv_A_lut.end()) {
    return _inv_A_lut[s];
  }

  std::uintmax_t iterations = 1000;
  auto low = std::lower_bound(_segment_lut.begin(), _segment_lut.end(), s);
  const double segment = low - _segment_lut.begin();
  double result = 0.0;
  result = bisect([this, s](double t) { return A(t) - s; }, segment - 1,
                  segment, eps_tolerance<float>(), iterations)
               .first;

  _inv_A_lut[s] = result;

  return result;
}

static std::map<double, double> _curvature_lut;
double Baseframe::curvature(const double s) {
  if (_curvature_lut.find(s) != _curvature_lut.end()) {
    return _curvature_lut[s];
  }

  const auto d_P = P_prime(s);
  double d_x = d_P.x();
  double d_y = d_P.y();

  if (d_x == 0.0 && d_y == 0.0) {  // avoid division by 0.0
    d_x = std::numeric_limits<double>::denorm_min();
    d_y = std::numeric_limits<double>::denorm_min();
  }

  const auto dd_P = P_pprime(s);
  const double dd_x = dd_P.x();
  const double dd_y = dd_P.y();

  const double kappa =
      (d_x * dd_y - dd_x * d_y) / pow((pow(d_x, 2) + pow(d_y, 2)), 3.0 / 2.0);

  _curvature_lut[s] = kappa;

  return kappa;
}

// TODO(roman): check for numerical problems
std::pair<double, double> Baseframe::localize(const point position,
                                              const double v,
                                              const double heading,
                                              const double dt,
                                              const double last_known_s) const {
  auto D_prime = [ this, x_0 = position.x(), y_0 = position.y() ](double s) {
    auto p = P(s);
    double x = p.x();
    double y = p.y();

    auto p_prime = P_prime(s);
    double x_prime = p_prime.x();
    double y_prime = p_prime.y();

    auto p_pprime = P_pprime(s);
    double x_pprime = p_pprime.x();
    double y_pprime = p_pprime.y();

    double dist_prime = 2 * (x - x_0) * x_prime + 2 * (y - y_0) * y_prime;
    double dist_pprime = 2 * ((x - x_0) * x_pprime + pow(x_prime, 2) +
                              (y - y_0) * y_pprime + pow(y_prime, 2));

    return std::pair<double, double>(dist_prime, dist_pprime);
  };

  std::uintmax_t iterations = 1000;
  double minimum_extra_offset = 10;
  double lower_barrier = last_known_s - minimum_extra_offset / 4; // added /4
  if (lower_barrier <= 0.0) lower_barrier = 0.0;

  double initial_guess = last_known_s + v * dt;
  if (initial_guess > length()) initial_guess = length();
  double upper_barrier = initial_guess + minimum_extra_offset;
  if (upper_barrier > length()) upper_barrier = length();

  //std::cout << lower_barrier << ' ' << initial_guess << ' ' << upper_barrier << '\n';

  auto result = newton_raphson_iterate(
      D_prime, lower_barrier, initial_guess, upper_barrier,
      std::numeric_limits<double>::digits / 4, iterations); // was /2

  //std::cout <<  result << '\n';

  auto p = P(result);

  return std::pair<double, double>(result, bg::distance(p, position));
}

}  // namespace planner


#include <vector>
#include <iostream>
#include <functional>
#include <solver.hpp>
#include <math.h>
#include <complex>

#include <Eigen/Dense>

#include <boost/multiprecision/eigen.hpp>
#include <boost/multiprecision/mpfr.hpp>
#include <boost/multiprecision/mpc.hpp>

typedef boost::multiprecision::mpc_complex_500 C;
typedef boost::multiprecision::mpfr_float_500 R;

int main() {
    std::function<C(Eigen::Matrix<C,2,1>&)> f, g;

    f = [] (Eigen::Matrix<C,2,1> &x) -> C {
        return x(0)*x(1) - C(6)/10;
    };

    g = [] (Eigen::Matrix<C,2,1> &x) -> C {
        return x(0)*x(0) + x(1)*x(1) - C(2);
    };

    std::vector<decltype(f)> vf;

    vf.push_back(f);
    vf.push_back(g);

    Eigen::Matrix<C,2,1> x0, x;

    x0(0) = 1;
    x0(1) = 1;

    auto s = solver::Solver2<C,R,2>(vf);
    s.set_h(R("1e-200"));
    s.set_tol(R("1e-100"));
    x = s.solve(x0);

    return 0;
}


// Copyright © 2016-2021 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#pragma once

#include <Eigen/Dense>
#include <limits>
#include <vinecopulib/misc/nlohmann_json.hpp>
#include <vinecopulib/misc/triangular_array.hpp>

namespace vinecopulib {

//! @brief A class for R-vine structures.
//!
//! RVineStructure objects encode the tree structure of the vine, i.e. the
//! conditioned/conditioning variables of each edge. It is represented by a
//! triangular array. An exemplary array is
//! ```
//! 4 4 4 4
//! 3 3 3
//! 2 2
//! 1
//! ```
//! which encodes the following pair-copulas:
//! ```
//! | tree | edge | pair-copulas   |
//! |------|------|----------------|
//! | 0    | 0    | `(1, 4)`       |
//! |      | 1    | `(2, 4)`       |
//! |      | 2    | `(3, 4)`       |
//! | 1    | 0    | `(1, 3; 4)`    |
//! |      | 1    | `(2, 3; 4)`    |
//! | 2    | 0    | `(1, 2; 3, 4)` |
//! ```
//! Denoting by `M[i, j]` the array entry in row `i` and column `j`,
//! the pair-copula index for edge `e` in tree `t` of a `d` dimensional vine
//! is `(M[d - 1 - e, e], M[t, e]; M[t - 1, e], ..., M[0, e])`. Less
//! formally,
//! 1. Start with the counter-diagonal element of column `e` (first conditioned
//!    variable).
//! 2. Jump up to the element in row `t` (second conditioned variable).
//! 3. Gather all entries further up in column `e` (conditioning set).
//!
//! A valid R-vine array must satisfy several conditions which are checked
//! when `RVineStructure()` is called:
//! 1. It only contains numbers between 1 and d.
//! 2. The diagonal must contain the numbers 1, ..., d.
//! 3. The diagonal entry of a column must not be contained in any
//!    column further to the right.
//! 4. The entries of a column must be contained in all columns to the left.
//! 5. The proximity condition must hold: For all t = 1, ..., d - 2 and
//!    e = 0, ..., d - t - 1 there must exist an index j > d, such that
//!    `(M[t, e], {M[0, e], ..., M[t-1, e]})` equals either
//!    `(M[d-j-1, j], {M[0, j], ..., M[t-1, j]})` or
//!    `(M[t-1, j], {M[d-j-1, j], M[0, j], ..., M[t-2, j]})`.
//!
//! An R-vine array is said to be in natural order when the anti-diagonal
//! entries are \f$ 1, \dots, d \f$ (from left to right). The exemplary arrray
//! above is in natural order. Any R-vine array can be characterized by the
//! diagonal entries (called order) and the entries below the diagonal of the
//! corresponding R-vine array in natural order. Since most algorithms work
//! with the structure in natural order, this is how RVineStructure stores the
//! structure internally.
class RVineStructure
{
public:
  explicit RVineStructure(
    const size_t& d = static_cast<size_t>(1),
    const size_t& trunc_lvl = std::numeric_limits<size_t>::max());
  explicit RVineStructure(
    const Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& mat,
    bool check = true);
  explicit RVineStructure(
    const std::vector<size_t>& order,
    const size_t& trunc_lvl = std::numeric_limits<size_t>::max(),
    bool check = true);
  RVineStructure(const std::vector<size_t>& order,
                 const TriangularArray<size_t>& struct_array,
                 bool natural_order = false,
                 bool check = true);
  explicit RVineStructure(const std::string& filename, const bool check = true);
  explicit RVineStructure(const nlohmann::json& input, const bool check = true);

  nlohmann::json to_json() const;
  void to_file(const std::string& filename) const;

  size_t get_dim() const;
  size_t get_trunc_lvl() const;
  std::vector<size_t> get_order() const;
  TriangularArray<size_t> get_struct_array(bool natural_order = false) const;
  TriangularArray<size_t> get_min_array() const;
  TriangularArray<short unsigned> get_needed_hfunc1() const;
  TriangularArray<short unsigned> get_needed_hfunc2() const;
  Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic> get_matrix() const;

  size_t struct_array(size_t tree,
                      size_t edge,
                      bool natural_order = false) const;
  size_t min_array(size_t tree, size_t edge) const;
  bool needed_hfunc1(size_t tree, size_t edge) const;
  bool needed_hfunc2(size_t tree, size_t edge) const;

  void truncate(size_t trunc_lvl);
  std::string str() const;

  static RVineStructure simulate(size_t d,
                                 bool natural_order = false,
                                 std::vector<int> seeds = std::vector<int>());

protected:
  size_t find_trunc_lvl(
    const Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& mat) const;
  std::vector<size_t> get_order(
    const Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& mat) const;
  TriangularArray<size_t> to_rvine_array(
    const Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& mat) const;

  TriangularArray<size_t> to_natural_order() const;
  static TriangularArray<size_t> make_dvine_struct_array(size_t d,
                                                         size_t trunc_lvl);
  static TriangularArray<size_t> make_cvine_struct_array(size_t d,
                                                         size_t trunc_lvl);
  TriangularArray<size_t> compute_min_array() const;
  TriangularArray<short unsigned> compute_needed_hfunc1() const;
  TriangularArray<short unsigned> compute_needed_hfunc2() const;

  void check_if_quadratic(
    const Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& mat) const;
  void check_lower_tri(
    const Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& mat) const;
  void check_upper_tri() const;
  void check_columns() const;
  void check_antidiagonal() const;
  void check_proximity_condition() const;

  std::vector<size_t> order_;
  size_t d_;
  size_t trunc_lvl_;
  TriangularArray<size_t> struct_array_;
  TriangularArray<size_t> min_array_;
  // can't use bool b/c the comittee messed up std::vector<bool>
  TriangularArray<short unsigned> needed_hfunc1_;
  TriangularArray<short unsigned> needed_hfunc2_;
};

std::ostream&
operator<<(std::ostream& os, const RVineStructure& rvs);

//! @brief A class for D-vine structures.
//!
//! D-vines are a special class of R-vines where each tree is a path. A D-vine
//! structure is determined entirely by the order of variables. For example, if
//! the order is `(1, 2, 3, 4)`, the first tree in the vine is 1-2-3-4 and all
//! further trees are unique due to the proximity condition.
//!
//! Note that `DVineStructure` objects inherit the methods and attributes of
//! `RVineStructure` objects.
class DVineStructure : public RVineStructure
{
public:
  explicit DVineStructure(const std::vector<size_t>& order);
  DVineStructure(const std::vector<size_t>& order, size_t trunc_lvl);
};

//! @brief A class for C-vine structures.
//!
//! C-vines are a special class of R-vines where each tree is a star. A C-vine
//! structure is determined entirely by the order of variables. For example, if
//! the order is `{1, 2, 3, 4}`, the first tree in the vine connects variable
//! 4 with all others, the second tree connects variable 3 with all others,
//! etc.
//!
//! Note that `CVineStructure` objects inherit the methods and attributes of
//! `RVineStructure` objects.
class CVineStructure : public RVineStructure
{
public:
  explicit CVineStructure(const std::vector<size_t>& order);
  CVineStructure(const std::vector<size_t>& order, size_t trunc_lvl);
};
}

#include <vinecopulib/vinecop/implementation/rvine_structure.ipp>


//
//  tmesh.hpp
//
//  Created by r. on 09/05/14
//

#ifndef round1_tmesh_hpp
#define round1_tmesh_hpp

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <vector>

namespace spacetime
{
	namespace ublas = boost::numeric::ublas;

	class tmesh : public ublas::vector<double>
	{
	public:
		typedef ublas::vector<double> parent_type;
	public:
		struct interval
		{
		public:
			// Should be hidden
			double a, b, len, mid;
		public:
			interval(double a, double b) : a(a), b(b), len(b-a), mid((a+b)/2) {}
		};
	public:
		void
		operator=(const tmesh& m) {
			parent_type::resize(m.size());
			parent_type::assign(m);
		}
	public:
		bool isvalid() const
		{
			const parent_type& x = (parent_type)(*this);
			if (x.empty()) return false;
			if (x.size() <= 1) return false;
			for (unsigned int i = 1; i != x.size(); ++i)
			{
				if (!(x(i-1) <= x(i))) return false;
			}
			return true;
		}
	public:
		unsigned int getni() const
		// Get the number of intervals
		{
			assert(isvalid());
			assert(size() >= 1);
			return (unsigned int)(size() - 1);
		}
	public:
		void makeuniform(double a, double b, unsigned int nodes)
		{
			assert(nodes >= 2);
			parent_type y(nodes);
			for (unsigned int n = 0; n != nodes; ++n)
				y[n] = a + n * ((b - a) / (nodes - 1));
			this->swap(y);
		}

		void refine()
		{
			assert(isvalid());
			parent_type vrefd(1 + 2 * getni());
			{
				const parent_type& x = (parent_type)(*this);
				unsigned int j = 0;
				vrefd(j++) = x(0);
				for (unsigned int i = 1; i != x.size(); ++i)
				{
					vrefd(j++) = 0.5 * (x(i-1) + x(i));
					vrefd(j++) = x(i);
				}
				assert(j == vrefd.size());
			}
			this->swap(vrefd);
		}

		interval getI(unsigned int n) const
		// n-th subinterval, n >= 1
		{
			assert(isvalid());
			const parent_type& x = (parent_type)(*this);
			assert((1 <= n) && (n < x.size()));
			return interval(x(n-1), x(n));
		}

		std::vector<interval> getIs() const
		// vector of all subintervals, canonically ordered
		{
			assert(isvalid());
			std::vector<interval> Is;
			{
				const parent_type& x = (parent_type)(*this);
				for (unsigned int n = 1; n != x.size(); ++n)
					Is.push_back(interval(x(n-1), x(n)));
			}
			return Is;
		}

		//namespace ublas = boost::numeric::ublas;
		typedef ublas::compressed_matrix<double, ublas::row_major> sparse_matrix;
		//
		sparse_matrix
		naive_prolongation_to() {
			const tmesh& m = (*this);
			// Number of elements
			unsigned K = (m.size() - 1);
			// ... should be even
			assert((K % 2) == 0);
			// Number of elements on coarse mesh
			unsigned k = K / 2;

			unsigned nf = K+1;
			unsigned nc = k+1;

			ublas::mapped_matrix<double> P(nf, nc);

			// Find coarse nodes
			for (unsigned j = 0; j != nc; ++j) {
				unsigned i = 2 * j;
				if (j != 0) P(i-1, j) = 0.5;
				P(i+0, j) = 1;
				if (j+1 != nc) P(i+1, j) = 0.5;
			}

			return P;
		}
	};
}

#endif


#ifndef MODEL_DISTRIBUTION_HPP
#define MODEL_DISTRIBUTION_HPP

#include "gtest/gtest_prod.h"
#include "types.hpp"

#include <algorithm>
#include <boost/log/trivial.hpp>
#include <numeric>
#include <random>
#include <unordered_map>
#include <utility>

namespace FilterModel {

/**
 * Represents the probability distribution encoded by the latent variable model adapted from in
 * Perkins et al.
 */
class ModelDistribution {
   public:
    /**
     * Constructs a new ModelDistribution object from a vector of category counts.
     *
     * Arguments:
     *  data - vector of category counts
     *  generator - random number generator
     *  comparison - test parameter, should usually ignore. If true, runs both exact and approximate
     *    methods for evaluating the probability, compares them, and stops the program if the
     *    estimates are too far apart.
     */
    ModelDistribution(const std::vector<category_counts_t> &data,
                      std::default_random_engine &generator, const Options &options);

    /**
     * Calculates log(p(k | alpha, epsilon, delta)) for every object in the data for every value
     * of alpha in alphas_per_object for that object. Returns a vector over objects of vectors
     * over log conditional probabilities per value of alpha.
     *
     * For example, if alphas_per_object = {{{1,0}}, {0,1}}, {{1,0}},
     * {{1,0}, {0,1}, {1, 1}} then the output might be {{-2, -8}, {-7}, {-1.2,
     * -2.1, -5.7}} where {-2, -8} are the log conditional probabilities for alpha={1,0} and
     * alpha={0,1} for the first object.
     */
    std::vector<std::vector<double>> distribution(
        const std::vector<std::vector<alpha_t>> &alphas_per_object, double epsilon,
        const delta_t &delta) const;

    /**
     * Calculates log(p(k | alpha, epsilon, delta)) for every object in the data for the given
     * alpha. Returns a vector over objects of vectors over log conditional probabilities per value
     * of alpha.
     *
     * For example, if alphas = {{0,1,0}, {1,1,1}} then the output might be {{-2, -7}, {-4, -5}}
     * where log(p(k | alpha={0,1,0}, ...)) = -2 for the first object,
     * log(p(k | alpha={0,1,0}, ...)) = -4 for the second obect, log(p(k | alpha={1,1,1}, ...)) = -7
     * for the first object, and so on.
     */
    std::vector<std::vector<double>> distribution(const std::vector<alpha_t> &alphas,
                                                  double epsilon, const delta_t &delta) const;

    // The following are public only so I can test them easier. FRIEND_TEST exists, but it doesn't
    // work right for static methods.

    static double calculate_log_p_k_positive_given_alpha_n_positive(int n_positive,
                                                                    const alpha_t &alpha);
    // This is non-static only so it has access to the comparison testing parameter.
    double calculate_log_sum_over_k_negative(int n_positive, int n_negative, const alpha_t &alpha,
                                             const delta_t &delta,
                                             const category_counts_t &object_counts) const;
    static double calculate_sum_over_k_negative_exact(int n_positive, int n_negative,
                                                      const alpha_t &alpha, const delta_t &delta,
                                                      const category_counts_t &object_counts);

    // Uses MVI3 integration
    static double calculate_sum_over_k_negative_approx(int n_positive, int n_negative,
                                                       const alpha_t &alpha, const delta_t &delta,
                                                       const category_counts_t &object_counts);
    /**
     * Given n^+, n^-, alpha, and k, loop over all possible values for k^-, and call f() on each.
     *
     * This is hard-coded for |alpha|=3. Doing this in a generic way was incredibly slow.
     */
    static void iterate_over_k_negatives(std::function<void(int, int, int)> f, int n_positive,
                                         int n_negative, const alpha_t &alpha,
                                         const category_counts_t &object_counts);

    /**
     * Removes components of v for which alpha_i has the wrong value.
     */
    template <class T>
    static std::vector<T> filter_by_alpha(const alpha_t &alpha, const std::vector<T> &v,
                                          const bool value = true) {
        assert(alpha.size() == v.size());

        std::vector<T> filtered;
        for (int i = 0; i < alpha.size(); ++i) {
            if (alpha.at(i) == value) {
                filtered.push_back(v.at(i));
            }
        }
        return filtered;
    }

   private:
    const std::vector<category_counts_t> &data;
    const Options options;
    std::default_random_engine &generator;

    /**
     * Test for if using a normal approximation (and mvi3 integration) is permissible.
     *
     * There is no real standard for when the multinomial to multivariate guassian approximation
     * is valid, so we adapt the standard binomial to guassian rule, which is that there is a
     * mean of at least 5 elements in every dimension.
     *
     * TODO(joschnei): Verify experimentally that this is a good heuristic.
     */
    bool can_use_normal_approx(int n_negative, const delta_t &delta) const;

    // The below are currently not in use beacuse of randomness problems.

    // Uses naive Monte-Carlo integration
    double calculate_sum_over_k_negative_approx_2(int n_positive, int n_negative,
                                                  const alpha_t &alpha, const delta_t &delta,
                                                  const category_counts_t &object_counts);
    /**
     * Creates a hyperplane array representing the constraints on k_minus for use in mvi3
     * integration.
     *
     * Each of the four constraints on k_negative can be represented as a set of linear
     * inequalities in the values of k_negative. This function creates those linear equalities
     * so that they can be passed as constraints to mvi3, a method for integrating a multinomial
     * pdf over a convex set. See function defintion for details of the exact constraints.
     */
    static std::vector<std::vector<double>> get_hyperplanes(int n_negative,
                                                            category_counts_t object_counts);
};
}  // namespace FilterModel

#endif


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
/*

The example demonstrates the use of LAPACK (via FLENS) with different
storage orders. Note that column major ordering of A is equivalent to
row major ordering of A^T.

right eigen:

A v = lambda v

left eigen:

u^H A = lambda u^H

      | A | A^T
----------------
left  | u | v^*
----------------
right | v | u^*
----------------

*/

#include "Blitz2FLENS.h"

#include "MathExtensions.h"
#include "BlitzTiny.h"
#include "ComplexArrayExtensions.tcc"
#include "Randomized.h"

#include "Range.h"

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

#include <boost/bind.hpp>

using namespace std;
using namespace randomized;
using namespace blitz2flens;
using namespace blitzplusplus;
using namespace mathutils;

using blitz::ColumnMajorArray;

const double epsilonCmp=1e-12;

const int RANK=3;

typedef CArray<  RANK> CAR ;
typedef DArray<  RANK> DAR ;
typedef CArray<2*RANK> CA2R;

typedef DenseVectorMF<dcomp >::type CDenseVector;
typedef DenseVectorMF<double>::type DDenseVector;
typedef GeMatrixMF<dcomp,RowMajor>::type GeMatrixRM;
typedef GeMatrixMF<dcomp,ColMajor>::type GeMatrixCM;

typedef HeMatrixMF<RowMajor>::type HeMatrixRM;

Randomized::Ptr ran(MakerGSL()(1001));

ExtTiny<RANK> dims(6,4,5);

// RowMajor, C/C++Array

BOOST_AUTO_TEST_CASE( RowMajorTest )
{
  CAR  vecBlitz(dims);
  CA2R matBlitz(concatenateTinies(dims,dims));
  CA2R vlBlitz(matBlitz.shape()), vrBlitz(matBlitz.shape());

  CDenseVector vecFLENS(blitz2flens::vector(vecBlitz));

  GeMatrixRM
    vlFLENS(matrix<RowMajor>(vlBlitz)),
    vrFLENS(matrix<RowMajor>(vrBlitz));

  fillWithRandom(matBlitz,ran);

  {
    CA2R a(matBlitz.copy());
    GeMatrixRM aFLENS(matrix<RowMajor>(a));
    cerr<<"Entering ev routine ... "; BOOST_CHECK(!ev(true,true,aFLENS,vecFLENS,vlFLENS,vrFLENS)); cerr<<"exiting, checking result ... ";
  }

  {
    CA2R resTensor(matBlitz.shape());
    CArray<9> temp9(concatenateTinies(matBlitz.shape(),dims));
    {
      using namespace blitz::tensor;
      temp9=matBlitz(i,j,k,o,p,q)*conj(vlBlitz(l,m,n,o,p,q))/vecBlitz(l,m,n);
      resTensor=CA2R(sum(sum(sum(temp9,q),p),o));

      BOOST_CHECK(!fcmp(1-max(abs(vlBlitz.transpose(3,4,5,0,1,2)-conj(resTensor))),1,epsilonCmp));

      temp9=matBlitz(o,p,q,i,j,k)*vrBlitz(l,m,n,o,p,q)/vecBlitz(l,m,n);
      resTensor=CA2R(sum(sum(sum(temp9,q),p),o));
    }
    BOOST_CHECK(!fcmp(1-max(abs(vrBlitz.transpose(3,4,5,0,1,2)-resTensor)),1,epsilonCmp));

    cerr<<"Nonsymmetric eigenproblem in RowMajor OK!\n";

  }
}

// ColMajor, FortranArray

BOOST_AUTO_TEST_CASE( ColMajorTest )
{
  CAR  vecBlitz(dims,ColumnMajorArray<RANK>());
  CA2R matBlitz(concatenateTinies(dims,dims),ColumnMajorArray<2*RANK>());
  CA2R vlBlitz(matBlitz.shape(),ColumnMajorArray<2*RANK>()), vrBlitz(matBlitz.shape(),ColumnMajorArray<2*RANK>());

  CDenseVector vecFLENS(blitz2flens::vector(vecBlitz));

  GeMatrixCM
    vlFLENS(matrix<ColMajor>(vlBlitz)),
    vrFLENS(matrix<ColMajor>(vrBlitz));

  fillWithRandom(matBlitz,ran);

  {
    CA2R a(matBlitz.copy());
    GeMatrixCM aFLENS(matrix<ColMajor>(a));

    cerr<<"Entering ev routine ... "; BOOST_CHECK(!ev(true,true,aFLENS,vecFLENS,vlFLENS,vrFLENS)); cerr<<"exiting, checking result ... ";
  }

  {
    CA2R resTensor(matBlitz.shape());
    CArray<9> temp9(concatenateTinies(matBlitz.shape(),dims));
    {
      using namespace blitz::tensor;
      temp9=matBlitz(i,j,k,o,p,q)*vrBlitz(o,p,q,l,m,n)/vecBlitz(l,m,n);
      resTensor=CA2R(sum(sum(sum(temp9,q),p),o));

      BOOST_CHECK(!fcmp(1-max(abs(vrBlitz-resTensor)),1,epsilonCmp));

      temp9=matBlitz(o,p,q,i,j,k)*conj(vlBlitz(o,p,q,l,m,n))/vecBlitz(l,m,n);
      resTensor=CA2R(sum(sum(sum(temp9,q),p),o));
    }

    BOOST_CHECK(!fcmp(1-max(abs(vlBlitz-conj(resTensor))),1,epsilonCmp));

    cerr<<"\"                            ColMajor OK!\n";

  }
}

BOOST_AUTO_TEST_CASE( HermitianTest )
{
  CAR  vecBlitz1(dims);
  DAR  vecBlitz2(dims);
  CA2R matBlitz(concatenateTinies(dims,dims));

  fillWithRandom(matBlitz,ran);

  matBlitz+=hermitianConjugate(matBlitz);

  {
    CA2R a(matBlitz.copy());

    CDenseVector vecFLENS(blitz2flens::vector(vecBlitz1));
    GeMatrixRM aFLENS(matrix<RowMajor>(a));

    cerr<<"Entering ev routine ... "; ev(false,false,aFLENS,vecFLENS,aFLENS,aFLENS); cerr<<"exiting, checking result ... ";
  }
  BOOST_CHECK(!fcmp(1-max(abs(imag(vecBlitz1))),1,epsilonCmp));
  cerr<<"Hermitian eigenproblem eigenvalues all real.\n";

  {
    CA2R a(matBlitz.copy());

    DDenseVector vecFLENS(blitz2flens::vector(vecBlitz2));
    HeMatrixRM aFLENS(hermitianMatrix<RowMajor>(a));

    cerr<<"Entering ev routine ... "; ev(true,aFLENS,vecFLENS); cerr<<"exiting, checking result ... ";

    {
      CA2R resTensor(matBlitz.shape());
      CArray<9> temp9(concatenateTinies(matBlitz.shape(),dims));
      {
        using namespace blitz::tensor;
        temp9=matBlitz(o,p,q,i,j,k)*a(l,m,n,o,p,q)/vecBlitz2(l,m,n);
        resTensor=CA2R(sum(sum(sum(temp9,q),p),o));

        BOOST_CHECK(!fcmp(1-max(abs(a.transpose(3,4,5,0,1,2)-resTensor)),1,epsilonCmp));

      }

    }

  }

  cerr<<"\"                      eigenproblem OK.\n";

  std::sort(vecBlitz1.data(),vecBlitz1.data()+vecBlitz1.size(),realCompare);

  // note: vecBlitz2 is already sorted

  BOOST_CHECK(!fcmp(1-max(abs(vecBlitz2-real(vecBlitz1))),1,epsilonCmp));

  cerr<<"Hermitian eigenproblem eigenvalues match.\n";

}