// This example consists of a single constant velocity target which
// moves under piecewise constant velocity in 3D. Its position is
// measured by an idealised GPS receiver.

#include <Eigen/StdVector>
#include <iostream>

#include <stdint.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/solvers/pcg/linear_solver_pcg.h>
#include <g2o/stuff/sampler.h>

#include "targetTypes6D.hpp"
#include "continuous_to_discrete.h"

using namespace Eigen;
using namespace std;
using namespace g2o;

int main()
{
  // Set up the parameters of the simulation
  int numberOfTimeSteps = 1000;
  const double processNoiseSigma = 1;
  const double accelerometerNoiseSigma = 1;
  const double gpsNoiseSigma = 1;
  const double dt = 1;

  // Set up the optimiser and block solver
  SparseOptimizer optimizer;
  optimizer.setVerbose(false);

  typedef BlockSolver< BlockSolverTraits<6, 6> > BlockSolver;
  BlockSolver::LinearSolverType * linearSolver
      = new LinearSolverCholmod<BlockSolver::PoseMatrixType>();
  BlockSolver* blockSolver = new BlockSolver(linearSolver);
  OptimizationAlgorithm* optimizationAlgorithm = new OptimizationAlgorithmGaussNewton(blockSolver);
  optimizer.setAlgorithm(optimizationAlgorithm);

  // Sample the start location of the target
  Vector6d state;
  state.setZero();
  for (int k = 0; k < 3; k++)
    {
      state[k] = 1000 * sampleGaussian();
    }

  // Construct the first vertex; this corresponds to the initial
  // condition and register it with the optimiser
  VertexPositionVelocity3D* stateNode = new VertexPositionVelocity3D();
  stateNode->setEstimate(state);
  stateNode->setId(0);
  optimizer.addVertex(stateNode);

  // Set up last estimate
  VertexPositionVelocity3D* lastStateNode = stateNode;

  // Iterate over the simulation steps
  for (int k = 1; k <= numberOfTimeSteps; ++k)
    {
      // Simulate the next step; update the state and compute the observation
      Vector3d processNoise(processNoiseSigma*sampleGaussian(),
                            processNoiseSigma*sampleGaussian(),
                            processNoiseSigma*sampleGaussian());

      for (int m = 0; m < 3; m++)
        {
          state[m] += dt * (state[m+3] + 0.5 * dt * processNoise[m]);
        }

      for (int m = 0; m < 3; m++)
        {
          state[m+3] += dt * processNoise[m];
        }

      // Construct the accelerometer measurement
      Vector3d accelerometerMeasurement;
      for (int m = 0; m < 3; m++)
        {
          accelerometerMeasurement[m] = processNoise[m] + accelerometerNoiseSigma * sampleGaussian();
        }

      // Construct the GPS observation
      Vector3d gpsMeasurement;
      for (int m = 0; m < 3; m++)
        {
          gpsMeasurement[m] = state[m] + gpsNoiseSigma * sampleGaussian();
        }

      // Construct vertex which corresponds to the current state of the target
      VertexPositionVelocity3D* stateNode = new VertexPositionVelocity3D();

      stateNode->setId(k);
      stateNode->setMarginalized(false);
      optimizer.addVertex(stateNode);

      TargetOdometry3DEdge* toe = new TargetOdometry3DEdge(dt, accelerometerNoiseSigma);
      toe->setVertex(0, lastStateNode);
      toe->setVertex(1, stateNode);
      VertexPositionVelocity3D* vPrev= dynamic_cast<VertexPositionVelocity3D*>(lastStateNode);
      VertexPositionVelocity3D* vCurr= dynamic_cast<VertexPositionVelocity3D*>(stateNode);
      toe->setMeasurement(accelerometerMeasurement);
      optimizer.addEdge(toe);

      // compute the initial guess via the odometry
      g2o::OptimizableGraph::VertexSet vPrevSet;
      vPrevSet.insert(vPrev);
      toe->initialEstimate(vPrevSet,vCurr);

      lastStateNode = stateNode;

      // Add the GPS observation
      GPSObservationEdgePositionVelocity3D* goe = new GPSObservationEdgePositionVelocity3D(gpsMeasurement, gpsNoiseSigma);
      goe->setVertex(0, stateNode);
      optimizer.addEdge(goe);
    }

  // Configure and set things going
  optimizer.initializeOptimization();
  optimizer.setVerbose(true);
  optimizer.optimize(5);
  cerr << "number of vertices:" << optimizer.vertices().size() << endl;
  cerr << "number of edges:" << optimizer.edges().size() << endl;

  // Print the results

  cout << "state=\n" << state << endl;

#if 0
  for (int k = 0; k < numberOfTimeSteps; k++)
    {
      cout << "computed estimate " << k << "\n"
           << dynamic_cast<VertexPositionVelocity3D*>(optimizer.vertices().find(k)->second)->estimate() << endl;
       }
#endif

  Vector6d v1 = dynamic_cast<VertexPositionVelocity3D*>(optimizer.vertices().find((std::max)(numberOfTimeSteps-2,0))->second)->estimate();
  Vector6d v2 = dynamic_cast<VertexPositionVelocity3D*>(optimizer.vertices().find((std::max)(numberOfTimeSteps-1,0))->second)->estimate();
  cout << "v1=\n" << v1 << endl;
  cout << "v2=\n" << v2 << endl;
  cout << "delta state=\n" << v2-v1 << endl;
}


#ifndef BURST_BENCHMARK_UTILITY_RANDOM_URD_ORDER_STATISTIC_DISTRIBUTION_HPP
#define BURST_BENCHMARK_UTILITY_RANDOM_URD_ORDER_STATISTIC_DISTRIBUTION_HPP

#include <boost/math/special_functions/beta.hpp>

#include <cstddef>
#include <random>
#include <tuple>

namespace utility
{
    template <typename RealType>
    class urd_order_statistic_distribution;

    template <typename RealType>
    class urd_order_statistic_distribution_param
    {
    public:
        using distribution_type = urd_order_statistic_distribution<RealType>;

        urd_order_statistic_distribution_param
        (
            std::size_t n,
            std::size_t k,
            RealType a,
            RealType b
        ):
            m_n(n),
            m_k(k),
            m_a(a),
            m_b(b)
        {
        }

        std::size_t n () const
        {
            return m_n;
        }

        std::size_t k () const
        {
            return m_k;
        }

        RealType a () const
        {
            return m_a;
        }

        RealType b () const
        {
            return m_b;
        }

        friend bool
            operator ==
            (
                const urd_order_statistic_distribution_param & left,
                const urd_order_statistic_distribution_param & right
            )
        {
            return
                std::tie(left.m_n, left.m_k, left.m_a, left.m_b) ==
                std::tie(right.m_n, right.m_k, right.m_a, right.m_b);
        }

        friend bool
            operator !=
            (
                const urd_order_statistic_distribution_param & left,
                const urd_order_statistic_distribution_param & right
            )
        {
            return !(left == right);
        }

    private:
        std::size_t m_n;
        std::size_t m_k;
        RealType m_a;
        RealType m_b;
    };

    /*!
        \brief
            Распределение порядковых статистик выборки из стандартного непрерывного равномерного
            распределения.

        \details
            "urd" означает "uniform random distribution".
            https://ru.wikipedia.org/wiki/Порядковая_статистика#Пример
    */
    template <typename RealType = double>
    class urd_order_statistic_distribution
    {
    public:
        using result_type = RealType;
        using param_type = urd_order_statistic_distribution_param<result_type>;

        /*!
            \brief
                Создание распределения с параметрами

            \param n
                Размер выборки.
            \param k
                Номер порядковой статистики.
            \param a
                Минимальное значение равномерного распределения.
            \param b
                Максимальное значение равномерного распределения.
         */
        urd_order_statistic_distribution
        (
            std::size_t n,
            std::size_t k,
            result_type a,
            result_type b
        ):
            m_param(n, k, a, b)
        {
        }

        explicit urd_order_statistic_distribution (const param_type & p):
            m_param(p)
        {
        }

        void reset ()
        {
        }

        template <typename URNG>
        result_type operator () (URNG & g)
        {
            return (*this)(g, m_param);
        }

        template <typename URNG>
        result_type operator () (URNG & g, const param_type & p)
        {
            auto ibeta = m_uniform(g);
            auto beta = boost::math::ibeta_inv(p.k(), p.n() - p.k() + 1, ibeta);
            return beta * (p.b() - p.a()) + p.a();
        }

        std::size_t n () const
        {
            return m_param.n();
        }

        std::size_t k () const
        {
            return m_param.k();
        }

        result_type a () const
        {
            return m_param.a();
        }

        result_type b () const
        {
            return m_param.b();
        }

        param_type param () const
        {
            return m_param;
        }

        void param (const param_type & p)
        {
            m_param = p;
        }

        result_type min () const
        {
            return a();
        }

        result_type max () const
        {
            return b();
        }

        friend bool
            operator ==
            (
                const urd_order_statistic_distribution & left,
                const urd_order_statistic_distribution& right
            )
        {
            return left.m_param == right.m_param;
        }

        friend bool
            operator !=
            (
                const urd_order_statistic_distribution & left,
                const urd_order_statistic_distribution& right
            )
        {
            return !(left == right);
        }

    private:
        using uniform_real_distribution_type = std::uniform_real_distribution<result_type>;

        param_type m_param;
        uniform_real_distribution_type m_uniform{0, 1};
    };
}

#endif // BURST_BENCHMARK_UTILITY_RANDOM_URD_ORDER_STATISTIC_DISTRIBUTION_HPP


#include <iostream>
#include <armadillo>

using namespace std;
using namespace arma;

int main()
{
    /* Constructor */

    arma::mat input;
    /*
    input << 1 << 446 << 42 << arma::endr
          << 2 <<  16 << 63 << arma::endr
          << 3 <<  13 << 63 << arma::endr
          << 4 <<  21 << 21 << arma::endr
          << 1 <<  13 << 11 << arma::endr
          << 32 << 45 << 42 << arma::endr
          << 22 << 16 << 63 << arma::endr
          << 32 << 13 << 42 << arma::endr;
    */
    input << 1  << 19  << arma::endr
          << 2  << 20  << arma::endr
          << 3  << 21  << arma::endr
          << 4  << 22  << arma::endr
          << 5  << 23  << arma:: endr
          << 6  << 24  << arma::endr
          << 7  << 25  << arma::endr
          << 8  << 26  << arma:: endr
          << 9  << 27  << arma::endr
          << 10 << 28  << arma::endr
          << 11 << 29  << arma:: endr
          << 12 << 30  << arma::endr
          << 13 << 31  << arma::endr
          << 14 << 32  << arma:: endr
          << 15 << 33  << arma::endr
          << 16 << 34  << arma::endr
          << 17 << 35  << arma:: endr
          << 18 << 36  << arma::endr;
    cout << "-----------------------------------" << endl;
    cout << "Input shape : " << input.n_rows << " " << input.n_cols << endl;
    cout << "-----------------------------------" << endl;

    const size_t size = 3; // number of channels
    const double eps = 1e-5;
    const double momentum = 0.1;
    arma::mat weights, runningMean, runningVariance, gamma, beta;
    weights.set_size(size + size, 1); // (size + size, 1)
    runningMean.zeros(size, 1); // (size, 1)
    runningVariance.ones(size, 1); // (size, 1)

    /* Reset() */

    gamma = arma::mat(weights.memptr(), size, 1, false, false);  // (size, 1)
    beta = arma::mat(weights.memptr() + gamma.n_elem, size, 1, false, false); // (size, 1)
    gamma.fill(1.0);
    beta.fill(0.0);

    /* Forward */

    // Step-0 : Preparation of temporary cubes for input and output
    const size_t batchSize = input.n_cols;
    const size_t inputSize = input.n_rows / size; // (inputWidth * inputHeight)
    arma::mat output;
    output.set_size(arma::size(input));
    arma::cube inputTemp(const_cast<arma::mat&>(input).memptr(),input.n_rows / size, size, batchSize, false, false);
    arma::cube outputTemp(const_cast<arma::mat&>(output).memptr(),input.n_rows / size, size, input.n_cols, false, false);
    outputTemp = inputTemp; // n_rows = inputSize, n_cols = size, n_slices = batchSize // (4, 2, 3)

    cout << "N     : " << batchSize << endl;
    cout << "C     : " << size << endl;
    cout << "H x W : " << inputSize << endl;
    cout << "-----------------------------------" << endl;
    cout << "Input Cube - " << endl << "each slice is an image of the batch of "<< batchSize << " images" << endl << "each column of a slice is one of the " << size << " channels of the image, HxW is flattened into this single column" << endl;
    cout << "-----------------------------------" << endl;
    inputTemp.print();
    cout << "-----------------------------------" << endl;

    // PURE FORWARD FOR INSTANCE NORM

    arma::cube mean(1, size, batchSize);
    arma::cube variance(1, size, batchSize);
    for (size_t s = 0; s < inputTemp.n_slices; s++)
    {
        arma::mat& currentInputSlice = inputTemp.slice(s);
        arma::mat& currentOutputSlice = outputTemp.slice(s);

        // Step -1 :  Calculate mean and variance
        mean.slice(s) = arma::mean(currentInputSlice,0);
        variance.slice(s) = arma::var(currentInputSlice, 1, 0);

        // Step 2 : Normalisation
        currentOutputSlice -= arma::repmat(mean.slice(s), input.n_rows / size, 1);
        currentOutputSlice /= arma::sqrt(arma::repmat(variance.slice(s), input.n_rows / size, 1) + eps);

        // Step 3 : Scaling
        currentOutputSlice %= arma::repmat(gamma.t(), input.n_rows / size, 1);
        currentOutputSlice += arma::repmat(beta.t(), input.n_rows / size, 1);
    }
    cout << "Input Mean : " << endl;
    cout << mean << endl;
    cout << "-----------------------------------" << endl;
    cout << "Input Variance : " << endl;
    cout << variance << endl;
    cout << "-----------------------------------" << endl;
    cout << "Output Cube - " << endl << "each slice is an image of the batch of "<< batchSize << " images" << endl << "each column of a slice is one of the " << size << " channels of the image, HxW is flattened into this single column" << endl;
    cout << "-----------------------------------" << endl;
    outputTemp.print();
    cout << "-----------------------------------" << endl;

    cout << "Output shape : " << output.n_rows << " " << output.n_cols << endl;
    cout << "-----------------------------------" << endl;

    return 0;
}


#ifndef STAN_MATH_FWD_SCAL_FUN_INV_SQRT_HPP
#define STAN_MATH_FWD_SCAL_FUN_INV_SQRT_HPP

#include <stan/math/fwd/core.hpp>

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

namespace stan {
  namespace math {

    template <typename T>
    inline
    fvar<T>
    inv_sqrt(const fvar<T>& x) {
      using std::sqrt;
      T sqrt_x(sqrt(x.val_));
      return fvar<T>(1 / sqrt_x, -0.5 * x.d_ / (x.val_ * sqrt_x));
    }
  }
}
#endif


// TMA_calculator.cpp : This file contains the 'main' function. Program execution begins and ends there.
//

#include "pch.h"
#include <iostream>
#include <math.h>
#include <vector>
#include <thread>
#include <dlib/matrix.h>
#include <dlib/optimization.h>
#include <dlib/global_optimization.h>
#include <wchar.h>
#include <locale.h>
#include <io.h>
#include <cstdio>
#include <cwchar>
#include <fcntl.h>

using namespace std;

vector<double> bearing = vector<double>(20, 0.0); //target bearing
vector<double> bearing_noisy = vector<double>(20, 0.0); //target bearing with random inaccuracies
vector<double> recording_time = vector<double>(20, 0.0);

//ownship coordinate (m,n)
vector<double> m = vector<double>(20, 0.0);
vector<double> n = vector<double>(20, 0.0);

unsigned int _j = 2; //iteration index
double own_ship_hdg = 0.0; //ownship heading
double last_travel_distance = 0.0; //ownship travel straight distance from last position
double last_travel_direction = 0.0; //ownship travel straight direction from last position

typedef dlib::matrix<double, 0, 1> column_vector;

// Simple upper and lower limiter
double limit(double input, double lower_limit, double upper_limit)
{
    if (input > upper_limit)
    {
        return upper_limit;
    }
    else if (input < lower_limit)
    {
        return lower_limit;
    }
    else
    {
        return input;
    }
}

vector<double> calculate_last_brg(const double &L1_distance, const double &spd, const double &crs)
{
    vector<double> out = vector<double>(3, 0.0);
    double u = spd * sin(crs * deg_to_rad); //target speed component on x axis
    double v = spd * cos(crs * deg_to_rad); //target speed component on y axis

    //target coordinate (a,b) at first bearing
    double a = L1_distance * sin(bearing[0] * deg_to_rad); //L1_distance = target distance at first bearing
    double b = L1_distance * cos(bearing[0] * deg_to_rad);

    //target coordinate (x,y) at last bearing
    double x = a + u * recording_time[_j];
    double y = b + v * recording_time[_j];

    out[0] = x;
    out[1] = y;
    out[2] = sqrt(pow(y - n[_j], 2) + pow(x - m[_j], 2));

    return out;
}

vector<double> calculate_last_brg_noisy(const double &L1_distance, const double &spd, const double &crs)
{
    vector<double> out = vector<double>(3, 0.0);
    double u = spd * sin(crs * deg_to_rad); //target speed component on x axis
    double v = spd * cos(crs * deg_to_rad); //target speed component on y axis

    //target coordinate (a,b) at first bearing
    double a = L1_distance * sin(bearing_noisy[0] * deg_to_rad); //L1_distance = target distance at first bearing
    double b = L1_distance * cos(bearing_noisy[0] * deg_to_rad);

    //target coordinate (x,y) at last bearing
    double x = a + u * recording_time[_j];
    double y = b + v * recording_time[_j];

    out[0] = x;
    out[1] = y;
    out[2] = sqrt(pow(y - n[_j], 2) + pow(x - m[_j], 2));

    return out;
}

int main()
{
    _setmode(_fileno(stdout), _O_U16TEXT); //support for chinese characters

    double optimize_L1_distance;
    double optimize_spd;
    double optimize_current_distance;
    double optimize_x;
    double optimize_y;

    //target function to minimize using BFGS algorithm
    auto target_function = [](const column_vector& mStartingPoint)
    {
        const double L1_distance = mStartingPoint(0);
        const double spd = mStartingPoint(1);
        const double crs = mStartingPoint(2);

        double u = spd * sin(crs * deg_to_rad);
        double v = spd * cos(crs * deg_to_rad);
        double a = L1_distance * sin(bearing[0] * deg_to_rad);
        double b = L1_distance * cos(bearing[0] * deg_to_rad);

        double total_error = 0.0;

        for (unsigned int i = 0; i < _j + 1; i++)
        {
            double x = a + u * recording_time[i];
            double y = b + v * recording_time[i];
            double line_error = (y - n[i]) * sin(bearing[i] * deg_to_rad) - (x - m[i]) * cos(bearing[i] * deg_to_rad);
            total_error += pow(line_error, 2);
        }

        //double penalty_for_spd = pow(limit(0.5 - spd, 0.0, 999999.0) * 100.0, 2) + pow(limit(spd - 6.0, 0.0, 999999.0) * 100.0, 2); //set speed limit: from 0.5 to 6.0 m/s
        //double penalty_for_range = pow(limit(500 - L1_distance, 0.0, 999999.0) * 0.1, 2) + pow(limit(L1_distance - 10000.0, 0.0, 999999.0) * 0.1, 2); //set limit for target distance at t1: from 500m to 10km

        return total_error;
    };

    auto target_function_noisy = [](const column_vector& mStartingPoint)
    {
        const double L1_distance = mStartingPoint(0);
        const double spd = mStartingPoint(1);
        const double crs = mStartingPoint(2);

        double u = spd * sin(crs * deg_to_rad);
        double v = spd * cos(crs * deg_to_rad);
        double a = L1_distance * sin(bearing_noisy[0] * deg_to_rad);
        double b = L1_distance * cos(bearing_noisy[0] * deg_to_rad);

        double total_error = 0.0;

        for (unsigned int i = 0; i < _j + 1; i++)
        {
            double x = a + u * recording_time[i];
            double y = b + v * recording_time[i];
            double line_error = (y - n[i]) * sin(bearing_noisy[i] * deg_to_rad) - (x - m[i]) * cos(bearing_noisy[i] * deg_to_rad);
            total_error += pow(line_error, 2);
        }
        return total_error;
    };

#ifdef _CHINESE
    std::wcout << L"说明：本舰自上一次所在位置的移动方向 = 从上一个观测点指向当前位置的绝对方位，可在海图中对两个观测点连线获得。v0.3版本加入误差分布计算，对观测到的方位角施加-0.5到0.5度范围内的随机误差，并进行1000次循环计算误差分布概率。" << endl;
    std::wcout << endl;

    std::wcout << L"时间点time t1 = 0 (sec)" << endl;

    std::wcout << L"本舰航向 (deg): ";
    std::cin >> own_ship_hdg;
    std::wcout << endl;

    std::wcout << L"敌舰相对方位角 (deg): ";
    std::cin >> bearing[0];
    std::wcout << endl;
    bearing[0] += own_ship_hdg;

    std::wcout << L"本舰自上一次所在位置的移动方向 = 0 (deg)" << endl;
    std::wcout << L"本舰距上一次所在位置的直线距离 = 0 (meter)" << endl;

    std::wcout << L"*******************************" << endl;

    std::wcout << L"时间点time t2 (sec): ";
    std::cin >> recording_time[1];
    std::wcout << endl;

    std::wcout << L"本舰航向 (deg): ";
    std::cin >> own_ship_hdg;
    std::wcout << endl;

    std::wcout << L"敌舰相对方位角 (deg): ";
    std::cin >> bearing[1];
    std::wcout << endl;
    bearing[1] += own_ship_hdg;

    std::wcout << L"本舰自上一次所在位置的移动方向 (deg): ";
    std::cin >> last_travel_direction;
    std::wcout << endl;

    std::wcout << L"本舰距上一次所在位置的直线距离 (meter): ";
    std::cin >> last_travel_distance;
    std::wcout << endl;
#else
    std::wcout << "time t1 = 0 (sec)" << endl;

    std::wcout << "ownship heading at t1 (deg): ";
    std::cin >> own_ship_hdg;
    std::wcout << endl;

    std::wcout << "target relative bearing at t1 (deg): ";
    std::cin >> bearing[0];
    std::wcout << endl;
    bearing[0] += own_ship_hdg;

    std::wcout << "true bearing from ownship last observation position to current position = 0 (deg)" << endl;
    std::wcout << "straight distance from ownship last observation position to current position = 0 (meter)" << endl;

    std::wcout << "*******************************" << endl;

    std::wcout << "time t2 (sec): ";
    std::cin >> recording_time[1];
    std::wcout << endl;

    std::wcout << "ownship heading at t2: ";
    std::cin >> own_ship_hdg;
    std::wcout << endl;

    std::wcout << "target relative bearing at t2 (deg): ";
    std::cin >> bearing[1];
    std::wcout << endl;
    bearing[1] += own_ship_hdg;

    std::wcout << "true bearing from ownship last observation position to current position (deg): ";
    std::cin >> last_travel_direction;
    std::wcout << endl;

    std::wcout << "straight distance from ownship last observation position to current position (meter): ";
    std::cin >> last_travel_distance;
    std::wcout << endl;
#endif

    m[1] = last_travel_distance * sin(last_travel_direction * deg_to_rad);
    n[1] = last_travel_distance * cos(last_travel_direction * deg_to_rad);

    //start input iteration
    for (unsigned int j = 2; j < 20; j++)
    {
        _j = j;

#ifdef _CHINESE
        std::wcout << L"*******************************" << endl;

        std::wcout << L"时间点time t" << j + 1 << L" (sec): ";
        std::cin >> recording_time[j];
        std::wcout << endl;

        std::wcout << L"本舰航向 (deg): ";
        std::cin >> own_ship_hdg;
        std::wcout << endl;

        std::wcout << L"敌舰相对方位角 (deg): ";
        std::cin >> bearing[j];
        std::wcout << endl;
        bearing[j] += own_ship_hdg;

        std::wcout << L"本舰自上一次所在位置的移动方向 (deg): ";
        std::cin >> last_travel_direction;
        std::wcout << endl;

        std::wcout << L"本舰距上一次所在位置的直线距离 (meter): ";
        std::cin >> last_travel_distance;
        std::wcout << endl;
#else
        std::wcout << "*******************************" << endl;

        std::wcout << "time t" << j + 1 << " (sec): ";
        std::cin >> recording_time[j];
        std::wcout << endl;

        std::wcout << "ownship heading at t" << j + 1 << ": ";
        std::cin >> own_ship_hdg;
        std::wcout << endl;

        std::wcout << "target relative bearing at t" << j + 1 << " (deg): ";
        std::cin >> bearing[j];
        std::wcout << endl;
        bearing[j] += own_ship_hdg;

        std::wcout << "true bearing from ownship last observation position to current position (deg): ";
        std::cin >> last_travel_direction;
        std::wcout << endl;

        std::wcout << "straight distance from ownship last observation position to current position (meter): ";
        std::cin >> last_travel_distance;
        std::wcout << endl;
#endif

        m[j] = m[j - 1] + last_travel_distance * sin(last_travel_direction * deg_to_rad);
        n[j] = n[j - 1] + last_travel_distance * cos(last_travel_direction * deg_to_rad);

        std::wcout << endl;
        column_vector starting_point = { 1000.0,1.0,0.0 };
        vector<double> optimal_crs;

        //multiple starting point for BFGS algorithm to find for multiple local minimal. (We need to keep all possible results for TMA)

        for (double L1_distance = 1000.0; L1_distance <= 10000.0; L1_distance += 500.0)
        {
            for (double spd = 1.0; spd < 10.0; spd += 2.0)
            {
                for (double crs = 0.0; crs <= 360.0; crs += 60.0)
                {
                    starting_point = { L1_distance,spd,crs };

                    dlib::find_min_using_approximate_derivatives(dlib::bfgs_search_strategy(), dlib::objective_delta_stop_strategy(1e-7), target_function, starting_point, -1);
                    vector<double> last_brg = calculate_last_brg(starting_point(0), starting_point(1), starting_point(2));

                    //adjust course result within 0-360 range
                    while (starting_point(2) < 0)
                    {
                        starting_point(2) += 360;
                    }

                    while (starting_point(2) >= 360)
                    {
                        starting_point(2) -= 360;
                    }

                    starting_point(2) = round(starting_point(2) * 100.0) / 100.0;

                    if (starting_point(0) > 1.0 && starting_point(1) > 0.0 && find(optimal_crs.begin(), optimal_crs.end(), starting_point(2)) == optimal_crs.end()) {
                        optimal_crs.push_back(starting_point(2));
                        optimize_L1_distance = starting_point(0);
                        optimize_spd = starting_point(1);
                        optimize_current_distance = last_brg[2];
                        optimize_x = last_brg[0];
                        optimize_y = last_brg[1];

                        if (abs(m[j]) < 0.1 && abs(n[j]) < 0.1)
                        {
#ifdef _CHINESE
                            std::wcout << L"敌舰航向target true course: " << starting_point(2) << L"deg" << endl;
#else
                            std::wcout << "target true course: " << starting_point(2) << "deg" << endl;
#endif
                        }
                        else
                        {
#ifdef _CHINESE
                            std::wcout << L"敌舰航向target true course: " << starting_point(2) << L"deg, 速度speed: " << optimize_spd * ms_to_kts << L"knots, 距离distance: " << optimize_current_distance << L"m" << endl;
#else
                            std::wcout << "target true course: " << starting_point(2) << "deg, speed: " << optimize_spd * ms_to_kts << "knots, distance: " << optimize_current_distance << "m" << endl;
#endif
                        }
                    }
                }
            }
        }

        //error analysis
        if (optimal_crs.size() > 0)
        {
            double last_opt_crs = optimal_crs[optimal_crs.size() - 1];
            int total_error_count = 0;
            int error_within_75m = 0;
            int error_within_150m = 0;
            int error_within_300m = 0;
            int error_within_5deg = 0;
            int error_within_10deg = 0;
            int error_within_20deg = 0;
            int error_within_05kts = 0;
            int error_within_1kts = 0;
            int error_within_2kts = 0;

            for (double L1_distance = optimize_L1_distance; L1_distance <= optimize_L1_distance + 1.0; L1_distance += 0.1)
            {
                for (double spd = optimize_spd; spd <= optimize_spd + 0.1; spd += 0.01)
                {
                    for (double crs = last_opt_crs; crs <= last_opt_crs + 1.0; crs += 0.1)
                    {
                        starting_point = { L1_distance,spd,crs };

                        for (unsigned int k = 0; k < _j + 1; k++)
                        {
                            bearing_noisy[k] = bearing[k] + (rand() % 11 - 5) / 10.0;
                        }

                        dlib::find_min_using_approximate_derivatives(dlib::bfgs_search_strategy(), dlib::objective_delta_stop_strategy(1e-7), target_function_noisy, starting_point, -1);
                        vector<double> last_brg = calculate_last_brg_noisy(starting_point(0), starting_point(1), starting_point(2));

                        //adjust course result within 0-360 range
                        while (starting_point(2) < 0)
                        {
                            starting_point(2) += 360;
                        }

                        while (starting_point(2) >= 360)
                        {
                            starting_point(2) -= 360;
                        }

                        if (starting_point(0) > 1.0 && starting_point(1) > 0.0) {
                            double last_x = last_brg[0];
                            double last_y = last_brg[1];
                            double distance_error = sqrt(pow(last_x - optimize_x, 2) + pow(last_y - optimize_y, 2));
                            double course_error = min(abs(starting_point(2) - last_opt_crs), 360.0 - abs(starting_point(2) - last_opt_crs));
                            double spd_error = abs(starting_point(1) - optimize_spd);
                            if (distance_error < 75.0)
                            {
                                error_within_75m += 1;
                            }

                            if (distance_error < 150.0)
                            {
                                error_within_150m += 1;
                            }

                            if (distance_error < 300.0)
                            {
                                error_within_300m += 1;
                            }

                            if (course_error < 5.0)
                            {
                                error_within_5deg += 1;
                            }

                            if (course_error < 10.0)
                            {
                                error_within_10deg += 1;
                            }

                            if (course_error < 20.0)
                            {
                                error_within_20deg += 1;
                            }

                            if (spd_error < 0.5)
                            {
                                error_within_05kts += 1;
                            }

                            if (spd_error < 1.0)
                            {
                                error_within_1kts += 1;
                            }

                            if (spd_error < 2.0)
                            {
                                error_within_2kts += 1;
                            }

                            total_error_count += 1;
                        }
                    }
                }
            }
            double error_prob_75m = error_within_75m * 100.0 / total_error_count;
            double error_prob_150m = error_within_150m * 100.0 / total_error_count;
            double error_prob_300m = error_within_300m * 100.0 / total_error_count;
            double error_prob_5deg = error_within_5deg * 100.0 / total_error_count;
            double error_prob_10deg = error_within_10deg * 100.0 / total_error_count;
            double error_prob_20deg = error_within_20deg * 100.0 / total_error_count;
            double error_prob_05kts = error_within_05kts * 100.0 / total_error_count;
            double error_prob_1kts = error_within_1kts * 100.0 / total_error_count;
            double error_prob_2kts = error_within_2kts * 100.0 / total_error_count;
            std::wcout << endl;

#ifdef _CHINESE
            std::wcout << L"航向误差分布位于5度以内的概率probability of target course error within 5deg: " << error_prob_5deg << " %" << endl;
            std::wcout << L"航向误差分布位于10度以内的概率probability of target course error within 10deg: " << error_prob_10deg << " %" << endl;
            std::wcout << L"航向误差分布位于20度以内的概率probability of target course error within 20deg: " << error_prob_20deg << " %" << endl;
#else
            std::wcout << "probability of target course error within 5deg: " << error_prob_5deg << " %" << endl;
            std::wcout << "probability of target course error within 10deg: " << error_prob_10deg << " %" << endl;
            std::wcout << "probability of target course error within 20deg: " << error_prob_20deg << " %" << endl;
#endif
            std::wcout << endl;
            if (abs(m[j]) < 0.1 && abs(n[j]) < 0.1)
            {
                std::wcout << j + 1 << L" bearings when stationary can only get course solution." << endl;
            }
            else
            {
#ifdef _CHINESE
                std::wcout << L"位置误差分布位于75米以内的概率probability of target positional error within 75m: " << error_prob_75m << " %" << endl;
                std::wcout << L"位置误差分布位于150米以内的概率probability of target positional error within 150m: " << error_prob_150m << " %" << endl;
                std::wcout << L"位置误差分布位于300米以内的概率probability of target positional error within 300m: " << error_prob_300m << " %" << endl;
                std::wcout << endl;
                std::wcout << "速度误差分布位于0.5节以内的概率probability of target speed error within 0.5kts: " << error_prob_05kts << " %" << endl;
                std::wcout << "速度误差分布位于1节以内的概率probability of target speed error within 1kts: " << error_prob_1kts << " %" << endl;
                std::wcout << "速度误差分布位于2节以内的概率probability of target speed error within 2kts: " << error_prob_2kts << " %" << endl;
#else
                std::wcout << "probability of target positional error within 75m: " << error_prob_75m << " %" << endl;
                std::wcout << "probability of target positional error within 150m: " << error_prob_150m << " %" << endl;
                std::wcout << "probability of target positional error within 300m: " << error_prob_300m << " %" << endl;
                std::wcout << endl;
                std::wcout << "probability of target speed error within 0.5kts: " << error_prob_05kts << " %" << endl;
                std::wcout << "probability of target speed error within 1kts: " << error_prob_1kts << " %" << endl;
                std::wcout << "probability of target speed error within 2kts: " << error_prob_2kts << " %" << endl;
#endif
            }
            std::wcout << endl;
        }
        else
        {
#ifdef _CHINESE
            std::wcout << L"无解！" << endl;
#else
        std::wcout << L"No solution found!" << endl;
#endif
        }
    }
}


#include <iostream>
#include <utility>

#include <boost/math/constants/constants.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

using boost::multiprecision::cpp_bin_float_single;
using boost::multiprecision::cpp_bin_float_double;

typedef boost::multiprecision::cpp_bin_float_single f32;
typedef boost::multiprecision::cpp_bin_float_double f64;

int main(void)
{
#if BUGTEST_PRECISION == 32
    //f32 pi32 = boost::math::constants::pi<f32, boost::math::policies::policy<boost::math::policies::digits2<32> > >();
    f32 pi32 = boost::math::constants::pi<f32>();
    std::cout << pi32 << std::endl;
#elif BUGTEST_PRECISION == 64
    //f64 pi64 = boost::math::constants::pi<f64, boost::math::policies::policy<boost::math::policies::digits2<64> > >();
    f64 pi64 = boost::math::constants::pi<f64>();
    std::cout << pi64 << std::endl;
#else
#error Set BUGTEST_PRECISION to 32 or 64.
#endif
    return 0;
}


/** @file spanning_trees.cc
 * @author David F. Gleich
 * @date 2008-09-29
 * @copyright Stanford University, 2006-2008
 * Implement the BGL spanning tree wrappers.
 */

/** History
 *  2006-04-20: Initial version
 *  2006-11-10: Fixed bug with incorrect number of edges returned,
 *    when the input graph has multiple components.
 *    The nedges output parameter is now set correctly for all algorithms.
 *    Although, it depends on a somewhat dubious "hack" to detect
 *    unused portions of the output iterator.
 *  2007-07-09: Switched to simple_csr_matrix graph type
 *    Switched to kruskal mst from boost mod to fix bug with output iterator
 *  2007-11-16: Added root vertex option to prim's MST
 *  2008-10-01: Changed copy_to_ijval to use mbglIndex instead of int.
 *    Removed old commented regions.
 */

#include "include/matlab_bgl.h"

#include <yasmic/simple_csr_matrix_as_graph.hpp>
#include <yasmic/iterator_utility.hpp>

#include <yasmic/boost_mod/kruskal_min_spanning_tree.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>

#include <vector>

/*template <class Graph, class Edge>
class spanning_tree_insert_iterator
    : public boost::iterator_facade<
        spanning_tree_insert_iterator<Edge>
      , Edge
      , boost::forward_traversal_tag
    >
{
public:
    int* i; int* j; double* val;
    Graph& g;

    spanning_tree_insert_iterator(int* _i, int* _j, double* _val)
        : i(_i), j(_j), val(_val)
    {}

private:
    friend class boost::iterator_core_access;

    void increment()
    {
        i++;
        j++;
        val++;
    }

    bool equal(spanning_tree_insert_iterator const& other)
    {
        return (i == other.i && j == other.j && val == other.val);
    }

    Edge

};*/

template <class Graph, class EdgeWeightPropMap, class Iterator>
mbglIndex copy_to_ijval(Graph& g, EdgeWeightPropMap ewpm, Iterator oi,
                   Iterator oi_end, mbglIndex* i, mbglIndex* j, double* val)
{
    using namespace boost;

    mbglIndex ei;
    for (ei= 0; oi != oi_end; ++oi, ++ei) {
        typename graph_traits<Graph>::edge_descriptor e = *oi;
        i[ei] = source(e,g);
        j[ei] = target(e,g);
        val[ei] = ewpm[e];
    }

    return (ei);
}

int kruskal_mst(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight, /* connectivity params */
    mbglIndex* i, mbglIndex* j, double* val /* tree output */,
    mbglIndex* nedges)
{
    using namespace yasmic;
    using namespace boost;

    // create the graph g
    typedef simple_csr_matrix<mbglIndex,double> crs_weighted_graph;
    crs_weighted_graph g(nverts, nverts, ia[nverts], ia, ja, weight);

    //
    // warning, this code assumes that the default constructor for
    // an edges has source == target, otherwise we will detect
    // incorrect edges in the next step.
    //
    std::vector<graph_traits<crs_weighted_graph>::edge_descriptor>
        oi(nverts-1);

    std::vector<graph_traits<crs_weighted_graph>::edge_descriptor>::iterator
        oi_end = kruskal_minimum_spanning_tree(g,oi.begin());

    //*nedges = nverts-1;
    //*nedges = (int)(oi_end - oi.begin());

    // warning, this code assumes that the default constructor for
    // an edges has source == target, otherwise we will detect
    // incorrect edges in the next step.
    *nedges = copy_to_ijval(g,get(edge_weight,g),
        oi.begin(), oi_end, i, j, val);

    return (0);
}

int prim_mst_rooted(mbglIndex nverts, mbglIndex *ja, mbglIndex *ia,
    double *weight, /* connectivity params */
    mbglIndex* i, mbglIndex* j, double* val, mbglIndex *nedges, /* tree output */
    mbglIndex root /* tree root */)
{
  using namespace yasmic;
  using namespace boost;

  typedef simple_csr_matrix<mbglIndex, double> crs_weighted_graph;
  crs_weighted_graph g(nverts, nverts, ia[nverts], ia, ja, weight);

  std::vector<mbglIndex> pred(nverts);

  prim_minimum_spanning_tree(g, make_iterator_property_map(pred.begin(), get(
      vertex_index, g)), root_vertex(root));

  mbglIndex edge_num = 0;
  for (mbglIndex pi = 0; pi < nverts; pi++) {
    if (pred[pi] == pi) {
      // this edge isn't present
    } else {
      assert(edge_num<nverts-1);

      i[edge_num] = pi;
      j[edge_num] = pred[pi];
      val[edge_num] = 0.0;

      for (mbglIndex k = ia[pred[pi]]; k < ia[pred[pi] + 1]; k++) {
        if (ja[k] == pi) {
          val[edge_num] = weight[k];
          break;
        }
      }

      edge_num++;
    }
  }

  *nedges = edge_num;

  return (0);
}

/**
 * Compute a minimum spanning tree starting from vertex 0 using Prim's algorithm
 */
int prim_mst(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight, /* connectivity params */
    mbglIndex* i, mbglIndex* j, double* val, mbglIndex *nedges /* tree output */)
{
    // for our graph type, calling for a rooted tree with root 0 is identical
    // to calling for the default root.
    return prim_mst_rooted(nverts, ja, ia, weight, i, j, val, nedges, 0);
}


/**
 * This file contains functions for solving stochastic optimization problems provided the user extends the
 * "Parameters" class to update the model underlying the parameters and provides both an objective function and a
 * Jacobian thereof that conforms to the expected format for the optimization algorithm.
 */

#ifndef PROMETHEUS_OPTIMIZATION_HPP
#define PROMETHEUS_OPTIMIZATION_HPP

#include <Eigen/Dense>
#include "stopwatch.hpp"

// zach's-attempt-at-math
namespace zaamath {

    /**
     * template class responsible for maintaining the parameters used during optimization and updating the parameters
     * (and optionally the underlying model of those parameters) once an updated set of parameters are passed in by the
     * optimization algorithm
     * @tparam RowIndexType the number of rows of the parameter vector (number of parameters)
     */
    template <int RowIndexType>
    class Parameters {
    protected:
        Eigen::Matrix<double, RowIndexType, 1> eigen_params_;

    public:
        Parameters(Eigen::Matrix<double, RowIndexType, 1>& eigen_params) : eigen_params_(eigen_params) {

        }

        virtual ~Parameters() = default;

        virtual void update(Eigen::Matrix<double, RowIndexType, 1>& eigen_params) {
            eigen_params_ = eigen_params;
        }

        Eigen::Matrix<double, RowIndexType, 1> eigen_params() {
            return eigen_params_;
        }
    };

    /**
     * This class implements the stochastic gradient descent optimization algorithm known as ADAM (short for Adaptive
     * Moment Estimation) as well as holds meta-data for the algorithm such as termination conditions and
     * hyper-parameters.
     *
     * The class performs inplace optimization, updating the Parameters object passed in by reference by the user every
     * iteration. Additional the class stores member variables about the status of the algorithm at each iteration,
     * which can be read by the client to discover things about the covergence of the algorthim. Therefore, if one seeks
     * to perform more than one optimization with the same Adam instance, one must first reset those member variables
     * with the
     */
    class Adam {
    private:
        bool show_trace_{true};
        int show_trace_every_{15};
        int f_x_tol_interval_{20};
        double f_x_tol_{1e-10};
        int x_tol_interval_{20};
        double x_tol_{1e-12};
        int max_iterations_{10000};
        double alpha_{0.01}; // learning rate / step size
        std::size_t batch_size_{1}; // not sure how to implement
        double beta1_{0.9};
        double beta2_{0.999};
        double epsilon_{1e-8};
        Eigen::Matrix<double, Eigen::Dynamic, 1> f_x_prev_; // don't initialize to any size yet
        Eigen::Matrix<double, Eigen::Dynamic, 1> times_;

        /**
         * implements the termination conditions for the loop in the optimization algorithm, returning true if the
         * process as converged or run out of iterations and false otherwise
         * @tparam RealRowIndexType the number of columns in the parameter vector
         * @param f_x_prev a running series of past error of all the previous iteration of the algorithm
         * @param x_prev the previous "x_tol_interval_" parameter values
         * @param iterations the iteration number that we are currently on
         * @return true if we have reached any termination criterion, false otherwise
         */
        template <int RealRowIndexType>
        bool reached_termination(Eigen::Matrix<double, Eigen::Dynamic, 1>& f_x_prev, Eigen::Matrix<double, Eigen::Dynamic, RealRowIndexType>& x_prev, int iterations) {
            if (iterations >= max_iterations_) {
                std::cout << "Quitting b/c reached max iterations" << std::endl;
                return true;
            }

            if (iterations >= f_x_tol_interval_) {
                Eigen::Matrix<double, Eigen::Dynamic, 1> last_n_items = f_x_prev(Eigen::lastN(f_x_tol_interval_));
                if ((zaamath::range(last_n_items).cwiseAbs().array() < f_x_tol_).all()) {
                    std::cout << "Quitting b/c Error Converged" << std::endl;
                    std::cout << " - Range: " << zaamath::range(last_n_items).cwiseAbs().array() << " < " << f_x_tol_ << std::endl;
                    return true;
                }
            }

            if (iterations >= x_tol_interval_) {
                if ((zaamath::range(x_prev).cwiseAbs().array() < x_tol_).all()) {
                    std::cout << "Quitting b/c Parameters Converged" << std::endl;
                    std::cout << " - Range: " << zaamath::range(x_prev).cwiseAbs().array() << " < " << x_tol_ << std::endl;
                    return true;
                }
            }

            return false;
        }

    public:

        /**
         * clears the member variables having to do with the performance of the optimization at each iteration so that
         * the object may be used again for another optimization.
         */
        void clear() {
//            f_x_prev_ = Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero();
            f_x_prev_.resize(0);
//            times_ = Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero();
            times_.resize(0);
        }

        /**
         * implements the vanilla Adam optimization algorithm
         * @tparam RowIndexType the number of parameters (the number of rows of the input parameter vector)
         * @param initial_params the initial parameter set
         * @param obj_func the objective function
         * @param jacobian the jacobian of the objective function w.r.t. the parameters
         */
        template <int RowIndexType>
        void optimize(Parameters<RowIndexType>& initial_params, std::function<double(Parameters<RowIndexType>&)>& obj_func, std::function<Eigen::Matrix<double, RowIndexType, 1>(Parameters<RowIndexType>&, int)>& jacobian) {

            // initialize to zero
            Eigen::Matrix<double, RowIndexType, 1> mt = Eigen::Matrix<double, RowIndexType, 1>::Zero();
            Eigen::Matrix<double, RowIndexType, 1> vt = Eigen::Matrix<double, RowIndexType, 1>::Zero();
            Eigen::Matrix<double, RowIndexType, 1> mthat = Eigen::Matrix<double, RowIndexType, 1>::Zero();
            Eigen::Matrix<double, RowIndexType, 1> vthat = Eigen::Matrix<double, RowIndexType, 1>::Zero();

            // Grab the last 'x_tol_interval' inputs
            Eigen::Matrix<double, Eigen::Dynamic, RowIndexType> x_prev = Eigen::Matrix<double, Eigen::Dynamic, RowIndexType>::Zero(x_tol_interval_, RowIndexType);
            x_prev.row(0) = initial_params.eigen_params();

            // track number of iterations
            int iterations{0};

            // trace table headers
            std::size_t header_widths[4] = {
                std::string(" Iteration ").size()-1,
                std::string("    Time    ").size()-std::string(" [us] ").size(),
                std::string("   Error   ").size()-1,
                std::string(" Min. Error ").size()-1
            };

            // Grab the stopwatch
            StopWatch stop_watch;

            while (!reached_termination(f_x_prev_, x_prev, iterations)) {
                stop_watch.start();
                Eigen::Matrix<double, RowIndexType, 1> grad = jacobian(initial_params, iterations);
                std::size_t batch{1};
                while (batch < batch_size_) {
                    grad += jacobian(initial_params, iterations + batch);
                    ++batch;
                }

                // update mt and vt
                mt = beta1_ * mt + (1 - beta1_) * grad;
                vt = beta2_ * vt + (1 - beta2_) * grad.array().square().matrix();

                // update mthat and vthat
                mthat = mt * 1 / (1 + std::pow(beta1_, iterations + 1));
                vthat = vt * 1 / (1 + std::pow(beta2_, iterations + 1));

                // update theta
                Eigen::Matrix<double, RowIndexType, 1> new_engine_params = initial_params.eigen_params() - (alpha_ * mthat.array() * (vthat.array().sqrt() + epsilon_).inverse()).matrix();
                initial_params.update(new_engine_params);

                stop_watch.stop();
                times_.conservativeResize(times_.rows()+1, Eigen::NoChange);
                times_(times_.rows()-1) = stop_watch.duration() * 1e-3;

                f_x_prev_.conservativeResize(f_x_prev_.rows()+1, Eigen::NoChange);
                f_x_prev_(f_x_prev_.rows()-1) = obj_func(initial_params);

                if (show_trace_) {
                    if (iterations == 0) {
                        std::cout << "| Iteration |    Time    |   Error   | Min. Error |" <<  std::endl;
                        std::cout << "+-----------+------------+-----------+------------+" << std::endl;
                        std::cout << "|";
                        std::cout << std::setw(header_widths[0]) << (iterations+1) << " ";
                        std::cout << "|";
                        double duration = times_(0);
                        std::cout << std::setw(header_widths[1]) << std::setprecision(4) << duration << " [us] ";
                        std::cout << "|";
                        double error = f_x_prev_(0);
                        std::cout << std::setw(header_widths[2]) << std::setprecision(4) << error << " ";
                        std::cout << "|";
                        std::cout << std::setw(header_widths[3]) << std::setprecision(4) << error << " ";
                        std::cout << "|" << std::endl;
                    } else if ((iterations + 1) % show_trace_every_ == 0) {
                        std::cout << "|";
                        std::cout << std::setw(header_widths[0]) << (iterations+1) << " ";
                        std::cout << "|";
                        double duration = times_.middleRows(iterations+1-show_trace_every_+1, show_trace_every_-1).sum();
                        std::cout << std::setw(header_widths[1]) << std::setprecision(4) << duration << " [us] ";
                        std::cout << "|";
                        double error = f_x_prev_(f_x_prev_.rows()-1);
                        std::cout << std::setw(header_widths[2]) << std::setprecision(4) << error << " ";
                        std::cout << "|";
                        double min_error = f_x_prev_.minCoeff();
                        std::cout << std::setw(header_widths[3]) << std::setprecision(4) << min_error << " ";
                        std::cout << "|" << std::endl;
                    }
                }
                ++iterations;
            }
        }

        Eigen::Matrix<double, Eigen::Dynamic, 1> f_x_prev() {
            return f_x_prev_;
        }

        Eigen::Matrix<double, Eigen::Dynamic, 1> times() {
            return times_;
        }
    };

};

#endif //PROMETHEUS_OPTIMIZATION_HPP


/**
 * @file gausslobattoparabolic.cc
 * @brief NPDE exam TEMPLATE CODE FILE
 * @author Oliver Rietmann
 * @date 22.07.2020
 * @copyright Developed at SAM, ETH Zurich
 */

#include "gausslobattoparabolic.h"

#include <lf/assemble/assemble.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <Eigen/SparseLU>
#include <functional>
#include <memory>
#include <utility>

namespace GaussLobattoParabolic {

/* SAM_LISTING_BEGIN_1 */
lf::assemble::COOMatrix<double> initMbig(
    std::shared_ptr<const lf::uscalfe::FeSpaceLagrangeO1<double>> fe_space) {
  const lf::assemble::DofHandler &dofh = fe_space->LocGlobMap();
#if SOLUTION
  // Diffusion coefficient =0, reaction coefficient = 1
  lf::mesh::utils::MeshFunctionConstant alpha(0.0), gamma(1.0);
  lf::uscalfe::ReactionDiffusionElementMatrixProvider entity_matrix_provider(
      fe_space, alpha, gamma);
  // Compute mass matrix for full finite element space
  lf::assemble::COOMatrix<double> M =
      lf::assemble::AssembleMatrixLocally<lf::assemble::COOMatrix<double>,
                                          decltype(entity_matrix_provider)>(
          0, dofh, entity_matrix_provider);
  // Find mesh nodes on the boundary
  const lf::mesh::utils::CodimMeshDataSet<bool> bd_flags =
      lf::mesh::utils::flagEntitiesOnBoundary(fe_space->Mesh(), 2);
  // Predicate for selecting matrix rows induced by test functions associated
  // with nodes on the boundary
  auto pred = [&bd_flags, &dofh](int i, int j) {
    return bd_flags(dofh.Entity(i));
  };
  // Set the corresponding triplets to zero using LehrFEM++ helper function
  M.setZero(pred);
#else
  //====================
  // Your code goes here
  // Replace this dummy assignment for M:
  int N = dofh.NumDofs();
  lf::assemble::COOMatrix<double> M(N, N);
  //====================
#endif

  return M;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
lf::assemble::COOMatrix<double> initAbig(
    std::shared_ptr<const lf::uscalfe::FeSpaceLagrangeO1<double>> fe_space) {
  const lf::assemble::DofHandler &dofh = fe_space->LocGlobMap();
#if SOLUTION
  // Diffusion coefficient =1, reaction coefficient = 0
  lf::mesh::utils::MeshFunctionConstant alpha(1.0), gamma(0.0);
  lf::uscalfe::ReactionDiffusionElementMatrixProvider entity_matrix_provider(
      fe_space, alpha, gamma);
  // Compute Galerkin matrix for -Laplacian on full FE space
  lf::assemble::COOMatrix<double> A =
      lf::assemble::AssembleMatrixLocally<lf::assemble::COOMatrix<double>,
                                          decltype(entity_matrix_provider)>(
          0, dofh, entity_matrix_provider);
  // Find mesh nodes on the boundary
  const lf::mesh::utils::CodimMeshDataSet<bool> bd_flags =
      lf::mesh::utils::flagEntitiesOnBoundary(fe_space->Mesh(), 2);
  // Predicate for selecting matrix rows induced by test functions associated
  // with nodes on the boundary
  auto pred = [&bd_flags, &dofh](int i, int j) {
    return bd_flags(dofh.Entity(i));
  };
  // Set the corresponding triplets to zero using LehrFEM++ helper function
  A.setZero(pred);
  // Set "boundary block" to the identity matrix
  for (int i = 0; i < dofh.NumDofs(); ++i) {
    if (bd_flags(dofh.Entity(i))) A.AddToEntry(i, i, 1.0);
  }
#else
  //====================
  // Your code goes here
  // Replace this dummy assignment for A:
  int N = dofh.NumDofs();
  lf::assemble::COOMatrix<double> A(N, N);
  //====================
#endif

  return A;
}
/* SAM_LISTING_END_2 */

/* SAM_LISTING_BEGIN_3 */
RHSProvider::RHSProvider(const lf::assemble::DofHandler &dofh,
                         std::function<double(double)> g)
    : g_(std::move(g)) {
#if SOLUTION
  // Finde nodes on the boundary
  const lf::mesh::utils::CodimMeshDataSet<bool> bd_flags =
      lf::mesh::utils::flagEntitiesOnBoundary(dofh.Mesh(), 2);
  int N = dofh.NumDofs();
  // Initialize the fixed vector, components for degrees of freedom associated
  // with nodes on the boundary are set to 1, all other to 0
  zero_one_ = Eigen::VectorXd(N);
  for (int i = 0; i < N; ++i) {
    zero_one_(i) = bd_flags(dofh.Entity(i)) ? 1.0 : 0.0;
  }
#else
  //====================
  // Your code goes here
  //====================
#endif
}

Eigen::VectorXd RHSProvider::operator()(double t) const {
#if SOLUTION
  // Just rescale the stored vector
  return g_(t) * zero_one_;
#else
  //====================
  // Your code goes here
  // Replace this dummy return value:
  return Eigen::VectorXd(0);
  //====================
#endif
}
/* SAM_LISTING_END_3 */

}  // namespace GaussLobattoParabolic


#include <cstdlib>
#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/blas/level1.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include <boost/numeric/bindings/upper.hpp>
#include <boost/numeric/bindings/lapack/driver.hpp>
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;
namespace lapack=boost::numeric::bindings::lapack;

int main(int argc, char *argv[]) {
  typedef ublas::vector<double> vector;
  typedef ublas::matrix<double, ublas::column_major> matrix;
  typedef typename vector::size_type size_type;

  rand_normal<double>::reset();
  size_type n=128;
  matrix A(n, n);
  for (size_type j=0; j<n; ++j) {
    for (size_type i=0; i<=j; ++i) {
      A(i, j)=rand_normal<double>::get();
      A(j, i)=A(i, j);
    }
  }
  {
    vector lambda(n);
    matrix A_bak(A);
    int info=lapack::syev('V', lapack::upper(A), lambda);
    if (info==0) {
      for (int i=0; i<n; ++i) {
	// res <- A*vr(i) - lambda(i)*vr(i)
	ublas::matrix_column<matrix> v(A, i);
	vector res(v);
	blas::gemv(1., A_bak, v, -lambda(i), res);
	std::cout << "norm of residual (right eigen vector " << i
		  << " ): " << blas::nrm2(res) << '\n';
      }
    } else
      if (info>0)
	std::cout << "unable to compute all eigen values\n";
      else
	std::cout << "illegal arguments\n";
  }
  return EXIT_SUCCESS;
}


#ifndef STAN_MATH_PRIM_SCAL_FUN_LOG_SUM_EXP_HPP
#define STAN_MATH_PRIM_SCAL_FUN_LOG_SUM_EXP_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/scal/fun/log1p_exp.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <boost/math/tools/promotion.hpp>

namespace stan {
namespace math {

/**
 * Calculates the log sum of exponetials without overflow.
 *
 * \f$\log (\exp(a) + \exp(b)) = m + \log(\exp(a-m) + \exp(b-m))\f$,
 *
 * where \f$m = max(a, b)\f$.
 *
 *
   \f[
   \mbox{log\_sum\_exp}(x, y) =
   \begin{cases}
     \ln(\exp(x)+\exp(y)) & \mbox{if } -\infty\leq x, y \leq \infty \\[6pt]
     \textrm{NaN} & \mbox{if } x = \textrm{NaN or } y = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{log\_sum\_exp}(x, y)}{\partial x} =
   \begin{cases}
     \frac{\exp(x)}{\exp(x)+\exp(y)} & \mbox{if } -\infty\leq x, y \leq \infty
 \\[6pt] \textrm{NaN} & \mbox{if } x = \textrm{NaN or } y = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{log\_sum\_exp}(x, y)}{\partial y} =
   \begin{cases}
     \frac{\exp(y)}{\exp(x)+\exp(y)} & \mbox{if } -\infty\leq x, y \leq \infty
 \\[6pt] \textrm{NaN} & \mbox{if } x = \textrm{NaN or } y = \textrm{NaN}
   \end{cases}
   \f]
 *
 * @param a the first variable
 * @param b the second variable
 */
template <typename T1, typename T2>
inline return_type_t<T1, T2> log_sum_exp(const T2& a, const T1& b) {
  if (a == NEGATIVE_INFTY)
    return b;
  if (a == INFTY && b == INFTY)
    return INFTY;
  if (a > b)
    return a + log1p_exp(b - a);
  return b + log1p_exp(a - b);
}

}  // namespace math
}  // namespace stan

#endif


#ifndef TVMTL_MANIFOLD_SPHERE_HPP
#define TVMTL_MANIFOLD_SPHERE_HPP

#include <cmath>
#include <complex>
#include <iostream>

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

#include "enumerators.hpp"

namespace tvmtl {

// Specialization SPHERE
template < int N>
struct Manifold< SPHERE, N> {

    public:
	static const MANIFOLD_TYPE MyType;
	static const int manifold_dim ;
	static const int value_dim; // TODO: maybe rename to embedding_dim

	static const bool non_isometric_embedding;

	// Scalar type of manifold
	//typedef double scalar_type;
	typedef double scalar_type;
	typedef double dist_type;
	typedef std::complex<double> complex_type;
	typedef std::vector<double> weight_list;

	// Value Typedef
	typedef Eigen::Matrix< scalar_type, N, 1>				value_type;
	typedef value_type&							ref_type;
	typedef const value_type&						cref_type;
	typedef std::vector<value_type, Eigen::aligned_allocator<value_type> >	value_list;

	// Tangent space typedefs
	typedef Eigen::Matrix < scalar_type, N, N-1> tm_base_type;
	typedef tm_base_type& tm_base_ref_type;

	// Derivative Typedefs
	typedef value_type			     deriv1_type;
	typedef deriv1_type&			     deriv1_ref_type;

	typedef Eigen::Matrix<scalar_type, N, N>     deriv2_type;
	typedef deriv2_type&			     deriv2_ref_type;
	typedef	Eigen::Matrix<scalar_type, N-1, N-1> restricted_deriv2_type;

	// Manifold distance functions (for IRLS)
	inline static dist_type dist_squared(cref_type x, cref_type y);
	inline static void deriv1x_dist_squared(cref_type x, cref_type y, deriv1_ref_type result);
	inline static void deriv1y_dist_squared(cref_type x, cref_type y, deriv1_ref_type result);

	inline static void deriv2xx_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);
	inline static void deriv2xy_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);
	inline static void deriv2yy_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);

	// Manifold exponentials und logarithms ( for Proximal point)
	template <typename DerivedX, typename DerivedY>
	inline static void exp(const Eigen::MatrixBase<DerivedX>& x, const Eigen::MatrixBase<DerivedY>& y, Eigen::MatrixBase<DerivedX>& result);
	inline static void log(cref_type x, cref_type y, ref_type result);

	inline static void convex_combination(cref_type x, cref_type y, double t, ref_type result);

	// Implementations of the Karcher mean
	// Slow list version
	inline static void karcher_mean(ref_type x, const value_list& v, double tol=1e-10, int maxit=15);
	inline static void weighted_karcher_mean(ref_type x, const weight_list& w, const value_list& v, double tol=1e-10, int maxit=15);
	// Variadic templated version
	template <typename V, class... Args>
	inline static void karcher_mean(V& x, const Args&... args);
	template <typename V>
	inline static void variadic_karcher_mean_gradient(V& x, const V& y);
	template <typename V, class... Args>
	inline static void variadic_karcher_mean_gradient(V& x, const V& y1, const Args&... args);

	// Basis transformation for restriction to tangent space
	inline static void tangent_plane_base(cref_type x, tm_base_ref_type result);

	// Projection
	inline static void projector(ref_type x);

	// Interpolation pre- and postprocessing
	inline static void interpolation_preprocessing(ref_type x) {};
	inline static void interpolation_postprocessing(ref_type x) {};

};

/*-----IMPLEMENTATION SPHERE----------*/

// Static constants, Outside definition to avoid linker error

template <int N>
const MANIFOLD_TYPE Manifold < SPHERE, N>::MyType = SPHERE;

template <int N>
const int Manifold < SPHERE, N>::manifold_dim = N-1;

template <int N>
const int Manifold < SPHERE, N>::value_dim = N;

template <int N>
const bool Manifold < SPHERE, N>::non_isometric_embedding = false;

// Squared Sphere distance function
template <int N>
inline typename Manifold < SPHERE, N>::dist_type Manifold < SPHERE, N>::dist_squared( cref_type x, cref_type y ){
    scalar_type xdoty = x.dot(y);
    if (xdoty < - 1.0) xdoty = -1.0;
    if (xdoty >  1.0) xdoty = 1.0;
    dist_type d = std::acos(xdoty);
    #ifdef TVMTL_MANIFOLD_DEBUG
	    if(!std::isfinite(d)){
	    std::cout << "\nDX Non-Series: " << std::endl;
	    std::cout << "x "<< x  << std::endl;
	    std::cout << "y "<< y << std::endl;
	    std::cout << "x^Ty "<< x.dot(y) << std::endl;
	    std::cout << "xdoty "<< xdoty << std::endl;
	    std::cout << "result" << d << std::endl;
	    }
	#endif
    return d*d;
}

// Derivative of Squared Sphere distance w.r.t. first argument
// TODO: Extende implementation of series to 1.0-eps
template <int N>
inline void Manifold < SPHERE, N>::deriv1x_dist_squared( cref_type x, cref_type y, deriv1_ref_type result){
    scalar_type xdoty = x.dot(y);
    bool useSeries = false;

    if (xdoty < - 1.0) xdoty = -1.0;
    if (xdoty >= 1.0) { xdoty = 1.0; useSeries = true; }

    if(!useSeries){
	scalar_type acos = std::acos(xdoty);
	result =  -2.0 * acos / std::sqrt(1.0 - xdoty * xdoty) * y;
	#ifdef TVMTL_MANIFOLD_DEBUG
	    if(!std::isfinite(result(0))){
	    std::cout << "\nDX Non-Series: " << std::endl;
	    std::cout << "x "<< x  << std::endl;
	    std::cout << "y "<< y << std::endl;
	    std::cout << "x^Ty "<< x.dot(y) << std::endl;
	    std::cout << "xdoty "<< xdoty << std::endl;
	    std::cout << "acos(x) " << acos << std::endl;
	    std::cout << "sqrt(1-x^2)" << std::sqrt(1.0 - xdoty * xdoty) << std::endl;
	    std::cout << "result" << result << std::endl;
	    }
	#endif
    }
    else{
	result = -2.0 * y;
	#ifdef TVMTL_MANIFOLD_DEBUG
	    if(!std::isfinite(result(0)))
		std::cout << "DX Series:" << result << std::endl;
	#endif
    }
}
// Derivative of Squared Sphere distance w.r.t. second argument
template <int N>
inline void Manifold < SPHERE, N>::deriv1y_dist_squared( cref_type x, cref_type y, deriv1_ref_type result){
    scalar_type xdoty = x.dot(y);
    bool useSeries = false;

    if (xdoty < - 1.0) xdoty = -1.0;
    if (xdoty >=  1.0) { xdoty = 1.0; useSeries = true; }

    if(!useSeries){
	scalar_type acos = std::acos(xdoty);
	result =  -2.0 * acos / std::sqrt(1.0 - xdoty * xdoty) * x;
    }
    else
	result = -2.0 * x;
}

// Second Derivative of Squared Sphere distance w.r.t first argument
template <int N>
inline void Manifold < SPHERE, N>::deriv2xx_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    scalar_type xdoty = x.dot(y);
    bool useSeries = false;

    if (xdoty < - 1.0) xdoty = -1.0;
    if (xdoty >=  1.0) { xdoty = 1.0; useSeries = true; }

    if(!useSeries){
	scalar_type onemx2 = 1.0 - xdoty * xdoty;
	scalar_type acos = std::acos(xdoty);
	scalar_type da =  -2.0 * acos / std::sqrt(onemx2);
	result = (2.0 + da * xdoty) / onemx2 * y * y.transpose() - da * xdoty * deriv2_type::Identity();
    }
    else
	result = 2.0/3.0 * y * y.transpose() + 2.0 * xdoty * deriv2_type::Identity();

}
// Second Derivative of Squared Sphere distance w.r.t first and second argument
template <int N>
inline void Manifold < SPHERE, N>::deriv2xy_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    scalar_type xdoty = x.dot(y);
    bool useSeries = false;

    if (xdoty < - 1.0) xdoty = -1.0;
    if (xdoty >=  1.0) { xdoty = 1.0; useSeries = true; }

    if(!useSeries){
	scalar_type onemx2 = 1.0 - xdoty * xdoty;
	scalar_type acos = std::acos(xdoty);
	scalar_type da =  -2.0 * acos / std::sqrt(onemx2);
	result = (2.0 + da * xdoty) / onemx2 * y * x.transpose() + da * deriv2_type::Identity();
    }
    else
	result = 2.0/3.0 * y * x.transpose() - 2.0 * deriv2_type::Identity();
}
// Second Derivative of Squared Sphere distance w.r.t second argument
template <int N>
inline void Manifold < SPHERE, N>::deriv2yy_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    scalar_type xdoty = x.dot(y);
    bool useSeries = false;

    if (xdoty < - 1.0) xdoty = -1.0;
    if (xdoty >=  1.0) { xdoty = 1.0; useSeries = true; }

    if(!useSeries){
	scalar_type onemx2 = 1.0 - xdoty * xdoty;
	scalar_type acos = std::acos(xdoty);
	scalar_type da =  -2.0 * acos / std::sqrt(onemx2);
	result = (2.0 + da * xdoty) / onemx2 * x * x.transpose() - da * xdoty * deriv2_type::Identity();

    }
    else
	result = 2.0/3.0 * x * x.transpose() + 2.0 * xdoty * deriv2_type::Identity();

}

// Exponential and Logarithm Map
template <int N>
template <typename DerivedX, typename DerivedY>
inline void Manifold <SPHERE, N>::exp(const Eigen::MatrixBase<DerivedX>& x, const Eigen::MatrixBase<DerivedY>& y, Eigen::MatrixBase<DerivedX>& result){
    //result=(x+y).normalized();
    scalar_type n = y.norm();
    if(n!=0)
	result = std::cos(n) * x + std::sin(n) * y / n;
    else
	result = x;
}

template <int N>
inline void Manifold <SPHERE, N>::log(cref_type x, cref_type y, ref_type result){
    //result = (y-x).normalized();
    scalar_type xdoty = x.dot(y);
    bool useSeries = false;

    if (xdoty < - 1.0) xdoty = -1.0;
    if (xdoty >=  1.0) { xdoty = 1.0; useSeries = true; }

    if(!useSeries){
	scalar_type fac = std::acos(xdoty) / std::sqrt(1.0 - xdoty * xdoty);
	result = fac * (y - xdoty * x);
    }
    else
	result =  y - xdoty * x;
}

// Tangent Plane restriction
// TODO: Implement general QR Composition here
template <int N>
inline void Manifold <SPHERE, N>::tangent_plane_base(cref_type x, tm_base_ref_type result){
    result = tm_base_type::Identity();
}

template <> // Special Version for S^2 utilizing the cross product
inline void Manifold <SPHERE, 3>::tangent_plane_base(cref_type x, tm_base_ref_type result){
    //int c = static_cast<int>(std::abs(x.coeff(0)) > 0.5);
    int c = static_cast<int>(std::abs(x.coeff(2)) > 0.5 || std::abs(x.coeff(1)) > 0.5);
    result.col(0) = value_type(0, x.coeff(2), -x.coeff(1)) * c + value_type(x.coeff(2), 0, -x.coeff(0)) * (1-c);
    result.col(0).normalize();
    result.col(1) = x.cross(result.col(0)).normalized();

    if(x.norm()==0)
	result = tm_base_type::Zero();

    #ifdef TVMTL_MANIFOLD_DEBUG
	    if(!std::isfinite(result(0,0))){
	    std::cout << "\n\nx "<< x  << std::endl;
	    std::cout << "col0 V1 "<<  value_type(0, x.coeff(2), -x.coeff(1)) << std::endl;
	    std::cout << "col0 V2 "<<  value_type(x.coeff(2), 0, -x.coeff(0)) << std::endl;
	    std::cout << "col0 norm " << result.col(0) << std::endl;
	    std::cout << "cross prod" << x.cross(result.col(0)) << std::endl;
	    std::cout << "cross prod norm" << result.col(1) << std::endl;
	    }
    #endif
}

// Convex geodesic combinations
template <int N>
inline void Manifold <SPHERE, N>::convex_combination(cref_type x, cref_type y, double t, ref_type result){
   if(t == 0.5){
	result = x + y;
	projector(result);
   }
   else{
	value_type l;
	log(x, y, l);
	exp(x, l * t, result);
   }
}

// Karcher mean implementations
template <int N>
inline void Manifold<SPHERE, N>::karcher_mean(ref_type x, const value_list& v, double tol, int maxit){
    value_type L, temp;

    int k = 0;
    double error = 0.0;
    do{
	scalar_type m1 = x.sum();
	L = value_type::Zero();
	for(int i = 0; i < v.size(); ++i){
	    log(x, v[i], temp);
	    L += temp;
	}
	exp(x, 1.0 / v.size() * L, temp);
	x = temp;
	error = std::abs(x.sum() - m1);
	++k;
    } while(error > tol && k < maxit);

}

template <int N>
inline void Manifold<SPHERE, N>::weighted_karcher_mean(ref_type x, const weight_list& w, const value_list& v, double tol, int maxit){
    value_type L, temp;

    int k = 0;
    double error = 0.0;
    do{
	scalar_type m1 = x.sum();
	L = value_type::Zero();
	for(int i = 0; i < v.size(); ++i){
	    log(x, v[i], temp);
	    L += w[i] * temp;
	}
	exp(x, 1.0 / v.size() * L, temp);
	x = temp;
	error = std::abs(x.sum() - m1);
	++k;
    } while(error > tol && k < maxit);

}

template <int N>
template <typename V, class... Args>
inline void Manifold<SPHERE, N>::karcher_mean(V& x, const Args&... args){
    V temp, sum;

    int numArgs = sizeof...(args);
    int k = 0;
    double error = 0.0;
    double tol = 1e-10;
    int maxit = 15;
    do{
	scalar_type m1 = x.sum();
	sum = x;
	variadic_karcher_mean_gradient(sum, args...);
	exp(x, 1.0 / numArgs * sum, temp);
	x = temp;
	error = std::abs(x.sum() - m1);
	++k;
    } while(error > tol && k < maxit);
}

template <int N>
template <typename V>
inline void Manifold<SPHERE, N>::variadic_karcher_mean_gradient(V& x, const V& y){
    V temp;
    log(x, y, temp);
    x = temp;
}

template <int N>
template <typename V, class... Args>
inline void Manifold<SPHERE, N>::variadic_karcher_mean_gradient(V& x, const V& y1, const Args& ... args){
    V temp1, temp2;
    temp2 = x;

    log(x, y1, temp1);

    variadic_karcher_mean_gradient(temp2, args...);
    temp1 += temp2;
    x = temp1;
}

template <int N>
inline void Manifold <SPHERE, N>::projector(ref_type x){

    #ifdef TVMTL_MANIFOLD_DEBUG
	if(!std::isfinite(x(0))) std::cout << "Projector recieved nan" << std::endl;
    #endif

    scalar_type norm = x.norm();
    if(norm!=0.0) x.normalize();
    else x.setConstant(1.0 / 256.0).normalize();

    #ifdef TVMTL_MANIFOLD_DEBUG
	if(!std::isfinite(x(0))) std::cout << "Projector returns nan" << std::endl;
    #endif
}

} // end namespace tvmtl

#endif


//==================================================================================================
/*!
  @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_CONSTANT_TAN_3PIO_8_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_TAN_3PIO_8_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    Constant \f$\tan3\frac\pi{8} = \sqrt2 + 1\f$.

    @par Semantic:

    For type T:

    @code
    T r = Tan_3pio_8<T>();
    @endcode

    is similar to:

    @code
    T r = 1.0+sqrt(2.0);
    @endcode

    @return a value of type T

**/
  template<typename T> T Tan_3pio_8();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      Constant \f$\tan3\frac\pi{8} = \sqrt2 + 1\f$.

      Generate the  constant tan_3pio_8.

      @return The Tan_3pio_8 constant for the proper type
    **/
    const boost::dispatch::functor<tag::tan_3pio_8_> tan_3pio_8 = {};
  }
} }
#endif

#include <boost/simd/constant/definition/tan_3pio_8.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


#include <iostream>
#include <fstream>
#include<iomanip>

#include <cstdlib>
#include <stdio.h>
#include <string>
#include <vector>

#include <time.h>
#include <sys/time.h>

#include "mkl.h"
#include <omp.h>
//#include "mkl_service.h"
#include <fftw3.h>
#include <armadillo>
using namespace std;
using namespace arma;

double gettime(){
    struct timeval tv;
    gettimeofday(&tv,NULL);
    return tv.tv_sec*1000+tv.tv_usec/1000.0;
};

double *** al(int &n1,int &n2,int &n3){
    double ***p;
    p=new double**[n1];
    for (int i=0;i<n1;i++){
        p[i] = new double *[n2];
        for (int j=0;j<n2;j++) {
            p[i][j] = new double[n3];
        }
    }
    return p;
}

//template <typename T>
void del(double *** p, int &n1,int &n2){
    for(int i=0; i<n1; i++) {
        for (int j = 0; j < n2; j++) {
            delete [] p[i][j];
        }
        delete [] p[i];
    }
    delete  [] p;
    p=NULL;
}

double *** loadfile(int &n1, int &n2, int &n3, char *path){

    int count = 0;
    FILE* fp;
    char str[100];

    double ***v = al(n1,n2,n3);

    fp = fopen(path,"r");

    string tmp;
    while (fscanf(fp, "%s", str) != EOF)
    {
        int NUM=count;
        int k=NUM%(n1*n2);
        k=(NUM-k)/(n1*n2);
        int j=(NUM-k*n1*n2)%n2;
        int i=(NUM-k*n1*n2)/n2;
        tmp=str;
        v[i][j][k]=(double)atof(tmp.c_str());
        count++;
        if(count==n1*n2*n3){break;}
    }

    fclose(fp);

    return v;
}

void display(double *** v, int &n1, int &n2, int &n3){

    cout << "Tensor: " << endl;

    for (int k = 0; k < n3; k++) {
        for (int i = 0; i < n1; i++) {
            for (int j = 0; j < n2; j++) {
                cout << v[i][j][k] << " ";
            }
            cout << endl;
        }
        cout << endl;
    }
}

int main()
{
    double t0, t1, t2, t3, t4;
    int turn=2000;
    int n1 = turn, n2 = turn, n3 = 110;
    int N0 = floor(n3/2.0) + 1;

    char path[1000] = "/home/jcfei/Documents/MATLAB/data/a2000.txt";

    double ***M = loadfile(n1, n2, n3, path);
    cout << "loadfile" << endl;

    double ***v_t = al(n1, n2, N0);
    double ***v_t1 = al(n1, n2, N0);

    t0 = gettime();
    fftw_complex out[N0]; //fftw_alloc_real()
    double *in = fftw_alloc_real(n3);

    fftw_plan p_fft;
    p_fft = fftw_plan_dft_r2c_1d(n3, in, out, FFTW_ESTIMATE);
//    p=fftw_plan_dft_1d(n3,in,out,FFTW_FORWARD,FFTW_MEASURE);

//#pragma omp parallel for num_threads(8)
    for (int i = 0; i < n1; i++) {
        for (int j = 0; j < n2; j++) {
            in = M[i][j];
            fftw_execute_dft_r2c(p_fft, in, out);
            for (int k = 0; k < N0; k++) {
                v_t[i][j][k] = out[k][0];
                v_t1[i][j][k] = out[k][1];
            }
        }
    }
    del(M,n1,n2);
//    t1=gettime();
//    cout<<"fft time: "<<t1-t0<<endl;

    double ***uf = al(n1, n1, N0);
    double ***uf1 = al(n1, n1, N0);
    double ***theta = al(n1, n2, N0);
    double ***vf = al(n2, n2, N0);
    double ***vf1 = al(n2, n2, N0);

    cx_mat TMP = zeros<cx_mat>(n1, n2);
    cx_mat TMPU = zeros<cx_mat>(n1, n1);
    cx_mat TMPV = zeros<cx_mat>(n2, n2);
    colvec TMPT;

//    t2=gettime();
//    cout<<"alloc space: "<<t2-t1<<endl;

//#pragma omp parallel for num_threads(8)
    for (int k = 0; k < N0; k++) {
        for (int i = 0; i < n1; i++) {
            for (int j = 0; j < n2; j++) {
                TMP(i, j).real(v_t[i][j][k]);
                TMP(i, j).imag(v_t1[i][j][k]);
            }
        }
        svd(TMPU, TMPT, TMPV, TMP, "dc");
//        svd(TMPU,TMPT,TMPV,TMP,"std");

        for (int i = 0; i < n1; i++) {
            for (int j = 0; j < n1; j++) {
                uf[i][j][k] = TMPU(i, j).real();
                uf1[i][j][k] = TMPU(i, j).imag();
            }
        }
        for (int i = 0; i < n2; i++) {
            for (int j = 0; j < n2; j++) {
                vf[i][j][k] = TMPV(i, j).real();
                vf1[i][j][k] = TMPV(i, j).imag();
            }
        }
        if (n1 <= n2) {
            for (int i = 0; i < n1; i++) {
                theta[i][i][k] = TMPT(i);
            }
        } else {
            for (int i = 0; i < n2; i++) {
                theta[i][i][k] = TMPT(i);
            }
        }
    }

    del(v_t, n1, n2);
    del(v_t1, n1, n2);

    fftw_complex out1[N0]; //fftw_alloc_real()
    double *in1 = fftw_alloc_real(n3);
    p_fft = fftw_plan_dft_c2r_1d(n3, out1, in1, FFTW_ESTIMATE);

//    #pragma omp parallel for num_threads(8)
//    #pragma omp parallel for num_threads(2)

    double ***U = al(n1, n1, n3);

    for (int i = 0; i < n1; i++) {
        for (int j = 0; j < n1; j++) {
            for (int k = 0; k < N0; k++) {
                out1[k][0] = uf[i][j][k];
                out1[k][1] = uf1[i][j][k];
            }
            fftw_execute_dft_c2r(p_fft, out1, in1);

            for (int k = 0; k < n3; k++) {
                U[i][j][k] = 1.0 / n3 * in1[k];
            }
        }
    }

    del(uf, n1, n1);
    del(uf1, n1, n1);

    double ***V = al(n2, n2, n3);
    for (int i = 0; i < n2; i++) {
        for (int j = 0; j < n2; j++) {
            for (int k = 0; k < N0; k++) {
                out1[k][0] = vf[i][j][k];
                out1[k][1] = vf1[i][j][k];
            }
            fftw_execute_dft_c2r(p_fft, out1, in1);
            for (int k = 0; k < n3; k++) {
                V[i][j][k] = 1.0 / n3 * in1[k];
            }
        }
    }
    del(vf, n2, n2);
    del(vf1, n2, n2);

    double ***Theta = al(n1, n2, n3);
    for (int i = 0; i < n1; i++) {
        for (int j = 0; j < n2; j++) {
            for (int k = 0; k < N0; k++) {
                out1[k][0] = theta[i][j][k];
                out1[k][1] = 0;
            }
            fftw_execute_dft_c2r(p_fft, out1, in1);
            for (int k = 0; k < n3; k++) {
                Theta[i][j][k] = 1.0 / n3 * in1[k];
            }
        }
    }
    del(theta, n1, n2);

    fftw_destroy_plan(p_fft);

    t4 = gettime();
//    cout<<"ifft time: "<<t4-t3<<endl;
    cout << "Total time: " << t4 - t0 << endl;

    //fft transform result write to txt
//    ofstream a;
//    a.open("/home/jcfei/Desktop/TensorC++/txtToarray/Theta-video.txt");
//    for (int k=0; k<n3;k++){
//        for (int i=0; i<n1;i++){
//            for(int j=0; j<n2;j++){
//                a<<setiosflags(ios::right)<<setw(15)<<Theta[i][j][k]<<"  ";
//            }
//            a<<endl;
//        }
//    }
//    a.close();
//
//    ofstream b;
//    b.open("/home/jcfei/Desktop/TensorC++/txtToarray/U-video.txt");
//    for (int k=0; k<n3;k++){
//        for (int i=0; i<n1;i++){
//            for(int j=0; j<n1;j++){
//                b<<setiosflags(ios::right)<<setw(10)<<U[i][j][k]<<"  ";
//            }
//            b<<endl;
//        }
//    }
//    b.close();
//
//    ofstream c;
//    c.open("/home/jcfei/Desktop/TensorC++/txtToarray/V-video.txt");
//    for (int k=0; k<n3;k++){
//        for (int i=0; i<n2;i++){
//            for(int j=0; j<n2;j++){
//                c<<setiosflags(ios::right)<<setw(10)<<V[i][j][k]<<"  ";
//            }
//            c<<endl;
//        }
//    }
//    c.close();
    return 0;
}


/*!
 * @file diffusion_problem_ms.hpp
 * @brief Contains implementation of the main object for multiscale FEM.
 * @author Konrad Simon
 * @date August 2019
 */

#ifndef INCLUDE_DIFFUSION_PROBLEM_MS_HPP_
#define INCLUDE_DIFFUSION_PROBLEM_MS_HPP_

// Deal.ii
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/work_stream.h>
#include <deal.II/base/multithread_info.h>
#include <deal.II/base/timer.h>

#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/affine_constraints.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/grid/grid_generator.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>

#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>

// STL
#include <cmath>
#include <fstream>
#include <iostream>
#include <vector>

// My Headers
#include "matrix_coeff.hpp"
#include "right_hand_side.hpp"
#include "neumann_bc.hpp"
#include "dirichlet_bc.hpp"
#include "diffusion_problem_basis.hpp"

/*!
 * @namespace DiffusionProblem
 * @brief Contains implementation of the main object
 * and all functions to solve a
 * Dirichlet-Neumann problem on a unit square.
 */
namespace DiffusionProblem
{
using namespace dealii;

/*!
 * @class DiffusionProblemMultiscale
 * @brief Main class to solve
 * Dirichlet-Neumann problem on a unit square with
 * multiscale FEM.
 */
template <int dim>
class DiffusionProblemMultiscale
{
public:
	DiffusionProblemMultiscale (unsigned int n_refine, unsigned int n_refine_local);
	void run ();

private:
	// *********************************************
	// This is for threading the basis computation
	struct BasisScratchData
	{
		BasisScratchData () {}; // No implementation
		BasisScratchData (const BasisScratchData& /*scratch_data*/) {}; // No implementation
	};

	struct BasisCopyData
	{

	};

	void compute_local_basis(const typename std::vector<DiffusionProblemBasis<dim>>::iterator &it_basis,
								BasisScratchData	&scratch_data,
								BasisCopyData	&copy_data);
	void output_local_solution(const typename std::vector<DiffusionProblemBasis<dim>>::iterator &it_basis,
								BasisScratchData	&scratch_data,
								BasisCopyData	&copy_data);
	void fake_copy (const BasisCopyData&) {}; // No implementation
	// *********************************************

	void make_grid ();
	void initialize_basis_problem ();
	void compute_basis ();
	void setup_system ();
	void assemble_system ();
	void solve_iterative ();

	void send_global_weights_to_cell ();

	void output_global_coarse () const;
	void output_global_fine ();

	Triangulation<dim>   			triangulation;
	FE_Q<dim>            			fe;
	DoFHandler<dim>      			dof_handler;

	AffineConstraints<double> 		constraints;

	SparsityPattern      			sparsity_pattern;
	SparseMatrix<double> 			system_matrix;

	/*!
	 * Solution vector containing weights at the dofs.
	 */
	Vector<double>       			solution;

	/*!
	 * Contains all parts of the right-hand side needed to
	 * solve the linear system.
	 */
	Vector<double>       			system_rhs;

	/*!
	 * Number of global refinements.
	 */
	const unsigned int n_refine;

	/*!
	 * Number of local refinements.
	 */
	const unsigned int n_refine_local;

	/*!
	 * STL Vector holding basis functions for each coarse cell.
	 */
	std::vector<DiffusionProblemBasis<dim>> 	cell_basis_vector;
};

/*!
 * Constructor.
 */
template <int dim>
DiffusionProblemMultiscale<dim>::DiffusionProblemMultiscale (unsigned int n_refine, unsigned int n_refine_local) :
  fe (1),
  dof_handler (triangulation),
  n_refine(n_refine),
  n_refine_local(n_refine_local),
  cell_basis_vector(std::pow(2,dim*n_refine))
{}

/*!
 * Set all relevant data to local basis object and initialize the basis fully.
 */
template <int dim>
void DiffusionProblemMultiscale<dim>::initialize_basis_problem ()
{
	// First set up all cell problems serially
	typename Triangulation<dim>::active_cell_iterator
									cell = dof_handler.begin_active(),
									endc = dof_handler.end();
	unsigned int cell_number = 0;
	for (; cell!=endc; ++cell)
	{
		cell_basis_vector[cell_number].set_n_local_refinements (n_refine_local);
		cell_basis_vector[cell_number].set_cell_data (cell, cell_number);
		cell_basis_vector[cell_number].set_basis_data ();

		if (cell_number==0)
			cell_basis_vector[cell_number].set_output_flag (true);

		++cell_number;
	}
}

/*!
 * @brief Function pre-computes basis functions.
 */
template <int dim>
void DiffusionProblemMultiscale<dim>::compute_basis ()
{
	// Now run them in threads
	typename std::vector<DiffusionProblemBasis<dim>>::iterator
										it_basis = cell_basis_vector.begin (),
										it_endbasis = cell_basis_vector.end ();
	WorkStream::run(it_basis,
					it_endbasis,
					*this,
					&DiffusionProblemMultiscale<dim>::compute_local_basis,
					&DiffusionProblemMultiscale<dim>::fake_copy,
					BasisScratchData(),
					BasisCopyData());
}

/*!
 * Pre-compute the local basis. This function
 * is only used for threading.
 */
template <int dim>
void
DiffusionProblemMultiscale<dim>::compute_local_basis(const typename
														std::vector<DiffusionProblemBasis<dim>>::iterator &it_basis,
														BasisScratchData&,
														BasisCopyData&)
{
	it_basis->run ();
}

/*!
 * @brief Set up the grid with a certain number of refinements.
 *
 * Generate a triangulation of \f$[0,1]^{\rm{dim}}\f$ with edges/faces
 * numbered form \f$1,\dots,2\rm{dim}\f$.
 */
template <int dim>
void DiffusionProblemMultiscale<dim>::make_grid ()
{
	GridGenerator::hyper_cube (triangulation, 0, 1, /* colorize */ true);

	triangulation.refine_global (n_refine);

	std::cout << "Number of active cells: "
			<< triangulation.n_active_cells()
			<< std::endl;
}

/*!
 * @brief Setup sparsity pattern and system matrix.
 *
 * Compute sparsity pattern and reserve memory for the sparse system matrix
 * and a number of right-hand side vectors. Also build a constraint object
 * to take care of Dirichlet boundary conditions.
 */
template <int dim>
void DiffusionProblemMultiscale<dim>::setup_system ()
{
	dof_handler.distribute_dofs (fe);

	std::cout << std::endl
			<< "Number of active global cells:   "
			<< triangulation.n_active_cells()
			<< std::endl
			<< "Number of degrees of freedom:   " << dof_handler.n_dofs()
			<< std::endl
			<< std::endl;

	constraints.clear();
	DoFTools::make_hanging_node_constraints(dof_handler, constraints);

	/*
	 * Set up Dirichlet boundary conditions.
	 */
	const Coefficients::DirichletBC<dim> dirichlet_bc;
	for (unsigned int i = 0; i<dim; ++i)
	{
		VectorTools::interpolate_boundary_values(dof_handler,
													/*boundary id*/ 2*i, // only even boundary id
													dirichlet_bc,
													constraints);
	}

	constraints.close();

	DynamicSparsityPattern dsp(dof_handler.n_dofs());
	DoFTools::make_sparsity_pattern (dof_handler,
									dsp,
									constraints,
									/*keep_constrained_dofs =*/ true); // for time stepping this is essential to be true

	sparsity_pattern.copy_from(dsp);

	system_matrix.reinit (sparsity_pattern);

	solution.reinit (dof_handler.n_dofs());
	system_rhs.reinit (dof_handler.n_dofs());
}

/*!
 * @brief Assemble the system matrix and the static right hand side.
 *
 * Assembly routine to build the time-independent (static) part.
 * Neumann boundary conditions will be put on edges/faces
 * with odd number. Constraints are not applied here yet.
 */
template <int dim>
void DiffusionProblemMultiscale<dim>::assemble_system ()
{
	QGauss<dim - 1> face_quadrature_formula(fe.degree + 1);

	FEFaceValues<dim> 	fe_face_values(fe,
										face_quadrature_formula,
										update_values | update_quadrature_points |
										update_normal_vectors |
										update_JxW_values);

	const unsigned int   	dofs_per_cell = fe.dofs_per_cell;
	const unsigned int 		n_face_q_points = face_quadrature_formula.size();

	FullMatrix<double>   cell_matrix (dofs_per_cell, dofs_per_cell);
	Vector<double>       cell_rhs (dofs_per_cell);

	std::vector<types::global_dof_index> local_dof_indices (dofs_per_cell);

	/*
	 * Neumann BCs and vector to store the values.
	 */
	const Coefficients::NeumannBC<dim> 	neumann_bc;
	std::vector<double>  	neumann_values(n_face_q_points);

	// initialize basis iterator
	typename std::vector<DiffusionProblemBasis<dim>>::iterator
									it_basis = cell_basis_vector.begin();

	/*
	 * Integration over cells.
	 */
	for (const auto &cell: dof_handler.active_cell_iterators())
	{
		cell_matrix = 0;
		cell_rhs = 0;

		cell_matrix = it_basis->get_global_element_matrix ();
		cell_rhs = it_basis->get_global_element_rhs ();

		/*
		 * Boundary integral for Neumann values for odd boundary_id.
		 */
		for (unsigned int face_number = 0;
			 face_number < GeometryInfo<dim>::faces_per_cell;
			 ++face_number)
		{
			if (cell->face(face_number)->at_boundary() &&
					(
						(cell->face(face_number)->boundary_id() == 1) ||
						(cell->face(face_number)->boundary_id() == 3) ||
						(cell->face(face_number)->boundary_id() == 5)
					)
				)
			{
				fe_face_values.reinit(cell, face_number);

				// Fill in values at this particular face.
				neumann_bc.value_list(fe_face_values.get_quadrature_points(),
										   neumann_values);

				for (unsigned int q_face_point = 0; q_face_point < n_face_q_points; ++q_face_point)
				{
					for (unsigned int i = 0; i < dofs_per_cell; ++i)
					{
						cell_rhs(i) += neumann_values[q_face_point] // g(x_q)
										* fe_face_values.shape_value(i, q_face_point) // phi_i(x_q)
										* fe_face_values.JxW(q_face_point); // dS
					} // end ++i
				} // end ++q_face_point
			} // end if
		} // end ++face_number

		// get global indices
		cell->get_dof_indices (local_dof_indices);
		/*
		 * Now add the cell matrix and rhs to the right spots
		 * in the global matrix and global rhs. Constraints will
		 * be taken care of later.
		 */
		for (unsigned int i = 0; i < dofs_per_cell; ++i)
		{
			for (unsigned int j = 0; j < dofs_per_cell; ++j)
			{
				system_matrix.add(local_dof_indices[i],
							local_dof_indices[j],
							cell_matrix(i, j));
			}
			system_rhs(local_dof_indices[i]) += cell_rhs(i);
		}
	} // end ++cell
}

/*!
 * @brief Iterative solver.
 *
 * CG-based solver with SSOR-preconditioning.
 */
template <int dim>
void
DiffusionProblemMultiscale<dim>::solve_iterative ()
{
	SolverControl           solver_control (1000, 1e-12);
	SolverCG<>              solver (solver_control);

	PreconditionSSOR<> preconditioner;
	preconditioner.initialize(system_matrix, 1.2);

	solver.solve (system_matrix,
				solution,
				system_rhs,
				preconditioner);

	constraints.distribute (solution);

	std::cout << "   "
			<< "(global problem)   "
			<< solver_control.last_step()
			<< " coarse CG iterations needed to obtain convergence."
			<< std::endl;
}

/*!
 * @brief Send coarse weights to corresponding local cell.
 *
 * After the coarse (global) weights have been computed they
 * must be set to the local basis object and stored there.
 * This is necessary to write the local multiscale solution.
 */
template <int dim>
void
DiffusionProblemMultiscale<dim>::send_global_weights_to_cell ()
{
	// For each cell we get dofs_per_cell values
	const unsigned int   dofs_per_cell   = fe.dofs_per_cell;
	std::vector<types::global_dof_index> 	local_dof_indices (dofs_per_cell);

	// active cell iterator
	typename DoFHandler<dim>::active_cell_iterator
								cell = dof_handler.begin_active (),
								endc = dof_handler.end ();
	// initialize basis iterator
	typename std::vector<DiffusionProblemBasis<dim>>::iterator
									it_basis = cell_basis_vector.begin ();
	for (; cell!=endc; ++cell)
	{
		// Get local
		cell->get_dof_indices (local_dof_indices);

		std::vector<double> extracted_weights (dofs_per_cell, 0);
		solution.extract_subvector_to (local_dof_indices, extracted_weights);
		it_basis->set_global_weights (extracted_weights);

		// increase syncronously
		++it_basis;
	}
}

/*!
 * @brief Write coarse solution to disk.
 *
 * Write results for coarse solution to disk in vtu-format.
 */
template <int dim>
void
DiffusionProblemMultiscale<dim>::output_global_coarse () const
{
	DataOut<dim> data_out;
	data_out.attach_dof_handler (dof_handler);
	data_out.add_data_vector (solution, "solution");
	data_out.build_patches ();

	std::ofstream output (dim == 2 ?
					"solution-ms_coarse-2d.vtu" :
					"solution-ms_coarse-3d.vtu");

	data_out.write_vtu (output);
}

/*!
 * Output function to write local multiscale solution. Only used for threading output.
 */
template <int dim>
void
DiffusionProblemMultiscale<dim>::output_local_solution (
		const typename std::vector<DiffusionProblemBasis<dim>>::iterator &it_basis,
		BasisScratchData&,
		BasisCopyData&)
{
	it_basis->output_global_solution_in_cell ();
}

/*!
 * Write all local multiscale solution (threaded) and
 * a global pvtu-record.
 */
template <int dim>
void
DiffusionProblemMultiscale<dim>::output_global_fine ()
{
	// List of filenames of local outputs for master file
	std::vector<std::string> filenames_on_cell;

	// Now run them in threads
	typename std::vector<DiffusionProblemBasis<dim>>::iterator
											it_basis = cell_basis_vector.begin (),
											it_endbasis = cell_basis_vector.end ();
	WorkStream::run(it_basis,
					it_endbasis,
					*this,
					&DiffusionProblemMultiscale<dim>::output_local_solution,
					&DiffusionProblemMultiscale<dim>::fake_copy,
					BasisScratchData(),
					BasisCopyData());

	// Active cell iterator
	typename DoFHandler<dim>::active_cell_iterator
								cell = dof_handler.begin_active (),
								endc = dof_handler.end ();
	// Initialize const basis iterator again
	it_basis = cell_basis_vector.begin ();

	for (; cell!=endc; ++cell)
	{
		// Get the global file name
		filenames_on_cell.push_back ( it_basis->get_filename_global () );

		++it_basis;
	}

	// Build a*.pvtu file that points to all output files
	DataOut<dim> data_out;
	data_out.attach_dof_handler (dof_handler);

	// Names of solution components
	data_out.add_data_vector (solution, "solution");

	std::string filename_master = (dim == 2 ?
			"solution-ms_fine-2d" :
			"solution-ms_fine-3d");
	filename_master += ".pvtu";

	std::ofstream master_output (filename_master.c_str ());

	data_out.write_pvtu_record (master_output, filenames_on_cell);
}

/*!
 * @brief Run function of the object.
 *
 * Run the computation after object is built.
 */
template <int dim>
void DiffusionProblemMultiscale<dim>::run ()
{
	std::cout << std::endl
			<< "==========================================="
			<< std::endl;

	std::cout << "Solving problem in "
			<< dim << " space dimensions."
			<< std::endl;

	make_grid ();

	setup_system ();

	initialize_basis_problem ();
	compute_basis ();

	assemble_system ();

	// Now solve
	constraints.condense(system_matrix, system_rhs);
	solve_iterative ();

	send_global_weights_to_cell ();

	output_global_coarse ();
	output_global_fine ();

	std::cout << std::endl
			<< "==========================================="
			<< std::endl;
}

} // end namespace DiffusionProblem

#endif /* INCLUDE_DIFFUSION_PROBLEM_MS_HPP_ */


#include "apch.h"
#include "Maths.h"
	//#include <Eigen/Core>
	//#include <Eigen/Geometry>
	//#include <Eigen/Dense>

namespace A {

	Eigen::Affine3f CreateOrthographicProjection(float left, float right, float bottom, float top, float z_near, float z_far)
	{
		AP_PROFILE_FN();
		Eigen::Affine3f proj = Eigen::Affine3f::Identity();
		proj(0, 0) = 2 / (right - left);
		proj(1, 1) = 2 / (top - bottom);
		proj(2, 2) = 2 / (z_near - z_far);
		proj(0, 3) = (right + left) / (left - right);
		proj(1, 3) = (top + bottom) / (bottom - top);
		proj(2, 3) = (z_far + z_near) / (z_near - z_far);
		proj(3, 3) = 1;
		return proj;
	}

}