//------------------------------------------------------------------
//  **MEDYAN** - Simulation Package for the Mechanochemical
//               Dynamics of Active Networks, v4.0
//
//  Copyright (2015-2018)  Papoian Lab, University of Maryland
//
//                 ALL RIGHTS RESERVED
//
//  See the MEDYAN web page for more information:
//  http://www.medyan.org
//------------------------------------------------------------------

#ifdef TESTING

//#define DO_THIS_NRM_TEST
#ifdef DO_THIS_NRM_TEST

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/covariance.hpp>
#include <boost/accumulators/statistics/variates/covariate.hpp>

using namespace boost::accumulators;

#include "gtest/gtest.h"

#include "common.h"

#include "Species.h"
#include "Reaction.h"
#include "ChemNRMImpl.h"
#include "ChemSim.h"

#ifdef TRACK_DEPENDENTS
// the NRM algorithm fundamentally depends on the ability to track dependents

TEST(ChemNRMTest, StoichiometryInvariants) {
    SpeciesBulk A1("A1",  100);
    SpeciesBulk A2("A2", 0);
    SpeciesBulk A3("A3", 0);
    Reaction<1,1> r1 = { {&A1,&A2}, 10.0 };
    Reaction<1,1> r2 = { {&A2,&A1}, 15.0 };
    Reaction<1,1> r3 = { {&A1,&A3}, 20.0 };

    ChemSim* chemsim = new ChemSim();
    chemsim->setInstance(new ChemNRMImpl());

    chemsim->addReaction(&r1);
    chemsim->addReaction(&r2);
    chemsim->addReaction(&r3);

    chemsim->initialize();
    chemsim->runSteps(30);
    EXPECT_EQ(100,A1.getN()+A2.getN()+A3.getN());
}

// Testing A<->B steady state
TEST(ChemNRMTest, SimpleSteadyState) {
    int Nstart = 16;
    SpeciesBulk A1("A1",  Nstart);
    SpeciesBulk A2("A2", 0);
    // A1 <-> A2 with the same forward and backward rates; [A]~[B] at steady state
    Reaction<1,1> r1 = { {&A1,&A2}, 100.0 };
    Reaction<1,1> r2 = { {&A2,&A1}, 100.0 };

    ChemSim* chemsim = new ChemSim();
    chemsim->setInstance(new ChemNRMImpl());

    chemsim->addReaction(&r1);
    chemsim->addReaction(&r2);

    chemsim->initialize();

    chemsim->runSteps(1000);

    accumulator_set<int, stats<tag::variance(immediate)>> accA1;
    accumulator_set<int, stats<tag::mean>> accA2;
    accumulator_set<int, stats<tag::covariance<double, tag::covariate1> > > accCov;
    int N_SAMPLE_POINTS=1000;
    for(int i=0;i<N_SAMPLE_POINTS;++i){
        chemsim->runSteps(100);
        accA1(A1.getN());
        accCov(A1.getN(), covariate1 = A2.getN());
    }
    double A1mean = mean(accA1);
    double A1var = variance(accA1);
    double var_expected = double(Nstart)/2/2;
    double mean_error = sqrt(var_expected/N_SAMPLE_POINTS);
    EXPECT_TRUE(fabs(A1mean-Nstart/2)<10*mean_error);
    EXPECT_TRUE(fabs(var_expected-A1var)<0.1*var_expected);
    // within 10% of the expected variance
    EXPECT_FLOAT_EQ(-1.0,covariance(accCov)/(A1var));

}

// Testing transient dynamics for A<->B
TEST(ChemNRMTest, SimpleTransient) {
    const long long int N_SAMPLE_POINTS=pow(10,6);
    const long long int Nstart = 10;
    const double tau_snapshot = 0.48; //seconds

    SpeciesBulk A1("A1",  Nstart);
    SpeciesBulk A2("A2", 0);
    // A1 <-> A2 with the same forward and backward rates; [A]~[B] at steady state
    Reaction<1,1> r1 = { {&A1,&A2}, 2.5 };
    Reaction<1,1> r2 = { {&A2,&A1}, 2.5 };

    ChemSim* chemsim = new ChemSim();
    chemsim->setInstance(new ChemNRMImpl());

    chemsim->addReaction(&r1);
    chemsim->addReaction(&r2);

    vector<long long int> n_hist(Nstart+1);

    accumulator_set<double, stats<tag::mean>> accTau;
    long long int N_penultimate;
    for(long long int i=0;i<N_SAMPLE_POINTS;++i){
        A1.setN(Nstart);
        A2.setN(0);
        chemsim->initialize();
        do {
            N_penultimate=A1.getN();
            chemsim->runSteps(1);
        } while (tau()<tau_snapshot);
        ++n_hist[N_penultimate];
        accTau(tau());
    }

    double sum=0;
    for(auto num: n_hist)
        sum+=double(num)/N_SAMPLE_POINTS;

    // The results below are for N=10 (coming from both analytical formula
    // and numerical integration)
    vector<double> n_hist_analyt {0.0003773 ,  0.00452585,  0.02443017,  0.0781464 ,  0.1640442 , 0.23613261,  0.23604161,  0.1617947,  0.07277957,  0.01940041, 0.00232715};
    double relative_error=0.15; //i.e. allow a 15% relative error
    for(int n=0; n<(Nstart+1); ++n){
        double p_est=double(n_hist[n])/N_SAMPLE_POINTS;
        double p_analyt=n_hist_analyt[n];
        EXPECT_NEAR(p_est,p_analyt,relative_error*p_analyt);
    }
}

// Testing transient dynamics for the A->B->C cycle, where A, B, and C
// can only take two values, n=0,1
TEST(ChemNRMTest, CyclicTransient) {
    const long long int N_SAMPLE_POINTS=pow(10,6);
    const double tau_snapshot = 0.25; //seconds
    //long long int print_freq = pow(10,7);
    SpeciesBulk A1("A1", 1, 1);
    SpeciesBulk A2("A2", 0, 1);
    SpeciesBulk A3("A3", 0, 1);
    Reaction<1,1> r1 = { {&A1,&A2}, 4.5 };
    Reaction<1,1> r2 = { {&A2,&A3}, 2.5 };
    Reaction<1,1> r3 = { {&A3,&A1}, 0.5 };

    ChemSim* chemsim = new ChemSim();
    chemsim->setInstance(new ChemNRMImpl());

    chemsim->addReaction(&r1);
    chemsim->addReaction(&r2);
    chemsim->addReaction(&r3);

    long long int n_a1_hist=0;
    long long int n_a2_hist=0;
    long long int n_a3_hist=0;

    for(long long int i=0;i<N_SAMPLE_POINTS;++i){
        A1.setN(1);
        A2.setN(0);
        A3.setN(0);
        long long int n_a1_pentult=0;
        long long int n_a2_pentult=0;
        long long int n_a3_pentult=0;
        chemsim->initialize();
        long long int events=0;
        do {
            n_a1_pentult=A1.getN();
            n_a2_pentult=A2.getN();
            n_a3_pentult=A3.getN();
            chemsim->runSteps(1);
            ++events;
        } while (tau()<tau_snapshot);
        n_a1_hist+=n_a1_pentult;
        n_a2_hist+=n_a2_pentult;
        n_a3_hist+=n_a3_pentult;
    }

    double pa1=static_cast<double>(n_a1_hist)/N_SAMPLE_POINTS;
    double pa2=static_cast<double>(n_a2_hist)/N_SAMPLE_POINTS;
    double pa3=static_cast<double>(n_a3_hist)/N_SAMPLE_POINTS;

    double pa1_numeric = 0.33169986;
    double pa2_numeric = 0.47589009;
    double pa3_numeric = 0.19241006;

    double relative_error=0.01; //i.e. allow a 1% relative error
    EXPECT_NEAR(pa1,pa1_numeric,relative_error*pa1_numeric);
    EXPECT_NEAR(pa2,pa2_numeric,relative_error*pa1_numeric);
    EXPECT_NEAR(pa3,pa3_numeric,relative_error*pa1_numeric);

}

// Testing transient dynamics for the X<->A->B->C cycle, where A, B, and C
// can only take two values, n=0,1
#ifdef TRACK_UPPER_COPY_N
TEST(ChemNRMTest, ComplexCyclicTransient) {
    const long long int N_SAMPLE_POINTS=pow(10,6);
    const long long int Nstart = 3;
    const double tau_snapshot = 0.5; //seconds
    SpeciesBulk X("X", Nstart); // X's copy number is not restricted
    SpeciesBulk A("A", 0, 1);
    SpeciesBulk B("B", 0, 1);
    SpeciesBulk C("C", 0, 1);

    float kxa=0.8; // s^-1
    float kax=0.3; // s^-1
    float kab=4.5; // s^-1
    float kbc=2.5; // s^-1
    float kca=0.5; // s^-1
    Reaction<1,1> xa = { {&X,&A}, kxa };
    Reaction<1,1> ax = { {&A,&X}, kax };
    Reaction<1,1> r1 = { {&A,&B}, kab };
    Reaction<1,1> r2 = { {&B,&C}, kbc };
    Reaction<1,1> r3 = { {&C,&A}, kca };

    ChemSim* chemsim = new ChemSim();
    chemsim->setInstance(new ChemNRMImpl());

    chemsim->addReaction(&r1);
    chemsim->addReaction(&r2);
    chemsim->addReaction(&r3);
    chemsim->addReaction(&xa);
    chemsim->addReaction(&ax);

    vector<long long int> x_hist(Nstart+1);
    long long int n_a1_hist=0;
    long long int n_a2_hist=0;
    long long int n_a3_hist=0;

    for(long long int i=0;i<N_SAMPLE_POINTS;++i){
        A.setN(0);
        B.setN(0);
        C.setN(0);
        X.setN(Nstart);
        long long int n_a_pentult=0;
        long long int n_b_pentult=0;
        long long int n_c_pentult=0;
        long long int x_pentult=0;
        chemsim->initialize();
        long long int events=0;
        do {
            x_pentult=X.getN();
            n_a_pentult=A.getN();
            n_b_pentult=B.getN();
            n_c_pentult=C.getN();
            bool success = chemsim->runSteps(1);
            if(!success){
                cout << "chem.runSteps(1) has failed, i= " << i << endl;
                chemsim->printReactions();
                break;
            }
            ++events;
        } while (tau()<tau_snapshot);
        ++x_hist[x_pentult];
        n_a1_hist+=n_a_pentult;
        n_a2_hist+=n_b_pentult;
        n_a3_hist+=n_c_pentult;
    }

    vector<double> p_nrm;

    for(int n=0; n<(Nstart+1); ++n){
        double p_est=double(x_hist[n])/N_SAMPLE_POINTS;
        p_nrm.push_back(p_est);
    }

    double pa1=static_cast<double>(n_a1_hist)/N_SAMPLE_POINTS;
    double pa2=static_cast<double>(n_a2_hist)/N_SAMPLE_POINTS;
    double pa3=static_cast<double>(n_a3_hist)/N_SAMPLE_POINTS;
    p_nrm.push_back(pa1);
    p_nrm.push_back(pa2);
    p_nrm.push_back(pa3);

    // The results below are for ...
    vector<double> p_numeric {0.001687323512088279, 0.12264078507458409, 0.55515007879166167, 0.3205218126216664, 0.32672439967797662, 0.30766594955383336, 0.17110327024528463};
    double relative_error=0.05; //i.e. allow a 5% relative error

    for(int n=0; n<(Nstart+4); ++n){
        EXPECT_NEAR(p_nrm[n],p_numeric[n],relative_error*p_numeric[n]);
    }
}
#endif // of TRACK_UPPER_COPY_N

#endif // TRACK_DEPENDENTS

#endif //DO_THIS_NRM_TEST
#endif //TESTING


//
// Created by Alex Beccaro on 18/01/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/1-50/12/problem12.hpp"

BOOST_AUTO_TEST_SUITE( Problem12 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem12::solve(5);
        BOOST_CHECK_EQUAL(res, 28);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem12::solve();
        BOOST_CHECK_EQUAL(res, 76576500);
    }

BOOST_AUTO_TEST_SUITE_END()


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

// The functions in this file are useful for implementing new
// algorithms and finding simple stats about the graph.

#ifndef LNCENTRALITY_DEGREE_STATS_HPP
#define LNCENTRALITY_DEGREE_STATS_HPP

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

#include <fstream>
#include <numeric>

#include "readgraph.hpp"

using namespace boost::accumulators;

namespace lncentrality {

typedef graph_traits<LitGraph>::degree_size_type Degree;

// A class that provides a function object for iterating over all
// vertices in the graph and printing the in/out degrees.
template <class Graph> class degrees {
public:
  typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
  typedef typename graph_traits<Graph>::degree_size_type Degree;

  degrees(Graph &g_) : g(g_) {}

  void operator()(const Vertex &v) const {
    using namespace boost;
    typename property_map<Graph, vertex_index_t>::type vertex_id =
        get(vertex_index, g);

    std::cout << "vertex: " << get(vertex_id, v) << " :: " << g[v].name
              << std::endl;

    std::cout << " in + out degree " << degree(v, g) << std::endl;
    std::cout << " out degree " << out_degree(v, g) << std::endl;
    std::cout << " in degree " << in_degree(v, g) << std::endl;
  }

private:
  Graph &g;
};

// For each vertex in the graph, set in/out/total degree in the
// vectors provided.
void get_degrees(vector<Degree> &in_degrees, vector<Degree> &out_degrees,
                 vector<Degree> &degrees, LitGraph &g) {
  graph_traits<LitGraph>::vertex_iterator i, end;
  for (boost::tie(i, end) = vertices(g); i != end; ++i) {
    in_degrees.push_back(in_degree(*i, g));
    out_degrees.push_back(out_degree(*i, g));
    degrees.push_back(degree(*i, g));
  }
}

// Compute simple stats about degrees for each vertex in the graph and
// print those.
map<string, double> get_degrees_stats(LitGraph &g) {
  graph_traits<LitGraph>::vertex_iterator i, end;
  accumulator_set<Degree, stats<tag::mean, tag::variance>> in_degrees,
      out_degrees, degrees;
  map<string, double> result;
  for (boost::tie(i, end) = vertices(g); i != end; ++i) {
    in_degrees(in_degree(*i, g));
    out_degrees(out_degree(*i, g));
    degrees(degree(*i, g));
  }
  result["in_degrees_mean"] = mean(in_degrees);
  result["in_degrees_variance"] = variance(in_degrees);
  result["out_degrees_mean"] = mean(out_degrees);
  result["out_degrees_variance"] = variance(out_degrees);
  result["degrees_mean"] = mean(degrees);
  result["degrees_variance"] = variance(degrees);
  return result;
}

} // namespace lncentrality

#endif


#include <boost/math/quadrature/trapezoidal.hpp>


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

  @copyright 2015 NumScale SAS
  @copyright 2015 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_FUNCTION_SCALAR_NTHROOT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_FUNCTION_SCALAR_NTHROOT_HPP_INCLUDED
#include <boost/simd/function/fast.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/function/scalar/is_inf.hpp>
#endif
#ifndef BOOST_SIMD_NO_INVALID
#include <boost/simd/function/scalar/is_nan.hpp>
#endif
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/scalar/abs.hpp>
#include <boost/simd/function/scalar/is_ltz.hpp>
#include <boost/simd/function/scalar/is_odd.hpp>
#include <boost/simd/function/scalar/minusone.hpp>
#include <boost/simd/function/scalar/pow.hpp>
#include <boost/simd/function/scalar/rec.hpp>
#include <boost/simd/function/scalar/sign.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( nthroot_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::integer_<A1> >
                          )
  {
    inline A0 operator() ( A0 a0, A1 a1) const BOOST_NOEXCEPT
    {
#ifndef BOOST_SIMD_NO_INVALID
      if (is_nan(a0)) return a0;
#endif
      auto is_ltza0 = is_ltz(a0);
      auto is_odda1 = is_odd(a1);
      if (is_ltza0 && !is_odda1) return Nan<A0>();
      A0 x = bs::abs(a0);
      if (x == One<A0>()) return a0;
      if (!a1) return (x < One<A0>()) ? Zero<A0>() : sign(a0)*Inf<A0>();
      if (!a0) return Zero<A0>();
#ifndef BOOST_SIMD_NO_INFINITIES
      if (is_inf(a0)) return (a1) ? a0 : One<A0>();
#endif
      A0 aa1 = static_cast<A0>(a1);
      A0 y = bs::pow(x,rec(aa1));
      // Correct numerical errors (since, e.g., 64^(1/3) is not exactly 4)
      // by one iteration of Newton's method
      if (y) y -= (bs::pow(y, aa1) - x) / (aa1* bs::pow(y,minusone(aa1)));
      return (is_ltza0 && is_odda1)? -y : y;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( nthroot_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::integer_<A1> >
                          , boost::simd::fast_tag
                          )
  {
    inline A0 operator() ( A0 a0, A1 a1
                         , fast_tag const&) const BOOST_NOEXCEPT
    {
      auto is_ltza0 = is_ltz(a0);
      auto is_odda1 = is_odd(a1);
      if (is_ltza0 && !is_odda1) return Nan<A0>();
      A0 x = bs::abs(a0);
      if (x == One<A0>()) return a0;
      if (!a1) return (x < One<A0>()) ? Zero<A0>() : sign(a0)*Inf<A0>();
      if (!a0) return Zero<A0>();
      A0 aa1 = static_cast<A0>(a1);
      A0 y = bs::pow(x,rec(aa1));
      return (is_ltza0 && is_odda1)? -y : y;
    }
  };
} } }

#endif


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

#pragma once

#include <boost/iterator/zip_iterator.hpp>
#include <cstddef>

#include "DataStructures/ComplexDataVector.hpp"
#include "DataStructures/DataVector.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "DataStructures/Tensor/TypeAliases.hpp"
#include "Evolution/Systems/Cce/BoundaryData.hpp"
#include "Evolution/Systems/Cce/WorldtubeBufferUpdater.hpp"
#include "Helpers/Evolution/Systems/Cce/WriteToWorldtubeH5.hpp"
#include "NumericalAlgorithms/Spectral/SwshCollocation.hpp"
#include "PointwiseFunctions/AnalyticSolutions/GeneralRelativity/KerrSchild.hpp"
#include "PointwiseFunctions/GeneralRelativity/GeneralizedHarmonic/Phi.hpp"
#include "PointwiseFunctions/GeneralRelativity/GeneralizedHarmonic/Pi.hpp"
#include "PointwiseFunctions/GeneralRelativity/SpacetimeMetric.hpp"
#include "Utilities/FileSystem.hpp"
#include "Utilities/Gsl.hpp"

namespace Cce {
namespace TestHelpers {
template <typename... Structure>
Tensor<ComplexModalVector, Structure...> tensor_to_goldberg_coefficients(
    const Tensor<DataVector, Structure...>& nodal_data, size_t l_max) noexcept {
  Tensor<ComplexModalVector, Structure...> goldberg_modal_data{
      square(l_max + 1)};
  SpinWeighted<ComplexDataVector, 0> transform_buffer{
      Spectral::Swsh::number_of_swsh_collocation_points(l_max)};
  for (size_t i = 0; i < nodal_data.size(); ++i) {
    transform_buffer.data() = std::complex<double>(1.0, 0.0) * nodal_data[i];
    goldberg_modal_data[i] =
        Spectral::Swsh::libsharp_to_goldberg_modes(
            Spectral::Swsh::swsh_transform(l_max, 1, transform_buffer), l_max)
            .data();
  }
  return goldberg_modal_data;
}

template <typename... Structure>
Tensor<ComplexModalVector, Structure...> tensor_to_libsharp_coefficients(
    const Tensor<DataVector, Structure...>& nodal_data,
    const size_t l_max)  // NOLINT(readability-avoid-const-params-in-decls)
    noexcept {
  Tensor<ComplexModalVector, Structure...> libsharp_modal_data{
      Spectral::Swsh::size_of_libsharp_coefficient_vector(l_max)};
  SpinWeighted<ComplexDataVector, 0> transform_buffer{
      Spectral::Swsh::number_of_swsh_collocation_points(l_max)};
  for (size_t i = 0; i < nodal_data.size(); ++i) {
    transform_buffer.data() = std::complex<double>(1.0, 0.0) * nodal_data[i];
    libsharp_modal_data[i] =
        Spectral::Swsh::swsh_transform(l_max, 1, transform_buffer).data();
  }
  return libsharp_modal_data;
}

template <typename AnalyticSolution>
void create_fake_time_varying_gh_nodal_data(
    const gsl::not_null<tnsr::aa<DataVector, 3>*> spacetime_metric,
    const gsl::not_null<tnsr::iaa<DataVector, 3>*> phi,
    const gsl::not_null<tnsr::aa<DataVector, 3>*> pi,
    const AnalyticSolution& solution, const double extraction_radius,
    const double amplitude, const double frequency, const double time,
    const size_t l_max) noexcept {
  const size_t number_of_angular_points =
      Spectral::Swsh::number_of_swsh_collocation_points(l_max);
  // create the vector of collocation points that we want to interpolate to

  tnsr::I<DataVector, 3> collocation_points{number_of_angular_points};
  const auto& collocation = Spectral::Swsh::cached_collocation_metadata<
      Spectral::Swsh::ComplexRepresentation::Interleaved>(l_max);
  for (const auto collocation_point : collocation) {
    get<0>(collocation_points)[collocation_point.offset] =
        extraction_radius * (1.0 + amplitude * sin(frequency * time)) *
        sin(collocation_point.theta) * cos(collocation_point.phi);
    get<1>(collocation_points)[collocation_point.offset] =
        extraction_radius * (1.0 + amplitude * sin(frequency * time)) *
        sin(collocation_point.theta) * sin(collocation_point.phi);
    get<2>(collocation_points)[collocation_point.offset] =
        extraction_radius * (1.0 + amplitude * sin(frequency * time)) *
        cos(collocation_point.theta);
  }

  const auto kerr_schild_variables = solution.variables(
      collocation_points, 0.0, gr::Solutions::KerrSchild::tags<DataVector>{});

  const Scalar<DataVector>& lapse =
      get<gr::Tags::Lapse<DataVector>>(kerr_schild_variables);
  const Scalar<DataVector>& dt_lapse =
      get<::Tags::dt<gr::Tags::Lapse<DataVector>>>(kerr_schild_variables);
  const auto& d_lapse = get<gr::Solutions::KerrSchild::DerivLapse<DataVector>>(
      kerr_schild_variables);

  const auto& shift = get<gr::Tags::Shift<3, ::Frame::Inertial, DataVector>>(
      kerr_schild_variables);
  const auto& dt_shift =
      get<::Tags::dt<gr::Tags::Shift<3, ::Frame::Inertial, DataVector>>>(
          kerr_schild_variables);
  const auto& d_shift = get<gr::Solutions::KerrSchild::DerivShift<DataVector>>(
      kerr_schild_variables);

  const auto& spatial_metric =
      get<gr::Tags::SpatialMetric<3, ::Frame::Inertial, DataVector>>(
          kerr_schild_variables);
  const auto& dt_spatial_metric = get<
      ::Tags::dt<gr::Tags::SpatialMetric<3, ::Frame::Inertial, DataVector>>>(
      kerr_schild_variables);
  const auto& d_spatial_metric =
      get<gr::Solutions::KerrSchild::DerivSpatialMetric<DataVector>>(
          kerr_schild_variables);

  gr::spacetime_metric(spacetime_metric, lapse, shift, spatial_metric);
  GeneralizedHarmonic::phi(phi, lapse, d_lapse, shift, d_shift, spatial_metric,
                           d_spatial_metric);
  GeneralizedHarmonic::pi(pi, lapse, dt_lapse, shift, dt_shift, spatial_metric,
                          dt_spatial_metric, *phi);
}

template <typename AnalyticSolution>
void create_fake_time_varying_modal_data(
    const gsl::not_null<tnsr::ii<ComplexModalVector, 3>*>
        spatial_metric_coefficients,
    const gsl::not_null<tnsr::ii<ComplexModalVector, 3>*>
        dt_spatial_metric_coefficients,
    const gsl::not_null<tnsr::ii<ComplexModalVector, 3>*>
        dr_spatial_metric_coefficients,
    const gsl::not_null<tnsr::I<ComplexModalVector, 3>*> shift_coefficients,
    const gsl::not_null<tnsr::I<ComplexModalVector, 3>*> dt_shift_coefficients,
    const gsl::not_null<tnsr::I<ComplexModalVector, 3>*> dr_shift_coefficients,
    const gsl::not_null<Scalar<ComplexModalVector>*> lapse_coefficients,
    const gsl::not_null<Scalar<ComplexModalVector>*> dt_lapse_coefficients,
    const gsl::not_null<Scalar<ComplexModalVector>*> dr_lapse_coefficients,
    const AnalyticSolution& solution, const double extraction_radius,
    const double amplitude, const double frequency, const double time,
    const size_t l_max, const bool convert_to_goldberg = true,
    const bool apply_normalization_bug = false) noexcept {
  const size_t number_of_angular_points =
      Spectral::Swsh::number_of_swsh_collocation_points(l_max);
  // create the vector of collocation points that we want to interpolate to

  tnsr::I<DataVector, 3> collocation_points{number_of_angular_points};
  const auto& collocation = Spectral::Swsh::cached_collocation_metadata<
      Spectral::Swsh::ComplexRepresentation::Interleaved>(l_max);
  for (const auto collocation_point : collocation) {
    get<0>(collocation_points)[collocation_point.offset] =
        extraction_radius * (1.0 + amplitude * sin(frequency * time)) *
        sin(collocation_point.theta) * cos(collocation_point.phi);
    get<1>(collocation_points)[collocation_point.offset] =
        extraction_radius * (1.0 + amplitude * sin(frequency * time)) *
        sin(collocation_point.theta) * sin(collocation_point.phi);
    get<2>(collocation_points)[collocation_point.offset] =
        extraction_radius * (1.0 + amplitude * sin(frequency * time)) *
        cos(collocation_point.theta);
  }

  const auto kerr_schild_variables = solution.variables(
      collocation_points, 0.0, gr::Solutions::KerrSchild::tags<DataVector>{});

  const Scalar<DataVector>& lapse =
      get<gr::Tags::Lapse<DataVector>>(kerr_schild_variables);
  const Scalar<DataVector>& dt_lapse =
      get<::Tags::dt<gr::Tags::Lapse<DataVector>>>(kerr_schild_variables);
  const auto& d_lapse = get<gr::Solutions::KerrSchild::DerivLapse<DataVector>>(
      kerr_schild_variables);

  const auto& shift = get<gr::Tags::Shift<3, ::Frame::Inertial, DataVector>>(
      kerr_schild_variables);
  const auto& dt_shift =
      get<::Tags::dt<gr::Tags::Shift<3, ::Frame::Inertial, DataVector>>>(
          kerr_schild_variables);
  const auto& d_shift = get<gr::Solutions::KerrSchild::DerivShift<DataVector>>(
      kerr_schild_variables);

  const auto& spatial_metric =
      get<gr::Tags::SpatialMetric<3, ::Frame::Inertial, DataVector>>(
          kerr_schild_variables);
  const auto& dt_spatial_metric = get<
      ::Tags::dt<gr::Tags::SpatialMetric<3, ::Frame::Inertial, DataVector>>>(
      kerr_schild_variables);
  const auto& d_spatial_metric =
      get<gr::Solutions::KerrSchild::DerivSpatialMetric<DataVector>>(
          kerr_schild_variables);

  DataVector normalization_factor{number_of_angular_points, 1.0};
  if (apply_normalization_bug) {
    normalization_factor = 0.0;
    const auto inverse_spatial_metric =
        determinant_and_inverse(spatial_metric).second;
    for (size_t i = 0; i < 3; ++i) {
      for (size_t j = 0; j < 3; ++j) {
        normalization_factor +=
            inverse_spatial_metric.get(i, j) * collocation_points.get(i) *
            collocation_points.get(j) /
            square(extraction_radius *
                   (1.0 + amplitude * sin(frequency * time)));
      }
    }
    normalization_factor = sqrt(normalization_factor);
  }

  Scalar<DataVector> dr_lapse{number_of_angular_points};
  get(dr_lapse) = (get<0>(collocation_points) * get<0>(d_lapse) +
                   get<1>(collocation_points) * get<1>(d_lapse) +
                   get<2>(collocation_points) * get<2>(d_lapse)) /
                  (extraction_radius * normalization_factor);
  tnsr::I<DataVector, 3> dr_shift{number_of_angular_points};
  for (size_t i = 0; i < 3; ++i) {
    dr_shift.get(i) = (get<0>(collocation_points) * d_shift.get(0, i) +
                       get<1>(collocation_points) * d_shift.get(1, i) +
                       get<2>(collocation_points) * d_shift.get(2, i)) /
                      (extraction_radius * normalization_factor);
  }
  tnsr::ii<DataVector, 3> dr_spatial_metric{number_of_angular_points};
  for (size_t i = 0; i < 3; ++i) {
    for (size_t j = i; j < 3; ++j) {
      dr_spatial_metric.get(i, j) =
          (get<0>(collocation_points) * d_spatial_metric.get(0, i, j) +
           get<1>(collocation_points) * d_spatial_metric.get(1, i, j) +
           get<2>(collocation_points) * d_spatial_metric.get(2, i, j)) /
          (extraction_radius * normalization_factor);
    }
  }

  if (convert_to_goldberg) {
    *lapse_coefficients =
        TestHelpers::tensor_to_goldberg_coefficients(lapse, l_max);
    *dt_lapse_coefficients =
        TestHelpers::tensor_to_goldberg_coefficients(dt_lapse, l_max);
    *dr_lapse_coefficients =
        TestHelpers::tensor_to_goldberg_coefficients(dr_lapse, l_max);

    *shift_coefficients =
        TestHelpers::tensor_to_goldberg_coefficients(shift, l_max);
    *dt_shift_coefficients =
        TestHelpers::tensor_to_goldberg_coefficients(dt_shift, l_max);
    *dr_shift_coefficients =
        TestHelpers::tensor_to_goldberg_coefficients(dr_shift, l_max);

    *spatial_metric_coefficients =
        TestHelpers::tensor_to_goldberg_coefficients(spatial_metric, l_max);
    *dt_spatial_metric_coefficients =
        TestHelpers::tensor_to_goldberg_coefficients(dt_spatial_metric, l_max);
    *dr_spatial_metric_coefficients =
        TestHelpers::tensor_to_goldberg_coefficients(dr_spatial_metric, l_max);
  } else {
    *lapse_coefficients =
        TestHelpers::tensor_to_libsharp_coefficients(lapse, l_max);
    *dt_lapse_coefficients =
        TestHelpers::tensor_to_libsharp_coefficients(dt_lapse, l_max);
    *dr_lapse_coefficients =
        TestHelpers::tensor_to_libsharp_coefficients(dr_lapse, l_max);

    *shift_coefficients =
        TestHelpers::tensor_to_libsharp_coefficients(shift, l_max);
    *dt_shift_coefficients =
        TestHelpers::tensor_to_libsharp_coefficients(dt_shift, l_max);
    *dr_shift_coefficients =
        TestHelpers::tensor_to_libsharp_coefficients(dr_shift, l_max);

    *spatial_metric_coefficients =
        TestHelpers::tensor_to_libsharp_coefficients(spatial_metric, l_max);
    *dt_spatial_metric_coefficients =
        TestHelpers::tensor_to_libsharp_coefficients(dt_spatial_metric, l_max);
    *dr_spatial_metric_coefficients =
        TestHelpers::tensor_to_libsharp_coefficients(dr_spatial_metric, l_max);
  }
}

template <typename AnalyticSolution>
void write_test_file(const AnalyticSolution& solution,
                     const std::string& filename, const double target_time,
                     const double extraction_radius, const double frequency,
                     const double amplitude, const size_t l_max) noexcept {
  const size_t goldberg_size = square(l_max + 1);
  tnsr::ii<ComplexModalVector, 3> spatial_metric_coefficients{goldberg_size};
  tnsr::ii<ComplexModalVector, 3> dt_spatial_metric_coefficients{goldberg_size};
  tnsr::ii<ComplexModalVector, 3> dr_spatial_metric_coefficients{goldberg_size};
  tnsr::I<ComplexModalVector, 3> shift_coefficients{goldberg_size};
  tnsr::I<ComplexModalVector, 3> dt_shift_coefficients{goldberg_size};
  tnsr::I<ComplexModalVector, 3> dr_shift_coefficients{goldberg_size};
  Scalar<ComplexModalVector> lapse_coefficients{goldberg_size};
  Scalar<ComplexModalVector> dt_lapse_coefficients{goldberg_size};
  Scalar<ComplexModalVector> dr_lapse_coefficients{goldberg_size};

  // write times to file for several steps before and after the target time
  if (file_system::check_if_file_exists(filename)) {
    file_system::rm(filename, true);
  }
  // scoped to close the file
  {
    TestHelpers::WorldtubeModeRecorder recorder{filename, l_max};
    for (size_t t = 0; t < 30; ++t) {
      const double time = 0.1 * t + target_time - 1.5;
      TestHelpers::create_fake_time_varying_modal_data(
          make_not_null(&spatial_metric_coefficients),
          make_not_null(&dt_spatial_metric_coefficients),
          make_not_null(&dr_spatial_metric_coefficients),
          make_not_null(&shift_coefficients),
          make_not_null(&dt_shift_coefficients),
          make_not_null(&dr_shift_coefficients),
          make_not_null(&lapse_coefficients),
          make_not_null(&dt_lapse_coefficients),
          make_not_null(&dr_lapse_coefficients), solution, extraction_radius,
          amplitude, frequency, time, l_max);
      for (size_t i = 0; i < 3; ++i) {
        for (size_t j = i; j < 3; ++j) {
          recorder.append_worldtube_mode_data(
              detail::dataset_name_for_component("/g", i, j), time,
              spatial_metric_coefficients.get(i, j), l_max);
          recorder.append_worldtube_mode_data(
              detail::dataset_name_for_component("/Drg", i, j), time,
              dr_spatial_metric_coefficients.get(i, j), l_max);
          recorder.append_worldtube_mode_data(
              detail::dataset_name_for_component("/Dtg", i, j), time,
              dt_spatial_metric_coefficients.get(i, j), l_max);
        }
        recorder.append_worldtube_mode_data(
            detail::dataset_name_for_component("/Shift", i), time,
            shift_coefficients.get(i), l_max);
        recorder.append_worldtube_mode_data(
            detail::dataset_name_for_component("/DrShift", i), time,
            dr_shift_coefficients.get(i), l_max);
        recorder.append_worldtube_mode_data(
            detail::dataset_name_for_component("/DtShift", i), time,
            dt_shift_coefficients.get(i), l_max);
      }
      recorder.append_worldtube_mode_data(
          detail::dataset_name_for_component("/Lapse"), time,
          get(lapse_coefficients), l_max);
      recorder.append_worldtube_mode_data(
          detail::dataset_name_for_component("/DrLapse"), time,
          get(dr_lapse_coefficients), l_max);
      recorder.append_worldtube_mode_data(
          detail::dataset_name_for_component("/DtLapse"), time,
          get(dt_lapse_coefficients), l_max);
    }
  }
}
}  // namespace TestHelpers
}  // namespace Cce


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

#include <boost/hana/maybe.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/monad/laws.hpp>
using namespace boost::hana;

BOOST_HANA_CONSTEXPR_LAMBDA auto f = [](auto x) {
    return just(x + int_<1>);
};

BOOST_HANA_CONSTEXPR_LAMBDA auto g = [](auto x) {
    return just(x * int_<3>);
};

int main() {
    BOOST_HANA_CONSTANT_ASSERT(Monad::laws::check(nothing, int_<1>, f, g));
    BOOST_HANA_CONSTANT_ASSERT(Monad::laws::check(just(int_<1>), int_<1>, f, g));
    BOOST_HANA_CONSTEXPR_ASSERT(Monad::laws::check(just(1), int_<1>, f, g));
}


#include <boost/math/complex/atanh.hpp>


#include <iostream>
#include "greeter/greeter.hpp"

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

#include <plog/Log.h> // Step1: include the headers
#include "plog/Initializers/RollingFileInitializer.h"

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

    plog::init(plog::debug, "myapp_log.txt");
    PLOG_INFO << "Program beginning.";

    greeter("Now running our demo program!");

    std::cout << "Testing Boost Matrix example!" << std::endl;
    matrix<double> m (3, 3);
    for (unsigned i = 0; i < m.size1 (); ++ i)
        for (unsigned j = 0; j < m.size2 (); ++ j) {
            m(i, j) = 3 * i + j;
            PLOG_DEBUG << "Indices " << i << " " << j;
        }
    std::cout << m << std::endl;

    greeter("Demo program ends!");
    PLOG_INFO << "Program ending.";

    return 0;
}


#include <UnitTest++/UnitTest++.h>
#include <stdexcept>
#include "../random.h"

#include <boost/filesystem.hpp>
#include <boost/random/poisson_distribution.hpp>
#include <boost/math/distributions/poisson.hpp>
#include <vector>

#include<unuran.h>

#include <iostream>
#include <fstream>

using namespace coela;
using namespace std;

SUITE(random)
{

    string test_suite_output_dir = "random_tests/";

    TEST(Notify_Suite_Has_Been_Run) {
        cout << "*** \"random number gen\" unit tests running ***" <<endl;
        boost::filesystem::create_directories(test_suite_output_dir);
    }

    TEST(Unuran_Seed) {
        //Need to do this within a function, hence declaration within a TEST macro.
        std::vector < unsigned long > seed;

        seed.push_back(1111);
        seed.push_back(222);
        seed.push_back(333);
        seed.push_back(444);
        seed.push_back(555);
        seed.push_back(666);

        unuran::
        StreamWrapper::set_unuran_package_seed(seed, false);

    }

    TEST(Unuran_Seed_Manipulation) {
        using namespace coela::unuran;

        std::vector < unsigned long > seed;
        seed.push_back(1111);
        seed.push_back(222);
        seed.push_back(333);
        seed.push_back(444);
        seed.push_back(555);
        seed.push_back(666);

        int n_advancements = 3;

        StreamWrapper::set_unuran_package_seed(seed, false);
        StreamWrapper rns1;
        StreamWrapper::advance_package_seed(n_advancements);
        StreamWrapper alternate_rns1;

        UniformRandomVariate urv1(0,1.0,rns1);
        UniformRandomVariate alternate_urv1(0,1.0,alternate_rns1);

        StreamWrapper::set_unuran_package_seed(seed, false);
        StreamWrapper rns2;
        StreamWrapper::advance_package_seed(n_advancements);
        StreamWrapper alternate_rns2;

        UniformRandomVariate urv2(0,1.0,rns2);
        UniformRandomVariate alternate_urv2(0,1.0,alternate_rns2);

        vector<double> results1, results2;

        for (size_t i=0; i!=10; ++i) {
            results1.push_back(urv1());
            results2.push_back(urv2());
        }

        CHECK(results1==results2);

        vector<double> alt_results1, alt_results2;

        for (size_t i=0; i!=10; ++i) {
            alt_results1.push_back(alternate_urv1());
            alt_results2.push_back(alternate_urv2());
        }

        CHECK(alt_results1==alt_results2);

        CHECK(alt_results1!=results1); //(sanity check)
        CHECK(alt_results2!=results2);
    }

//
//    TEST(Unuran_Uniform_Distribution) {
//        using namespace coela::unuran;
//
//        StreamWrapper rns;
//
//        uniform_random_variate rv_0_1(0, 1.0, rns);
//
//        coela::HistogramContainer14bit hist;
//
//        size_t n_its = 1e6;
//
//        int n_bins=10;
//
//        //visual check
////        for (size_t i=0; i!=10;++i){
////            cout<<"rv_0_1: " <<rv_0_1()<<endl;
////        }
//
//        for (size_t i=0; i!=n_its; ++i) {
//            hist.count_int_value(floor(rv_0_1()*n_bins));
//        }
//
//        int n_per_bin = n_its / n_bins;
//
//        for (int ind=0; ind<n_bins; ++ind) {
//            CHECK_CLOSE(n_per_bin, hist[ind], n_per_bin/100);
//        }
//
//        hist.write_to_file(test_suite_output_dir+"uniform_dist_tenths.txt");
//    }
//

//
//    TEST(Boost_Poisson_Distribution_mean_1){
//        double mean =1.0;
////        coela::simple_01_uniform_random_variate rv(172);
//
//
//        unuran_poisson_random_variate poisson_gen(mean);
//
//
//        coela::HistogramContainer14bit hist;
//
//        size_t n_its = 1e6;
//
//        boost::math::poisson_distribution<> poisson_pdf(mean);
//
//        for (size_t i=0; i!=n_its; ++i){
//            hist.count_int_value((int)poisson_gen());
//
//        }
//
//        for (int ind=0; ind<100; ++ind){
//            double predicted_num = boost::math::pdf(poisson_pdf,ind)*n_its;
//            CHECK_CLOSE(predicted_num, (double)hist[ind] , max(predicted_num/10, 5.0));
//        }
//
//        hist.write_to_file(test_suite_output_dir+"poisson_dist_mean"+string_utils::ftoa(mean,2)+".txt");
//
//    }

//    TEST(Boost_Poisson_Distribution_mean_11_and_a_quarter){
//        double mean =11.25;
//        coela::simple_01_uniform_random_variate rv(123);
//
//
//        boost::poisson_distribution<> poisson_gen(mean);
//
//
//        coela::HistogramContainer14bit hist;
//
//        size_t n_its = 1e6;
//
//        boost::math::poisson_distribution<> poisson_pdf(mean);
//
//        int zeroes=0;
//        for (size_t i=0; i!=n_its; ++i){
//            assert(fmod(poisson_gen(rv),1.0) == 0.0);
//            int result = poisson_gen(rv);
//            if (result==0) ++zeroes;
//            hist.count_int_value(result);
//
//        }
//
//        cerr<<"zeros "<<zeroes<<endl;
//
//        for (int ind=0; ind<100; ++ind){
//            double predicted_num = boost::math::pdf(poisson_pdf,ind)*n_its;
//            CHECK_CLOSE(predicted_num, (double)hist[ind] , max(predicted_num/10, 10.0));
//        }
//
//        hist.write_to_file(test_suite_output_dir+"boost_poisson_dist_mean"+string_utils::ftoa(mean,4)+".txt");
//
//    }

//    TEST(unuran_poisson_11_quarter) {
//
//        std::vector < unsigned long > seed;
//        seed.push_back(1111);
//        seed.push_back(222);
//        seed.push_back(333);
//        seed.push_back(444);
//        seed.push_back(555);
//        seed.push_back(666);
//
//        unuran::
//        StreamWrapper::set_unuran_package_seed(seed, false);
//
//        using namespace random;
//        double mean =11.25;
//
//        unuran::StreamWrapper rns1;
//
//        unuran::PoissonRandomVariate gen(mean, rns1);
//        coela::HistogramContainer14bit hist;
//
//        size_t n_its = 1e6;
//
//        int zeroes=0;
//        for (size_t i=0; i!=n_its; ++i) {
//            assert(fmod(gen(),1.0) == 0.0);
//            int result = gen();
//            if (result==0) { ++zeroes; }
//            hist.count_int_value(result);
//
//        }
//
////        cerr<<"zeros "<<zeroes<<endl; //turns out the boost method produced far too many zero counts
//
//        boost::math::poisson_distribution<> poisson_pdf(mean);
//
//        for (int ind=0; ind<100; ++ind) {
//            double predicted_num = boost::math::pdf(poisson_pdf,ind)*n_its;
//            CHECK_CLOSE(predicted_num, (double)hist[ind] , max(predicted_num/10, 10.0));
//        }
//
//        hist.write_to_file(test_suite_output_dir+"unuran_poisson_dist_mean"+string_utils::ftoa(
//                               mean,4)+".txt");
//
//    }

    TEST(unuran_poisson_seed_varies) {
        //Confirm that different poisson variates using the same stream will create different outputs:

        double mean = 23.1234;

        unuran::StreamWrapper rns1, rns2;

        //NB changing the seed may cause some tests to fail due to overly tight / crude statistical checks.

        //Here we can explicitly use different streams, or use default behaviour
        //(which  is to feed both from the default random number stream)

        //The advantage to explicitly specifying different streams? MULTI-THREADING.
        //(Each stream object looks after its own state)
        //But also, using a specific stream object makes the user aware of the need to supply a seed
        //(and possibly vary said seed between simulations)

        unuran::PoissonRandomVariate p_gen1(mean, rns1);
        unuran::PoissonRandomVariate p_gen2(mean, rns1);
//        unuran_poisson_random_variate p_gen1(mean);
//        unuran_poisson_random_variate p_gen2(mean);

        vector<int> p1, p2;

        for (size_t i=0; i!=10; ++i) {
            p1.push_back(p_gen1());
            p2.push_back(p_gen2());

            //visual check:
//            cout<<i<<" - rns poisson 1: "<<gen1()<<"; rns poisson 2: "<<gen2()<<endl;
        }

        CHECK(p1!=p2);
    }

    TEST(unuran_gaussian) {

        unuran::StreamWrapper rns;

        double mean(5.0), sigma(2.5);
        unuran::GaussianRandomVariate grv(mean, sigma ,rns);

        size_t n_its=1e5;
        vector<double> vals; vals.reserve(n_its);

        ofstream datfile(string(test_suite_output_dir+"gaussian_vals.txt").c_str());

        for (size_t i=0; i!=n_its; ++i) {
            vals.push_back(grv());
            datfile<<vals.back()<<"\n";
        }

        datfile.close();

//        cout<<"gaussian limits: "<< endl;
//        cout<<"mean: "<< 2* sigma/sqrt((double)n_its)<<endl;
//        cout<<"sigma: "<< 2* sigma*sigma/sqrt((double)n_its) <<endl;

//        CHECK_CLOSE(mean, vector_mean(vals),  2*sigma/sqrt((double)n_its));
//        CHECK_CLOSE(sigma, vector_std_dev(vals),  sigma*sigma/sqrt((double)n_its));

        //Now test the parameter update: (produces weird warning messages, stuff it.)
//        vals.clear();
//        mean = 55;
//        sigma = 15;
//
//        grv.update_params(mean,sigma);
//        for (size_t i=0; i!=n_its;++i){
//            vals.push_back( grv());
//        }
//        CHECK_CLOSE(mean, vector_mean(vals),  sigma/sqrt((double)n_its));
//        CHECK_CLOSE(sigma, vector_std_dev(vals),  sigma*sigma/sqrt((double)n_its));
    }

}


#include <fstream>
#include <iostream>
#include <experimental/filesystem>
#include <stdio.h>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>

#include "../tasks.hh"
#include "../classes/BinnedTypedMatrix.hh"

namespace fs = std::experimental::filesystem;

using namespace EMC;

using namespace boost::numeric::ublas;

/*int EMC::triangulate(bool forceOverwrite, std::string input, std::string output) {

	if (!forceOverwrite && fs::exists(output)) {
		std::cout << "Output file already exists, use -f to force overwrite." << std::endl;
		return 1;
	}

	std::ifstream infile(input);
	BinnedTypedMatrix inputMatrix = BinnedTypedMatrix::readFromFile(infile);
	infile.close();

    std::ofstream outputF(output);
    inputMatrix.writeToFile(outputF);
    outputF.close();

    return 0;
}
*/


// A numerically integrated implementation of the circuit described in
// "Rolling Your Own Circuit Simulator with Eigen and Boost.ODEInt"
// using Eigen to store and manipulate circuit values
// Author: Jeff Trull <edaskel@att.net>

/*
Copyright (c) 2014 Jeffrey E. Trull

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

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

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

#include <iostream>
#include <array>
#include <boost/numeric/odeint.hpp>
#include <Eigen/Dense>

#include "mna.hpp"

typedef std::array<double, 2> state_t;   // 0 = V_out, 1 = I_L

struct circuit {
    circuit(double r, double l, double c) {

        using namespace Eigen;

        typedef Matrix<double, 4, 4> matrix4_t;
        matrix4_t G = matrix4_t::Zero();
        matrix4_t C = matrix4_t::Zero();

        // state assignment: 0, 1 = node voltages; 2 = I_L, 3 = I_in
        stamp_r(G, C, 0, 1, r);
        stamp_c(G, C, 1, c);
        stamp_l(G, C, 1, 2, l);
        stamp_i(G, C, 0, 3);           // V_in, I_in

        // input application vector - connects single input to appropriate equation
        typedef Matrix<double, 4, 1> vector4_t;
        vector4_t B;
        B << 0, 0, 0, -1;

        // output extraction vector - connects state element to output
        Matrix<double, 1, 4> D;
        D << 0, 1, 0, 0;         // state element idx 1 = V_out

        // Now we have C*dX/dt = -G*X + B*u, and the output is = D * X

        // regularize so C is non-singular
        // First perform Gaussian elimination with full pivoting
        // This will put non-zero elements in the upper left, generally
        auto lu_fact = C.fullPivLu();
        matrix4_t Cprime = lu_fact.matrixLU().template triangularView<Upper>();

        // repeat process on G matrix and u vector using the factored components
        // LU produces a factorization P^-1 * L * U * Q^-1
        matrix4_t L = lu_fact.matrixLU().template triangularView<UnitLower>();
        matrix4_t P = lu_fact.permutationP();
        matrix4_t Q = lu_fact.permutationQ();
        matrix4_t Gprime = L.fullPivLu().solve(P * G * Q);   // permute rows and columns
        vector4_t Bprime = L.fullPivLu().solve(P * B);       // rows only
        Matrix<double, 1, 4> Dprime = D * Q;

        // Use the "rank" (# independent rows/columns) to determine where to split
        int sz = lu_fact.rank();
        int remainder = 4 - sz;

        // Produce smaller (and hopefully non-singular) matrices for calculation
        MatrixXd Cnew = Cprime.topLeftCorner(sz, sz);   // the rest is zero

        // break up Gprime into chunks based on the size of the nonzero portion of Cprime
        MatrixXd G11  = Gprime.topLeftCorner(sz, sz);
        MatrixXd G12  = Gprime.topRightCorner(sz, remainder);
        MatrixXd G21  = Gprime.bottomLeftCorner(remainder, sz);
        MatrixXd G22  = Gprime.bottomRightCorner(remainder, remainder);

        // solve for the (sz) state variables we will retain
        auto G22LU = G22.fullPivLu();
        MatrixXd Gnew = G11 - G12 * G22LU.solve(G21);

        // adjust input and output connections
        Matrix<double, Dynamic, 1> Bnew;
        Bnew = Bprime.topRows(sz) - G12 * G22LU.solve(Bprime.bottomRows(remainder));
        Matrix<double, 1, Dynamic> Dnew;
        Dnew = Dprime.leftCols(sz) - Dprime.rightCols(remainder) * G22LU.solve(G21);

        // verify the new C is non-singular
        assert(Cnew.rows() == Cnew.fullPivLu().rank());

        // factor Cnew out of our equation by multiplying both sides by Cnew^-1
        // new equation will be dX/dt = - Cnew^-1 * Gnew * X + Cnew^-1 * Bnew * u
        drift_term_ = - Cnew.fullPivLu().solve(Gnew);
        input_term_ =   Cnew.fullPivLu().solve(Bnew);

        // Vout may have been moved in the reduction process, so we must supply a map
        s2o_        = Dnew;
    }

    double state2output(state_t const& x) const {
        Eigen::Map<const Eigen::Matrix<double, 2, 1> > xvec(x.data());

        return (s2o_ * xvec)(0, 0);
    }

    void operator()(state_t const& x, state_t& dxdt, double t) {
        using namespace Eigen;

        Map<const Matrix<double, 2, 1> > xvec(x.data());
        Map<Matrix<double, 2, 1> > result(dxdt.data());

        result = drift_term_ * xvec + input_term_ ;   // input is always 1V
    }

private:
    Eigen::MatrixXd drift_term_;            // connects current state to development over time
    Eigen::MatrixXd input_term_;            // connects input to development
    Eigen::Matrix<double, 1, Eigen::Dynamic> s2o_; // transforms state to output
};

int main() {
    using namespace boost::numeric::odeint;
    circuit ckt(100.0, 20e-6, 20e-9);
    state_t x{0.0, 0.0};                    // initial conditions

    integrate( ckt, x, 0.0, 10e-6, 0.1e-6,  // time range and increment
               [ckt](state_t const& x, double t) {
                   std::cout << t << " " << ckt.state2output(x) << std::endl;
               });
}


/* Copyright (C) 2012-2017 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.
 */
/* matmul1D.cpp - Data-movement operations on arrays of slots
 */
#include <algorithm>
#include <NTL/BasicThreadPool.h>
#include "EncryptedArray.h"
#include "matmul.h"
//#include "multiAutomorph.h"

#if 0

// Translate a value x in Zm* to the index e s.t. g_i^e=x (mod m)
// in a native dimension, returns e in 0..ord_i-1
// in a non-native dimension, returns e in -ord_i+1..ord_i-1
// returns -ord_i if it can't find such an e

// For now, this is just done by brute-force search, which is
// probably good enough.
static
long val2index(const PAlgebra& zMStar, long dim, long x)
{
  FHE_TIMER_START;

  long m = zMStar.getM();
  long g = zMStar.ZmStarGen(dim);
  long ord = zMStar.OrderOf(dim);

  long e = 0;
  long g2e = 1;

  mulmod_precon_t gminv = PrepMulModPrecon(g, m);

  while (e < ord && g2e != x) {
    e++;
    g2e = MulModPrecon(g2e, g, m, gminv);
  }

  if (e < ord) return e;

  if (zMStar.SameOrd(dim)) return -ord;

  x = MulMod(x, g2e, m);

  e = 0;
  g2e = 1;

  while (e < ord && g2e != x) {
    e++;
    g2e = MulModPrecon(g2e, g, m, gminv);
  }

  if (e < ord) return e-ord;

  return -ord;

}

// A class that implements the basic (sparse) 1D matrix-vector functions
template<class type> class matmul1D_impl {
  PA_INJECT(type)
  const MatrixCacheType buildCache;
  std::unique_ptr<CachedzzxMatrix> zCache;
  std::unique_ptr<CachedDCRTMatrix> dCache;

  MatMul<type>& mat;
  const EncryptedArrayDerived<type>& ea;

public:
  matmul1D_impl(MatMulBase& _mat, MatrixCacheType tag, long dim)
    : buildCache(tag), mat(dynamic_cast< MatMul<type>& >(_mat)),
      ea(_mat.getEA().getDerived(type()))
  {
    long D = ea.sizeOfDimension(dim);
    long sz = ea.nativeDimension(dim) ? D : (2*D-1);

    if (buildCache==cachezzX)
      zCache.reset(new CachedzzxMatrix(NTL::INIT_SIZE,sz));
    else if (buildCache==cacheDCRT)
      dCache.reset(new CachedDCRTMatrix(NTL::INIT_SIZE,sz));
  }

  // Get the i'th diagonal along dimension dim, encoded as a
  // single constant. All blocks use the same transofmration.
  // Returns true if this is a zero diagonal, false otherwise
  bool processDiagonal1(zzX& cPoly, long dim, long i, long D)
  {
    vector<RX> tmpDiag(D);
    bool zDiag = true; // is this a zero diagonal?
    long nzLast = -1;  // index of last non-zero entry
    RX entry;

    // Process the entries in this diagonal one at a time
    for (long j = 0; j < D; j++) { // process entry j
      bool zEntry = mat.get(entry, mcMod(j-i, D), j); // entry [j-i mod D, j]
      assert(zEntry || deg(entry) < ea.getDegree());
      // get(...) returns true if the entry is empty, false otherwise

      if (!zEntry && IsZero(entry)) zEntry = true;// zero is an empty entry too

      if (!zEntry) {   // not a zero entry
        zDiag = false; // mark diagonal as non-empty

        // clear entries between last nonzero entry and this one
        for (long jj = nzLast+1; jj < j; jj++) clear(tmpDiag[jj]);
        tmpDiag[j] = entry;
        nzLast = j;
      }
    }
    if (zDiag) return true; // zero diagonal, nothing to do

    // clear trailing zero entries
    for (long jj = nzLast+1; jj < D; jj++) clear(tmpDiag[jj]);

    vector<RX> diag(ea.size());
    if (D==1) diag.assign(ea.size(), tmpDiag[0]); // dimension of size one
    else for (long j = 0; j < ea.size(); j++)
           diag[j] = tmpDiag[ ea.coordinate(dim,j) ];
           // rearrange the indexes based on the current dimension

    ea.encode(cPoly, diag);
    return false; // a nonzero diagonal
  }

  // Get the i'th diagonal along dimension dim, encoded as a
  // single constant. Different blocks use different transofmrations.
  // Returns true if this is a zero diagonal, false otherwise
  bool processDiagonal2(zzX& poly, long dim, long idx, long D)
  {
    bool zDiag = true; // is this a zero diagonal?
    long nzLast = -1;  // index of last non-zero entry
    RX entry;

    // Process the entries in this diagonal one at a time
    long blockIdx, innerIdx;
    vector<RX> diag(ea.size());
    for (long j=0; j<ea.size(); j++) {
      if (D==1) {
	blockIdx=j; innerIdx = 0;
      } else {
	std::tie(blockIdx, innerIdx) // std::pair<long,long> idxes
	  = ea.getContext().zMStar.breakIndexByDim(j, dim);
	//	blockIdx = idxes.first;  // which transformation
	//	innerIdx = idxes.second; // index along diemnssion dim
      }
      // process entry j
      bool zEntry=mat.multiGet(entry,mcMod(innerIdx-idx,D),innerIdx,blockIdx);
      // entry [i,j-i mod D] in the block corresponding to blockIdx
      // multiGet(...) returns true if the entry is empty, false otherwise

      // If non-zero, make sure the degree is not too large
      assert(zEntry || deg(entry) < ea.getDegree());

      if (!zEntry && IsZero(entry)) zEntry = true; // zero is an empty entry too

      if (!zEntry) {   // not a zero entry
	zDiag = false; // mark diagonal as non-empty

	// clear entries between last nonzero entry and this one
	for (long jj = nzLast+1; jj < j; jj++) clear(diag[jj]);
	nzLast = j;
	diag[j] = entry;
      }
    }
    if (zDiag) return true; // zero diagonal, nothing to do

    // clear trailing zero entries
    for (long jj = nzLast+1; jj < ea.size(); jj++) clear(diag[jj]);
    ea.encode(poly, diag);
    return false; // a nonzero diagonal
  }

  void fixup(zzX& cpoly, const RX& c, long i, long signed_i, long dim)
  {
    const PAlgebraModDerived<type>& tab = ea.getTab();
    const vector< vector< RX > >& maskTable = tab.getMaskTable();
    const RXModulus& PhimXMod = tab.getPhimXMod();

    long D = ea.sizeOfDimension(dim);

    if (signed_i > 0) {
      RX mask = maskTable[dim][i];
      MulMod(mask, mask, c, PhimXMod);
      convert(cpoly, mask);
    }
    else {
      RX mask;
      NTL::negate(mask, maskTable[dim][i]);
      MulMod(mask, mask, c, PhimXMod);
      add(mask, mask, c);
      convert(cpoly, mask);
    }
  }

  void multiply(Ctxt* ctxt, long dim, bool oneTransform)
  {
    FHE_TIMER_START;

    assert(dim >= 0 && dim < ea.dimension());
    RBak bak; bak.save(); ea.getTab().restoreContext(); // backup NTL modulus

    std::unique_ptr<Ctxt> res, shCtxt;
    if (ctxt!=nullptr) { // we need to do an actual multiplication
      ctxt->cleanUp(); // not sure, but this may be a good idea
      res.reset(new Ctxt(ZeroCtxtLike, *ctxt));
      shCtxt.reset(new Ctxt(ZeroCtxtLike, *ctxt));
    }

    // Check if we have the relevant constant in cache
    CachedzzxMatrix* zcp;
    CachedDCRTMatrix* dcp;
    mat.getCache(&zcp, &dcp);

    // set up the AutoIterator, if we have a ctxt
    std::unique_ptr<AutoIterator> autoIterator;
    if (ctxt) {
      FHE_NTIMER_START(AutoIterator_build);
      autoIterator.reset(AutoIterator::build(*ctxt, ctxt->getPubKey().getTree4dim(dim)));
    }

    // Process the diagonals one at a time
    if (ea.nativeDimension(dim)) {
      zzX cpoly;
      long D = ea.sizeOfDimension(dim);

      for (long cnt = 0; cnt < D; cnt++) { // process one diagonal
	long i; //process diagonal i

	if (!ctxt) {
	  i = cnt;
	}
	else if (cnt == 0) {
	  i = 0;
	  *shCtxt = *ctxt;
	}
	else {
          FHE_NTIMER_START(AutoIterator_next);
	  long x = autoIterator->next(*shCtxt);
	  assert(x !=0);
	  i = val2index(ea.getContext().zMStar, dim, x);
	  assert(i >= 0);
	}

	zzX* zxPtr=nullptr;
	DoubleCRT* dxPtr=nullptr;

	if (dcp != nullptr)         // DoubleCRT cache exists
	  dxPtr = (*dcp)[i].get();
	else if (zcp != nullptr)    // zzx cache exists but no DoubleCRT
	  zxPtr = (*zcp)[i].get();
	else { // no cache, compute const
	  bool zero = oneTransform? processDiagonal1(cpoly, dim, i, D)
				  : processDiagonal2(cpoly, dim, i, D);
	  if (!zero) zxPtr = &cpoly; // if it is not a zero value, point to it
	}

	// if zero diagonal, nothing to do for this iteration
	if (zxPtr==nullptr && dxPtr==nullptr)
	  continue;

	// Non-zero diagonal, store it in cache and/or multiply/add it

	if (ctxt) {
	  if (dxPtr!=nullptr) shCtxt->multByConstant(*dxPtr);
	  else                shCtxt->multByConstant(*zxPtr);

	  *res += *shCtxt;

	}

	if (buildCache==cachezzX) {
	  (*zCache)[i].reset(new zzX(*zxPtr));
	}
	else if (buildCache==cacheDCRT) {
	  (*dCache)[i].reset(new DoubleCRT(*zxPtr, ea.getContext()));
	}
      } // end of loop over diagonals
    }
    else {

      zzX cpoly;
      long D = ea.sizeOfDimension(dim);

      std::vector< unique_ptr<RX> > local_cache;
      if (!dcp && !zcp) {
        local_cache.resize(D);
      }

      for (long cnt = 0; cnt < 2*D-1; cnt++) { // process one diagonal
	long i; //process diagonal i
        long signed_i;

	if (!ctxt) {
          if (cnt == 0) {
             i = 0;
             signed_i = 0;
          }
          else {
             i = (cnt+1)/2;
             signed_i = (cnt % 2) ? (i - D) : i;
          }
	}
	else if (cnt == 0) {
	  i = 0;
          signed_i = 0;
	  *shCtxt = *ctxt;
	}
	else {
	  long x = autoIterator->next(*shCtxt);
	  assert(x !=0);
	  signed_i = val2index(ea.getContext().zMStar, dim, x);
          i = signed_i;
          if (i < 0) i += D;
	}

        long i_off = signed_i + (D-1);

	zzX* zxPtr=nullptr;
	DoubleCRT* dxPtr=nullptr;

	if (dcp != nullptr)         // DoubleCRT cache exists
	  dxPtr = (*dcp)[i_off].get();
	else if (zcp != nullptr)    // zzx cache exists but no DoubleCRT
	  zxPtr = (*zcp)[i_off].get();
	else if (local_cache[i]) {
          if (*local_cache[i] != 0) {
             convert(cpoly, *local_cache[i]);
             zxPtr = &cpoly;
             if (i) fixup(cpoly, *local_cache[i], i, signed_i, dim);
          }
          local_cache[i].reset(); // we've used it, so we can kill it
        }
        else { // no cache, compute const
	  bool zero = oneTransform? processDiagonal1(cpoly, dim, i, D)
				  : processDiagonal2(cpoly, dim, i, D);
	  if (!zero) {
            zxPtr = &cpoly; // if it is not a zero value, point to it
            local_cache[i].reset(new RX());
            convert(*local_cache[i], cpoly);
            if (i) fixup(cpoly, *local_cache[i], i, signed_i, dim);
          }
          else {
            local_cache[i].reset(new RX());
          }
	}

	// if zero diagonal, nothing to do for this iteration
	if (zxPtr==nullptr && dxPtr==nullptr)
	  continue;

	// Non-zero diagonal, store it in cache and/or multiply/add it

	if (ctxt) {
	  if (dxPtr!=nullptr) shCtxt->multByConstant(*dxPtr);
	  else                shCtxt->multByConstant(*zxPtr);
	  *res += *shCtxt;
	}
	if (buildCache==cachezzX) {
	  (*zCache)[i_off].reset(new zzX(*zxPtr));
	}
	else if (buildCache==cacheDCRT) {
	  (*dCache)[i_off].reset(new DoubleCRT(*zxPtr, ea.getContext()));
	}
      } // end of loop over diagonals
    }

    if (ctxt) // copy result back to ctxt
      *ctxt = *res;

    // "install" the cache (if needed)
    if (buildCache == cachezzX)
      mat.installzzxcache(zCache);
    else if (buildCache == cacheDCRT)
      mat.installDCRTcache(dCache);
  } // end of multiply(...)
};

// Wrapper functions around the implemenmtation class
static void matmul1d(Ctxt* ctxt, MatMulBase& mat, long dim,
		     bool oneTransform, MatrixCacheType buildCache)
{
  MatMulLock locking(mat, buildCache);

  // If locking.getType()!=cacheEmpty then we really do need to
  // build the cache, and we also have the lock for it.

  if (locking.getType()==cacheEmpty && ctxt==nullptr) //  nothing to do
    return;

  switch (mat.getEA().getTag()) {
    case PA_GF2_tag: {
      matmul1D_impl<PA_GF2> M(mat, locking.getType(), dim);
      M.multiply(ctxt, dim, oneTransform);
      break;
    }
    case PA_zz_p_tag: {
      matmul1D_impl<PA_zz_p> M(mat, locking.getType(), dim);
      M.multiply(ctxt, dim, oneTransform);
      break;
    }
    default:
      throw std::logic_error("matmul1d: neither PA_GF2 nor PA_zz_p");
  }
}
void buildCache4MatMul1D(MatMulBase& mat, long dim, MatrixCacheType buildCache)
{ matmul1d(nullptr, mat, dim, true, buildCache); }

void matMul1D(Ctxt& ctxt, MatMulBase& mat,long dim, MatrixCacheType buildCache)
{ matmul1d(&ctxt, mat, dim, true, buildCache); }

void buildCache4MatMulti1D(MatMulBase& mat,long dim,MatrixCacheType buildCache)
{ matmul1d(nullptr, mat, dim, false, buildCache); }

void matMulti1D(Ctxt& ctxt,MatMulBase& mat,long dim,MatrixCacheType buildCache)
{ matmul1d(&ctxt, mat, dim, false, buildCache); }

/********************************************************************
 ********************************************************************/
// Applying matmul to plaintext, useful for debugging

template<class type>
class matmul1D_pa_impl {
public:
  PA_INJECT(type)

  static void multiply(NewPlaintextArray& pa, MatMul<type>& mat,
		       long dim, bool oneTrans)
  {
    const EncryptedArrayDerived<type>& ea = mat.getEA().getDerived(type());
    RBak bak; bak.save(); ea.getTab().restoreContext();

    long n = ea.size();
    long D = ea.sizeOfDimension(dim);

    vector< vector<RX> > data1(n/D);
    for (long k = 0; k < n/D; k++)
      data1[k].resize(D);

    // copy the data into a vector of 1D vectors
    vector<RX>& data = pa.getData<type>();
    for (long i = 0; i < n; i++) {
      long k,j;
      std::tie(k,j) = ea.getContext().zMStar.breakIndexByDim(i, dim);
      data1[k][j] = data[i];       // k= along dim, j = the rest of i
    }

    // multiply each one of the vectors by the same matrix
    for (long k = 0; k < n/D; k++) {
      for (long j = 0; j < D; j++) { // simple matrix-vector multiplication
	std::pair<long,long> p(k,j);
	long idx = ea.getContext().zMStar.assembleIndexByDim(p, dim);

	RX acc, val, tmp;
	acc = 0;
        for (long i = 0; i < D; i++) {
          bool zero = oneTrans? mat.get(val, i, j) : mat.multiGet(val,i,j,k);
          if (!zero) {
            NTL::mul(tmp, data1[k][i], val);
            NTL::add(acc, acc, tmp);
          }
        }
        rem(data[idx], acc, ea.getG()); // store the result in the data array
      }
    }
  }
};
// A wrapper around the implementation class
static void matmul1d(NewPlaintextArray& pa, MatMulBase& mat,
		     long dim, bool oneTrans)
{
  switch (mat.getEA().getTag()) {
    case PA_GF2_tag: {
      matmul1D_pa_impl<PA_GF2>::multiply(pa,
                      dynamic_cast< MatMul<PA_GF2>& >(mat), dim, oneTrans);
      return;
    }
    case PA_zz_p_tag: {
      matmul1D_pa_impl<PA_zz_p>::multiply(pa,
                      dynamic_cast<MatMul<PA_zz_p>&>(mat), dim, oneTrans);
      return;
    }
    default:
      throw std::logic_error("matMul1D: neither PA_GF2 nor PA_zz_p");
  }
}

void matMul1D(NewPlaintextArray& pa, MatMulBase& mat, long dim)
{
  matmul1d(pa, mat, dim, true);
}
void matMulti1D(NewPlaintextArray& pa, MatMulBase& mat, long dim)
{
  matmul1d(pa, mat, dim, false);
}

#else

// baby-step giant-step implementation

struct PtxtPtr {
  enum Type { ZERO=0, ZZX=1, DCRT=2 };
  zzX*       zp;
  DoubleCRT* dp;

  PtxtPtr(): zp(nullptr), dp(nullptr) {}
};

// A class that implements the basic (sparse) 1D matrix-vector functions
template<class type> class matmul1D_impl {
  PA_INJECT(type)
  const MatrixCacheType buildCache;
  std::unique_ptr<CachedzzxMatrix> zCache;
  std::unique_ptr<CachedDCRTMatrix> dCache;

  MatMul<type>& mat;
  const EncryptedArrayDerived<type>& ea;

public:
  matmul1D_impl(MatMulBase& _mat, long dim, MatrixCacheType tag)
    : buildCache(tag), mat(dynamic_cast< MatMul<type>& >(_mat)),
      ea(_mat.getEA().getDerived(type()))
  {
    if (buildCache==cachezzX) {
      zCache.reset(new CachedzzxMatrix(NTL::INIT_SIZE,ea.sizeOfDimension(dim)));
    } else if (buildCache==cacheDCRT) {
      dCache.reset(new CachedDCRTMatrix(NTL::INIT_SIZE,ea.sizeOfDimension(dim)));
    }
  }

  // Get the i'th diagonal along dimension dim, encoded as a
  // single constant. All blocks use the same transofmration.
  // Returns true if this is a zero diagonal, false otherwise
  bool processDiagonal1(zzX& cPoly, long dim, long i, long D, long rotAmt)
  {
    vector<RX> tmpDiag(D);
    bool zDiag = true; // is this a zero diagonal?
    long nzLast = -1;  // index of last non-zero entry
    RX entry;

    // Process the entries in this diagonal one at a time
    for (long j = 0; j < D; j++) { // process entry j
      long rotJ = (j+rotAmt) % D;  // need to rotate constant by rotAmt
      bool zEntry = mat.get(entry, mcMod(rotJ-i, D), rotJ); // entry [j-i mod D, j]
      assert(zEntry || deg(entry) < ea.getDegree());
      // get(...) returns true if the entry is empty, false otherwise

      if (!zEntry && IsZero(entry)) zEntry = true;// zero is an empty entry too

      if (!zEntry) {   // not a zero entry
        zDiag = false; // mark diagonal as non-empty

        // clear entries between last nonzero entry and this one
        for (long jj = nzLast+1; jj < j; jj++) clear(tmpDiag[jj]);
        tmpDiag[j] = entry;
        nzLast = j;
      }
    }
    if (zDiag) return true; // zero diagonal, nothing to do

    // clear trailing zero entries
    for (long jj = nzLast+1; jj < D; jj++) clear(tmpDiag[jj]);

    vector<RX> diag(ea.size());
    if (D==1) diag.assign(ea.size(), tmpDiag[0]); // dimension of size one
    else for (long j = 0; j < ea.size(); j++)
           diag[j] = tmpDiag[ ea.coordinate(dim,j) ];
           // rearrange the indexes based on the current dimension

    ea.encode(cPoly, diag);
    return false; // a nonzero diagonal
  }

  // Get the i'th diagonal along dimension dim, encoded as a
  // single constant. Different blocks use different transofmrations.
  // Returns true if this is a zero diagonal, false otherwise
  bool processDiagonal2(zzX& poly, long dim, long idx, long D, long rotAmt)
  {
    bool zDiag = true; // is this a zero diagonal?
    long nzLast = -1;  // index of last non-zero entry
    RX entry;

    // Process the entries in this diagonal one at a time
    long blockIdx, innerIdx;
    vector<RX> diag(ea.size());
    for (long j=0; j<ea.size(); j++) {
      if (D==1) {
	blockIdx=j; innerIdx = 0;
      } else {
	std::tie(blockIdx, innerIdx) // std::pair<long,long> idxes
	  = ea.getContext().zMStar.breakIndexByDim(j, dim);
	//	blockIdx = idxes.first;  // which transformation
	//	innerIdx = idxes.second; // index along diemnssion dim
        innerIdx = (innerIdx+rotAmt) % D;  // need to rotate constant by rotAmt
      }
      // process entry j
      bool zEntry=mat.multiGet(entry,mcMod(innerIdx-idx,D),innerIdx,blockIdx);
      // entry [i,j-i mod D] in the block corresponding to blockIdx
      // multiGet(...) returns true if the entry is empty, false otherwise

      // If non-zero, make sure the degree is not too large
      assert(zEntry || deg(entry) < ea.getDegree());

      if (!zEntry && IsZero(entry)) zEntry = true; // zero is an empty entry too

      if (!zEntry) {   // not a zero entry
	zDiag = false; // mark diagonal as non-empty

	// clear entries between last nonzero entry and this one
	for (long jj = nzLast+1; jj < j; jj++) clear(diag[jj]);
	nzLast = j;
	diag[j] = entry;
      }
    }
    if (zDiag) return true; // zero diagonal, nothing to do

    // clear trailing zero entries
    for (long jj = nzLast+1; jj < ea.size(); jj++) clear(diag[jj]);

    ea.encode(poly, diag);
    return false; // a nonzero diagonal
  }

  // Returns in vec all the constants needed for a single giant-step.
  // The return value is ZERO(=0) if all these constants are zero.
  // Otherwise it is ZZX(=1) if using zzX or DCRT(=2) if using DoubleCRT
  PtxtPtr::Type getConsts(std::vector<PtxtPtr>& vec, std::vector<zzX>& polys,
                          CachedzzxMatrix* zcp, CachedDCRTMatrix* dcp,
                          long dim, long jg, long D, bool oneTrans)
  {
    PtxtPtr::Type typ = PtxtPtr::ZERO;
    long g = polys.size(); // how many constants do we need
    vec.assign(g,PtxtPtr());

    if (dcp!=nullptr) {        // we have DCRT cache
      for (long i=0, idx=jg; i<g && idx<D; i++, idx++) {
        if ((vec[i].dp = (*dcp)[idx].get()) != nullptr)
          typ = PtxtPtr::DCRT;
      }
    } else if (zcp!=nullptr) { // we have ZZX cache
      for (long i=0, idx=jg; i<g && idx<D; i++, idx++) {
        if ((vec[i].zp = (*zcp)[idx].get()) != nullptr)
          typ = PtxtPtr::ZZX;
      }
    } else {                   // no cache, compute consts
      for (long i=0, idx=jg; i<g && idx<D; i++, idx++) {
        bool zero = oneTrans? processDiagonal1(polys[i], dim, idx, D, i)
                            : processDiagonal2(polys[i], dim, idx, D, i);
        if (!zero) {
          vec[i].zp = &(polys[i]); // if not a zero, point to it
          typ = PtxtPtr::ZZX;
        }
      }
    }
    return typ;
  }

  void multiply(Ctxt* ctxt, long dim, bool oneTransform)
  {
    assert(dim >= 0 && dim <= ea.dimension());
    RBak bak; bak.save(); ea.getTab().restoreContext(); // backup NTL modulus

    long D = (dim==ea.dimension())? 1 : ea.sizeOfDimension(dim);
    long g = mat.getGstep();
    if (g<1 || g>=D) g=1; // sanity check
    long dDivg = divc(D,g);

    // Process the diagonals in baby-step/giant-step ordering.
    //   sum_{i=0}^{d-1} const_i rot^i(X)
    //   = \sum_{i=0}^{g-1} \sum_{j=0}^{d/g -1} const_{i+g*j} rot^{i+g*j}(X)
    //   = \sum_{i=0}^{g-1} rot^i(sum_j rot^{-i}(const_{i+g*j}) rot^{g*j}(X))
    //
    // so for i=0..g-1 we let
    //    Y_i = sum_j rot^{-i}(const_{i+g*j}) rot^{g*j}(X)
    // then compute \sum_{i=0}^{g-1} rot^i(Y_i).
    //
    // Computing the Y_i's, we initialize an accumulator for each Y_i,
    // then compute the rotations X_j = rot^{g*j}(X), j=0,...,d/g-1.
    // Each X_j is multiplied by all the constants rot^{-i}(const_{i+g*j}),
    // i=0,...,g-1, and the i'th product is added to the accumulator for
    // the corresponding Y_i.

    long lastRotate = 0;
    std::vector<Ctxt> acc; // accumulators
    if (ctxt!=nullptr) {   // we need to do an actual multiplication
      ctxt->cleanUp();     // not sure, but this may be a good idea
      Ctxt tmp(ZeroCtxtLike, *ctxt);
      acc.resize(g, tmp);
    }

    // Check if we have the relevant constant in cache
    CachedzzxMatrix* zcp;
    CachedDCRTMatrix* dcp;
    mat.getCache(&zcp, &dcp);

    // Process the diagonals in giant-step/baby-step order
    std::vector<zzX> cpolys(g); // scratch space for encoding consts
    std::vector<PtxtPtr> ptrs;  // pointers to these constants
    for (long j = 0; j < dDivg; j++) { // giant steps
      long jg = j*g;            // beginning index of this giant step

      // get all the constants rot^{-i}(const_{i+g*j}) for this step
      PtxtPtr::Type ty = getConsts(ptrs, cpolys, zcp, dcp,
                                   dim, jg, D, oneTransform);

      if (ty==PtxtPtr::ZERO) continue; // all consts are zero

      // Store constants in cache and/or multiply/add them

      if (ctxt!=nullptr) {  // rotate by jg, multiply & add
        Ctxt shCtxt(*ctxt); // temporary to hold current rot^{g*j}(X)

        if (j>0) {
          shCtxt = *ctxt;
          ea.rotate1D(shCtxt, dim, jg); // rotate by i
	} // if j==0 we already have *shCtxt == *ctxt

        if (ty==PtxtPtr::DCRT) for (long i=0; i<min(g,D-jg); i++) {
            if (ptrs[i].dp!=nullptr) {
              Ctxt tmp(shCtxt);
              tmp.multByConstant(*(ptrs[i].dp));
              acc[i] += tmp;
            }
          }
        else /*ty==PtxtPtr::ZZX*/ for (long i=0; i<min(g,D-jg); i++) {
            if (ptrs[i].zp!=nullptr) {
              Ctxt tmp(shCtxt);
              tmp.multByConstant(*(ptrs[i].zp));
              acc[i] += tmp;
            }
          }
      }

      if (buildCache==cachezzX) for (long i=0; i<min(g,D-jg); i++) {
          if (ptrs[i].zp!=nullptr)
            (*zCache)[i+jg].reset(new zzX(*(ptrs[i].zp)));
        }
      else if (buildCache==cacheDCRT) for (long i=0; i<min(g,D-jg); i++) {
          if (ptrs[i].zp!=nullptr)
            (*dCache)[i+jg].reset(new DoubleCRT(*(ptrs[i].zp),ea.getContext()));
        }
      // end of giant-step loop
    }
    // Compute the result as \sum_{i=0}^{g-1} rho^i(Y_i)
    if (ctxt!=nullptr) {
      *ctxt = acc[0];
      for (long i = 1; i < g; i++) {
        ea.rotate1D(acc[i], dim, i);
        *ctxt += acc[i];
      }
    }
    // "install" the cache if needed
    if (buildCache == cachezzX)
      mat.installzzxcache(zCache);
    else if (buildCache == cacheDCRT)
      mat.installDCRTcache(dCache);
  } // end of multiply(...)
};

// Wrapper functions around the implemenmtation class
static void matmul1d(Ctxt* ctxt, MatMulBase& mat, long dim, bool oneTransform,
                     MatrixCacheType buildCache)
{
  MatMulLock locking(mat, buildCache);

  // If locking.getType()!=cacheEmpty then we really do need to
  // build the cache, and we also have the lock for it.

  if (locking.getType()==cacheEmpty && ctxt==nullptr) //  nothing to do
    return;

  switch (mat.getEA().getTag()) {
    case PA_GF2_tag: {
      matmul1D_impl<PA_GF2> M(mat, dim, locking.getType());
      M.multiply(ctxt, dim, oneTransform);
      break;
    }
    case PA_zz_p_tag: {
      matmul1D_impl<PA_zz_p> M(mat, dim, locking.getType());
      M.multiply(ctxt, dim, oneTransform);
      break;
    }
    default:
      throw std::logic_error("matmul1d: neither PA_GF2 nor PA_zz_p");
  }
}
void buildCache4MatMul1D(MatMulBase& mat, long dim,
                         MatrixCacheType buildCache)
{ matmul1d(nullptr, mat, dim, true, buildCache); }

void matMul1D(Ctxt& ctxt, MatMulBase& mat,long dim,
              MatrixCacheType buildCache)
{ matmul1d(&ctxt, mat, dim, true, buildCache); }

void buildCache4MatMulti1D(MatMulBase& mat,long dim,
                           MatrixCacheType buildCache)
{ matmul1d(nullptr, mat, dim, false, buildCache); }

void matMulti1D(Ctxt& ctxt,MatMulBase& mat,long dim,
                MatrixCacheType buildCache)
{ matmul1d(&ctxt, mat, dim, false, buildCache); }

/********************************************************************
 ********************************************************************/
// Applying matmul to plaintext, useful for debugging

template<class type>
class matmul1D_pa_impl {
public:
  PA_INJECT(type)

  static void multiply(NewPlaintextArray& pa, MatMul<type>& mat,
		       long dim, bool oneTrans)
  {
    const EncryptedArrayDerived<type>& ea = mat.getEA().getDerived(type());
    RBak bak; bak.save(); ea.getTab().restoreContext();

    long n = ea.size();
    long D = ea.sizeOfDimension(dim);

    vector< vector<RX> > data1(n/D);
    for (long k = 0; k < n/D; k++)
      data1[k].resize(D);

    // copy the data into a vector of 1D vectors
    vector<RX>& data = pa.getData<type>();
    for (long i = 0; i < n; i++) {
      long k,j;
      std::tie(k,j) = ea.getContext().zMStar.breakIndexByDim(i, dim);
      data1[k][j] = data[i];       // k= along dim, j = the rest of i
    }

    // multiply each one of the vectors by the same matrix
    for (long k = 0; k < n/D; k++) {
      for (long j = 0; j < D; j++) { // simple matrix-vector multiplication
	std::pair<long,long> p(k,j);
	long idx = ea.getContext().zMStar.assembleIndexByDim(p, dim);

	RX acc, val, tmp;
	acc = 0;
        for (long i = 0; i < D; i++) {
          bool zero = oneTrans? mat.get(val, i, j) : mat.multiGet(val,i,j,k);
          if (!zero) {
            NTL::mul(tmp, data1[k][i], val);
            NTL::add(acc, acc, tmp);
          }
        }
        rem(data[idx], acc, ea.getG()); // store the result in the data array
      }
    }
  }
};
// A wrapper around the implementation class
static void matmul1d(NewPlaintextArray& pa, MatMulBase& mat,
		     long dim, bool oneTrans)
{
  switch (mat.getEA().getTag()) {
    case PA_GF2_tag: {
      matmul1D_pa_impl<PA_GF2>::multiply(pa,
                      dynamic_cast< MatMul<PA_GF2>& >(mat), dim, oneTrans);
      return;
    }
    case PA_zz_p_tag: {
      matmul1D_pa_impl<PA_zz_p>::multiply(pa,
                      dynamic_cast<MatMul<PA_zz_p>&>(mat), dim, oneTrans);
      return;
    }
    default:
      throw std::logic_error("matMul1D: neither PA_GF2 nor PA_zz_p");
  }
}

void matMul1D(NewPlaintextArray& pa, MatMulBase& mat, long dim)
{
  matmul1d(pa, mat, dim, true);
}
void matMulti1D(NewPlaintextArray& pa, MatMulBase& mat, long dim)
{
  matmul1d(pa, mat, dim, false);
}

#endif


/*********************************************************************
*  Copyright (c) 2017 Robert Bosch GmbH.
*  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 under the License.
* *********************************************************************/

#include <gtest/gtest.h>

#include <Eigen/Dense>

#include "steering_functions/filter/ekf.hpp"
#include "steering_functions/steering_functions.hpp"
#include "steering_functions/utilities/utilities.hpp"

using namespace std;
using namespace steer;

#define EPS_JACOBI 1e-4                  // [-]
#define EPS_PERTURB 1e-7                 // [-]
#define SAMPLES 1e6                      // [-]
#define OPERATING_REGION_X 20.0          // [m]
#define OPERATING_REGION_Y 20.0          // [m]
#define OPERATING_REGION_THETA 2 * M_PI  // [rad]
#define OPERATING_REGION_KAPPA 2.0       // [1/m]
#define OPERATING_REGION_DELTA_S 0.4     // [m]
#define OPERATING_REGION_SIGMA 2.0       // [1/m^2]
#define random(lower, upper) (rand() * (upper - lower) / RAND_MAX + lower)
#define random_boolean() rand() % 2

typedef Eigen::Matrix<double, 2, 2> Matrix2d;
typedef Eigen::Matrix<double, 3, 3> Matrix3d;
typedef Eigen::Matrix<double, 3, 2> Matrix32d;

State get_random_state()
{
  State state;
  state.x = random(-OPERATING_REGION_X / 2.0, OPERATING_REGION_X / 2.0);
  state.y = random(-OPERATING_REGION_Y / 2.0, OPERATING_REGION_Y / 2.0);
  state.theta = random(-OPERATING_REGION_THETA / 2.0, OPERATING_REGION_THETA / 2.0);
  state.kappa = random(-OPERATING_REGION_KAPPA / 2.0, OPERATING_REGION_KAPPA / 2.0);
  state.d = 0.0;
  return state;
}

Control get_random_control()
{
  Control control;
  control.delta_s = random(-OPERATING_REGION_DELTA_S / 2.0, OPERATING_REGION_DELTA_S / 2.0);
  control.sigma = random(-OPERATING_REGION_SIGMA / 2.0, OPERATING_REGION_SIGMA / 2.0);
  return control;
}

State integrate_ODE(const State &state, const Control &control, double integration_step)
{
  State state_next;
  double sigma(control.sigma);
  double d(sgn(control.delta_s));
  if (fabs(sigma) > get_epsilon())
  {
    end_of_clothoid(state.x, state.y, state.theta, state.kappa, sigma, d, integration_step, &state_next.x,
                    &state_next.y, &state_next.theta, &state_next.kappa);
  }
  else
  {
    if (fabs(state.kappa) > get_epsilon())
    {
      end_of_circular_arc(state.x, state.y, state.theta, state.kappa, d, integration_step, &state_next.x, &state_next.y,
                          &state_next.theta);
    }
    else
    {
      end_of_straight_line(state.x, state.y, state.theta, d, integration_step, &state_next.x, &state_next.y);
    }
  }
  state_next.theta =
      pify(state.theta + state.kappa * d * integration_step + 0.5 * sigma * d * integration_step * integration_step);
  state_next.kappa = state.kappa + sigma * integration_step;
  state_next.d = d;

  return state_next;
}

Matrix3d get_num_motion_jacobi_x(const State &state, const Control &control, double integration_step)
{
  Matrix3d F_x;
  Matrix3d perturb(EPS_PERTURB * Matrix3d::Identity());

  State f_x = integrate_ODE(state, control, integration_step);
  for (int i = 0; i < F_x.cols(); ++i)
  {
    // perturb x, y, theta
    State state_perturb;
    state_perturb.x = state.x + perturb(0, i);
    state_perturb.y = state.y + perturb(1, i);
    state_perturb.theta = state.theta + perturb(2, i);
    state_perturb.kappa = state.kappa;

    State f_x_perturb = integrate_ODE(state_perturb, control, integration_step);
    F_x(0, i) = (f_x_perturb.x - f_x.x) / perturb(i, i);
    F_x(1, i) = (f_x_perturb.y - f_x.y) / perturb(i, i);
    F_x(2, i) = (f_x_perturb.theta - f_x.theta) / perturb(i, i);
  }
  return F_x;
}

Matrix32d get_num_motion_jacobi_u(const State &state, const Control &control, double integration_step)
{
  Matrix32d F_u;
  Matrix2d perturb(EPS_PERTURB * Matrix2d::Identity());

  State f_u = integrate_ODE(state, control, integration_step);
  for (int i = 0; i < F_u.cols(); ++i)
  {
    // perturb delta_s and kappa
    Control control_perturb;
    control_perturb.delta_s = control.delta_s + perturb(0, i);
    control_perturb.sigma = control.sigma;
    double integration_step_perturb = fabs(control_perturb.delta_s);

    State state_perturb;
    state_perturb.x = state.x;
    state_perturb.y = state.y;
    state_perturb.theta = state.theta;
    state_perturb.kappa = state.kappa + perturb(1, i);

    State f_u_perturb = integrate_ODE(state_perturb, control_perturb, integration_step_perturb);
    F_u(0, i) = (f_u_perturb.x - f_u.x) / perturb(i, i);
    F_u(1, i) = (f_u_perturb.y - f_u.y) / perturb(i, i);
    F_u(2, i) = (f_u_perturb.theta - f_u.theta) / perturb(i, i);
  }
  return F_u;
}

TEST(Jacobian, F_x)
{
  EKF ekf;
  for (int i = 0; i < SAMPLES; i++)
  {
    State state = get_random_state();
    Control control = get_random_control();

    state.kappa = random_boolean() * state.kappa;
    control.sigma = random_boolean() * control.sigma;
    double integration_step = fabs(control.delta_s);

    Matrix3d F_x_ana(Matrix3d::Zero());
    Matrix32d F_u_ana(Matrix32d::Zero());
    ekf.get_motion_jacobi(state, control, integration_step, F_x_ana, F_u_ana);
    Matrix3d F_x_num = get_num_motion_jacobi_x(state, control, integration_step);
    for (int i = 0; i < F_x_ana.rows(); ++i)
    {
      for (int j = 0; j < F_x_ana.cols(); ++j)
      {
        EXPECT_LE(fabs(F_x_ana(i, j) - F_x_num(i, j)), EPS_JACOBI);
      }
    }
  }
}

TEST(Jacobian, F_u)
{
  EKF ekf;
  for (int i = 0; i < SAMPLES; i++)
  {
    State state = get_random_state();
    Control control = get_random_control();

    state.kappa = random_boolean() * state.kappa;
    control.sigma = random_boolean() * control.sigma;
    double integration_step = fabs(control.delta_s);

    Matrix3d F_x_ana(Matrix3d::Zero());
    Matrix32d F_u_ana(Matrix32d::Zero());
    ekf.get_motion_jacobi(state, control, integration_step, F_x_ana, F_u_ana);
    Matrix32d F_u_num = get_num_motion_jacobi_u(state, control, integration_step);
    for (int i = 0; i < F_u_ana.rows(); ++i)
    {
      for (int j = 0; j < F_u_ana.cols(); ++j)
      {
        EXPECT_LE(fabs(F_u_ana(i, j) - F_u_num(i, j)), EPS_JACOBI);
      }
    }
  }
}

int main(int argc, char **argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


//
//! Copyright © 2017
//! Brandon Kohn
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef GEOMETRIX_ARITHMETIC_MATRIX_TRACE_HPP
#define GEOMETRIX_ARITHMETIC_MATRIX_TRACE_HPP

#include <geometrix/tensor/matrix_traits.hpp>
#include <boost/concept/assert.hpp>

namespace geometrix {

    namespace detail{

        template<std::size_t I, typename Sum, typename Matrix>
        struct trace_sum
        {
        private:
            using next_t = trace_sum<I-1, decltype(std::declval<typename type_at<Matrix, I-1, I-1>::type>() + std::declval<Sum>()), Matrix>;
        public:

            using type = typename next_t::type;

            static type apply(const Matrix& m, const Sum& s)
            {
                return next_t::apply(m, s + get<I-1,I-1>(m));
            }
        };

        template<typename Sum, typename Matrix>
        struct trace_sum<0, Sum, Matrix>
        {
            using type = decltype(std::declval<typename type_at<Matrix, 0, 0>::type>() + std::declval<Sum>());

            static type apply(const Matrix&, const Sum& s)
            {
                return s;
            }
        };
    }//! namespace detail;

    //! Trace of a Matrix - Sum of the diagonals.
    namespace result_of {

        template <typename Matrix>
        struct trace
            : ::geometrix::detail::trace_sum
              <
                geometric_traits<Matrix>::row_dimension::value - 2
              , typename type_at<Matrix, geometric_traits<Matrix>::row_dimension::value - 1, geometric_traits<Matrix>::row_dimension::value - 1>::type
              , Matrix
              >
        {};

    }//! namespace result_of;

    template <typename Matrix>
    inline typename result_of::trace<Matrix>::type trace(const Matrix& m)
    {
        BOOST_CONCEPT_ASSERT(( SquareMatrixConcept<Matrix> ));
        auto v = get<geometric_traits<Matrix>::row_dimension::value - 1,geometric_traits<Matrix>::row_dimension::value - 1>(m);
        return detail::trace_sum<geometric_traits<Matrix>::row_dimension::value - 1, decltype(v), Matrix>::apply(m, v);
    }

}//namespace geometrix;

#endif//GEOMETRIX_ARITHMETIC_MATRIX_TRACE_HPP


#include "plane_fitting.h"

#include <Eigen/Dense>
#include "util/mirrored_memory.h"

namespace dart {

void fitPlane(float3 & planeNormal,
              float & planeIntercept,
              const float4 * dObsVertMap,
              const float4 * dObsNormMap,
              const int width,
              const int height,
              const float distanceThreshold,
              const float normalThreshold,
              const int maxIters,
              const float regularization,
              int * dbgAssociated) {

    dart::MirroredVector<float> result(4 + 16 + 1);

    for (int iter=0; iter<maxIters; ++iter) {

        cudaMemset(result.devicePtr(),0,(1+4+16)*sizeof(float));

        fitPlaneIter(planeNormal,
                     planeIntercept,
                     dObsVertMap,
                     dObsNormMap,
                     width,
                     height,
                     distanceThreshold,
                     normalThreshold,
                     regularization,
                     result.devicePtr(),
                     dbgAssociated);

        result.syncDeviceToHost();

        Eigen::MatrixXf JTJ = regularization*Eigen::MatrixXf::Identity(4,4);
        Eigen::VectorXf eJ = Eigen::VectorXf::Zero(4,1);

        for (int i=0; i<4; ++i) {
            eJ(i) = result.hostPtr()[i];
            for(int j=0; j<4; ++j) {
                JTJ(i,j) += result.hostPtr()[4 + j + i*4];
            }
        }

        //std::cout << "JTJ: \n" << JTJ << std::endl;
        //std::cout << "eJ: \n" << eJ << std::endl;

        Eigen::VectorXf update = -JTJ.ldlt().solve(eJ);

        //std::cout << "error: " << result.hostPtr()[4+16] << std::endl;
        //std::cout << "update: " << update << std::endl;

        planeNormal.x += update(0);
        planeNormal.y += update(1);
        planeNormal.z += update(2);
        planeIntercept += update(3);

        planeNormal = normalize(planeNormal);

        //std::cout << "new normal " << planeNormal.x << ", " << planeNormal.y << ", " << planeNormal.z << std::endl;
        //std::cout << "new intercept " << planeIntercept << std::endl;
    }

}

}


#pragma once

#include "../typedstateobserver.hh"
#include "../../StateObject/HardwareState/hardwarestate.hh"

#include <Eigen/Core>
#include <Eigen/Geometry>

namespace bold
{
  enum class OrientationTechnique
  {
    Madgwick = 0,
    Sum = 1
  };

  template<typename> class Setting;;

  /** Tracks the orientation of the torso using data from the IMU.
   */
  class OrientationTracker : public TypedStateObserver<HardwareState>
  {
  public:
    OrientationTracker();

    Eigen::Quaterniond getQuaternion() const;

    void reset();

  private:
    void observeTyped(std::shared_ptr<HardwareState const> const& state, SequentialTimer& timer) override;

    void updateMadgwick(std::shared_ptr<HardwareState const> const& state);

    void updateSum(std::shared_ptr<HardwareState const> const& state);

    // estimated orientation quaternion elements
    float SEq_1, SEq_2, SEq_3, SEq_4;
    Setting<OrientationTechnique>* d_technique;
  };
}