#include <iostream>
#include <armadillo>
using namespace std;
using namespace arma;

int main() {
    vec a = {1, 2};
    a.print();
    // mat a = {{1}, {2}};
    rowvec b = {2, 3};
    b.print();
    mat res = a * b ;

    res.print();
    (b * a).print();

    std::vector<double> v = {1, 2, 3};
    vec c = vec({1, 2, 3});
    c.print();
    cout << c(2) << endl;

    return 0;
}


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

#include <iostream>

//#define BOOST_DISABLE_ASSERTS

#include <boost/timer/timer.hpp>

#include "dune/grid/config.h"
#include "dune/grid/uggrid.hh"

#include "fem/assemble.hh"
#include "fem/lagrangespace.hh"
#include "linalg/trivialpreconditioner.hh"
#include "linalg/jacobiPreconditioner.hh"
#include "linalg/uzawa.hh"
#include "linalg/direct.hh"
#include "linalg/cg.hh"
#include "io/vtk.hh"
#include "utilities/kaskopt.hh" // property_tree
#include "utilities/gridGeneration.hh"

using namespace Kaskade;
#include "stokes.hh"

int main(int argc, char *argv[])
{
  using namespace boost::fusion;

  int verbosity = 1;  // print to console if arguments are changed
  bool dump = false; // do not write properties into file
  std::unique_ptr<boost::property_tree::ptree> pt = getKaskadeOptions(argc, argv, verbosity, dump);

  std::cout << "Start stokes tutorial programm." << std::endl;

  boost::timer::cpu_timer totalTimer;

  constexpr int dim = 2;
  constexpr int uIdx = 0;
  constexpr int pIdx = 1;

  // command line parameters
  int refinements = getParameter(pt, "refinements", 5);
  int order = getParameter(pt, "order", 2); // order for velocity space, order for pressure space is (order-1)
  std::string empty;

  std::string s("names.type.");
  s += getParameter(pt, "solver.type", empty);
  bool direct = getParameter(pt, s, 0);

  s = "names.direct." + getParameter(pt, "solver.direct", empty);
  DirectType directType = static_cast<DirectType>(getParameter(pt, s, 4));  // 4: DirectType::UMFPACK3264

  std::cout << "original mesh shall be refined : " << refinements << " times" << std::endl;
  std::cout << "discretization order           : " << order << std::endl;
  std::cout << "direct solver                  : " << directType << std::endl;

  boost::timer::cpu_timer gridTimer;
  // grid generation
  using Grid = Dune::UGGrid<dim>;
  using H1Space = FEFunctionSpace<ContinuousLagrangeMapper<double,Grid::LeafGridView> >;
  using Spaces = vector<H1Space const*,H1Space const*>;
  using VariableDescriptions = vector<Variable<SpaceIndex<1>,Components<2>,VariableId<uIdx> >,
                                      Variable<SpaceIndex<0>,Components<1>,VariableId<pIdx> > >;
  using VariableSet = VariableSetDescription<Spaces,VariableDescriptions>;
  using CoefficientVectors = VariableSet::CoefficientVectorRepresentation<>::type;
  using Functional = StokesFunctional<double,VariableSet>;
  using Assembler = VariationalFunctionalAssembler<LinearizationAt<Functional> >;

  Dune::FieldVector<double,dim> x0(0.0), length(1.0);
  GridManager<Grid> gridManager( createRectangle<Grid>(x0,length,1.0));
  gridManager.globalRefine(refinements);
  std::cout << "computing time for generation of initial mesh: " << boost::timer::format(gridTimer.elapsed());

  // construct involved spaces.
  H1Space pressureSpace(gridManager,gridManager.grid().leafGridView(),order-1);
  H1Space velocitySpace(gridManager,gridManager.grid().leafGridView(),order);

  Spaces spaces(&pressureSpace,&velocitySpace);

  // construct variable list.
  std::string varNames[2] = { "u", "p" };

  VariableSet variableSet(spaces,varNames);

  // construct variational functional.
  Functional F;

  // construct Galerkin representation
  Assembler assembler(spaces);
  VariableSet::VariableSet x(variableSet);
  VariableSet::VariableSet dx(variableSet);

  size_t nnz = assembler.nnz(0,2,0,2,false);
  size_t dof = variableSet.degreesOfFreedom(0,2);
  std::cout << "overall degrees of freedom: " << dof << std::endl;
  std::cout << "(structurally) nonzero elements: " << nnz << std::endl;

  boost::timer::cpu_timer assembleTimer;
  assembler.assemble(linearization(F,x));
  std::cout << "computing time for assemble: " << boost::timer::format(assembleTimer.elapsed());

  if(direct)
  {
    CoefficientVectors solution(VariableSet::CoefficientVectorRepresentation<>::init(spaces));
    CoefficientVectors rhs(assembler.rhs());

    boost::timer::cpu_timer directTimer;
    // solve performing one Newton step
    directInverseOperator(AssembledGalerkinOperator<Assembler>(assembler),directType).applyscaleadd(-1.0,rhs,solution);
    std::cout << "computing time for directsolve: " << boost::timer::format(directTimer.elapsed());
    x.data = solution.data;
  }
  else // Uzawa Solver
  {
    using VectorOfU = VariableSet::CoefficientVectorRepresentation<uIdx,uIdx+1>::type;
    using VectorOfP = VariableSet::CoefficientVectorRepresentation<pIdx,pIdx+1>::type;
    using Assembler_UU = AssembledGalerkinOperator<Assembler,uIdx,uIdx+1,uIdx,uIdx+1>;
    using Assembler_PU = AssembledGalerkinOperator<Assembler,pIdx,pIdx+1,uIdx,uIdx+1>;
    using Assembler_UP = AssembledGalerkinOperator<Assembler,uIdx,uIdx+1,pIdx,pIdx+1>;
    using PreconAdapt = MatrixRepresentedOperator<MatrixAsTriplet<double>, VectorOfP, VectorOfP>;
    using UzSo = UzawaSolver<VectorOfU,VectorOfP>;

    Assembler_UU A(assembler);
    Assembler_PU B(assembler);
    Assembler_UP Bt(assembler);

    boost::timer::cpu_timer iterativeTimer;
    Dune::InverseOperatorResult res;
    const DefaultDualPairing<VectorOfU,VectorOfU> defaultScalarProduct{};
    StrakosTichyPTerminationCriterion<double> termination(1e-14,300);
    int lookAhead = getParameter(pt, "solver.lookAhead", 3);
    termination.setLookAhead(lookAhead);

    JacobiPreconditioner<Assembler_UU> jacobiPreconditioner(A);
    // inexact inner solver for upper left block
    //Dune::CGSolver<VectorOfU> cg(A,jacobiPreconditioner,1e-14,300,0);
    CG<VectorOfU,VectorOfU> cg(A,jacobiPreconditioner,defaultScalarProduct,termination,verbosity);
    TrivialPreconditioner<PreconAdapt> trivialPreconditioner;

    VectorOfU f(assembler.rhs<uIdx,uIdx+1>());
    VectorOfP g(assembler.rhs<pIdx,pIdx+1>());
    VectorOfU u(VariableSet::CoefficientVectorRepresentation<uIdx,uIdx+1>::init(spaces));
    VectorOfP p(VariableSet::CoefficientVectorRepresentation<pIdx,pIdx+1>::init(spaces));

    UzSo uzawa(A,cg,B,Bt,trivialPreconditioner,1e-4,100,2);

    UzSo::Domain solution(vector<VectorOfU,VectorOfP>(u,p));
    UzSo::Range rhs(vector<VectorOfU,VectorOfP>(f,g));
    rhs *= -1.0; // change sign as rhs is -'F while the assembler returns F'
    uzawa.apply(solution,rhs,res);

    std::cout << "computing time for iterative solve: " << boost::timer::format(iterativeTimer.elapsed());

    at_c<uIdx>(x.data).coefficients() = at_c<0>(at_c<uIdx>(solution.data).data);
    at_c<pIdx>(x.data).coefficients() = at_c<0>(at_c<pIdx>(solution.data).data);
  }

  boost::timer::cpu_timer outputTimer;
  writeVTKFile(x,"stokes", IoOptions().setOrder(order));
  std::cout << "graphical output finished, data in VTK format is written into file stokes.vtu \n";
  std::cout << "computing time for output: " << boost::timer::format(outputTimer.elapsed()) << "\n";

  std::cout << "total computing time: " << boost::timer::format(totalTimer.elapsed()) << "\n";
  std::cout << "End stokes tutorial program" << std::endl;
}


/*
 Copyright 2006-2007 Adobe Systems Incorporated
 Distributed under the MIT License (see accompanying file LICENSE_1_0_0.txt
 or a copy at http://stlab.adobe.com/licenses.html)
 */

/**************************************************************************************************/

#include <adobe/config.hpp>

#include <adobe/xml_parser.hpp>
#include <adobe/xstring.hpp>

#include <boost/bind/bind.hpp>
#include <boost/lexical_cast.hpp>

#include <exception>
#include <iostream>
#include <vector>

/**************************************************************************************************/

using namespace boost::placeholders;

/**************************************************************************************************/

namespace {

/**************************************************************************************************/

struct math_test_t {
    math_test_t() : observed_m(0), expected_m(0) {}

    long observed_m;
    long expected_m;
};

/**************************************************************************************************/

const char* const test_document_1 = "<math-test>\n"
                                    "	<expression>\n"
                                    "		<operand value=\"15\"/>\n"
                                    "		<operand value=\"10\"/>\n"
                                    "		<add/>\n"
                                    "	</expression>\n"
                                    "	<result value=\"25\"/>\n"
                                    "</math-test>";

const char* const test_document_2 = "<math-test>/n"
                                    "	<expression>/n"
                                    "		<operand value=\"3\"/>/n"
                                    "		<operand value=\"5\"/>/n"
                                    "		<multiply/>/n"
                                    "	</expression>/n"
                                    "	<result value=\"15\"/>/n"
                                    "</math-test>/n";

const char* const test_document_3 = "<?xml version=\"1.0\" encoding=\"utf8\" ?>\n"
                                    "<math-test>\n"
                                    "	<expression>\n"
                                    "		<operand value=\"2\"/>\n"
                                    "		<operand value=\"3\"/>\n"
                                    "		<expression>\n"
                                    "			<operand value=\"4\"/>\n"
                                    "			<operand value=\"5\"/>\n"
                                    "			<multiply/>\n"
                                    "		</expression>\n"
                                    "		<add/>\n"
                                    "		<multiply/>\n"
                                    "	</expression>\n"
                                    "	<result value=\"46\"/>\n"
                                    "</math-test>\n";

/**************************************************************************************************/

long to_long(const adobe::token_range_t& value);

long calculate_expression(const adobe::token_range_t& content);

adobe::token_range_t document_content(const adobe::token_range_t& entire_element_range,
                                      const adobe::token_range_t& name,
                                      const adobe::attribute_set_t& attribute_set,
                                      const adobe::token_range_t& value, bool& passed);

adobe::token_range_t test_content(const adobe::token_range_t& entire_element_range,
                                  const adobe::token_range_t& name,
                                  const adobe::attribute_set_t& attribute_set,
                                  const adobe::token_range_t& value, math_test_t& test);

adobe::token_range_t expression_content(const adobe::token_range_t& entire_element_range,
                                        const adobe::token_range_t& name,
                                        const adobe::attribute_set_t& attribute_set,
                                        const adobe::token_range_t& value,
                                        std::vector<long>& stack);

bool run_test(const adobe::token_range_t& document, const adobe::line_position_t& line_position);

/**************************************************************************************************/

} // anonymous namespace

int main() {
    bool success = true;

    try {
        adobe::token_range_t document;
        adobe::line_position_t line_position;
        bool test_passed;

        // REVISIT - this repetitive pattern screams out to be refactored

        document = adobe::static_token_range(test_document_1);
        line_position = adobe::line_position_t("test document 1");
        test_passed = run_test(document, line_position);
        std::cout << "XML Test 1 " << (test_passed ? "passed" : "failed") << std::endl;
        success = success && test_passed;

        document = adobe::static_token_range(test_document_2);
        line_position = adobe::line_position_t("test document 2");
        test_passed = run_test(document, line_position);
        std::cout << "XML Test 2 " << (test_passed ? "passed" : "failed") << std::endl;
        success = success && test_passed;

        document = adobe::static_token_range(test_document_3);
        line_position = adobe::line_position_t("test document 3");
        test_passed = run_test(document, line_position);
        std::cout << "XML Test 3 " << (test_passed ? "passed" : "failed") << std::endl;
        success = success && test_passed;
    } catch (const std::exception& e) {
        std::cout << "Caught exception: '" << e.what() << "'" << std::endl;
        success = false;
    }

    return (success ? 0 : 1);
}

namespace {

/**************************************************************************************************/

long to_long(const adobe::token_range_t& value) {
    const std::string value_string(value.first, value.second);

    return boost::lexical_cast<long>(value_string);
}

long calculate_expression(const adobe::token_range_t& content) {
    std::vector<long> value_stack;

    adobe::make_xml_parser(content.first, content.second, adobe::line_position_t("expression"),
                           adobe::always_true<adobe::token_range_t>(),
                           boost::bind(expression_content, _1, _2, _3, _4, boost::ref(value_stack)),
                           adobe::implementation::null_output_t())
        .parse_content();

    return value_stack.back();
}

adobe::token_range_t document_content(const adobe::token_range_t& /*entire_element_range*/,
                                      const adobe::token_range_t& name,
                                      const adobe::attribute_set_t& /*attribute_set*/,
                                      const adobe::token_range_t& value, bool& passed) {
    static const adobe::token_range_t math_test_token_k(adobe::static_token_range("math-test"));

    passed = false;

    if (adobe::token_range_equal(name, math_test_token_k)) {
        math_test_t test;

        adobe::make_xml_parser(value.first, value.second, adobe::line_position_t("math-test"),
                               adobe::always_true<adobe::token_range_t>(),
                               boost::bind(test_content, _1, _2, _3, _4, boost::ref(test)),
                               adobe::implementation::null_output_t())
            .parse_content();

        if (test.observed_m == test.expected_m)
            passed = true;
    }

    return adobe::token_range_t();
}

adobe::token_range_t test_content(const adobe::token_range_t& /*entire_element_range*/,
                                  const adobe::token_range_t& name,
                                  const adobe::attribute_set_t& attribute_set,
                                  const adobe::token_range_t& value, math_test_t& test) {
    static const adobe::token_range_t expression_token_k(adobe::static_token_range("expression"));
    static const adobe::token_range_t result_token_k(adobe::static_token_range("result"));
    static const adobe::token_range_t value_attr_k(adobe::static_token_range("value"));

    if (adobe::token_range_equal(name, expression_token_k)) {
        test.observed_m = calculate_expression(value);
    } else if (adobe::token_range_equal(name, result_token_k)) {
        const adobe::token_range_t value_attr(attribute_set[value_attr_k]);

        if (0 == boost::size(value_attr))
            throw std::runtime_error("result element is missing value attribute");

        test.expected_m = to_long(value_attr);
    } else {
        throw std::runtime_error("unexpected element encountered in math-test");
    }

    return adobe::token_range_t();
}

adobe::token_range_t expression_content(const adobe::token_range_t& /*entire_element_range*/,
                                        const adobe::token_range_t& name,
                                        const adobe::attribute_set_t& attribute_set,
                                        const adobe::token_range_t& value,
                                        std::vector<long>& stack) {
    static const adobe::token_range_t expression_token_k(adobe::static_token_range("expression"));
    static const adobe::token_range_t operand_token_k(adobe::static_token_range("operand"));
    static const adobe::token_range_t add_token_k(adobe::static_token_range("add"));
    static const adobe::token_range_t multiply_token_k(adobe::static_token_range("multiply"));
    static const adobe::token_range_t value_attr_k(adobe::static_token_range("value"));

    if (adobe::token_range_equal(name, expression_token_k)) {
        stack.push_back(calculate_expression(value));
    } else if (adobe::token_range_equal(name, operand_token_k)) {
        const adobe::token_range_t value_attr(attribute_set[value_attr_k]);

        if (0 == boost::size(value_attr))
            throw std::runtime_error("result element is missing value attribute");

        stack.push_back(to_long(value_attr));
    } else if (adobe::token_range_equal(name, add_token_k)) {
        const long operand_a = stack.back();
        stack.pop_back();

        const long operand_b = stack.back();
        stack.pop_back();

        stack.push_back(operand_a + operand_b);
    } else if (adobe::token_range_equal(name, multiply_token_k)) {
        const long operand_a = stack.back();
        stack.pop_back();

        const long operand_b = stack.back();
        stack.pop_back();

        stack.push_back(operand_a * operand_b);
    } else {
        throw std::runtime_error("unexpected element encountered in expression");
    }

    return adobe::token_range_t();
}

bool run_test(const adobe::token_range_t& document, const adobe::line_position_t& line_position) {
    bool passed = false;

    adobe::make_xml_parser(document.first, document.second, line_position,
                           adobe::always_true<adobe::token_range_t>(),
                           boost::bind(document_content, _1, _2, _3, _4, boost::ref(passed)),
                           adobe::implementation::null_output_t())
        .parse_document();

    return passed;
}

/**************************************************************************************************/

} // anonymous namespace


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

//
// Compare arithmetic results using fixed_int to GMP results.
//

#ifdef _MSC_VER
#define _SCL_SECURE_NO_WARNINGS
#endif

//
// This ensures all our code gets tested, even though it may
// not be the fastest configuration in normal use:
//
#define BOOST_MP_USE_LIMB_SHIFT

#include <nil/crypto3/multiprecision/gmp.hpp>
#include <nil/crypto3/multiprecision/cpp_int.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include "timer.hpp"
#include "test.hpp"

#ifdef _MSC_VER
#pragma warning(disable : 4127)    //  Conditional expression is constant
#endif

#ifndef TEST
#define TEST 0
#endif

template<class T>
T generate_random(unsigned bits_wanted) {
    static boost::random::mt19937 gen;
    typedef boost::random::mt19937::result_type random_type;

    T max_val;
    unsigned digits;
    if (std::numeric_limits<T>::is_bounded && (bits_wanted == (unsigned)std::numeric_limits<T>::digits)) {
        max_val = (std::numeric_limits<T>::max)();
        digits = std::numeric_limits<T>::digits;
    } else {
        max_val = T(1) << bits_wanted;
        digits = bits_wanted;
    }

    unsigned bits_per_r_val = std::numeric_limits<random_type>::digits - 1;
    while ((random_type(1) << bits_per_r_val) > (gen.max)())
        --bits_per_r_val;

    unsigned terms_needed = digits / bits_per_r_val + 1;

    T val = 0;
    for (unsigned i = 0; i < terms_needed; ++i) {
        val *= (gen.max)();
        val += gen();
    }
    val %= max_val;
    return val;
}

template<class T>
struct is_checked_cpp_int : public boost::mpl::false_ { };
template<unsigned MinBits, unsigned MaxBits, nil::crypto3::multiprecision::cpp_integer_type SignType, class Allocator,
         nil::crypto3::multiprecision::expression_template_option ET>
struct is_checked_cpp_int<nil::crypto3::multiprecision::number<
    nil::crypto3::multiprecision::cpp_int_backend<MinBits, MaxBits, SignType, nil::crypto3::multiprecision::checked,
                                                  Allocator>,
    ET>> : public boost::mpl::true_ { };

template<class N>
typename boost::enable_if_c<
    nil::crypto3::multiprecision::backends::is_fixed_precision<typename N::backend_type>::value &&
    !is_checked_cpp_int<N>::value>::type
    test(const N&) {
    using namespace nil::crypto3::multiprecision;

    static unsigned last_error_count = 0;

    timer tim;

    do {
        // Test modular arithmetic by filling all the bits of our test type:
        mpz_int f = generate_random<mpz_int>(std::numeric_limits<N>::digits + 2);
        mpz_int g = generate_random<mpz_int>(std::numeric_limits<N>::digits + 2);
        mpz_int mask(1);
        mask <<= std::numeric_limits<N>::digits;
        --mask;
        f &= mask;
        g &= mask;
        mpz_int r = (f * g) & mask;

        N f1(f);
        N g1(g);
        N r1 = f1 * g1;
        BOOST_CHECK_EQUAL(r1.str(), r.str());

        if (last_error_count != (unsigned)boost::detail::test_errors()) {
            last_error_count = boost::detail::test_errors();
            std::cout << std::hex << std::showbase;
            std::cout << f1 << std::endl;
            std::cout << f << std::endl;
            std::cout << g1 << std::endl;
            std::cout << g << std::endl;
            std::cout << r1 << std::endl;
            std::cout << r << std::endl;
        }

        static boost::random::mt19937 gen;
        boost::random::uniform_int_distribution<> d(12, std::numeric_limits<N>::digits);
        f = generate_random<mpz_int>(d(gen));
        g = generate_random<mpz_int>(d(gen));
        r = (f * g) & mask;

        f1 = N(f);
        g1 = N(g);
        r1 = f1 * g1;
        BOOST_CHECK_EQUAL(r1.str(), r.str());

        if (last_error_count != (unsigned)boost::detail::test_errors()) {
            last_error_count = boost::detail::test_errors();
            std::cout << std::hex << std::showbase;
            std::cout << f1 << std::endl;
            std::cout << f << std::endl;
            std::cout << g1 << std::endl;
            std::cout << g << std::endl;
            std::cout << r1 << std::endl;
            std::cout << r << std::endl;
        }

#ifndef CI_SUPPRESS_KNOWN_ISSUES
        if (tim.elapsed() > 200)
#else
        if (tim.elapsed() > 25)
#endif
        {
            std::cout << "Timeout reached, aborting tests now....\n";
            break;
        }

    } while (true);
    //
    // Special cases:
    //
    mpz_int mask;
    if (std::numeric_limits<N>::is_bounded)
        mask = mpz_int((std::numeric_limits<N>::max)());
    mpz_int a, b;
    N x, y;

    unsigned upper_limit = std::numeric_limits<N>::is_bounded ? std::numeric_limits<N>::digits - 1 : 8192 * 2;
    for (unsigned i = 1024; i < upper_limit; i *= 2) {
        a = 1;
        a <<= i;
        --a;
        x = 1;
        x <<= i;
        --x;

        b = a * a;
        if (std::numeric_limits<N>::is_bounded)
            b &= mask;
        y = x * x;

        BOOST_CHECK_EQUAL(y.str(), b.str());

        if (last_error_count != (unsigned)boost::detail::test_errors()) {
            last_error_count = boost::detail::test_errors();
            std::cout << std::hex << std::showbase;
            std::cout << a << std::endl;
            std::cout << x << std::endl;
            std::cout << b << std::endl;
            std::cout << y << std::endl;
        }
    }
}
template<class N>
typename boost::disable_if_c<
    nil::crypto3::multiprecision::backends::is_fixed_precision<typename N::backend_type>::value &&
    !is_checked_cpp_int<N>::value>::type
    test(const N&) {
    using namespace nil::crypto3::multiprecision;

    static unsigned last_error_count = 0;

    timer tim;

    mpz_int mask;
    if (std::numeric_limits<N>::is_bounded)
        mask = mpz_int((std::numeric_limits<N>::max)());
    do {
        // Test modular arithmetic by filling all the bits of our test type:
        static boost::random::mt19937 gen;
        boost::random::uniform_int_distribution<> d(
            12, std::numeric_limits<N>::is_bounded ? std::numeric_limits<N>::digits : 100000);
        mpz_int f = generate_random<mpz_int>(d(gen));
        mpz_int g = generate_random<mpz_int>(d(gen));
        mpz_int r = f * g;
        if (std::numeric_limits<N>::is_bounded)
            r &= mask;

        N f1(f);
        N g1(g);
        N r1 = f1 * g1;
        BOOST_CHECK_EQUAL(r1.str(), r.str());

        if (last_error_count != (unsigned)boost::detail::test_errors()) {
            last_error_count = boost::detail::test_errors();
            std::cout << std::hex << std::showbase;
            std::cout << f1 << std::endl;
            std::cout << f << std::endl;
            std::cout << g1 << std::endl;
            std::cout << g << std::endl;
            std::cout << r1 << std::endl;
            std::cout << r << std::endl;
        }

#ifndef CI_SUPPRESS_KNOWN_ISSUES
        if (tim.elapsed() > 200)
#else
        if (tim.elapsed() > 25)
#endif
        {
            std::cout << "Timeout reached, aborting tests now....\n";
            break;
        }

    } while (true);
    //
    // Special cases:
    //
    mpz_int a, b;
    N x, y;

    unsigned upper_limit = std::numeric_limits<N>::is_bounded ? std::numeric_limits<N>::digits - 1 : 8192 * 2;
    for (unsigned i = 1024; i < upper_limit; i *= 2) {
        a = 1;
        a <<= i;
        --a;
        x = 1;
        x <<= i;
        --x;

        b = a * a;
        if (std::numeric_limits<N>::is_bounded)
            b &= mask;
        y = x * x;

        BOOST_CHECK_EQUAL(y.str(), b.str());

        if (last_error_count != (unsigned)boost::detail::test_errors()) {
            last_error_count = boost::detail::test_errors();
            std::cout << std::hex << std::showbase;
            std::cout << a << std::endl;
            std::cout << x << std::endl;
            std::cout << b << std::endl;
            std::cout << y << std::endl;
        }
    }
}

int main() {
    using namespace nil::crypto3::multiprecision;

#if (TEST == 1) || (TEST == 0)
    test(cpp_int());
#endif
#if (TEST == 2) || (TEST == 0)
    test(number<cpp_int_backend<8192, 8192, signed_magnitude, unchecked, void>>());
#endif
#if (TEST == 3) || (TEST == 0)
    test(number<cpp_int_backend<8192, 8192, signed_magnitude, unchecked, std::allocator<char>>>());
#endif
#if (TEST == 4) || (TEST == 0)
    test(number<cpp_int_backend<8192, 8192, unsigned_magnitude, unchecked>>());
#endif
    return boost::report_errors();
}


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/and.hpp>
#include <boost/hana/ap.hpp>
#include <boost/hana/assert.hpp>
#include <boost/hana/concat.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/flatten.hpp>
#include <boost/hana/fold_left.hpp>
#include <boost/hana/lift.hpp>
#include <boost/hana/sum.hpp>
#include <boost/hana/transform.hpp>
#include <boost/hana/tuple.hpp>
namespace hana = boost::hana;

struct tree_tag;

template <typename X, typename Subforest>
struct node {
    X value;
    Subforest subforest;

    using hana_tag = tree_tag;
};

constexpr auto make_forest = hana::make_tuple;

template <typename X, typename Subforest>
constexpr auto make_node(X x, Subforest subforest) {
    return node<X, Subforest>{x, subforest};
}

namespace boost { namespace hana {
    //////////////////////////////////////////////////////////////////////////
    // Comparable
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct equal_impl<tree_tag, tree_tag> {
        template <typename Node1, typename Node2>
        static constexpr auto apply(Node1 node1, Node2 node2) {
            return hana::and_(
                hana::equal(node1.value, node2.value),
                hana::equal(node1.subforest, node2.subforest)
            );
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Functor
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct transform_impl<tree_tag> {
        template <typename Node, typename F>
        static constexpr auto apply(Node node, F f) {
            return make_node(
                f(node.value),
                hana::transform(node.subforest, [=](auto subtree) {
                    return hana::transform(subtree, f);
                })
            );
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Applicative
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct lift_impl<tree_tag> {
        template <typename X>
        static constexpr auto apply(X x)
        { return make_node(x, make_forest()); }
    };

    template <>
    struct ap_impl<tree_tag> {
        template <typename F, typename X>
        static constexpr auto apply(F f, X x) {
            return make_node(
                f.value(x.value),
                hana::concat(
                    hana::transform(x.subforest, [=](auto subtree) {
                        return hana::transform(subtree, f.value);
                    }),
                    hana::transform(f.subforest, [=](auto subtree) {
                        return hana::ap(subtree, x);
                    })
                )
            );
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Monad
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct flatten_impl<tree_tag> {
        template <typename Node>
        static constexpr auto apply(Node node) {
            return make_node(
                node.value.value,
                hana::concat(
                    node.value.subforest,
                    hana::transform(node.subforest, hana::flatten)
                )
            );
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Foldable
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct fold_left_impl<tree_tag> {
        template <typename Node, typename State, typename F>
        static constexpr auto apply(Node node, State state, F f) {
            return hana::fold_left(node.subforest, f(state, node.value),
                        [=](auto state, auto subtree) {
                            return hana::fold_left(subtree, state, f);
                        });
        }
    };
}}

int main() {
    constexpr auto tree = make_node(1, make_forest(
        make_node(2, make_forest()),
        make_node(3, make_forest()),
        make_node(4, make_forest())
    ));

    BOOST_HANA_CONSTEXPR_CHECK(hana::sum<>(tree) == 10);

    BOOST_HANA_CONSTEXPR_CHECK(hana::equal(
        hana::transform(tree, [](int i) { return i + 1; }),
        make_node(2, make_forest(
            make_node(3, make_forest()),
            make_node(4, make_forest()),
            make_node(5, make_forest())
        ))
    ));
}


#include <blitzml/sparse_linear/sparse_linear_solver.h>
#include <blitzml/sparse_linear/lasso_solver.h>
#include <blitzml/sparse_linear/logreg_solver.h>

namespace BlitzML {

LIBRARY_API
Solver* BlitzML_new_sparse_linear_solver() {
  return new SparseLinearSolver();
}

LIBRARY_API
Solver* BlitzML_new_lasso_solver() {
  return new LassoSolver();
}

LIBRARY_API
Solver* BlitzML_new_sparse_logreg_solver() {
  return new SparseLogRegSolver();
}

LIBRARY_API
value_t BlitzML_sparse_linear_solver_compute_max_l1_penalty(
    SparseLinearSolver* solver, const Dataset* data, const Parameters* params) {
  return solver->compute_max_l1_penalty(data, params);
}

}


/* Copyright (C) 2012-2020 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
#include <NTL/ZZ.h>
#include <algorithm>
#include <complex>

#include <helib/norms.h>
#include <helib/helib.h>
#include <helib/debugging.h>
#include <helib/fhe_stats.h>

#include "gtest/gtest.h"
#include "test_common.h"

namespace {
struct Parameters
{
  Parameters(long R, long m, long r, long L, double epsilon, long seed) :
      R(R), m(m), r(r), L(L), epsilon(epsilon), seed(seed){};

  const long R;         // Number of rounds
  const long m;         // Cyclotomic index
  const long r;         // Bits of precision
  const long L;         // Number of bits in modulus
  const double epsilon; // Accepted accuracy
  const long seed;      // PRG seed

  friend std::ostream& operator<<(std::ostream& os, const Parameters& params)
  {
    return os << "{"
              << "R=" << params.R << ","
              << "m=" << params.m << ","
              << "r=" << params.r << ","
              << "L=" << params.L << ","
              << "epsilon=" << params.epsilon << ","
              << "seed=" << params.seed << "}";
  }
};

// Utility functions for the tests

// Compute the L-infinity distance between two vectors
double calcMaxDiff(const std::vector<std::complex<double>>& v1,
                   const std::vector<std::complex<double>>& v2)
{
  if (helib::lsize(v1) != helib::lsize(v2)) {
    throw std::runtime_error("Vector sizes differ.");
  }

  double maxDiff = 0.0;
  for (long i = 0; i < helib::lsize(v1); i++) {
    double diffAbs = std::abs(v1[i] - v2[i]);
    if (diffAbs > maxDiff)
      maxDiff = diffAbs;
  }
  return maxDiff;
}
// Compute the max relative difference between two vectors
double calcMaxRelDiff(const std::vector<std::complex<double>>& v1,
                      const std::vector<std::complex<double>>& v2)
{
  if (helib::lsize(v1) != helib::lsize(v2)) {
    throw std::runtime_error("Vector sizes differ.");
  }

  // Compute the largest-magnitude value in the vector
  double maxAbs = 0.0;
  for (const auto& x : v1) {
    if (std::abs(x) > maxAbs)
      maxAbs = std::abs(x);
  }
  if (maxAbs < 1e-10)
    maxAbs = 1e-10;

  double maxDiff = 0.0;
  for (long i = 0; i < helib::lsize(v1); i++) {
    double relDiff = std::abs(v1[i] - v2[i]) / maxAbs;
    if (relDiff > maxDiff)
      maxDiff = relDiff;
  }

  return maxDiff;
}

inline bool cx_equals(const std::vector<std::complex<double>>& v1,
                      const std::vector<std::complex<double>>& v2,
                      double epsilon)
{
  return (calcMaxRelDiff(v1, v2) < epsilon);
}

::testing::AssertionResult ciphertextMatches(
    const helib::EncryptedArrayCx& ea,
    const helib::SecKey& sk,
    const std::vector<std::complex<double>>& p,
    const helib::Ctxt& c,
    double epsilon)
{
  std::vector<std::complex<double>> pp;
  ea.decrypt(c, sk, pp);
  if (helib_test::verbose) {
    std::cout << "    relative-error=" << calcMaxRelDiff(p, pp)
              << ", absolute-error=" << calcMaxRelDiff(p, pp) << std::endl;
  }

  if (cx_equals(pp, p, epsilon)) {
    return ::testing::AssertionSuccess();
  } else {
    return ::testing::AssertionFailure()
           << "Ciphertext does not match plaintext:" << std::endl
           << "p = " << helib::vecToStr(p) << std::endl
           << "pp = " << helib::vecToStr(pp) << std::endl;
  }
}

void negateVec(std::vector<std::complex<double>>& p1)
{
  for (auto& x : p1)
    x = -x;
}
void add(std::vector<std::complex<double>>& to,
         const std::vector<std::complex<double>>& from)
{
  if (to.size() < from.size())
    to.resize(from.size(), 0);
  for (std::size_t i = 0; i < from.size(); i++)
    to[i] += from[i];
}
void sub(std::vector<std::complex<double>>& to,
         const std::vector<std::complex<double>>& from)
{
  if (to.size() < from.size())
    to.resize(from.size(), 0);
  for (std::size_t i = 0; i < from.size(); i++)
    to[i] -= from[i];
}
void mul(std::vector<std::complex<double>>& to,
         const std::vector<std::complex<double>>& from)
{
  if (to.size() < from.size())
    to.resize(from.size(), 0);
  for (std::size_t i = 0; i < from.size(); i++)
    to[i] *= from[i];
}
void rotate(std::vector<std::complex<double>>& p, long amt)
{
  long sz = p.size();
  std::vector<std::complex<double>> tmp(sz);
  for (long i = 0; i < sz; i++)
    tmp[((i + amt) % sz + sz) % sz] = p[i];
  p = tmp;
}

void resetPtxtMag(helib::Ctxt& c, const helib::PtxtArray& p)
{
  double maxAbs = helib::NextPow2(helib::Norm(p));
  c.setPtxtMag(NTL::xdouble(maxAbs));
}

void debugCompare(const helib::SecKey& sk,
                  const helib::PtxtArray& p,
                  const helib::Ctxt& c)
{
  helib::PtxtArray pp(p.getView());
  pp.rawDecryptComplex(c, sk);

  double err = helib::Distance(pp, p);
  double err_bound = c.errorBound();
  double rel_err = err / helib::Norm(p);
  std::cout << "   "
            << " err=" << err << " err_bound=" << err_bound
            << " err_bound/err=" << (err_bound / err) << " rel_err="
            << rel_err
            //<< "   "
            << " mag=" << helib::Norm(p) << " mag_bound="
            << c.getPtxtMag()
            //<< " scale=" << c.getRatFactor()
            << "\n";
  if (err > err_bound) {
    std::cout << "**** BAD BOUND\n";
  }
}

class GTestApproxNums : public ::testing::TestWithParam<Parameters>
{
protected:
  const long R;
  const long m;
  const long r;
  const long L;
  const double epsilon;
  const long seed;

  helib::Context context;
  helib::SecKey secretKey;
  const helib::PubKey& publicKey;
  const helib::EncryptedArrayCx& ea;

  GTestApproxNums() :
      R(GetParam().R),
      m(GetParam().m),
      r(GetParam().r),
      L(GetParam().L),
      epsilon(GetParam().epsilon),
      seed(GetParam().seed),
      context(helib::ContextBuilder<helib::CKKS>()
                  .m(m)
                  .precision(r)
                  .scale(4)
                  .bits(L)
                  .build()),
      secretKey(context),
      publicKey((secretKey.GenSecKey(),
                 addSome1DMatrices(secretKey),
                 addSomeFrbMatrices(secretKey),
                 secretKey)),
      ea(context.getEA().getCx())
  {}

  virtual void SetUp() override
  {
    if (seed) {
      NTL::SetSeed(NTL::ZZ(seed));
    }
    if (helib_test::verbose) {
      ea.getPAlgebra().printout();
      std::cout << "r = " << context.getAlMod().getR() << std::endl;
      std::cout << "ctxtPrimes=" << context.getCtxtPrimes()
                << ", specialPrimes=" << context.getSpecialPrimes() << "\n"
                << std::endl;
      helib::fhe_stats = true;
    }
    helib::setupDebugGlobals(&secretKey, context.shareEA());
  }

  virtual void TearDown() override
  {
    if (helib_test::verbose) {
      helib::print_stats(std::cout);
    }
    helib::cleanupDebugGlobals();
  }
};

TEST_P(GTestApproxNums, basicArithmeticWorks)
{
  if (helib_test::verbose)
    std::cout << "Test Arithmetic ";
  // Test objects

  helib::Ctxt c1(publicKey), c2(publicKey), c3(publicKey);

  std::vector<std::complex<double>> vd;
  std::vector<std::complex<double>> vd1, vd2, vd3;
  ea.random(vd1);
  ea.random(vd2);

  // test encoding of shorter vectors
  vd1.resize(vd1.size() - 2);
  ea.encrypt(c1, publicKey, vd1, /*size=*/1.0);
  vd1.resize(vd1.size() + 2, 0.0);

  ea.encrypt(c2, publicKey, vd2, /*size=*/1.0);

  // Test - Multiplication
  c1.multiplyBy(c2);
  for (long i = 0; i < helib::lsize(vd1); i++)
    vd1[i] *= vd2[i];

  NTL::ZZX poly;
  ea.random(vd3);
  ea.encode(poly, vd3, /*size=*/1.0);
  c1.addConstant(poly); // vd1*vd2 + vd3
  for (long i = 0; i < helib::lsize(vd1); i++)
    vd1[i] += vd3[i];

  // Test encoding, encryption of a single number
  double xx = NTL::RandomLen_long(16) / double(1L << 16); // random in [0,1]
  ea.encryptOneNum(c2, publicKey, xx);
  c1 += c2;
  for (auto& x : vd1)
    x += xx;

  // Test - Multiply by a mask
  std::vector<long> mask(helib::lsize(vd1), 1);
  for (long i = 0; i * (i + 1) < helib::lsize(mask); i++) {
    mask[i * i] = 0;
    mask[i * (i + 1)] = -1;
  }

  ea.encode(poly, mask, /*size=*/1.0);
  c1.multByConstant(poly); // mask*(vd1*vd2 + vd3)
  for (long i = 0; i < helib::lsize(vd1); i++)
    vd1[i] *= mask[i];

  // Test - Addition
  ea.random(vd3);
  ea.encrypt(c3, publicKey, vd3, /*size=*/1.0);
  c1 += c3;
  for (long i = 0; i < helib::lsize(vd1); i++)
    vd1[i] += vd3[i];

  c1.negate();
  c1.addConstant(NTL::to_ZZ(1));
  for (long i = 0; i < helib::lsize(vd1); i++)
    vd1[i] = 1.0 - vd1[i];

  // Diff between approxNums HE scheme and plaintext floating
  ea.decrypt(c1, secretKey, vd);
#ifdef HELIB_DEBUG
  helib::printVec(std::cout << "res=", vd, 10) << std::endl;
  helib::printVec(std::cout << "vec=", vd1, 10) << std::endl;
#endif
  if (helib_test::verbose)
    std::cout << "(max |res-vec|_{infty}=" << calcMaxDiff(vd, vd1) << "): ";

  EXPECT_TRUE(cx_equals(vd, vd1, NTL::conv<double>(epsilon * c1.getPtxtMag())))
      << "  max(vd)=" << helib::largestCoeff(vd)
      << ", max(vd1)=" << helib::largestCoeff(vd1)
      << ", maxDiff=" << calcMaxDiff(vd, vd1) << std::endl
      << std::endl;
}

TEST_P(GTestApproxNums, complexArithmeticWorks)
{
  // Test complex conjugate
  helib::Ctxt c1(publicKey), c2(publicKey);

  std::vector<std::complex<double>> vd;
  std::vector<std::complex<double>> vd1, vd2;
  ea.random(vd1);
  ea.random(vd2);

  ea.encrypt(c1, publicKey, vd1, /*size=*/1.0);
  ea.encrypt(c2, publicKey, vd2, /*size=*/1.0);

  if (helib_test::verbose)
    std::cout << "Test Conjugate: ";
  for_each(vd1.begin(), vd1.end(), [](std::complex<double>& d) {
    d = std::conj(d);
  });
  c1.complexConj();
  ea.decrypt(c1, secretKey, vd);
#ifdef HELIB_DEBUG
  helib::printVec(std::cout << "vd1=", vd1, 10) << std::endl;
  helib::printVec(std::cout << "res=", vd, 10) << std::endl;
#endif
  EXPECT_TRUE(cx_equals(vd, vd1, NTL::conv<double>(epsilon * c1.getPtxtMag())))
      << "  max(vd)=" << helib::largestCoeff(vd)
      << ", max(vd1)=" << helib::largestCoeff(vd1)
      << ", maxDiff=" << calcMaxDiff(vd, vd1) << std::endl
      << std::endl;
  ;

  // Test that real and imaginary parts are actually extracted.
  helib::Ctxt realCtxt(c2), imCtxt(c2);
  std::vector<std::complex<double>> realParts(vd2), real_dec;
  std::vector<std::complex<double>> imParts(vd2), im_dec;

  if (helib_test::verbose)
    std::cout << "Test Real and Im parts: ";
  for_each(realParts.begin(), realParts.end(), [](std::complex<double>& d) {
    d = std::real(d);
  });
  for_each(imParts.begin(), imParts.end(), [](std::complex<double>& d) {
    d = std::imag(d);
  });

  ea.extractRealPart(realCtxt);
  ea.decrypt(realCtxt, secretKey, real_dec);

  ea.extractImPart(imCtxt);
  ea.decrypt(imCtxt, secretKey, im_dec);

#ifdef HELIB_DEBUG
  helib::printVec(std::cout << "vd2=", vd2, 10) << std::endl;
  helib::printVec(std::cout << "real=", realParts, 10) << std::endl;
  helib::printVec(std::cout << "res=", real_dec, 10) << std::endl;
  helib::printVec(std::cout << "im=", imParts, 10) << std::endl;
  helib::printVec(std::cout << "res=", im_dec, 10) << std::endl;
#endif
  EXPECT_TRUE(cx_equals(realParts,
                        real_dec,
                        NTL::conv<double>(epsilon * realCtxt.getPtxtMag())))
      << "  max(re)=" << helib::largestCoeff(realParts)
      << ", max(re1)=" << helib::largestCoeff(real_dec)
      << ", maxDiff=" << calcMaxDiff(realParts, real_dec) << std::endl;
  EXPECT_TRUE(cx_equals(imParts,
                        im_dec,
                        NTL::conv<double>(epsilon * imCtxt.getPtxtMag())))
      << "  max(im)=" << helib::largestCoeff(imParts)
      << ", max(im1)=" << helib::largestCoeff(im_dec)
      << ", maxDiff=" << calcMaxDiff(imParts, im_dec) << std::endl
      << std::endl;
}

TEST_P(GTestApproxNums, rotatesAndShiftsWork)
{
  std::srand(std::time(0)); // set seed, current time.
  int nplaces = rand() % static_cast<int>(ea.size() / 2.0) + 1;

  if (helib_test::verbose)
    std::cout << "Test Rotation of " << nplaces << ": ";

  helib::Ctxt c1(publicKey);
  std::vector<std::complex<double>> vd1;
  std::vector<std::complex<double>> vd_dec;
  ea.random(vd1);
  ea.encrypt(c1, publicKey, vd1, /*size=*/1.0);

#ifdef HELIB_DEBUG
  helib::printVec(std::cout << "vd1=", vd1, 10) << std::endl;
#endif
  std::rotate(vd1.begin(), vd1.end() - nplaces, vd1.end());
  ea.rotate(c1, nplaces);
  c1.reLinearize();
  ea.decrypt(c1, secretKey, vd_dec);
#ifdef HELIB_DEBUG
  helib::printVec(std::cout << "vd1(rot)=", vd1, 10) << std::endl;
  helib::printVec(std::cout << "res: ", vd_dec, 10) << std::endl;
#endif

  EXPECT_TRUE(
      cx_equals(vd1, vd_dec, NTL::conv<double>(epsilon * c1.getPtxtMag())))
      << "  max(vd)=" << helib::largestCoeff(vd_dec)
      << ", max(vd1)=" << helib::largestCoeff(vd1)
      << ", maxDiff=" << calcMaxDiff(vd_dec, vd1) << std::endl
      << std::endl;
}

TEST_P(GTestApproxNums, generalOpsWorks)
{
  /************** Each round consists of the following:
   1. c1.multiplyBy(c0)
   2. c0 += random constant
   3. c2 *= random constant
   4. tmp = c1
   5. ea.rotate(tmp, random amount in [-nSlots/2, nSlots/2])
   6. c2 += tmp
   7. ea.rotate(c2, random amount in [1-nSlots, nSlots-1])
   8. c1.negate()
   9. c3.multiplyBy(c2)
   10. c0 -= c3
   **************/
  long nslots = ea.size();
  char buffer[32];

  std::vector<std::complex<double>> p0, p1, p2, p3;
  ea.random(p0);
  ea.random(p1);
  ea.random(p2);
  ea.random(p3);

  helib::Ctxt c0(publicKey), c1(publicKey), c2(publicKey), c3(publicKey);
  ea.encrypt(c0, publicKey, p0, /*size=*/1.0);
  ea.encrypt(c1, publicKey, p1, /*size=*/1.0);
  ea.encrypt(c2, publicKey, p2, /*size=*/1.0);
  ea.encrypt(c3, publicKey, p3, /*size=*/1.0);

  helib::resetAllTimers();
  HELIB_NTIMER_START(Circuit);

  for (long i = 0; i < R; i++) {

    if (helib_test::verbose)
      std::cout << "*** round " << i << "..." << std::endl;

    long shamt = NTL::RandomBnd(2 * (nslots / 2) + 1) - (nslots / 2);
    // random number in [-nslots/2..nslots/2]
    long rotamt = NTL::RandomBnd(2 * nslots - 1) - (nslots - 1);
    // random number in [-(nslots-1)..nslots-1]

    // two random constants
    std::vector<std::complex<double>> const1, const2;
    ea.random(const1);
    ea.random(const2);

    NTL::ZZX const1_poly, const2_poly;
    ea.encode(const1_poly, const1, /*size=*/1.0);
    ea.encode(const2_poly, const2, /*size=*/1.0);

    mul(p1, p0); // c1.multiplyBy(c0)
    c1.multiplyBy(c0);
    if (helib_test::verbose) {
      CheckCtxt(c1, "c1*=c0");
    }
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p1, c1, epsilon));

    add(p0, const1); // c0 += random constant
    c0.addConstant(const1_poly);
    if (helib_test::verbose) {
      CheckCtxt(c0, "c0+=k1");
    }
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p0, c0, epsilon));

    mul(p2, const2); // c2 *= random constant
    c2.multByConstant(const2_poly);
    if (helib_test::verbose) {
      CheckCtxt(c2, "c2*=k2");
    }
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p2, c2, epsilon));

    std::vector<std::complex<double>> tmp_p(p1); // tmp = c1
    helib::Ctxt tmp(c1);
    sprintf(buffer, "tmp=c1>>=%d", (int)shamt);
    rotate(tmp_p,
           shamt); // ea.shift(tmp, random amount in [-nSlots/2,nSlots/2])
    ea.rotate(tmp, shamt);
    if (helib_test::verbose) {
      CheckCtxt(tmp, buffer);
    }
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, tmp_p, tmp, epsilon));

    add(p2, tmp_p); // c2 += tmp
    c2 += tmp;
    if (helib_test::verbose) {
      CheckCtxt(c2, "c2+=tmp");
    }
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p2, c2, epsilon));

    sprintf(buffer, "c2>>>=%d", (int)rotamt);
    rotate(p2, rotamt); // ea.rotate(c2, random amount in [1-nSlots, nSlots-1])
    ea.rotate(c2, rotamt);
    if (helib_test::verbose) {
      CheckCtxt(c2, buffer);
    }
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p2, c2, epsilon));

    negateVec(p1); // c1.negate()
    c1.negate();
    if (helib_test::verbose) {
      CheckCtxt(c1, "c1=-c1");
    }
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p1, c1, epsilon));

    mul(p3, p2); // c3.multiplyBy(c2)
    c3.multiplyBy(c2);
    if (helib_test::verbose) {
      CheckCtxt(c3, "c3*=c2");
    }
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p3, c3, epsilon));

    sub(p0, p3); // c0 -= c3
    c0 -= c3;
    if (helib_test::verbose) {
      CheckCtxt(c0, "c0=-c3");
    }
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p0, c0, epsilon));
  }

  c0.cleanUp();
  c1.cleanUp();
  c2.cleanUp();
  c3.cleanUp();

  HELIB_NTIMER_STOP(Circuit);

  std::vector<std::complex<double>> pp0, pp1, pp2, pp3;

  ea.decrypt(c0, secretKey, pp0);
  ea.decrypt(c1, secretKey, pp1);
  ea.decrypt(c2, secretKey, pp2);
  ea.decrypt(c3, secretKey, pp3);

  if (helib_test::verbose) {
    std::cout << "Test " << R << " rounds of mixed operations, ";
  }
  EXPECT_TRUE(
      cx_equals(pp0, p0, NTL::conv<double>(epsilon * c0.getPtxtMag())) &&
      cx_equals(pp1, p1, NTL::conv<double>(epsilon * c1.getPtxtMag())) &&
      cx_equals(pp2, p2, NTL::conv<double>(epsilon * c2.getPtxtMag())) &&
      cx_equals(pp3, p3, NTL::conv<double>(epsilon * c3.getPtxtMag())))
      << "  max(p0)=" << helib::largestCoeff(p0)
      << ", max(pp0)=" << helib::largestCoeff(pp0)
      << ", maxDiff=" << calcMaxDiff(p0, pp0) << std::endl
      << "  max(p1)=" << helib::largestCoeff(p1)
      << ", max(pp1)=" << helib::largestCoeff(pp1)
      << ", maxDiff=" << calcMaxDiff(p1, pp1) << std::endl
      << "  max(p2)=" << helib::largestCoeff(p2)
      << ", max(pp2)=" << helib::largestCoeff(pp2)
      << ", maxDiff=" << calcMaxDiff(p2, pp2) << std::endl
      << "  max(p3)=" << helib::largestCoeff(p3)
      << ", max(pp3)=" << helib::largestCoeff(pp3)
      << ", maxDiff=" << calcMaxDiff(p3, pp3) << std::endl
      << std::endl;

  if (helib_test::verbose) {
    std::cout << std::endl;
    helib::printAllTimers();
    std::cout << std::endl;
  }
  helib::resetAllTimers();
}

TEST_P(GTestApproxNums, generalOpsWorkWithNewAPI)
{
  helib::PtxtArray p0(context), p1(context), p2(context), p3(context);
  p0.random();
  p1.random();
  p2.random();
  p3.random();

  helib::Ctxt c0(publicKey), c1(publicKey), c2(publicKey), c3(publicKey);
  p0.encrypt(c0);
  p1.encrypt(c1);
  p2.encrypt(c2);
  p3.encrypt(c3);

  for (long i = 0; i < R; i++) {

    if (helib_test::verbose)
      std::cout << "*** round " << i << "..." << std::endl;

    if (helib_test::reset) {
      resetPtxtMag(c0, p0);
      resetPtxtMag(c1, p1);
      resetPtxtMag(c2, p2);
      resetPtxtMag(c3, p3);
    }

    debugCompare(secretKey, p0, c0);
    debugCompare(secretKey, p1, c1);
    debugCompare(secretKey, p2, c2);
    debugCompare(secretKey, p3, c3);

    long nslots = context.getNSlots();

    // Random number in [-(nslots-1)..nslots-1]
    long rotamt = NTL::RandomBnd(2 * nslots - 1) - (nslots - 1);

    // Two random constants
    helib::PtxtArray const1(context), const2(context);
    const1.random();
    const2.random();

    helib::PtxtArray tmp1_p(p0);
    rotate(tmp1_p, rotamt);
    helib::Ctxt tmp1(c0);
    rotate(tmp1, rotamt);
    debugCompare(secretKey, tmp1_p, tmp1);

    tmp1_p += const1;
    tmp1 += const1;
    debugCompare(secretKey, tmp1_p, tmp1);

    p0 += const2;
    c0 += const2;
    debugCompare(secretKey, p0, c0);

    p0 *= tmp1_p;
    c0.multiplyBy(tmp1);
    debugCompare(secretKey, p0, c0);

    helib::PtxtArray tmp2_p(p1);
    tmp2_p *= const1;
    helib::Ctxt tmp2(c1);
    tmp2 *= const1;
    debugCompare(secretKey, tmp2_p, tmp2);

    rotate(p1, rotamt);
    rotate(c1, rotamt);
    debugCompare(secretKey, p1, c1);

    p1 += tmp2_p;
    c1 += tmp2;
    debugCompare(secretKey, p1, c1);

    helib::PtxtArray tmp3_p(p2);
    tmp3_p *= const2;
    helib::Ctxt tmp3(c2);
    tmp3 *= const2;
    debugCompare(secretKey, tmp3_p, tmp3);

    p2 *= p3;
    c2 *= c3;
    debugCompare(secretKey, p2, c2);

    p2 += tmp3_p;
    c2 += tmp3;
    debugCompare(secretKey, p2, c2);

    p3 *= const1;
    c3 *= const1;
    debugCompare(secretKey, p3, c3);

    if (helib_test::verbose) {
      // Check correctness after each round
      helib::PtxtArray pp0(context), pp1(context), pp2(context), pp3(context);

      pp0.rawDecryptComplex(c0, secretKey);
      pp1.rawDecryptComplex(c1, secretKey);
      pp2.rawDecryptComplex(c2, secretKey);
      pp3.rawDecryptComplex(c3, secretKey);

      EXPECT_TRUE(pp0 == helib::Approx(p0)) << "Round " << i;
      EXPECT_TRUE(pp1 == helib::Approx(p1)) << "Round " << i;
      EXPECT_TRUE(pp2 == helib::Approx(p2)) << "Round " << i;
      EXPECT_TRUE(pp3 == helib::Approx(p3)) << "Round " << i;
    }
  }

  helib::PtxtArray pp0(context), pp1(context), pp2(context), pp3(context);
  helib::PtxtArray ppp0(context), ppp1(context), ppp2(context), ppp3(context);

  pp0.decryptReal(c0, secretKey);
  pp1.decryptReal(c1, secretKey);
  pp2.decryptReal(c2, secretKey);
  pp3.decryptReal(c3, secretKey);

  if (helib_test::verbose) {
    ppp0.rawDecryptReal(c0, secretKey);
    ppp1.rawDecryptReal(c1, secretKey);
    ppp2.rawDecryptReal(c2, secretKey);
    ppp3.rawDecryptReal(c3, secretKey);

    std::cout << "======= rounded/raw differences\n"
              << helib::Distance(pp0, ppp0) << "\n"
              << helib::Distance(pp1, ppp1) << "\n"
              << helib::Distance(pp2, ppp2) << "\n"
              << helib::Distance(pp3, ppp3) << "\n";

    std::cout << "======= actual/raw differences\n"
              << helib::Distance(p0, ppp0) << "\n"
              << helib::Distance(p1, ppp1) << "\n"
              << helib::Distance(p2, ppp2) << "\n"
              << helib::Distance(p3, ppp3) << "\n";
  }

  EXPECT_TRUE(pp0 == helib::Approx(p0));
  EXPECT_TRUE(pp1 == helib::Approx(p1));
  EXPECT_TRUE(pp2 == helib::Approx(p2));
  EXPECT_TRUE(pp3 == helib::Approx(p3));
}

INSTANTIATE_TEST_SUITE_P(typicalParameters,
                         GTestApproxNums,
                         ::testing::Values(
                             // SLOW
                             Parameters(/*R=*/1,
                                        /*m=*/1024,
                                        /*r=*/10,
                                        /*L=*/150,
                                        /*epsilon=*/0.01,
                                        /*seed=*/0)
                             // FAST
                             // Parameters(1, 128,10, 150, 0.01, 0)
                             ));
// if (R<=0) R=1;
// if (R<=2)
//  L = 100*R;
// else
//  L = 220*(R-1);

} // namespace


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_EXPONENT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_EXPONENT_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/constant/maxexponent.hpp>
#include <boost/simd/constant/nbmantissabits.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/simd/exponentbits.hpp>
#include <boost/simd/function/simd/selsub.hpp>
#include <boost/simd/function/simd/shr.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/function/simd/if_zero_else.hpp>
#include <boost/simd/function/simd/is_invalid.hpp>
#endif
#if defined(__INTEL_COMPILER) && defined(BOOST_SIMD_HAS_AVX_SUPPORT) && !defined(BOOST_SIMD_HAS_AVX2_SUPPORT)
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/minus.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(exponent_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::integer_<A0>, X>
                          )
   {
      using result = bd::as_integer_t<A0, signed>;
     BOOST_FORCEINLINE result operator()(A0 const&) const
      {
        return Zero<result>();
      }
   };

   BOOST_DISPATCH_OVERLOAD(exponent_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      using result = bd::as_integer_t<A0, signed>;
      BOOST_FORCEINLINE result operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        using s_type = bd::scalar_of_t<A0>;
        const int nmb= int(Nbmantissabits<s_type>());
        result x = shr(exponentbits(a0), nmb);
        // workaround for ICC 14.0.3, tested on AVX
        #if defined(__INTEL_COMPILER) && defined(BOOST_SIMD_HAS_AVX_SUPPORT) && !defined(BOOST_SIMD_HAS_AVX2_SUPPORT)
        x = x - if_else(a0, Maxexponent<A0>(), Zero<result>());
        #else
        x = selsub(a0, x, Maxexponent<A0>());
        #endif
        #ifndef BOOST_SIMD_NO_INVALIDS
        return if_zero_else( is_invalid(a0), x );
        #else
        return x;
        #endif
      }
   };

} } }

#endif


//
// Copyright 2019-2020 Mateusz Loskot <mateusz at loskot dot net>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
#include <boost/gil.hpp>
#include <boost/gil/extension/numeric/convolve.hpp>

#include <boost/core/lightweight_test.hpp>

#include <tuple>
#include <type_traits>

#include "core/image/test_fixture.hpp"
#include "core/test_fixture.hpp"
#include "test_fixture.hpp"

namespace gil = boost::gil;
namespace fixture = boost::gil::test::fixture;

struct test_image_1x1_kernel_1x1_identity {
  template <typename Image> void operator()(Image const &) {
    using image_t = Image;
    auto const img = fixture::create_image<image_t>(1, 1, 7);
    image_t img_out(img);

    using pixel_t = typename image_t::value_type;
    using channel_t = typename gil::channel_type<pixel_t>::type;
    auto const kernel = fixture::create_kernel<channel_t>({1});
    gil::detail::convolve_1d<pixel_t>(gil::const_view(img_out), kernel,
                                      gil::view(img_out));

    // 1x1 kernel reduces convolution to multiplication
    BOOST_TEST(gil::const_view(img).front() ==
               gil::const_view(img_out).front());
  }
  static void run() {
    boost::mp11::mp_for_each<fixture::image_types>(
        test_image_1x1_kernel_1x1_identity{});
  }
};

struct test_image_1x1_kernel_3x3_identity {
  template <typename Image> void operator()(Image const &) {
    using image_t = Image;
    auto const img = fixture::create_image<Image>(1, 1, 7);
    image_t img_out(img);

    using pixel_t = typename image_t::value_type;
    using channel_t = typename gil::channel_type<pixel_t>::type;
    auto const kernel =
        fixture::create_kernel<channel_t>({0, 0, 0, 0, 1, 0, 0, 0, 0});
    gil::detail::convolve_1d<pixel_t>(gil::const_view(img_out), kernel,
                                      gil::view(img_out));

    BOOST_TEST(gil::const_view(img).front() ==
               gil::const_view(img_out).front());
  }
  static void run() {
    boost::mp11::mp_for_each<fixture::image_types>(
        test_image_1x1_kernel_3x3_identity{});
  }
};

struct test_image_3x3_kernel_3x3_identity {
  template <typename Image> void operator()(Image const &) {
    using image_t = Image;
    using pixel_t = typename image_t::value_type;
    using channel_t = typename gil::channel_type<pixel_t>::type;
    auto const img = fixture::generate_image<image_t>(
        3, 3, fixture::random_value<channel_t>{});
    image_t img_out(img);

    auto const kernel =
        fixture::create_kernel<channel_t>({0, 0, 0, 0, 1, 0, 0, 0, 0});
    gil::detail::convolve_1d<pixel_t>(gil::const_view(img_out), kernel,
                                      gil::view(img_out));

    BOOST_TEST(
        gil::equal_pixels(gil::const_view(img), gil::const_view(img_out)));
  }
  static void run() {
    boost::mp11::mp_for_each<fixture::image_types>(
        test_image_3x3_kernel_3x3_identity{});
  }
};

struct test_image_5x5_kernel_3x3_identity {
  template <typename Image> void operator()(Image const &) {
    using image_t = Image;
    using pixel_t = typename image_t::value_type;
    using channel_t = typename gil::channel_type<pixel_t>::type;
    auto const img = fixture::generate_image<image_t>(
        5, 5, fixture::random_value<channel_t>{});
    image_t img_out(img);

    auto const kernel =
        fixture::create_kernel<channel_t>({0, 0, 0, 0, 1, 0, 0, 0, 0});
    gil::detail::convolve_1d<pixel_t>(gil::const_view(img_out), kernel,
                                      gil::view(img_out));
    // TODO: Test different boundary options

    BOOST_TEST(
        gil::equal_pixels(gil::const_view(img), gil::const_view(img_out)));
  }
  static void run() {
    boost::mp11::mp_for_each<fixture::image_types>(
        test_image_5x5_kernel_3x3_identity{});
  }
};

int main() {
  test_image_1x1_kernel_1x1_identity::run();
  test_image_1x1_kernel_3x3_identity::run();
  test_image_3x3_kernel_3x3_identity::run();
  test_image_5x5_kernel_3x3_identity::run();

  return ::boost::report_errors();
}


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#ifndef CBR_CONTROL__DERIVATOR_HPP_
#define CBR_CONTROL__DERIVATOR_HPP_

#include <boost/hana/adapt_struct.hpp>

#include <cbr_utils/cyber_timer.hpp>

#include <memory>
#include <type_traits>
#include <utility>

namespace cbr
{

struct DerivatorParams
{
  double filter_tau{0.};

  void check_correctness() const
  {
  }
};

template<typename T, typename _clock_t = std::chrono::high_resolution_clock>
class Derivator
{
public:
  using clock_t = _clock_t;
  using timer_t = CyberTimerNoAvg<std::ratio<1>, double, clock_t>;

  Derivator() = default;
  Derivator(const Derivator &) = default;
  Derivator(Derivator &) = default;
  Derivator(Derivator &&) = default;
  Derivator & operator=(const Derivator &) = default;
  Derivator & operator=(Derivator &) = default;
  Derivator & operator=(Derivator &&) = default;
  ~Derivator() = default;

  template<typename T1>
  explicit Derivator(T1 && clock, const DerivatorParams & prm = {})
  : timer_(std::forward<T1>(clock)),
    prm_(prm)
  {
    prm_.check_correctness();
  }

  explicit Derivator(const DerivatorParams & prm)
  : prm_(prm)
  {
    prm_.check_correctness();
  }

  template<typename T1>
  void set_clock(T1 && clock)
  {
    timer_.set_clock(std::forward<T1>(clock));
  }

  void set_params(const DerivatorParams & prm)
  {
    prm.check_correctness();
    prm_ = prm;
  }

  const DerivatorParams & get_params() const
  {
    return prm_;
  }

  const T & update(const T & val, const typename timer_t::time_point tNow)
  {
    if (init_) {
      const double dt = timer_.toctic(tNow);

      if (prm_.filter_tau <= 0.) {  // no filtering
        if (dt == 0.) {
          derNm1_ = val - valNm1_;
        } else {
          derNm1_ = (val - valNm1_) / dt;
        }
        valNm1_ = val;
        return derNm1_;
      } else {  // with filtering
        const double e = std::exp(-dt / prm_.filter_tau);
        derNm1_ = (val - valNm1_) * e / prm_.filter_tau;
        valNm1_ = valNm1_ * e + (1 - e) * val;
        return derNm1_;
      }
    }

    timer_.tic(tNow);
    init_ = true;
    valNm1_ = val;
    derNm1_ = 0.;
    return derNm1_;
  }

  const T & update(const T & val)
  {
    return update(val, timer_.now());
  }

  const T & operator()(const T & val)
  {
    return update(val);
  }

  const T & getValue() const
  {
    return derNm1_;
  }

  void reset()
  {
    derNm1_ = 0.;
    init_ = false;
  }

  void reset(const T & val, const typename timer_t::time_point tNow)
  {
    timer_.tic(tNow);
    valNm1_ = val;
    derNm1_ = 0.;
    init_ = true;
  }

  void reset(const T & val)
  {
    reset(val, timer_.now());
  }

protected:
  timer_t timer_ = timer_t(clock_t{});
  DerivatorParams prm_;
  double valNm1_ = 0.;
  double derNm1_ = 0.;
  bool init_{false};
};

}  // namespace cbr

// cppcheck-suppress unknownMacro
BOOST_HANA_ADAPT_STRUCT(
  cbr::DerivatorParams,
  filter_tau
);

#endif  // CBR_CONTROL__DERIVATOR_HPP_


/**
 *
 * @file linsys.hpp
 *
 * @date April 1, 2019
 *
 * @brief Provides the interface to linear systems of equations derived
 * from performing KCLs at various nodes in the circuit.
 *
 * @author Matthew Kasper (mkasper@andrew.cmu.edu)
 *
 */

#ifndef _LINEAR_SYSTEM_H_
#define _LINEAR_SYSTEM_H_

#include <Eigen/Dense>
#include <unordered_map>
#include <stdio.h>
#include <errors.hpp>
#include <sstream>

/**
 * @brief Represents the system of linear equations Ax=B derived from
 * KCL that will be used to solve for unknowns in the circuit.
 */
struct LinearSystem {
	int ground;         /**< Circuit's ground node */
	Eigen::MatrixXd A;  /**< LHS matrix of system of linear equations */
	Eigen::VectorXd x;  /**< Solution vector */
	Eigen::VectorXd B;  /**< RHS vector of system of linear equations */

	/** @brief Maps the string representations of unknowns to their ids */
	std::unordered_map<std::string, int> unknowns_map;

	/** @brief Type representing unknown sized vector of strings */
	typedef Eigen::Matrix<std::string, Eigen::Dynamic, 1> VectorXs;
	VectorXs unknown_labels; /**< Vector of unknown labels mapping to `x` */

	/* construct a linear system */
	LinearSystem(int num_unknowns, int ground_id,
		std::unordered_map<std::string, int> unknowns);

	/* destroy a linear system */
	~LinearSystem() { }

	/* Zero our a linear system */
	void clear();

	/* get a string representation of the system */
	std::string to_string();

	/* solve the system of linear equations */
	Eigen::VectorXd& solve();

	/* add component contributions to the LHS/RHS of the system */
	void increment_lhs(int r, int c, double delta);
	void increment_rhs(int r, double delta);

	/**
	 * @brief Writes a linear system to an output stream.
	 *
	 * @param out The output stream.
	 * @param sys The linear system.
	 *
	 * @return The updated output stream.
	 */
	friend std::ostream& operator<<(std::ostream& out, LinearSystem& sys) {
		out << sys.to_string();
		return out;
	}
};

#endif


/*
    MIT License

    Copyright (c) 2021 Zhepei Wang (wangzhepei@live.com)

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in all
    copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    SOFTWARE.
*/

#ifndef FLATNESS_HPP
#define FLATNESS_HPP

#include <Eigen/Eigen>

#include <cmath>

namespace flatness
{
    class FlatnessMap
    {
    public:
        inline void reset(const double &vehicle_mass,
                          const double &gravitational_acceleration,
                          const double &horitonral_drag_coeff,
                          const double &vertical_drag_coeff,
                          const double &parasitic_drag_coeff,
                          const double &speed_smooth_factor)
        {
            mass = vehicle_mass;
            grav = gravitational_acceleration;
            dh = horitonral_drag_coeff;
            dv = vertical_drag_coeff;
            cp = parasitic_drag_coeff;
            veps = speed_smooth_factor;

            return;
        }
        //通过速度加速度和机动度计算角速度和旋转
        inline void forward(const Eigen::Vector3d &vel,
                            const Eigen::Vector3d &acc,
                            const Eigen::Vector3d &jer,
                            const double &psi,
                            const double &dpsi,
                            double &thr,
                            Eigen::Vector4d &quat,
                            Eigen::Vector3d &omg)
        {
            double w0, w1, w2, dw0, dw1, dw2;

            v0 = vel(0);
            v1 = vel(1);
            v2 = vel(2);
            a0 = acc(0);
            a1 = acc(1);
            a2 = acc(2);
            cp_term = sqrt(v0 * v0 + v1 * v1 + v2 * v2 + veps);
            w_term = 1.0 + cp * cp_term;
            w0 = w_term * v0;
            w1 = w_term * v1;
            w2 = w_term * v2;
            dh_over_m = dh / mass;
            zu0 = a0 + dh_over_m * w0;
            zu1 = a1 + dh_over_m * w1;
            zu2 = a2 + dh_over_m * w2 + grav;
            zu_sqr0 = zu0 * zu0;
            zu_sqr1 = zu1 * zu1;
            zu_sqr2 = zu2 * zu2;
            zu01 = zu0 * zu1;
            zu12 = zu1 * zu2;
            zu02 = zu0 * zu2;
            zu_sqr_norm = zu_sqr0 + zu_sqr1 + zu_sqr2;
            zu_norm = sqrt(zu_sqr_norm);
            z0 = zu0 / zu_norm;
            z1 = zu1 / zu_norm;
            z2 = zu2 / zu_norm;
            ng_den = zu_sqr_norm * zu_norm;
            ng00 = (zu_sqr1 + zu_sqr2) / ng_den;
            ng01 = -zu01 / ng_den;
            ng02 = -zu02 / ng_den;
            ng11 = (zu_sqr0 + zu_sqr2) / ng_den;
            ng12 = -zu12 / ng_den;
            ng22 = (zu_sqr0 + zu_sqr1) / ng_den;
            v_dot_a = v0 * a0 + v1 * a1 + v2 * a2;
            dw_term = cp * v_dot_a / cp_term;
            dw0 = w_term * a0 + dw_term * v0;
            dw1 = w_term * a1 + dw_term * v1;
            dw2 = w_term * a2 + dw_term * v2;
            dz_term0 = jer(0) + dh_over_m * dw0;
            dz_term1 = jer(1) + dh_over_m * dw1;
            dz_term2 = jer(2) + dh_over_m * dw2;
            dz0 = ng00 * dz_term0 + ng01 * dz_term1 + ng02 * dz_term2;
            dz1 = ng01 * dz_term0 + ng11 * dz_term1 + ng12 * dz_term2;
            dz2 = ng02 * dz_term0 + ng12 * dz_term1 + ng22 * dz_term2;
            f_term0 = mass * a0 + dv * w0;
            f_term1 = mass * a1 + dv * w1;
            f_term2 = mass * (a2 + grav) + dv * w2;
            thr = z0 * f_term0 + z1 * f_term1 + z2 * f_term2;
            tilt_den = sqrt(2.0 * (1.0 + z2));
            tilt0 = 0.5 * tilt_den;
            tilt1 = -z1 / tilt_den;
            tilt2 = z0 / tilt_den;
            c_half_psi = cos(0.5 * psi);
            s_half_psi = sin(0.5 * psi);
            quat(0) = tilt0 * c_half_psi;
            quat(1) = tilt1 * c_half_psi + tilt2 * s_half_psi;
            quat(2) = tilt2 * c_half_psi - tilt1 * s_half_psi;
            quat(3) = tilt0 * s_half_psi;
            c_psi = cos(psi);
            s_psi = sin(psi);
            omg_den = z2 + 1.0;
            omg_term = dz2 / omg_den;
            omg(0) = dz0 * s_psi - dz1 * c_psi -
                     (z0 * s_psi - z1 * c_psi) * omg_term;
            omg(1) = dz0 * c_psi + dz1 * s_psi -
                     (z0 * c_psi + z1 * s_psi) * omg_term;
            omg(2) = (z1 * dz0 - z0 * dz1) / omg_den + dpsi;

            return;
        }

        inline void backward(const Eigen::Vector3d &pos_grad,
                             const Eigen::Vector3d &vel_grad,
                             const double &thr_grad,
                             const Eigen::Vector4d &quat_grad,
                             const Eigen::Vector3d &omg_grad,
                             Eigen::Vector3d &pos_total_grad,
                             Eigen::Vector3d &vel_total_grad,
                             Eigen::Vector3d &acc_total_grad,
                             Eigen::Vector3d &jer_total_grad,
                             double &psi_total_grad,
                             double &dpsi_total_grad) const
        {
            double w0b, w1b, w2b, dw0b, dw1b, dw2b;
            double z0b, z1b, z2b, dz0b, dz1b, dz2b;
            double v_sqr_normb, cp_termb, w_termb;
            double zu_sqr_normb, zu_normb, zu0b, zu1b, zu2b;
            double zu_sqr0b, zu_sqr1b, zu_sqr2b, zu01b, zu12b, zu02b;
            double ng00b, ng01b, ng02b, ng11b, ng12b, ng22b, ng_denb;
            double dz_term0b, dz_term1b, dz_term2b, f_term0b, f_term1b, f_term2b;
            double tilt_denb, tilt0b, tilt1b, tilt2b, head0b, head3b;
            double cpsib, spsib, omg_denb, omg_termb;
            double tempb, tilt_den_sqr;

            tilt0b = s_half_psi * (quat_grad(3)) + c_half_psi * (quat_grad(0));
            head3b = tilt0 * (quat_grad(3)) + tilt2 * (quat_grad(1)) - tilt1 * (quat_grad(2));
            tilt2b = c_half_psi * (quat_grad(2)) + s_half_psi * (quat_grad(1));
            head0b = tilt2 * (quat_grad(2)) + tilt1 * (quat_grad(1)) + tilt0 * (quat_grad(0));
            tilt1b = c_half_psi * (quat_grad(1)) - s_half_psi * (quat_grad(2));
            tilt_den_sqr = tilt_den * tilt_den;
            tilt_denb = (z1 * tilt1b - z0 * tilt2b) / tilt_den_sqr + 0.5 * tilt0b;
            omg_termb = -((z0 * c_psi + z1 * s_psi) * (omg_grad(1))) -
                        (z0 * s_psi - z1 * c_psi) * (omg_grad(0));
            tempb = omg_grad(2) / omg_den;
            dpsi_total_grad = omg_grad(2);
            z1b = dz0 * tempb;
            dz0b = z1 * tempb + c_psi * (omg_grad(1)) + s_psi * (omg_grad(0));
            z0b = -(dz1 * tempb);
            dz1b = s_psi * (omg_grad(1)) - z0 * tempb - c_psi * (omg_grad(0));
            omg_denb = -((z1 * dz0 - z0 * dz1) * tempb / omg_den) -
                       dz2 * omg_termb / (omg_den * omg_den);
            tempb = -(omg_term * (omg_grad(1)));
            cpsib = dz0 * (omg_grad(1)) + z0 * tempb;
            spsib = dz1 * (omg_grad(1)) + z1 * tempb;
            z0b += c_psi * tempb;
            z1b += s_psi * tempb;
            tempb = -(omg_term * (omg_grad(0)));
            spsib += dz0 * (omg_grad(0)) + z0 * tempb;
            cpsib += -dz1 * (omg_grad(0)) - z1 * tempb;
            z0b += s_psi * tempb + tilt2b / tilt_den + f_term0 * (thr_grad);
            z1b += -c_psi * tempb - tilt1b / tilt_den + f_term1 * (thr_grad);
            dz2b = omg_termb / omg_den;
            z2b = omg_denb + tilt_denb / tilt_den + f_term2 * (thr_grad);
            psi_total_grad = c_psi * spsib + 0.5 * c_half_psi * head3b -
                             s_psi * cpsib - 0.5 * s_half_psi * head0b;
            f_term0b = z0 * (thr_grad);
            f_term1b = z1 * (thr_grad);
            f_term2b = z2 * (thr_grad);
            ng02b = dz_term0 * dz2b + dz_term2 * dz0b;
            dz_term0b = ng02 * dz2b + ng01 * dz1b + ng00 * dz0b;
            ng12b = dz_term1 * dz2b + dz_term2 * dz1b;
            dz_term1b = ng12 * dz2b + ng11 * dz1b + ng01 * dz0b;
            ng22b = dz_term2 * dz2b;
            dz_term2b = ng22 * dz2b + ng12 * dz1b + ng02 * dz0b;
            ng01b = dz_term0 * dz1b + dz_term1 * dz0b;
            ng11b = dz_term1 * dz1b;
            ng00b = dz_term0 * dz0b;
            jer_total_grad(2) = dz_term2b;
            dw2b = dh_over_m * dz_term2b;
            jer_total_grad(1) = dz_term1b;
            dw1b = dh_over_m * dz_term1b;
            jer_total_grad(0) = dz_term0b;
            dw0b = dh_over_m * dz_term0b;
            tempb = cp * (v2 * dw2b + v1 * dw1b + v0 * dw0b) / cp_term;
            acc_total_grad(2) = mass * f_term2b + w_term * dw2b + v2 * tempb;
            acc_total_grad(1) = mass * f_term1b + w_term * dw1b + v1 * tempb;
            acc_total_grad(0) = mass * f_term0b + w_term * dw0b + v0 * tempb;
            vel_total_grad(2) = dw_term * dw2b + a2 * tempb;
            vel_total_grad(1) = dw_term * dw1b + a1 * tempb;
            vel_total_grad(0) = dw_term * dw0b + a0 * tempb;
            cp_termb = -(v_dot_a * tempb / cp_term);
            tempb = ng22b / ng_den;
            zu_sqr0b = tempb;
            zu_sqr1b = tempb;
            ng_denb = -((zu_sqr0 + zu_sqr1) * tempb / ng_den);
            zu12b = -(ng12b / ng_den);
            tempb = ng11b / ng_den;
            ng_denb += zu12 * ng12b / (ng_den * ng_den) -
                       (zu_sqr0 + zu_sqr2) * tempb / ng_den;
            zu_sqr0b += tempb;
            zu_sqr2b = tempb;
            zu02b = -(ng02b / ng_den);
            zu01b = -(ng01b / ng_den);
            tempb = ng00b / ng_den;
            ng_denb += zu02 * ng02b / (ng_den * ng_den) +
                       zu01 * ng01b / (ng_den * ng_den) -
                       (zu_sqr1 + zu_sqr2) * tempb / ng_den;
            zu_normb = zu_sqr_norm * ng_denb -
                       (zu2 * z2b + zu1 * z1b + zu0 * z0b) / zu_sqr_norm;
            zu_sqr_normb = zu_norm * ng_denb + zu_normb / (2.0 * zu_norm);
            tempb += zu_sqr_normb;
            zu_sqr1b += tempb;
            zu_sqr2b += tempb;
            zu2b = z2b / zu_norm + zu0 * zu02b + zu1 * zu12b + 2 * zu2 * zu_sqr2b;
            w2b = dv * f_term2b + dh_over_m * zu2b;
            zu1b = z1b / zu_norm + zu2 * zu12b + zu0 * zu01b + 2 * zu1 * zu_sqr1b;
            w1b = dv * f_term1b + dh_over_m * zu1b;
            zu_sqr0b += zu_sqr_normb;
            zu0b = z0b / zu_norm + zu2 * zu02b + zu1 * zu01b + 2 * zu0 * zu_sqr0b;
            w0b = dv * f_term0b + dh_over_m * zu0b;
            w_termb = a2 * dw2b + a1 * dw1b + a0 * dw0b +
                      v2 * w2b + v1 * w1b + v0 * w0b;
            acc_total_grad(2) += zu2b;
            acc_total_grad(1) += zu1b;
            acc_total_grad(0) += zu0b;
            cp_termb += cp * w_termb;
            v_sqr_normb = cp_termb / (2.0 * cp_term);
            vel_total_grad(2) += w_term * w2b + 2 * v2 * v_sqr_normb + vel_grad(2);
            vel_total_grad(1) += w_term * w1b + 2 * v1 * v_sqr_normb + vel_grad(1);
            vel_total_grad(0) += w_term * w0b + 2 * v0 * v_sqr_normb + vel_grad(0);
            pos_total_grad(2) = pos_grad(2);
            pos_total_grad(1) = pos_grad(1);
            pos_total_grad(0) = pos_grad(0);

            return;
        }

    private:
        double mass, grav, dh, dv, cp, veps;

        double v0, v1, v2, a0, a1, a2, v_dot_a;
        double z0, z1, z2, dz0, dz1, dz2;
        double cp_term, w_term/*segma*||v||*/, dh_over_m;
        double zu_sqr_norm, zu_norm, zu0, zu1, zu2;
        double zu_sqr0, zu_sqr1, zu_sqr2, zu01, zu12, zu02;
        double ng00, ng01, ng02, ng11, ng12, ng22, ng_den;
        double dw_term, dz_term0, dz_term1, dz_term2, f_term0, f_term1, f_term2;
        double tilt_den, tilt0, tilt1, tilt2, c_half_psi, s_half_psi;
        double c_psi, s_psi, omg_den, omg_term;
    };
}

#endif


/* Copyright Institute of Sound and Vibration Research - All rights reserved */

#include <libefl/vector_functions.hpp>
#include <libefl/vector_functions_reference.hpp>

#include <libefl/aligned_array.hpp>

#include <boost/test/unit_test.hpp>
#include <libefl/lagrange_coefficient_calculator.hpp>

#include <algorithm>
#include <cmath>
#include <complex>
#include <iostream>
#include <valarray>

namespace visr
{
namespace efl
{
namespace test
{

BOOST_AUTO_TEST_CASE( instantiateL7 )
{
  constexpr std::size_t order = 7;

  bool const reverseCoefficients = true;

  using InterType = efl::LagrangeCoefficientCalculator<float, order, reverseCoefficients>;

  InterType lagInt;

  float const mu = -0.25;

  std::array<float, order+1> res;
  lagInt.calculateCoefficients( mu, &res[0] );

//  std::cout << "storage size: " << InterType::storageSize() << std::endl;
}

} // namespace test
} // namespace rbbl
} // namespace visr


// -*- mode: c++; indent-tabs-mode: nil; -*-
//
// Paragraph
// Copyright (c) 2016-2019 Illumina, Inc.
// All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// You may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//		http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied
// See the License for the specific language governing permissions and limitations
//
//

/**
 * Population statistics for genotype sets
 *
 * \author Sai Chen & Egor Dolzhenko & Peter Krusche
 * \email schen6@illumina.com & pkrusche@illumina.com & edolzhenko@illumina.com
 *
 */

#include "genotyping/PopulationStatistics.hh"

#include <algorithm>
#include <boost/math/distributions/chi_squared.hpp>
#include <numeric>

using std::vector;

namespace genotyping
{
PopulationStatistics::PopulationStatistics(GenotypeSet const& genotypes)
{
    num_valid_samples = 0;
    num_total_samples = static_cast<int>(genotypes.size());

    for (auto const& genotype : genotypes)
    {
        if (genotype.gt.empty())
        {
            continue;
        }
        num_valid_samples++;
        if (genotype_counts.find(genotype.gt) == genotype_counts.end())
        {
            genotype_counts[genotype.gt] = 1;
        }
        else
        {
            genotype_counts[genotype.gt]++;
        }

        for (auto const& gt : genotype.gt)
        {
            if (allele_counts.size() <= gt)
            {
                allele_counts.resize(gt + 1, 0);
            }
            allele_counts[gt]++;
        }
    }
}

Json::Value PopulationStatistics::toJson() const
{
    double hwe_p_chisq = getChisqPvalue();
    double hwe_p_fisher = -1;
    if (needFisherExactHWE())
    {
        hwe_p_fisher = getFisherExactPvalue();
    }
    double call_rate = getCallrate();

    Json::Value json_result;
    json_result["hwe"] = hwe_p_chisq;
    if (hwe_p_fisher == -1)
    {
        json_result["hwe_fisher"] = "";
    }
    else
    {
        json_result["hwe_fisher"] = hwe_p_fisher;
    }
    json_result["call_rate"] = call_rate;

    auto allele_frequencies = getAlleleFrequencies();
    json_result["allele_frequencies"] = Json::arrayValue;
    for (auto& a : allele_frequencies)
    {
        json_result["allele_frequencies"].append(a);
    }

    return json_result;
}

double PopulationStatistics::getChisqPvalue() const
{
    double chisq_val = 0;
    for (auto& gv : genotype_counts)
    {
        if (gv.first.size() != 2)
        {
            continue;
        }
        uint64_t h1 = gv.first[0];
        uint64_t h2 = gv.first[1];
        if (allele_counts[h1] == 0 || allele_counts[h2] == 0) // skip unobserved alleles
        {
            continue;
        }
        double e_count;
        if (h1 == h2)
        {
            e_count = ((double)allele_counts[h1] / num_valid_samples / 2)
                * ((double)allele_counts[h1] / num_valid_samples / 2) * num_valid_samples;
        }
        else
        {
            e_count = 2 * ((double)allele_counts[h1] / num_valid_samples / 2)
                * ((double)allele_counts[h2] / num_valid_samples / 2) * num_valid_samples;
        }

        double diff = e_count - gv.second;
        double norm_diff_square = (double)(diff * diff) / e_count;
        chisq_val += norm_diff_square;
    }
    boost::math::chi_squared chisq_distribution(1);
    double hwe_pval = 1 - boost::math::cdf(chisq_distribution, chisq_val);
    return hwe_pval;
}

/**
 * @return True if need to use Fisher's exact test for HWE P
 * For multi-alleles, always use chisq
 * For bi-allelic:
 *      if
 *          N<=30, or count of the rarest genotype <= 20, or rarest expected count <= 20, use fisher's exact
 *      else
 *          use chisq
 */
bool PopulationStatistics::needFisherExactHWE() const
{
    int num_observed_alleles = 0;
    for (auto a : allele_counts)
    {
        if (a > 0)
        {
            num_observed_alleles++;
        }
    }
    if (num_observed_alleles <= 1)
    {
        return false;
    }
    if (num_observed_alleles > 2)
    {
        return false;
    }

    if (num_valid_samples <= 30)
    {
        return true;
    }

    for (auto& g : genotype_counts)
    {
        if (g.second > 0 && g.second <= 20)
        {
            return true;
        }
    }

    auto minor_allele_index = minNonZeroAlleleIndex();
    double minor_allele_freq = (double)allele_counts[minor_allele_index] / 2 / num_valid_samples;
    if (minor_allele_freq * minor_allele_freq * num_valid_samples <= 20)
    {
        return true;
    }
    return false;
}

/**
 * from JE Wigginton 2005 AJHG
 */
double PopulationStatistics::getFisherExactPvalue() const
{
    auto minor_allele_index = minNonZeroAlleleIndex();
    auto p_major = std::max_element(allele_counts.begin(), allele_counts.end());
    int minor_allele_count = allele_counts[minor_allele_index];
    int major_allele_count = *p_major;

    GenotypeVector het_gv;
    het_gv.push_back(static_cast<uint64_t>(p_major - allele_counts.begin()));
    het_gv.push_back(static_cast<uint64_t>(minor_allele_index));
    std::sort(het_gv.begin(), het_gv.end());
    int observed_num_het = 0;
    for (auto& g : genotype_counts)
    {
        if (g.first.size() != 2)
        {
            continue;
        }
        if (g.first[0] == het_gv[0] && g.first[1] == het_gv[1])
        {
            observed_num_het = g.second;
            break;
        }
    }

    int num_expect_het = std::round(
        2 * ((double)minor_allele_count / num_valid_samples / 2) * ((double)major_allele_count / num_valid_samples / 2)
        * num_valid_samples);

    std::vector<double> scaled_pvals;
    double observe_scaled_pval = -1;

    // for num_het > expected het
    int prev_num_ref_hom = (minor_allele_count - num_expect_het) / 2;
    int prev_num_alt_hom = num_valid_samples - prev_num_ref_hom - num_expect_het;
    double prev_scaled_pval = 1;
    for (int num_het = num_expect_het; num_het <= minor_allele_count; num_het += 2)
    {
        if (num_het == num_expect_het)
        {
            scaled_pvals.push_back(1);
            continue;
        }
        int prev_num_het = num_het - 2;
        double iscale
            = prev_scaled_pval * (4 * prev_num_ref_hom * prev_num_alt_hom) / ((prev_num_het + 2) * (prev_num_het + 1));
        scaled_pvals.push_back(iscale);
        prev_scaled_pval = iscale;
        prev_num_ref_hom--;
        prev_num_alt_hom--;
        if (observe_scaled_pval == -1 && num_het == observed_num_het)
        {
            observe_scaled_pval = iscale;
        }
    }

    // for num_het < expected het
    prev_num_ref_hom = (minor_allele_count - num_expect_het) / 2;
    prev_num_alt_hom = num_valid_samples - prev_num_ref_hom - num_expect_het;
    prev_scaled_pval = 1;
    for (int num_het = num_expect_het; num_het >= 0; num_het -= 2)
    {
        if (num_het == num_expect_het)
        {
            continue;
        }
        int prev_num_het = num_het + 2;
        double iscale = prev_scaled_pval / 4 * prev_num_het / (prev_num_ref_hom + 1) * (prev_num_het - 1)
            / (prev_num_alt_hom + 1);
        scaled_pvals.push_back(iscale);
        prev_scaled_pval = iscale;
        prev_num_ref_hom++;
        prev_num_alt_hom++;
        if (observe_scaled_pval == -1 && num_het == observed_num_het)
        {
            observe_scaled_pval = iscale;
        }
    }

    // calculate HWE
    double hwe_scale_sum = 0;
    for (auto s : scaled_pvals)
    {
        if (s <= observe_scaled_pval)
        {
            hwe_scale_sum += s;
        }
    }
    double pval = hwe_scale_sum / std::accumulate(scaled_pvals.begin(), scaled_pvals.end(), (double)0);
    return pval;
}

/**
 * @return allele frequencies
 */
std::vector<double> PopulationStatistics::getAlleleFrequencies() const
{
    std::vector<double> result;
    const uint32_t sum = std::accumulate(allele_counts.begin(), allele_counts.end(), (uint32_t)0);
    result.reserve(allele_counts.size());
    for (auto ac : allele_counts)
    {
        if (sum > 0)
        {
            result.push_back(((double)ac) / sum);
        }
        else
        {
            result.push_back(0);
        }
    }
    return result;
}

/**
 * @return iterator to the lowest allele non-zero minor allele count
 */
size_t PopulationStatistics::minNonZeroAlleleIndex() const
{
    auto p_minor = std::min_element(allele_counts.begin(), allele_counts.end());
    if (*p_minor > 0)
    {
        return static_cast<size_t>(p_minor - allele_counts.begin());
    }
    p_minor = std::max_element(allele_counts.begin(), allele_counts.end());
    if (*p_minor == 0)
    {
        return static_cast<size_t>(0);
    }
    for (auto it = allele_counts.begin(); it != allele_counts.end(); it++)
    {
        if (*it < *p_minor)
        {
            p_minor = it;
        }
    }
    return static_cast<size_t>(p_minor - allele_counts.begin());
}
}


#include "ros/ros.h"
#include "laser_scanner_infoscreen/trackObjects.h"
#include <visualization_msgs/Marker.h>
#include <vector>
#include <cmath>
#include <utility>
#include <armadillo>
#include "laser_objects.hpp"

/*! \file track_objects_server.cpp
  \brief ROS Service node of object tracker.

  Handles low-level parsing LaserScan message, and detecting and tracking objects.
  First culling is limiting wihtin 120deg cone to the front. Then points are broken
  into continuities of which between 0.4m and 0.7m long are passed to laser_objects
  for further processing.

  \sa track_objects_client.cpp
  \sa laser_objects
*/

ros::Publisher *marker_pub_pointer;
laser_objects *repository = NULL;

float cart_dist(float x1, float x2, float y1, float y2) {
  return std::sqrt( std::pow(x2-x1,2) + std::pow(y2-y1,2));
}

bool track(laser_scanner_infoscreen::trackObjects::Request  &req,
           laser_scanner_infoscreen::trackObjects::Response &res)
{
	if (!repository) {
		repository = new laser_objects(req.scan_time);
	}
	visualization_msgs::Marker line_list;
	float beg_arc, end_arc;
	float beg_angle, end_angle;
	float angle_increment = req.angle_increment;
	std::vector<float> ranges;

	line_list.header.frame_id = "/laser";
	line_list.header.stamp = ros::Time::now();
	line_list.ns = "points_and_lines";
	line_list.action = visualization_msgs::Marker::ADD;
	line_list.pose.orientation.w = 0.5;
	line_list.id = 5;
	line_list.type = visualization_msgs::Marker::LINE_LIST;
	line_list.scale.x = 0.1;
	line_list.color.b = 1.0;
	line_list.color.a = 0.3;
	int start_index = 0;
	while(start_index * angle_increment + req.angle_min < -(1.0/3.0)*M_PI) {
		start_index++;
	}
	int end_index = start_index;
	while(end_index * angle_increment + req.angle_min < (1.0/3.0)*M_PI) {
		end_index++;
	}
	std::vector<float> front_ranges(req.ranges.begin()+start_index, req.ranges.begin()+end_index);
	// beg_angle = end_angle = req.angle_min;
	beg_angle = end_angle = req.angle_min + start_index*angle_increment;
	beg_arc = front_ranges[0];
	end_arc = front_ranges[0];
	// beg_arc = req.ranges[0];
	// end_arc = req.ranges[0];
	ranges.push_back(beg_arc);
	// for (float range : req.ranges) {
	for (float range : front_ranges) {
		if (std::abs(end_arc - range) < 0.10) {
			end_arc = range;
			ranges.push_back(range);
			end_angle = end_angle + angle_increment;
		} else {
      float x1 = ranges[0] * cos(beg_angle);
      float x2 = ranges.back() * cos(beg_angle + angle_increment * ranges.size());
      float y1 = ranges[0] * sin(beg_angle);
      float y2 = ranges.back() * sin(beg_angle + angle_increment * ranges.size());
      float width = cart_dist(x1, x2, y1, y2);
			if(width > 0.3f && width < 0.7f) {
				geometry_msgs::Point p;
				p.z = 0;
				p.x = ranges[0] * cos(beg_angle);
				p.y = ranges[0] * sin(beg_angle);
				line_list.points.push_back(p);
				p.x = ranges[ranges.size() - 1] * cos(end_angle);
				p.y = ranges[ranges.size() - 1] * sin(end_angle);
				line_list.points.push_back(p);
				float sum_x = 0.0;
				float sum_y = 0.0;
				for (int i = 0; i < ranges.size(); i++) {
					sum_x += ranges[i] * cos(beg_angle + i*angle_increment);
					sum_y += ranges[i] * sin(beg_angle + i*angle_increment);
				}
				if (sum_x) {
					marker_pub_pointer->publish(line_list);
					repository->update(std::make_pair(sum_x/ranges.size(), sum_y/ranges.size()));
				}
				//ROS_INFO("found object at: [%f, %f]", (float)sum_x / ranges.size(), (float)sum_y / ranges.size());
			}
			beg_angle = end_angle + angle_increment;
			end_angle = beg_angle;
			beg_arc = range;
			end_arc = beg_arc;
			ranges.clear();
			ranges.push_back(range);
		}

	}
	repository->update_repository();
	res.mobiles_x = repository->get_mobiles_x();
	res.mobiles_y = repository->get_mobiles_y();
	res.statics_x = repository->get_statics_x();
	res.statics_y = repository->get_statics_y();
	return true;
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "track_objects_server");
	ros::NodeHandle n;

	ros::ServiceServer service = n.advertiseService("track_objects", track);
	ROS_INFO("Ready to track objects.");
	ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("vizualization_objects", 10);
	marker_pub_pointer = &marker_pub;
	ros::spin();

	return 0;
}