#include <iostream>
#include <list>
#include <vector>
#include <algorithm>
#include <set>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/multi_point.hpp>
#include <boost/geometry/geometries/multi_polygon.hpp>

#include <boost/assign/std/vector.hpp>
#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/assign.hpp>

#include <boost/foreach.hpp>

#include <stdlib.h>
#include <bits/stdc++.h>

namespace bg = boost::geometry;

// use the following two commands to compile:
//
//WARNING: DO NOT USE O2. IT DESTROYS bg::correct FUNCTIONALITY
// g++ -lboost_system -c -fPIC polygoncalc.cpp -o polygoncalc.o -O2
// g++ -lboost_system -shared -Wl,-soname,libpolygoncalc.so -o libpolygoncalc.so polygoncalc.o -O2
// if it does not work, add -std=c++14 and add -I and -L directories

typedef boost::geometry::model::d2::point_xy<double> point_type;

typedef bg::model::polygon<point_type> polygon_type;

void dump( const std::string & label, const std::list< std::set< unsigned long > > & values )
{
    std::cout << label << std::endl;
    for( auto iter : values )
    {
        std::cout << "{ ";
        for( auto val : iter )
            std::cout << val << ", ";
        std::cout << "}, ";
    }
    std::cout << std::endl;
}

void combine( std::list< std::set< unsigned long > > & values )
{
    for( std::list< std::set< unsigned long > >::iterator iter = values.begin(); iter != values.end(); ++iter )
        for( std::list< std::set< unsigned long > >::iterator niter( iter ); ++niter != values.end(); )
            if( std::find_first_of( iter->begin(), iter->end(), niter->begin(), niter->end() ) != iter->end() )
            {
                iter->insert( niter->begin(), niter->end() );
                values.erase( niter );
                niter = iter;
            }
}

unsigned long merge(unsigned long* parent, unsigned long x)
{
    if (parent[x] == x)
        return x;
    return merge(parent, parent[x]);
}

void connectedcomponents(unsigned long n, std::vector<std::vector<unsigned long> >& edges, std::list<std::list<unsigned long>>& res)
{

    unsigned long parent[n];
    for (unsigned long i = 0; i < n; i++) {
        parent[i] = i;
    }
    for (auto x : edges) {
        parent[merge(parent, x[0])] = merge(parent, x[1]);
    }

    for (unsigned long i = 0; i < n; i++) {
        parent[i] = merge(parent, parent[i]);
    }
    std::map<unsigned long, std::list<unsigned long> > m;
    for (unsigned long i = 0; i < n; i++) {
        m[parent[i]].push_back(i);
    }
    for (auto it = m.begin(); it != m.end(); it++) {
        std::list<unsigned long> l = it->second;

        res.push_back(l);
    }
}

class PolygonCalc{

    public:
        const double helloworld(){
            return 0.123;
        }

        double test_calc(){

            polygon_type poly1 {{{0.0, 0.0}, {1.0, 0.0}, {1.0, 1.0}, {0.0, 1.0}}};
            polygon_type poly2 {{{5.5, 0.5}, {6.5, 0.5}, {6.5, 1.5}, {5.5, 1.5}}};

            bg::correct(poly1);
            bg::correct(poly2);

            std::deque<polygon_type> output;
            bg::intersection(poly1, poly2, output);

            double totalArea = 0.0;

            BOOST_FOREACH(polygon_type const& p, output)
            {

                totalArea += bg::area(p);
            }

            double d = bg::distance(poly1, poly2);

            std::cout << d << std::endl;

            return totalArea;

        }

        int test_nparray(double *A, int n){

            int i;
            double sum = 0.0;

            for (i=0; i<n; i++) {

                sum += A[i];

            }

            std::cout << "n: " << n << std::endl;

            std::cout << sum / n << std::endl;

            return n;

        }

        double min_poly_distance(double *poly1x, double *poly1y, double *poly2x, double *poly2y, int m, int n){

            int i;

            std::vector<point_type> points1;
            std::vector<point_type> points2;

            for (i=0; i<m; i++) {
                points1.push_back(point_type(poly1x[i], poly1y[i]));
            }

            for (i=0; i<n; i++) {
                points2.push_back(point_type(poly2x[i], poly2y[i]));
            }

            polygon_type poly1;
            polygon_type poly2;

            bg::assign_points(poly1, points1);
            bg::assign_points(poly2, points2);

            bg::correct(poly1);
            bg::correct(poly2);

            std::deque<polygon_type> output;
            bg::intersection(poly1, poly2, output);

            double totalArea = 0.0;

            BOOST_FOREACH(polygon_type const& p, output)
            {
                totalArea += bg::area(p);
            }

            if (totalArea > 0.0){
                return 0.0;
            }

            return bg::distance(poly1, poly2);

        }

        double poly_area(double *poly1x, double *poly1y, int m){

            int i;

            std::vector<point_type> points1;

            for (i=0; i<m; i++) {
                points1.push_back(point_type(poly1x[i], poly1y[i]));
            }

            polygon_type poly1;

            bg::assign_points(poly1, points1);

            bg::correct(poly1);

            double totalArea = bg::area(poly1);

            return totalArea;

        }

        double poly_intersection_area(double *poly1x, double *poly1y, double *poly2x, double *poly2y, int m, int n){

            int i;

            std::vector<point_type> points1;
            std::vector<point_type> points2;

            for (i=0; i<m; i++) {
                points1.push_back(point_type(poly1x[i], poly1y[i]));
            }

            for (i=0; i<n; i++) {
                points2.push_back(point_type(poly2x[i], poly2y[i]));
            }

            polygon_type poly1;
            polygon_type poly2;

            bg::assign_points(poly1, points1);
            bg::assign_points(poly2, points2);

            bg::correct(poly1);
            bg::correct(poly2);

            std::deque<polygon_type> output;
            bg::intersection(poly1, poly2, output);

            double totalArea = 0.0;

            BOOST_FOREACH(polygon_type const& p, output)
            {
                totalArea += bg::area(p);
            }

            return totalArea;

        }

        double poly_intersection_area_ratio(double *poly1x, double *poly1y, double *poly2x, double *poly2y, int m, int n){

            int i;

            std::vector<point_type> points1;
            std::vector<point_type> points2;

            for (i=0; i<m; i++) {
                points1.push_back(point_type(poly1x[i], poly1y[i]));
            }

            for (i=0; i<n; i++) {
                points2.push_back(point_type(poly2x[i], poly2y[i]));
            }

            polygon_type poly1;
            polygon_type poly2;

            bg::assign_points(poly1, points1);
            bg::assign_points(poly2, points2);

            bg::correct(poly1);
            bg::correct(poly2);

            std::deque<polygon_type> output;
            bg::intersection(poly1, poly2, output);

            double totalArea = 0.0;

            BOOST_FOREACH(polygon_type const& p, output)
            {
                totalArea += bg::area(p);
            }

            double area1 = bg::area(poly1);
            double area2 = bg::area(poly2);

            if (area1 > area2) {

                return totalArea / area2;

            } else {

                return totalArea / area1;

            }

            return totalArea;

        }

        unsigned long* old_group_elements(unsigned long *a, unsigned long *b,
            unsigned long *c, unsigned long *d, unsigned long n,
            double threshold_dist)
        {

            unsigned long *element_groups = new unsigned long[n];

            unsigned long i;

            std::vector<bool> v(n);
            std::fill(v.begin(), v.begin() + 2, true);

            std::vector<unsigned long> w(2);

            std::vector<point_type> points1;
            std::vector<point_type> points2;

            double height;

            double dist;

            double totalArea;

            std::vector< std::vector<unsigned long> > to_process = {};

            std::list< std::list<unsigned long> > res = {};

            std::cout << "threshold_dist: " << threshold_dist << std::endl;

            std::cout << "Computing polygon distances..." << std::endl;

            for (i = 0; i < n; i++) {

                to_process.push_back({i, i});

            }

            do {

                w = {};

                points1 = {};
                points2 = {};

                for (i = 0; i < n; i++) {

                    if (v[i]) {
                        w.push_back(i);
                    }

                }

                points1.push_back(point_type(a[w[0]], b[w[0]]));
                points1.push_back(point_type(c[w[0]], b[w[0]]));
                points1.push_back(point_type(a[w[0]], d[w[0]]));
                points1.push_back(point_type(c[w[0]], d[w[0]]));

                points2.push_back(point_type(a[w[1]], b[w[1]]));
                points2.push_back(point_type(c[w[1]], b[w[1]]));
                points2.push_back(point_type(a[w[1]], d[w[1]]));
                points2.push_back(point_type(c[w[1]], d[w[1]]));

                polygon_type poly1;
                polygon_type poly2;

                bg::assign_points(poly1, points1);
                bg::assign_points(poly2, points2);

                bg::correct(poly1);
                bg::correct(poly2);

                height = std::min(abs(double(d[w[0]] - b[w[0]])), abs(double(d[w[1]] - b[w[1]])));

                std::deque<polygon_type> output;
                bg::intersection(poly1, poly2, output);

                totalArea = 0.0;

                BOOST_FOREACH(polygon_type const& p, output)
                {
                    totalArea += bg::area(p);
                }

                if (totalArea > 0.0){
                    dist = 0.0;
                } else {
                    dist = bg::distance(poly1, poly2);
                }

                if (dist <= threshold_dist * height) {
                    to_process.push_back(w);
                }

            } while (std::prev_permutation(v.begin(), v.end()));

            std::cout << "Combining to nested list..." << std::endl;

            connectedcomponents(n, to_process, res);

            i = 0;

            for (auto const& el: res) {

                for (auto const& itm: el) {
                    element_groups[itm] = i;
                }

                i++;

            }

            return element_groups;

        }

        unsigned long* group_elements(unsigned long *a, unsigned long *b,
            unsigned long *c, unsigned long *d, unsigned long n,
            double threshold_dist, double slov_ratio)
        {

            unsigned long *element_groups = new unsigned long[n];

            unsigned long i;

            std::vector<bool> v(n);
            std::fill(v.begin(), v.begin() + 2, true);

            std::vector<unsigned long> w(2);

            std::vector<point_type> points1;
            std::vector<point_type> points2;

            double height, width, size_metric, min_height;

            double min_x_dist, min_y_dist, min_normal_dist;

            double pre_threshold_dist = std::max(5.0, threshold_dist);

            double dist;

            double totalArea;

            unsigned long y1, y2;

            std::vector< std::vector<unsigned long> > to_process = {};

            std::list< std::list<unsigned long> > res = {};

            std::cout << "threshold_dist: " << threshold_dist << std::endl;

            std::cout << "Computing polygon distances..." << std::endl;

            for (i = 0; i < n; i++) {

                to_process.push_back({i, i});

            }

            do {

                w = {};

                points1 = {};
                points2 = {};

                for (i = 0; i < n; i++) {

                    if (v[i]) {
                        w.push_back(i);
                    }

                }

                height = std::max(abs(double(d[w[0]] - b[w[0]])), abs(double(d[w[1]] - b[w[1]])));

                min_height = std::min(abs(double(d[w[0]] - b[w[0]])), abs(double(d[w[1]] - b[w[1]])));

                width = std::max(abs(double(c[w[0]] - a[w[0]])), abs(double(c[w[1]] - a[w[1]])));

                size_metric = std::max(height, width);

                min_x_dist = std::min({abs(double(a[w[0]] - c[w[1]])), abs(double(c[w[0]] - a[w[1]])), abs(double(a[w[0]] - a[w[1]])), abs(double(c[w[0]] - c[w[1]]))});

                min_y_dist = std::min({abs(double(b[w[0] - d[w[1]]])), abs(double(d[w[0]] - b[w[1]])), abs(double(b[w[0] - b[w[1]]])), abs(double(d[w[0]] - d[w[1]]))});

                min_normal_dist = std::min(min_x_dist, min_y_dist);

                if (min_normal_dist > pre_threshold_dist * size_metric) {
                    continue;
                }

                points1.push_back(point_type(a[w[0]], b[w[0]]));
                points1.push_back(point_type(c[w[0]], b[w[0]]));
                points1.push_back(point_type(a[w[0]], d[w[0]]));
                points1.push_back(point_type(c[w[0]], d[w[0]]));

                points2.push_back(point_type(a[w[1]], b[w[1]]));
                points2.push_back(point_type(c[w[1]], b[w[1]]));
                points2.push_back(point_type(a[w[1]], d[w[1]]));
                points2.push_back(point_type(c[w[1]], d[w[1]]));

                polygon_type poly1;
                polygon_type poly2;

                bg::assign_points(poly1, points1);
                bg::assign_points(poly2, points2);

                bg::correct(poly1);
                bg::correct(poly2);

                std::deque<polygon_type> output;
                bg::intersection(poly1, poly2, output);

                totalArea = 0.0;

                BOOST_FOREACH(polygon_type const& p, output)
                {
                    totalArea += bg::area(p);
                }

                if (totalArea > 0.0){
                    dist = 0.0;
                } else {
                    dist = bg::distance(poly1, poly2);
                }

                if (dist == 0) {
                    to_process.push_back(w);
                    continue;
                }

                if (b[w[0]] < d[w[0]]) {
                    y1 = b[w[0]];
                    y2 = d[w[0]];
                } else {
                    y1 = d[w[0]];
                    y2 = b[w[0]];
                }

                if (slov_ratio >= 0) {
                    if (! (((y1 <= b[w[1]] && b[w[1]] <= y2) && (y2 - b[w[1]]) / min_height >= slov_ratio ) || ((y1 <= d[w[1]] && d[w[1]] <= y2) && (d[w[1]] - y1) / min_height >= slov_ratio) || ((y1 <= b[w[1]]) && (y1 <= d[w[1]]) && (y2 >= b[w[1]]) && (y2 >= d[w[1]])))) {
                        continue;
                    }
                }

                if (dist <= threshold_dist * size_metric) {
                    to_process.push_back(w);
                }

            } while (std::prev_permutation(v.begin(), v.end()));

            std::cout << "Combining to nested list..." << std::endl;

            connectedcomponents(n, to_process, res);

            i = 0;

            for (auto const& el: res) {

                for (auto const& itm: el) {
                    element_groups[itm] = i;
                }

                i++;

            }

            return element_groups;

        }

};

extern "C" {
    PolygonCalc* PolygonCalc_new(){ return new PolygonCalc; }
    void PolygonCalc_delete(PolygonCalc *polygoncalc){ delete polygoncalc; }
    const double PolygonCalc_helloworld(PolygonCalc* polygoncalc){ return polygoncalc->helloworld(); }
    double PolygonCalc_test_calc(PolygonCalc* polygoncalc){ return polygoncalc->test_calc(); }
    int PolygonCalc_test_nparray(double *A, int n, PolygonCalc* polygoncalc){ return polygoncalc->test_nparray(A, n); }
    double PolygonCalc_min_poly_distance(PolygonCalc* polygoncalc, double *poly1x, double *poly1y, double *poly2x, double *poly2y, int m, int n){

        return polygoncalc->min_poly_distance(poly1x, poly1y, poly2x, poly2y, m, n);

    }
    double PolygonCalc_poly_area(PolygonCalc* polygoncalc, double *poly1x, double *poly1y, int m){ return polygoncalc->poly_area(poly1x, poly1y, m); }
    double PolygonCalc_poly_intersection_area(PolygonCalc* polygoncalc, double *poly1x, double *poly1y, double *poly2x, double *poly2y, int m, int n){

        return polygoncalc->poly_intersection_area(poly1x, poly1y, poly2x, poly2y, m, n);

    }
    double PolygonCalc_poly_intersection_area_ratio(PolygonCalc* polygoncalc, double *poly1x, double *poly1y, double *poly2x, double *poly2y, int m, int n){

        return polygoncalc->poly_intersection_area_ratio(poly1x, poly1y, poly2x, poly2y, m, n);

    }

    unsigned long* PolygonCalc_group_elements(PolygonCalc* polygoncalc, unsigned long *a, unsigned long *b, unsigned long *c, unsigned long *d, unsigned long n, double threshold_dist, double slov_ratio) {

        return polygoncalc->group_elements(a, b, c, d, n, threshold_dist, slov_ratio);

    }

    void free_long_array(unsigned long* pointer){

        delete[] pointer;

    }

}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_3.h>
#include <CGAL/Triangulation_vertex_base_with_info_3.h>
#include <Eigen/Dense>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkPolyData.h>
#include <vtkCellArray.h>
#include <vtkDoubleArray.h>
#include <vtkSmartPointer.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Triangulation_vertex_base_with_info_3<unsigned,K> Vb;
typedef CGAL::Triangulation_data_structure_3<Vb> Tds;
typedef Tds::Vertex_handle Vertex_handle;
typedef Tds::Cell_handle Cell_handle;
typedef CGAL::Delaunay_triangulation_3<K, Tds> Delaunay;
typedef Delaunay::Point Point;
typedef Eigen::Vector3d Vector3d;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::Matrix3d Matrix3d;
typedef Eigen::Matrix3Xd Matrix3Xd;
typedef Eigen::Map<Matrix3Xd> Map3Xd;

int main(){
    clock_t t1;
    t1 = clock();
    for(auto i=0; i < 10000; ++i){
        vtkNew<vtkPolyDataReader> reader;
        reader->SetFileName("Bad.vtk");
        reader->Update();
        auto poly = reader->GetOutput();
        auto N = poly->GetNumberOfPoints();
        auto pts = static_cast<double_t*>(poly->GetPoints()->GetData()->GetVoidPointer(0));
        Map3Xd points(pts,3,N);

        // Project points to unit sphere
        points.colwise().normalize();

        // Reset the center of the sphere to origin by translating
        Vector3d center = points.rowwise().mean();
        points = points.colwise() - center;

        // Rotate all points so that the point in 0th column is along z-axis
        Vector3d c = points.col(0);
        double_t cos_t = c(2);
        double_t sin_t = std::sqrt( 1 - cos_t*cos_t );
        Vector3d axis;
        axis << c(1), -c(0), 0.;
        Matrix3d rotMat, axis_cross, outer;
        axis_cross << 0. , -axis(2), axis(1),
                        axis(2), 0., -axis(0),
                        -axis(1), axis(0), 0.;

        outer.noalias() = axis*axis.transpose();

        rotMat = cos_t*Matrix3d::Identity() + sin_t*axis_cross +
                (1-cos_t)*outer;
        Matrix3Xd rPts(3,N);
        rPts = rotMat*points; // The points on a sphere rotated
	points = rPts;

        std::vector<std::pair<Point,unsigned>> spherePoints;
        spherePoints.push_back(std::make_pair(Point(0.,0.,0.),N));
        for( auto i=0; i < N; ++i){
            spherePoints.push_back( std::make_pair(Point(points(0,i),
                                                   points(1,i),
                                                   points(2,i)),
                                             i));
        }

        // Calculate the convex hull
        Delaunay T(spherePoints.begin(),spherePoints.end());

        // To extract the surface
        std::vector<Cell_handle> cells;
        T.tds().incident_cells( T.infinite_vertex(),
                                std::back_inserter(cells) );

        // Write to a vtk file
        vtkNew<vtkCellArray> triangles;
        for( auto c : cells ){
            auto infv = c->index(T.infinite_vertex());
            //triangles->InsertNextCell(3);
            for( auto j=0; j < 4; ++j){
                if (j == infv)
                    continue;
                triangles->InsertCellPoint(c->vertex(j)->info());
            }
        }
        poly->SetPolys(triangles);
        vtkNew<vtkPolyDataWriter> writer;
        writer->SetFileName("Mesh.vtk");
        writer->SetInputData(poly);
        writer->Write();
    }
    float diff(static_cast<float>(clock()) - static_cast<float>(t1));
    std::cout << "Time elapsed : " << diff / CLOCKS_PER_SEC
              << " seconds" << std::endl;
    return 0;
}


#include <cmath>
#include <chrono>
#include <thread>
#include <ctime>
#include <fstream>
#include <chrono>

#include <Eigen/Dense>

#include <cinder/app/App.h>
#include <cinder/app/RendererGl.h>
#include <cinder/gl/gl.h>
#include <cinder/CameraUi.h>
#include <cinder/params/Params.h>
#include <cinder/Log.h>
#include <cinder/ObjLoader.h>

#include <boost/archive/binary_iarchive.hpp>
#include <boost/serialization/unique_ptr.hpp>

#include <tinyformat.h>

#include "../resources/Resources.h"
#include <Utils.h>
#include <BackgroundWorker.h>
#include <TransferFunctionEditor.h>
#include <Integration.h>
#include <GridUtils.h>
#include <TimeIntegrator.h>
#include <SoftBodyGrid2D.h>
#include <SoftBodyMesh2D.h>
#include <SoftBody2DResults.h>

#include <IInverseProblem.h>
#include <InverseProblem_HumanSoftTissue.h>
#include <InverseProblem_ProbabilisticElastography.h>
#include <InverseProblem_Adjoint_YoungsModulus.h>
#include <InverseProblem_Adjoint_InitialConfiguration.h>
#include <InverseProblem_Adjoint_Dynamic.h>
#include "PartialObservations.h"
#include <GridVisualization.h>

using namespace ci;
using namespace ci::app;
using namespace std;
using namespace ar;
using namespace Eigen;

class InverseSoftBodyApp : public App {
public:
	InverseSoftBodyApp();
	void setup() override;
	void keyDown(KeyEvent event) override;
    void keyUp(KeyEvent event) override;
	void mouseDown( MouseEvent event ) override;
	void update() override;
	void draw() override;

private:

    typedef double real;

	//AntTweakBar settings
	params::InterfaceGlRef	params;
    bool printMode;

    enum class ComputationMode
    {
        COMPUTATION_MODE_GRID = 1,
        COMPUTATION_MODE_MESH = 2,
        COMPUTATION_MODE_BOTH = 3
    };
    int computationMode;

    //input - loading
    std::unique_ptr<SoftBody2DResults> inputResults;
    int inputTimestep;

    //input - generate
    int gridResolution;
    bool fitObjectToGrid;
    enum class Scene
    {
        SCENE_BAR,
        SCENE_TORUS
    };
    Scene scene;
    double torusOuterRadius;
    double torusInnerRadius;
    Vector2f rectCenter;
    Vector2f rectHalfSize;
    bool spaceBarPressed = false;
    TimeIntegrator::Integrator timeIntegratorType;
    TimeIntegrator::DenseLinearSolver denseLinearSolverType;
    TimeIntegrator::SparseLinearSolver sparseLinearSolverType;
    bool useSparseMatrices;
    int sparseSolverIterations;
    real sparseSolverTolerance;
    SoftBodySimulation::RotationCorrection rotationCorrectionMode;
    Vector2f gravity;
    Vector2f neumannForce;
    float youngsModulus;
    float poissonsRatio;
    float mass;
    float dampingAlpha;
    float dampingBeta;
    float timestep;
    bool gridExplicitDiffusion;
    bool gridHardDirichletBoundaries;
    SoftBodyGrid2D::AdvectionMode gridAdvectionMode;
    bool solveForwardStatic;
    SoftBodyMesh2D meshSolver;
    SoftBodyGrid2D gridSolver;

	bool enableDirichletBoundaries_;
	bool enableCollision_;
	real groundPlaneHeight;
	real groundPlaneAngle;
	real groundStiffness_;
	real collisionSoftminAlpha_;

	//reconstruction
	enum class ReconstructionMode
	{
		HUMAN_SOFT_TISSUE, //human soft tissue algorithm
		PROBABILISTIC_ELASTOGRAPHY,
        ADJOINT_YOUNGS_MODULUS,
        ADJOINT_INITIAL_POSITIONS,
		ADJOINT_DYNAMIC
	};
	ReconstructionMode reconstructionMode;
	std::map<ReconstructionMode, std::unique_ptr<IInverseProblem>> reconstructionAlgorithms;
	InverseProblemOutput resultMesh;
	InverseProblemOutput resultGrid;
	std::optional<GridUtils2D::grid_t> cleanedSdf;
    bool usePartialObservations;
    PartialObservations partialObservations;

    //rendering
	bool showGrid;
	enum class GridVisualizationMode
	{
		GRID_VISUALIZATION_U,
		GRID_VISUALIZATION_SOLUTION,
		GRID_VISUALIZATION_BOUNDARY,
		_GRID_VISUALIZATION_COUNT
	};
	GridVisualizationMode showGridSolution;
	bool showReconstructedSolution = false;
	std::mutex invalidateRenderingMutex;
	std::condition_variable invalidateRenderingConditionVariable;
	//visualization helpers
	GridVisualization visualization;

    //processing
    ar::BackgroundWorkerPtr worker;
	double meshElapsedSeconds = 0;
	double gridElapsedSeconds = 0;

	int reconstructSdfIterations = 5;

private:
    void invalidateRendering(bool wait);
    void drawSimulation();
	void loadSimulation();

    void resetSimulation();
    void runSimulation();

	void resetReconstruction();
	void performReconstruction();

	void reconstructSDF(int mode, int iterations);
    void testPlot();
};

InverseSoftBodyApp::InverseSoftBodyApp()
{
	//initial config
    printMode = false;
    computationMode = (int)ComputationMode::COMPUTATION_MODE_GRID;
    gridResolution = 22;
    fitObjectToGrid = false;
    scene = Scene::SCENE_BAR;
    torusOuterRadius = 0.1;
    torusInnerRadius = 0.03;
    rectCenter << 0.5, 0.7;
    rectHalfSize << 0.24, 0.06;
    showGrid = true;
    timeIntegratorType = TimeIntegrator::Integrator::Newmark1;
    denseLinearSolverType = TimeIntegrator::DenseLinearSolver::PartialPivLU;
    sparseLinearSolverType = TimeIntegrator::SparseLinearSolver::BiCGSTAB;
    useSparseMatrices = false;
    sparseSolverIterations = 100;
    sparseSolverTolerance = 1e-5;
    rotationCorrectionMode = SoftBodySimulation::RotationCorrection::Corotation;
    inputTimestep = 0;

    gridExplicitDiffusion = true;
    gridHardDirichletBoundaries = false;
    gridAdvectionMode = SoftBodyGrid2D::AdvectionMode::DIRECT_FORWARD;
    gravity = Vector2f(0, -30);
    neumannForce = Vector2f(0, 0);
    youngsModulus = 200;
    poissonsRatio = 0.45;
    mass = 1.0;
    dampingAlpha = 0.01;
    dampingBeta = 0.01;
    timestep = 0.01;
	solveForwardStatic = false;

	enableDirichletBoundaries_ = true;
	enableCollision_ = false;
	groundPlaneHeight = 0.6;
	groundPlaneAngle = 0;
	groundStiffness_ = 1000;
	collisionSoftminAlpha_ = 100;

	//create reconstruction algorithms
	reconstructionAlgorithms.emplace(ReconstructionMode::HUMAN_SOFT_TISSUE, std::make_unique<InverseProblem_HumanSoftTissue>());
	reconstructionAlgorithms.emplace(ReconstructionMode::PROBABILISTIC_ELASTOGRAPHY, std::make_unique<InverseProblem_ProbabilisticElastography>());
    reconstructionAlgorithms.emplace(ReconstructionMode::ADJOINT_YOUNGS_MODULUS, std::make_unique<InverseProblem_Adjoint_YoungsModulus>());
    reconstructionAlgorithms.emplace(ReconstructionMode::ADJOINT_INITIAL_POSITIONS, std::make_unique<InverseProblem_Adjoint_InitialConfiguration>());
	reconstructionAlgorithms.emplace(ReconstructionMode::ADJOINT_DYNAMIC, std::make_unique<InverseProblem_Adjoint_Dynamic>());
	reconstructionMode = ReconstructionMode::ADJOINT_DYNAMIC;
    usePartialObservations = true;
	showGridSolution = GridVisualizationMode::GRID_VISUALIZATION_BOUNDARY;

	resetSimulation();
}

void InverseSoftBodyApp::setup()
{
	//ANT TWEAK BAR
	//parameter ui, must happen before user-camera
	params = params::InterfaceGl::create(getWindow(), "Parameters", ivec2(350, 800));
    params->setOptions("", "refresh=0.05");
    params->addParam("PrintMode", &printMode).label("Print Mode");

	//input
    params->addButton("LoadSimulation", std::function<void()>([this]() {this->loadSimulation(); }), "label='Load Simulation'");
    params->addParam("InputStep", std::function<void(int)>([this](int v)
    {
        inputTimestep = v;
		showReconstructedSolution = false;
        invalidateRendering(false);
    }), std::function<int()>([this]()
    {
        return inputTimestep;
    })).label("Input Step").min(0);
    vector<string> computationModeEnums(4);
    computationModeEnums[(int)ComputationMode::COMPUTATION_MODE_GRID] = "grid";
    computationModeEnums[(int)ComputationMode::COMPUTATION_MODE_MESH] = "mesh";
    computationModeEnums[(int)ComputationMode::COMPUTATION_MODE_BOTH] = "overlay / both";
    params->addParam("Mode", computationModeEnums, (int*)&computationMode, "label='Mode'");

	//Soft Body properties - Generate Inputs
    params->addParam("InputResolution",
        std::function<void(int)>([this](int newValue) {this->gridResolution = newValue; this->resetSimulation(); }),
        std::function<int()>([this]() {return this->gridResolution; })
    ).min(4).group("Generate Input").label("Grid resolution").keyIncr("PGUP").keyDecr("PGDOWN");
    params->addParam("FitObjectToGrid",
        std::function<void(bool)>([this](bool newValue) {this->fitObjectToGrid = newValue; this->resetSimulation(); }),
        std::function<bool()>([this]() {return this->fitObjectToGrid; })
    ).group("Generate Input").label("Fit Object To Grid");
    vector<string> sceneEnums = { "Bar", "Torus" };
    params->addParam("Scene", sceneEnums, (int*)&scene).group("Generate Input").label("Scene")
        .accessors(std::function<void(int)>([this](int v)
    {
        scene = (Scene)v;
        resetSimulation();
        if (scene == Scene::SCENE_TORUS) {
            params->setOptions("InputTorusOuterRadius", "visible=true");
            params->setOptions("InputTorusInnerRadius", "visible=true");
            params->setOptions("InputRectCenterX", "visible=false");
            params->setOptions("InputRectHalfSizeX", "visible=false");
            params->setOptions("InputRectCenterY", "visible=false");
            params->setOptions("InputRectHalfSizeY", "visible=false");
        }
        else {
            params->setOptions("InputTorusOuterRadius", "visible=false");
            params->setOptions("InputTorusInnerRadius", "visible=false");
            params->setOptions("InputRectCenterX", "visible=true");
            params->setOptions("InputRectHalfSizeX", "visible=true");
            params->setOptions("InputRectCenterY", "visible=true");
            params->setOptions("InputRectHalfSizeY", "visible=true");
        }
    }), std::function<int()>([this]() {return (int)scene; }));
    params->addParam("InputTorusOuterRadius",
        std::function<void(double)>([this](double newValue) {this->torusOuterRadius = newValue; this->resetSimulation(); }),
        std::function<double()>([this]() {return this->torusOuterRadius; })
    ).min(0.01).max(0.5).step(0.01).group("Generate Input").label("Torus outer radius").visible(false);
    params->addParam("InputTorusInnerRadius",
        std::function<void(double)>([this](double newValue) {this->torusInnerRadius = newValue; this->resetSimulation(); }),
        std::function<double()>([this]() {return this->torusInnerRadius; })
    ).min(0.01).max(0.5).step(0.01).group("Generate Input").label("Torus inner radius").visible(false);
    params->addParam("InputRectCenterX",
        std::function<void(float)>([this](float newValue) {this->rectCenter.x() = newValue; this->resetSimulation(); }),
        std::function<float()>([this]() {return this->rectCenter.x(); })
    ).step(0.01).group("Generate Input").label("Rect center X").visible(true);
    params->addParam("InputRectCenterY",
        std::function<void(float)>([this](float newValue) {this->rectCenter.y() = newValue; this->resetSimulation(); }),
        std::function<float()>([this]() {return this->rectCenter.y(); })
    ).step(0.01).group("Generate Input").label("Rect center Y").visible(true);
    params->addParam("InputRectHalfSizeX",
        std::function<void(float)>([this](float newValue) {this->rectHalfSize.x() = newValue; this->resetSimulation(); }),
        std::function<float()>([this]() {return this->rectHalfSize.x(); })
    ).step(0.01).group("Generate Input").label("Rect half size X").visible(true);
    params->addParam("InputRectHalfSizeY",
        std::function<void(float)>([this](float newValue) {this->rectHalfSize.y() = newValue; this->resetSimulation(); }),
        std::function<float()>([this]() {return this->rectHalfSize.y(); })
    ).step(0.01).group("Generate Input").label("Rect half size Y").visible(true);
    params->addButton("InputReset", std::function<void()>([this]() {this->resetSimulation(); }), "label='Reset' group='Generate Input' key=r");

    params->addParam("SoftBodyGravity", &gravity.y()).step(0.001).group("Generate Input").label("Gravity");
    params->addParam("SoftBodyNeumannForce", &neumannForce.y()).step(0.001).group("Generate Input").label("Neumann Force")
        .accessors(std::function<void(float)>([this](float v)
    {
        neumannForce.y() = v;
        resetSimulation();
    }), std::function<float()>([this]() {
        return (float)neumannForce.y();
    }));
    params->addParam("SoftBodyYoungsModulus", &youngsModulus).min(0).step(0.01).group("Generate Input").label("Young's modulus");
    params->addParam("SoftBodyPoissonsRatio", &poissonsRatio).min(0.0001).max(0.4999).step(0.01).group("Generate Input").label("Poisson's ratio");
    params->addParam("SoftBodyMass", &mass).min(0.0001).step(0.01).group("Generate Input").label("Mass");
    params->addParam("SoftBodyDampingAlpha", &dampingAlpha).min(0).step(0.001).group("Generate Input").label("Damping on mass");
    params->addParam("SoftBodyDampingBeta", &dampingBeta).min(0).step(0.001).group("Generate Input").label("Damping on stiffness");
    vector<string> timeIntegratorTypeNames = { "Newmark 1", "Newmark 2", "Central Differences", "Linear Accelleration", "Newmark 3", "HHT-alpha" };
    params->addParam("SoftBodyTimeIntegration", timeIntegratorTypeNames, (int*)&timeIntegratorType).group("Generate Input").label("Time Integrator")
        .accessors(std::function<void(int)>([this](int v)
    {
        timeIntegratorType = (TimeIntegrator::Integrator)v;
        resetSimulation();
    }), std::function<int()>([this]() {return (int)timeIntegratorType; }));
#if SOFT_BODY_SUPPORT_SPARSE_MATRICES==1
    params->addParam("SoftBodyUseSparseMatrices", std::function<void(bool)>([this](bool v)
    {
        useSparseMatrices = v;
        if (v)
        {
            params->setOptions("SoftBodyDenseLinearSolver", "visible=false");
            params->setOptions("SoftBodySparseLinearSolver", "visible=true");
            params->setOptions("SoftBodySparseSolverIterations", "visible=true");
            params->setOptions("SoftBodySparseSolverTolerance", "visible=true");
        }
        else
        {
            params->setOptions("SoftBodyDenseLinearSolver", "visible=true");
            params->setOptions("SoftBodySparseLinearSolver", "visible=false");
            params->setOptions("SoftBodySparseSolverIterations", "visible=false");
            params->setOptions("SoftBodySparseSolverTolerance", "visible=false");
        }
    }), std::function<bool()>([this]()
    {
        return useSparseMatrices;
    })).group("Generate Input").label("Sparse matrices");
    vector<string> denseLinearSolverTypeNames = { "PartialPivLU", "FullPivLU", "HouseholderQR", "ColPivHousholderQR", "FullPivHouseholderQR", "CompleteOrthogonalDecomposition", "LLT", "LDLT" };
    params->addParam("SoftBodyDenseLinearSolver", denseLinearSolverTypeNames, (int*)&denseLinearSolverType).group("Generate Input").label("Dense Linear Solver");
    vector<string> sparseLinearSolverTypeNames = { "Conjugate Gradient", "BiCGSTAB ", "Sparse-LU" };
    params->addParam("SoftBodySparseLinearSolver", sparseLinearSolverTypeNames, (int*)&sparseLinearSolverType).group("Generate Input").label("Sparse Linear Solver");
    params->addParam("SoftBodySparseSolverIterations", &sparseSolverIterations).group("Generate Input").label("Sparese Solver iterations").min(0);
    params->addParam("SoftBodySparseSolverTolerance", &sparseSolverTolerance).group("Generate Input").label("Sparese Solver tolerance").min(0).step(0.00001);
#else
    vector<string> denseLinearSolverTypeNames = { "PartialPivLU", "FullPivLU", "HouseholderQR", "ColPivHousholderQR", "FullPivHouseholderQR", "CompleteOrthogonalDecomposition", "LLT", "LDLT" };
    params->addParam("SoftBodyDenseLinearSolver", denseLinearSolverTypeNames, (int*)&denseLinearSolverType).group("Generate Input").label("Dense Linear Solver");
#endif
    params->addParam("SoftBodyTimeStep", &timestep).min(0.001).step(0.001).group("Generate Input").label("Time step");
    params->addParam("SoftBodyGridExplicitDiffusion", &gridExplicitDiffusion, "group='Generate Input' label='Grid Displacement Diffusion' true='explicit (post-process)' false='implicit (matrix)'");
    params->addParam("SoftBodyGridHardDirichletBoundaries", &gridHardDirichletBoundaries).group("Generate Input").label("Grid Hard Dirichlet Boundaries");
    vector<string> softBodyAdvectionNames;
    for (int i = 0; i < static_cast<int>(SoftBodyGrid2D::AdvectionMode::_COUNT_); ++i)
        softBodyAdvectionNames.push_back(SoftBodyGrid2D::advectionModeName(static_cast<SoftBodyGrid2D::AdvectionMode>(i)));
    params->addParam("SoftBodyAdvectionMode", softBodyAdvectionNames, (int*)&gridAdvectionMode).group("Generate Input").label("Grid Advection");

    vector<string> rotationCorrectionNames = { "None", "Corotation" };
    params->addParam("SoftBodyRotationCorrection", rotationCorrectionNames, (int*)&rotationCorrectionMode).group("Generate Input").label("Rotation correction");

	params->addParam("SoftBodyDirichlet",
		std::function<void(bool)>([this](bool newValue) {this->enableDirichletBoundaries_ = newValue; this->resetSimulation(); }),
		std::function<bool()>([this]() {return this->enableDirichletBoundaries_; }))
		.group("Generate Input").label("Enable Dirichlet Boundaries");
	params->addParam("SoftBodyCollision", &enableCollision_).group("Generate Input").label("Enable Collision");
	params->addParam("InputGroundPlaneHeight", &groundPlaneHeight).min(0).max(1).step(0.001).group("Generate Input").label("Ground Height");
	params->addParam("InputGroundPlaneAngle", &groundPlaneAngle).min(-1).max(1).step(0.001).group("Generate Input").label("Ground Angle");
	params->addParam("SoftBodyCollisionGroundStiffness", &groundStiffness_).group("Generate Input").label("Ground Stiffness").min(0).step(0.001);
	params->addParam("SoftBodyCollisionSoftmaxAlpha", &collisionSoftminAlpha_).group("Generate Input").label("Collision Softmax-Alpha").min(1).max(1000).step(0.001);

	params->addParam("SoftBodyStaticDynamic", &solveForwardStatic).group("Generate Input").label("Solution Mode").optionsStr("true='Static' false='Dynamic'");

	//reconstruction
	vector<string> reconstructionModeNames = {
	    "Human Soft Tissue",
	    "Probabilistic Elastography",
        "Adjoint - Youngs Modulus",
        "Adjoint - Initial Positions",
		"Adjoint - Dynamic"
	};
	params->addParam("ReconstructionAlgorithm", reconstructionModeNames, (int*)&reconstructionMode)
		.group("Reconstruction").label("Algorithm")
		.accessors(std::function<void(int)>([this](int v) {
			reconstructionAlgorithms[reconstructionMode]->setParamsVisibility(params, false);
			reconstructionMode = static_cast<ReconstructionMode>(v);
			reconstructionAlgorithms[reconstructionMode]->setParamsVisibility(params, true);
			resetReconstruction();
		}), std::function<int()>([this]() {
			return static_cast<int>(reconstructionMode);
		}));
	for (auto& p : reconstructionAlgorithms) {
		p.second->setupParams(params, "Reconstruction");
		p.second->setParamsVisibility(params, false);
	}
	reconstructionAlgorithms[reconstructionMode]->setParamsVisibility(params, true);
    params->addParam("ReconstructionUsePartialObservation", &usePartialObservations).label("Partial Observation").group("Reconstruction");
	params->addButton("ReconstructionRun", std::function<void()>([this]() {this->performReconstruction(); }), "group='Reconstruction' label='Solve' key=RETURN");
	params->addButton("ReconstructionCancel", std::function<void()>([this]() {
		if (worker) worker->interrupt();
	}), "group='Reconstruction' label='Cancel'");

    //partial observations
    partialObservations.initParams(params);

	//rendering
    params->addParam("RenderingShowGrid", &showGrid).group("Rendering").label("Show grid");
	vector<string> gridVisualizationModeEnums((int)GridVisualizationMode::_GRID_VISUALIZATION_COUNT);
	gridVisualizationModeEnums[(int)GridVisualizationMode::GRID_VISUALIZATION_U] = "u";
	gridVisualizationModeEnums[(int)GridVisualizationMode::GRID_VISUALIZATION_SOLUTION] = "solution";
	gridVisualizationModeEnums[(int)GridVisualizationMode::GRID_VISUALIZATION_BOUNDARY] = "boundary";
	params->addParam("RenderingShowGridSolutionMode", gridVisualizationModeEnums, (int*)&showGridSolution, "label='Grid solution' group=Rendering");
	params->addParam("RenderingShowSolution", std::function<void(bool)>([this](bool v)
	{
		showReconstructedSolution = v;
		invalidateRendering(false);
	}), std::function<bool()>([this]()
	{
		return showReconstructedSolution;
	})).label("Show Reconstruction");

	params->addParam("RecontructSdfIterations", &reconstructSdfIterations).label("Reconstuct Sdf - Iterations").min(0).group("Test");
	params->addButton("ReconstructSdfViscosity", std::function<void()>([this]() {this->reconstructSDF(1, this->reconstructSdfIterations); }), "label='Reconstruct SDF - Viscosity' group=Test ");
	params->addButton("ReconstructSdfUpwind", std::function<void()>([this]() {this->reconstructSDF(2, this->reconstructSdfIterations); }), "label='Reconstruct SDF - Upwind' group=Test ");
	params->addButton("ReconstructSdfSussmann", std::function<void()>([this]() {this->reconstructSDF(3, this->reconstructSdfIterations); }), "label='Reconstruct SDF - Sussmann' group=Test ");
    params->addButton("ReconstructSdfFastMarching", std::function<void()>([this]() {this->reconstructSDF(4, this->reconstructSdfIterations); }), "label='Reconstruct SDF - Fast Marching' group=Test ");
    params->addButton("TestPlot", std::function<void()>([this]() {this->testPlot(); }), "label='Test Plot' group=Test ");
	params->setOptions("Test", "opened=false");

	visualization.setup();
}

void InverseSoftBodyApp::keyDown(KeyEvent event)
{
	App::keyDown(event);
    if (event.isHandled()) return;
	if (event.getChar() == 'f') {
		// Toggle full screen when the user presses the 'f' key.
		setFullScreen(!isFullScreen());
	}
	else if (event.getCode() == KeyEvent::KEY_ESCAPE) {
		// Exit full screen, or quit the application, when the user presses the ESC key.
		if (isFullScreen())
			setFullScreen(false);
		else
			quit();
	}
	else if (event.getChar() == 'p') {
		//Screenshot
		Surface surface = copyWindowSurface();
        //if (printMode) {
        //    //only save the grid
        //    int windowWidth = getWindow()->getWidth();
        //    int windowHeight = getWindow()->getHeight();
        //    int gridBoundary = 50;
        //    int gridSize = std::min(windowWidth, windowHeight) - 2 * gridBoundary;
        //    int gridOffsetX = windowWidth / 2;
        //    int gridOffsetY = windowHeight / 2;
        //    Surface surface2(gridSize + 10, gridSize + 10, false);
        //    surface2.copyFrom(surface, Area(gridOffsetX - 5, gridOffsetY - 5, gridOffsetX + gridSize + 10, gridOffsetY + gridSize + 10));
        //    surface = surface2;
        //}
		//construct filename
		time_t now = time(NULL);
		struct tm tstruct;
		char buf[100];
		tstruct = *localtime(&now);
		strftime(buf, sizeof(buf), "%d-%m-%Y_%H-%M-%S", &tstruct);
		string fileName = string("../screenshots/InverseSoftBodyApp-") + string(buf) + ".png";
		//write out
		writeImage(fileName, surface);
        CI_LOG_I("Screenshot saved to " << fileName);
	}
    else if (event.getCode() == KeyEvent::KEY_SPACE)
    {
        spaceBarPressed = true;
    }
}

void InverseSoftBodyApp::keyUp(KeyEvent event)
{
    App::keyUp(event);
    if (event.isHandled()) return;
    if (event.getCode() == KeyEvent::KEY_SPACE)
    {
        spaceBarPressed = false;
    }
}

void InverseSoftBodyApp::mouseDown( MouseEvent event )
{
	App::mouseDown(event);
}

void InverseSoftBodyApp::update()
{
    //perform time stepping
    if (spaceBarPressed) {
		runSimulation();
    }

	//update transfer function editor
	visualization.update();
}

void InverseSoftBodyApp::drawSimulation()
{
	// Draw sdf
	if (computationMode & (int)ComputationMode::COMPUTATION_MODE_GRID)
	{
		visualization.gridDrawSdf();
		if (showGrid)
			visualization.gridDrawGridLines();
		if (showGridSolution == GridVisualizationMode::GRID_VISUALIZATION_BOUNDARY)
			visualization.gridDrawObjectBoundary();
		if (!gridSolver.hasSolution())
			visualization.gridDrawBoundaryConditions(gridSolver.getGridDirichlet(), gridSolver.getGridNeumannX(), gridSolver.getGridNeumannY());
		if (gridSolver.hasSolution() && showGridSolution == GridVisualizationMode::GRID_VISUALIZATION_U) {
			gl::ScopedColor c;
			if (printMode)
				gl::color(0, 0, 0.5);
			else
				gl::color(1, 1, 1);
			visualization.gridDrawDisplacements();
		}
	}

	// Draw partial observations
	if (computationMode & (int)ComputationMode::COMPUTATION_MODE_GRID
		&& usePartialObservations) {
		gl::ScopedMatrices m;
		visualization.applyTransformation();
		gl::ScopedColor c;
		partialObservations.draw();
	}

	// Draw mesh
	if (computationMode & (int)ComputationMode::COMPUTATION_MODE_MESH) {
		visualization.meshDraw();
	}

	// Draw ground
	if (enableCollision_)
	{
		visualization.drawGround(groundPlaneHeight, groundPlaneAngle);
	}
}

void InverseSoftBodyApp::draw()
{
    using namespace ar::utils;
    if (printMode)
        gl::clear(Color(1, 1, 1));
    else
        gl::clear(Color(0, 0, 0));

    // WINDOW SPACE
    gl::disableDepthRead();
    gl::disableDepthWrite();
    gl::setMatricesWindow(getWindowSize(), true);

    //draw the simulation
    if (inputResults != nullptr)
    {
        drawSimulation();
    }

    // Draw the background worker's status
    if (worker && !worker->isDone()) {
        //draw waiting animation
        {
            gl::ScopedModelMatrix scopedMatrix;
            gl::ScopedColor scopedColor;
            gl::translate(25, getWindowHeight() - 50);
            int step; double dummy; step = static_cast<int>(std::modf(getElapsedSeconds(), &dummy) * 8);
            for (int i = 0; i < 8; ++i) {
                float c = ((i + step)%8) / 7.0f;
                gl::color(c, c, c);
                gl::drawSolidRoundedRect(Rectf(5, -2, 15, 2), 2);
                gl::rotate(-2.0f * M_PI / 8.0f);
            }
        }
        //draw status
        gl::ScopedColor scopedColor;
        if (printMode)
            gl::color(0, 0, 0);
        else
            gl::color(1, 1, 1);
        gl::drawString(worker->getStatus(), vec2(50, getWindowHeight() - 50));
    }

	//draw result
	{
		static Font font("Arial", 20);
		static ColorA color(1, 1, 1, 1);
		gl::ScopedColor scopedColor;
		if (printMode)
			gl::color(0, 0, 0);
		else
			gl::color(1, 1, 1);
		int gridSize = std::min(getWindowWidth(), getWindowHeight()) - 100;
		int x = (getWindowWidth() + gridSize) / 2 + 50;
		int y = 50;
		int stepY = 15;

		gl::drawString("Results Mesh", vec2(x, y), color, font); y += stepY;
		gl::drawString("<Prop>: <Computed> - <Truth> (<StdDev>)", vec2(x, y), color, font); y += stepY;
		if (resultMesh.youngsModulus_.has_value()) {
			gl::drawString(
				tfm::format("Young's Modulus: %.2f - %.2f (%.2f)", resultMesh.youngsModulus_.value(), inputResults->settings_.youngsModulus_, resultMesh.youngsModulusStdDev_.value_or(0))
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultMesh.poissonsRatio_.has_value()) {
			gl::drawString(
				tfm::format("Poisson Ratio: %.2f - %.2f", resultMesh.poissonsRatio_.value(), inputResults->settings_.poissonsRatio_)
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultMesh.mass_.has_value()) {
			gl::drawString(
				tfm::format("Mass: %.2f - %.2f (%.2f)", resultMesh.mass_.value(), inputResults->settings_.mass_, resultMesh.massStdDev_.value_or(0))
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultMesh.dampingAlpha_.has_value()) {
			gl::drawString(
				tfm::format("Mass Damping: %.2f - %.2f (%.2f)", resultMesh.dampingAlpha_.value(), inputResults->settings_.dampingAlpha_, resultMesh.dampingAlphaStdDev_.value_or(0))
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultMesh.dampingBeta_.has_value()) {
			gl::drawString(
				tfm::format("Stiffness Damping: %.2f - %.2f (%.2f)", resultMesh.dampingBeta_.value(), inputResults->settings_.dampingBeta_, resultMesh.dampingBetaStdDev_.value_or(0))
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultMesh.groundHeight.has_value())
		{
			gl::drawString(
				tfm::format("Ground Height: %.3f - %.3f", resultMesh.groundHeight.value(), inputResults->settings_.groundPlaneHeight_)
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultMesh.groundAngle.has_value())
		{
			gl::drawString(
				tfm::format("Ground Angle: %.3f - %.3f", resultMesh.groundAngle.value(), inputResults->settings_.groundPlaneAngle_)
				, vec2(x, y), color, font);
			y += stepY;
		}
		gl::drawString(tfm::format("Final Cost: %.3f", resultMesh.finalCost_), vec2(x, y), color, font); y += stepY;
		if (meshElapsedSeconds > 0)
			gl::drawString(tfm::format("Time: %.2f sec", meshElapsedSeconds), vec2(x, y), color, font); y += stepY;

		y += stepY;
		gl::drawString("Results Grid", vec2(x, y), color, font); y += stepY;
		gl::drawString("<Prop>: <Computed> - <Truth> (<StdDev>)", vec2(x, y), color, font); y += stepY;
		if (resultGrid.youngsModulus_.has_value()) {
			gl::drawString(
				tfm::format("Young's Modulus: %.2f - %.2f (%.2f)", resultGrid.youngsModulus_.value(), inputResults->settings_.youngsModulus_, resultGrid.youngsModulusStdDev_.value_or(0))
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultGrid.poissonsRatio_.has_value()) {
			gl::drawString(
				tfm::format("Poisson Ratio: %.2f - %.2f", resultMesh.poissonsRatio_.value(), inputResults->settings_.poissonsRatio_)
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultGrid.mass_.has_value()) {
			gl::drawString(
				tfm::format("Mass: %.2f - %.2f (%.2f)", resultGrid.mass_.value(), inputResults->settings_.mass_, resultGrid.massStdDev_.value_or(0))
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultGrid.dampingAlpha_.has_value()) {
			gl::drawString(
				tfm::format("Mass Damping: %.2f - %.2f (%.2f)", resultGrid.dampingAlpha_.value(), inputResults->settings_.dampingAlpha_, resultGrid.dampingAlphaStdDev_.value_or(0))
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultGrid.dampingBeta_.has_value()) {
			gl::drawString(
				tfm::format("Stiffness Damping: %.2f - %.2f (%.2f)", resultGrid.dampingBeta_.value(), inputResults->settings_.dampingBeta_, resultGrid.dampingBetaStdDev_.value_or(0))
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultGrid.groundHeight.has_value())
		{
			gl::drawString(
				tfm::format("Ground Height: %.3f - %.3f", resultGrid.groundHeight.value(), inputResults->settings_.groundPlaneHeight_)
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultGrid.groundAngle.has_value())
		{
			gl::drawString(
				tfm::format("Ground Angle: %.3f - %.3f", resultGrid.groundAngle.value(), inputResults->settings_.groundPlaneAngle_)
				, vec2(x, y), color, font);
			y += stepY;
		}
		gl::drawString(tfm::format("Final Cost: %.3f", resultGrid.finalCost_), vec2(x, y), color, font); y += stepY;
		if (gridElapsedSeconds > 0)
			gl::drawString(tfm::format("Time: %.2f sec", gridElapsedSeconds), vec2(x, y), color, font); y += stepY;
	}

	// Draw the interface
	visualization.setTfeVisible(!printMode);
	params->draw();
	visualization.drawTransferFunctionEditor();

	//notify waiting threads
	invalidateRenderingConditionVariable.notify_all();
}

void InverseSoftBodyApp::invalidateRendering(bool wait)
{
	//GRID
	SoftBodyGrid2D::grid_t referenceSdf = inputResults->gridReferenceSdf_;
	SoftBodyGrid2D::grid_t currentSdf;
	SoftBodyGrid2D::grid_t currentUx = GridUtils2D::grid_t::Zero(gridSolver.getGridResolution(), gridSolver.getGridResolution());
	SoftBodyGrid2D::grid_t currentUy = GridUtils2D::grid_t::Zero(gridSolver.getGridResolution(), gridSolver.getGridResolution());
	if (inputTimestep > 0) {
		if (cleanedSdf.has_value()) {
			currentSdf = cleanedSdf.value();
		}
		else if (showReconstructedSolution && resultGrid.resultGridSdf_.has_value()) {
			currentSdf = resultGrid.resultGridSdf_.value();
		}
		else if (showReconstructedSolution && resultGrid.initialGridSdf_.has_value()) {
			currentSdf = resultGrid.initialGridSdf_.value();
		}
		else {
			if (inputResults->gridResultsSdf_.size() > inputTimestep - 1) {
				currentSdf = inputResults->gridResultsSdf_[inputTimestep - 1];
				currentUx = inputResults->gridResultsUx_[inputTimestep - 1];
				currentUy = inputResults->gridResultsUy_[inputTimestep - 1];
			}
			else
				currentSdf = inputResults->gridReferenceSdf_;
		}
	}
	else
	{
		currentSdf = inputResults->gridReferenceSdf_;
	}
	if (gridSolver.hasSolution())
	{
		visualization.setGrid(gridSolver.getSdfReference(), gridSolver.getSdfSolution(), gridSolver.getUGridX(), gridSolver.getUGridY());
	}
	else
	{
		GridUtils2D::grid_t z = GridUtils2D::grid_t::Zero(gridSolver.getGridResolution(), gridSolver.getGridResolution());
		visualization.setGrid(gridSolver.getSdfReference(), gridSolver.getSdfReference(), z, z);
	}

	//MESH
	auto pos = inputResults->meshReferencePositions_;
	if (inputTimestep > 0) {
		if (showReconstructedSolution && resultMesh.resultMeshDisp_.has_value()) {
			for (size_t i = 0; i < pos.size(); ++i)
				pos[i] += resultMesh.resultMeshDisp_.value()[i];
		}
		else if (showReconstructedSolution && resultMesh.initialMeshPositions_.has_value()) {
			pos = resultMesh.initialMeshPositions_.value();
		}
		else {
			if (inputResults->meshResultsDisplacement_.size() > inputTimestep - 1)
				for (size_t i = 0; i < pos.size(); ++i)
					pos[i] += inputResults->meshResultsDisplacement_[inputTimestep - 1][i];
		}
	}
	visualization.setMesh(pos, inputResults->meshReferenceIndices_, meshSolver.getNodeStates());

	//wait for the rendering to happen
	if (wait)
	{
		std::unique_lock<std::mutex> lock(invalidateRenderingMutex);
		invalidateRenderingConditionVariable.wait_for(lock, 50ms);
	}
}

void InverseSoftBodyApp::loadSimulation()
{
	//get save path
	fs::path path = getOpenFilePath(fs::path("../saves/"));
	if (path.empty()) {
		CI_LOG_W("Saving cancelled by the user");
		return;
	}
	std::string pathS = path.string();
	CI_LOG_I("Load results from " << pathS);

	//load it
	std::ifstream ifs(pathS, std::ifstream::binary);
	boost::archive::binary_iarchive ia(ifs);
	ia >> inputResults;
	CI_LOG_I("Results loaded");
    params->setOptions("InputStep", "max=" + std::to_string(inputResults->numSteps_));
	inputTimestep = 0;

	resetReconstruction();
}

void InverseSoftBodyApp::resetSimulation()
{
	//reset solution
	inputResults = std::make_unique<SoftBody2DResults>();

	//create scene
	switch (scene)
	{
	case Scene::SCENE_BAR:
		gridSolver = SoftBodyGrid2D::CreateBar(
			rectCenter.cast<real>(), rectHalfSize.cast<real>(), gridResolution,
			fitObjectToGrid, enableDirichletBoundaries_, Vector2(0, 0), neumannForce.cast<real>());
		inputResults->initGridReference(gridSolver);
		meshSolver = SoftBodyMesh2D::CreateBar(
			rectCenter.cast<real>(), rectHalfSize.cast<real>(),
			gridResolution, fitObjectToGrid,
			enableDirichletBoundaries_, Vector2(0, 0), neumannForce.cast<real>());
		inputResults->initMeshReference(meshSolver);
		break;
	case Scene::SCENE_TORUS:
		gridSolver = SoftBodyGrid2D::CreateTorus(
			torusOuterRadius, torusInnerRadius, gridResolution,
			enableDirichletBoundaries_, Vector2(0, 0), neumannForce.cast<real>());
		inputResults->initGridReference(gridSolver);
		meshSolver = SoftBodyMesh2D::CreateTorus(
			torusOuterRadius, torusInnerRadius,
			gridResolution, enableDirichletBoundaries_, Vector2(0, 0), neumannForce.cast<real>().eval());
		inputResults->initMeshReference(meshSolver);
	}

	resetReconstruction();
	//invalidate texture and mesh
	showReconstructedSolution = false;
	inputTimestep = 0;
	invalidateRendering(false);
}

void InverseSoftBodyApp::runSimulation()
{
	if (worker != nullptr && !worker->isDone()) {
		//still running
		return;
	}

	if (solveForwardStatic && !enableDirichletBoundaries_)
	{
		CI_LOG_E("Can't solve for static solution if dirichlet boundaries are disabled");
		return;
	}

	//declare background task
	std::function<void(BackgroundWorker*)> task = [this](BackgroundWorker* worker) {
		//pass arguments
		meshSolver.setGravity(gravity.cast<real>());
		meshSolver.setMaterialParameters(youngsModulus, poissonsRatio);
		meshSolver.setMass(mass);
		meshSolver.setDamping(dampingAlpha, dampingBeta);
		meshSolver.setDenseLinearSolver(denseLinearSolverType);
		meshSolver.setSparseLinearSolver(sparseLinearSolverType);
		meshSolver.setTimeIntegrator(timeIntegratorType);
		meshSolver.setUseSparseMatrices(useSparseMatrices);
		meshSolver.setSparseSolveIterations(sparseSolverIterations);
		meshSolver.setSparseSolveTolerance(sparseSolverTolerance);
		meshSolver.setRotationCorrection(rotationCorrectionMode);
		meshSolver.setTimestep(timestep);
		meshSolver.setGroundPlane(groundPlaneHeight, groundPlaneAngle);
		meshSolver.setEnableCollision(enableCollision_);
		meshSolver.setCollisionResolution(SoftBodySimulation::CollisionResolution::SPRING_IMPLICIT);
		meshSolver.setCollisionVelocityDamping(0);
		meshSolver.setGroundStiffness(groundStiffness_);
		meshSolver.setCollisionSoftmaxAlpha(collisionSoftminAlpha_);
		gridSolver.setGravity(gravity.cast<real>());
		gridSolver.setMaterialParameters(youngsModulus, poissonsRatio);
		gridSolver.setMass(mass);
		gridSolver.setDamping(dampingAlpha, dampingBeta);
		gridSolver.setDenseLinearSolver(denseLinearSolverType);
		gridSolver.setSparseLinearSolver(sparseLinearSolverType);
		gridSolver.setTimeIntegrator(timeIntegratorType);
		gridSolver.setUseSparseMatrices(useSparseMatrices);
		gridSolver.setSparseSolveIterations(sparseSolverIterations);
		gridSolver.setSparseSolveTolerance(sparseSolverTolerance);
		gridSolver.setExplicitDiffusion(gridExplicitDiffusion);
		gridSolver.setHardDirichletBoundaries(gridHardDirichletBoundaries);
		gridSolver.setAdvectionMode(gridAdvectionMode);
		gridSolver.setRotationCorrection(rotationCorrectionMode);
		gridSolver.setTimestep(timestep);
		gridSolver.setGroundPlane(groundPlaneHeight, groundPlaneAngle);
		gridSolver.setEnableCollision(enableCollision_);
		gridSolver.setCollisionResolution(SoftBodySimulation::CollisionResolution::SPRING_IMPLICIT);
		gridSolver.setCollisionVelocityDamping(0);
		gridSolver.setGroundStiffness(groundStiffness_);
		gridSolver.setCollisionSoftmaxAlpha(collisionSoftminAlpha_);

		inputResults->settings_ = gridSolver.getSettings();

		//solve it
		if (int(computationMode) & int(ComputationMode::COMPUTATION_MODE_MESH)) {
			auto start1 = std::chrono::steady_clock::now();
			if (solveForwardStatic) {
				meshSolver.solveStaticSolution(worker);
				if (worker->isInterrupted()) return;
			}
			else {
				meshSolver.solveDynamicSolution(worker);
				if (worker->isInterrupted()) return;
			}
			auto duration1 = std::chrono::duration_cast<chrono::milliseconds>(std::chrono::steady_clock::now() - start1);
			meshElapsedSeconds = duration1.count() / 1000.0;
			inputResults->meshResultsDisplacement_.push_back(meshSolver.getCurrentDisplacements());
			inputResults->meshResultsVelocities_.push_back(meshSolver.getCurrentVelocities());
		}

		if (int(computationMode) & int(ComputationMode::COMPUTATION_MODE_GRID)) {
			auto start2 = std::chrono::steady_clock::now();
			if (solveForwardStatic) {
				gridSolver.solveStaticSolution(worker);
				if (worker->isInterrupted()) return;
			}
			else {
				gridSolver.solveDynamicSolution(worker);
				if (worker->isInterrupted()) return;
			}
			auto duration2 = std::chrono::duration_cast<chrono::milliseconds>(std::chrono::steady_clock::now() - start2);
            CI_LOG_D("Reference simulation result:\n" << gridSolver.getSdfSolution());
            CI_LOG_D("Reference uX:\n" << gridSolver.getUGridX());
            CI_LOG_D("Reference uY:\n" << gridSolver.getUGridY());
            CI_LOG_D("Settings: " << gridSolver.getSettings() << " " << gridSolver.getGridSettings());
			gridElapsedSeconds = duration2.count() / 1000.0;
			inputResults->gridResultsSdf_.push_back(gridSolver.getSdfSolution());
			inputResults->gridResultsUx_.push_back(gridSolver.getUGridX());
			inputResults->gridResultsUy_.push_back(gridSolver.getUGridY());
			inputResults->gridResultsUxy_.push_back(gridSolver.getUSolution());
            partialObservations.setSdf(gridSolver.getSdfReference(), gridSolver.getSdfSolution(), gridSolver.getUGridX(), gridSolver.getUGridY());
            inputResults->gridPartialObservations_.push_back(partialObservations.getObservations());
		}

		inputResults->numSteps_++;
		inputTimestep = inputResults->numSteps_;
		showReconstructedSolution = false;
		params->setOptions("InputStep", "max=" + std::to_string(inputResults->numSteps_));
		invalidateRendering(false);
	};

	//start background worker
	worker = make_shared<BackgroundWorker>(task);
	CI_LOG_I("Background worker started");
}

void InverseSoftBodyApp::resetReconstruction()
{
	CI_LOG_D("reset");
	worker = nullptr;

	cleanedSdf.reset();
	resultMesh = InverseProblemOutput();
	resultGrid = InverseProblemOutput();
	meshElapsedSeconds = 0;
	gridElapsedSeconds = 0;
	invalidateRendering(false);
}

void InverseSoftBodyApp::performReconstruction()
{
	if (worker != nullptr && !worker->isDone()) {
		//still running
		return;
	}

	if (this->inputTimestep == 0) {
		CI_LOG_I("no timestep selected");
		return;
	}

	resetReconstruction();

	//declare background task
	std::function<void(BackgroundWorker*)> task = [this](BackgroundWorker* worker) {
		//get algorithm
		IInverseProblem* alg = reconstructionAlgorithms[reconstructionMode].get();
		alg->setInput(inputResults.get());
        if (InverseProblem_Adjoint_Dynamic* algD = dynamic_cast<InverseProblem_Adjoint_Dynamic*>(alg))
        {
            algD->setGridUsePartialObservation(usePartialObservations);
        }
		//run them
		if (computationMode & (int)ComputationMode::COMPUTATION_MODE_MESH) {
			auto start1 = std::chrono::steady_clock::now();
			resultMesh = alg->solveMesh(this->inputTimestep-1, worker, [this](const InverseProblemOutput& intermediateSolution)
			{
				showReconstructedSolution = true;
				resultMesh = intermediateSolution;
				invalidateRendering(true);
			});
			auto duration1 = std::chrono::duration_cast<chrono::milliseconds>(std::chrono::steady_clock::now() - start1);
			meshElapsedSeconds = duration1.count() / 1000.0;
			if (worker->isInterrupted()) return;
		}
		if (computationMode & (int)ComputationMode::COMPUTATION_MODE_GRID) {
			auto start2 = std::chrono::steady_clock::now();
			resultGrid = alg->solveGrid(this->inputTimestep-1, worker, [this](const InverseProblemOutput& intermediateSolution)
			{
				showReconstructedSolution = true;
				resultGrid = intermediateSolution;
				invalidateRendering(true);
			});
			if (worker->isInterrupted()) return;
			auto duration2 = std::chrono::duration_cast<chrono::milliseconds>(std::chrono::steady_clock::now() - start2);
			gridElapsedSeconds = duration2.count() / 1000.0;
		}
		showReconstructedSolution = true;
		invalidateRendering(true);
	};

	//start background worker
	worker = make_shared<BackgroundWorker>(task);
	CI_LOG_I("Background worker started");
}

void InverseSoftBodyApp::reconstructSDF(int mode, int iterations)
{
	GridUtils2D::grid_t sdf;
	if (inputTimestep > 0) {
		if (showReconstructedSolution && resultGrid.resultGridSdf_.has_value()) {
			sdf = resultGrid.resultGridSdf_.value();
		}
		else if (showReconstructedSolution && resultGrid.initialGridSdf_.has_value()) {
			sdf = resultGrid.initialGridSdf_.value();
		}
		else {
			sdf = inputResults->gridResultsSdf_[inputTimestep - 1];
		}
	}
	else
	{
		sdf = inputResults->gridReferenceSdf_;
	}

	real viscosity = 0.01;
	if (iterations > 0) {
        if (mode == 1)
            sdf = GridUtils2D::recoverSDFViscosity(sdf, viscosity, iterations);
        else if (mode == 2)
            sdf = GridUtils2D::recoverSDFUpwind(sdf, iterations);
        else if (mode == 3)
            sdf = GridUtils2D::recoverSDFSussmann(sdf, viscosity, iterations);
        else if (mode == 4)
            sdf = GridUtils2D::recoverSDFFastMarching(sdf);
	}

	cleanedSdf = sdf;
	invalidateRendering(false);

	CI_LOG_I("SDF cleaned up");
}

void InverseSoftBodyApp::testPlot()
{
    std::function<void(BackgroundWorker*)> task = [this](BackgroundWorker* worker) {
		//get algorithm
		IInverseProblem* alg = reconstructionAlgorithms[reconstructionMode].get();
		if (InverseProblem_Adjoint_Dynamic* algD = dynamic_cast<InverseProblem_Adjoint_Dynamic*>(alg))
        {
            algD->setGridUsePartialObservation(usePartialObservations);
        }
        alg->setInput(inputResults.get());
		//run test plot
		alg->testPlot(this->inputTimestep, worker);
    };
    worker = make_shared<BackgroundWorker>(task);
    CI_LOG_I("Background worker started");
}

#if 1
CINDER_APP( InverseSoftBodyApp, RendererGl(RendererGl::Options().msaa(8)), [&](App::Settings *settings)
{
	settings->setWindowSize(1600, 900);
} )
#endif


/**
 * \ file SecondOrderFilter.cpp
 */

#include <ATK/EQ/SecondOrderFilter.h>
#include <ATK/EQ/IIRFilter.h>

#include <ATK/Mock/FFTCheckerFilter.h>
#include <ATK/Mock/SimpleSinusGeneratorFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#define PROCESSSIZE (1024*64)

BOOST_AUTO_TEST_CASE( IIRFilter_BandPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::SecondOrderBandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_Q(1);
  filter.set_cut_frequency(100);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.31610222820014583));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_BandPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::SecondOrderBandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_Q(1);
  filter.set_cut_frequency(100);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 0.8408964152537104));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_BandPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::SecondOrderBandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_Q(1);
  filter.set_cut_frequency(100);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 0.22326595903140814));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_BandPassPeakCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::SecondOrderBandPassPeakCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_Q(1);
  filter.set_cut_frequency(100);
  filter.set_gain(2);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 1.0074946766389419));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_BandPassPeakCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::SecondOrderBandPassPeakCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_Q(1);
  filter.set_cut_frequency(100);
  filter.set_gain(2);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 1.4142135623730931));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_BandPassPeakCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::SecondOrderBandPassPeakCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_Q(1);
  filter.set_cut_frequency(100);
  filter.set_gain(2);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 1.0018333926095173));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_AllPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::SecondOrderAllPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_Q(.1);
  filter.set_cut_frequency(100);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 1));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_AllPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::SecondOrderAllPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_Q(.1);
  filter.set_cut_frequency(100);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 1));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_AllPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::SecondOrderAllPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_Q(.1);
  filter.set_cut_frequency(100);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 1));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_LowPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::SecondOrderLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.09991943746806305));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_LowPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::SecondOrderLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 0.8408964152537146));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_LowPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::SecondOrderLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 0.04984673793807906));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_LowPassCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::SecondOrderLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(200, 0.49246840910199763));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_HighPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::SecondOrderHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(1000);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.8697981291708585));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_HighPassCoefficients_10k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(10000);

  ATK::IIRFilter<ATK::SecondOrderHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(1000);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(10000, 1.0344582218093583));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_HighPassCoefficients_500_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(500);

  ATK::IIRFilter<ATK::SecondOrderHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(1000);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(500, 0.5091584471108357));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_LowShelvingCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::SecondOrderLowShelvingCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_gain(.5);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.9999157109896207));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_LowShelvingCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::SecondOrderLowShelvingCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_gain(.5);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 0.7951544465306409));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_LowShelvingCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::SecondOrderLowShelvingCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_gain(.5);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(200, 0.9599245087100254));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_HighShelvingCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::SecondOrderHighShelvingCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(1000);
  filter.set_gain(.5);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.7953107582465426));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_HighShelvingCoefficients_10k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(10000);

  ATK::IIRFilter<ATK::SecondOrderHighShelvingCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(1000);
  filter.set_gain(.5);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(10000, 0.7071168098138222));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_HighShelvingCoefficients_500_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(500);

  ATK::IIRFilter<ATK::SecondOrderHighShelvingCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(1000);
  filter.set_gain(.5);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(500, 0.960256352408842));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}


#include <iostream>
#include <armadillo>
#include <config.h>
#include <string>
#include "png_weight.hpp"
#include "util.hpp"
#include "neural_net.hpp"
#include "mnist.hpp"

using std::string;
using elem_t = float;

void run() {
    string train_images_fn = data_dir + "/train-images-idx3-ubyte";
    string train_labels_fn = data_dir + "/train-labels-idx1-ubyte";
    string test_images_fn = data_dir + "/t10k-images-idx3-ubyte";
    string test_labels_fn = data_dir + "/t10k-labels-idx1-ubyte";

    auto train_data = mnist<elem_t>::create(train_images_fn, train_labels_fn);
    auto test_data = mnist<elem_t>::create(test_images_fn, test_labels_fn);
    auto train_net = neural_net<elem_t>(train_data, 0.1);

    // Train the neural net. Pass in a lambda to display progress
    train_net.train(200, [&](size_t i, size_t max_itr) -> void {
        // Display progress
        if (i == 0 || i % 20 == 0 || i == max_itr - 1) {
            std::cout << i << " cost function = " << train_net.cost()
                      << ", percent correct: " << train_net.predict()
                      << std::endl;
        }
    });

    std::cout << std::endl;

    // Create a 2nd neural network using the learned weights from training data
    auto test_net = neural_net<elem_t>(test_data);
    test_net.theta1 = train_net.theta1;
    test_net.theta2 = train_net.theta2;

    auto percentage = test_net.predict();

    std::cout << "percentage correct: " << percentage << std::endl;

    // Write out the weight to a png file
    weight_png("theta1.png", no_bias(train_net.theta1), 28, 28, 2, 2);
    weight_png("theta2.png", no_bias(train_net.theta2), 8, 8, 2, 2);
}

int main(int, char**) {
    try {
        run();
    } catch (std::runtime_error err) {
        std::cerr << err.what() << std::endl;
    }
}


#include "NormalPrior.h"

#include <iomanip>

#include <SmurffCpp/Utils/linop.h>
#include <SmurffCpp/IO/MatrixIO.h>
#include <SmurffCpp/Utils/counters.h>

#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <SmurffCpp/Utils/Distribution.h>
#include <SmurffCpp/Utils/Error.h>

using namespace smurff;

//  base class NormalPrior

NormalPrior::NormalPrior(std::shared_ptr<Session> session, uint32_t mode, std::string name)
   : ILatentPrior(session, mode, name)
{

}

void NormalPrior::init()
{
   //does not look that there was such init previously
   ILatentPrior::init();

   const int K = num_latent();
   m_mu = std::make_shared<Eigen::VectorXd>(K);
   hyperMu().setZero();

   Lambda.resize(K, K);
   Lambda.setIdentity();
   Lambda *= 10;

   // parameters of Inv-Whishart distribution
   WI.resize(K, K);
   WI.setIdentity();
   mu0.resize(K);
   mu0.setZero();
   b0 = 2;
   df = K;

   const auto &config = getSession().getConfig();
   if (config.hasPropagatedPosterior(getMode()))
   {
      mu_pp = std::make_shared<Eigen::MatrixXd>(matrix_utils::dense_to_eigen(*config.getMuPropagatedPosterior(getMode())));
      Lambda_pp = std::make_shared<Eigen::MatrixXd>(matrix_utils::dense_to_eigen(*config.getLambdaPropagatedPosterior(getMode())));
      m_name += " with posterior propagation";
   }
}

const Eigen::VectorXd NormalPrior::fullMu(int n) const
{
   if (getSession().getConfig().hasPropagatedPosterior(getMode()))
   {
      return mu_pp->col(n);
   }
   //else
   return hyperMu();
}

const Eigen::MatrixXd NormalPrior::getLambda(int n) const
{
   if (getSession().getConfig().hasPropagatedPosterior(getMode()))
   {
      return Eigen::Map<Eigen::MatrixXd>(Lambda_pp->col(n).data(), num_latent(), num_latent());
   }
   //else
   return Lambda;
}
void NormalPrior::update_prior()
{
   std::tie(hyperMu(), Lambda) = CondNormalWishart(num_item(), getUUsum(), getUsum(), mu0, b0, WI, df);
}

//n is an index of column in U matrix
void  NormalPrior::sample_latent(int n)
{
   const auto &mu_u = fullMu(n);
   const auto &Lambda_u = getLambda(n);

   Eigen::VectorXd &rr = rrs.local();
   Eigen::MatrixXd &MM = MMs.local();

   rr.setZero();
   MM.setZero();

   // add pnm
   data().getMuLambda(model(), m_mode, n, rr, MM);

   // add hyperparams
   rr.noalias() += Lambda_u * mu_u;
   MM.noalias() += Lambda_u;

   //Solve system of linear equations for x: MM * x = rr - not exactly correct  because we have random part
   //Sample from multivariate normal distribution with mean rr and precision matrix MM

   Eigen::LLT<Eigen::MatrixXd> chol;
   {
      chol = MM.llt(); // compute the Cholesky decomposition X = L * U
      if(chol.info() != Eigen::Success)
      {
         THROWERROR("Cholesky Decomposition failed!");
      }
   }

   chol.matrixL().solveInPlace(rr); // solve for y: y = L^-1 * b
   rr.noalias() += nrandn(num_latent());
   chol.matrixU().solveInPlace(rr); // solve for x: x = U^-1 * y

   U().col(n).noalias() = rr; // rr is equal to x
}

std::ostream &NormalPrior::status(std::ostream &os, std::string indent) const
{
   os << indent << m_name << std::endl;
   os << indent << "  mu: " <<  hyperMu().transpose() << std::endl;
   return os;
}


#include "Day16-FlawedFrequencyTransmission.h"

#include <AdventOfCodeCommon/DisableLibraryWarningsMacros.h>

__BEGIN_LIBRARIES_DISABLE_WARNINGS
#include <boost/algorithm/string.hpp>

#include <algorithm>
#include <array>
#include <numeric>
#include <vector>
#include <sstream>
__END_LIBRARIES_DISABLE_WARNINGS

namespace
{
const size_t NUM_ITERATIONS = 100;
const size_t PATTERN_OFFSET = 1;
const size_t BASE_PATTERN_SIZE = 4;
const std::array<int, BASE_PATTERN_SIZE> BASE_PATTERN{0, 1, 0, -1};

const size_t MULTIPLIER_OF_REAL_SIGNAL = 10'000;
}

namespace AdventOfCode
{
namespace Year2019
{
namespace Day16
{

std::vector<int> convertToVectorOfDigits(const std::string& s)
{
    std::vector<int> intVector;

    for (auto c : s)
    {
        intVector.push_back(c - '0');
    }

    return intVector;
}

std::string convertToString(std::vector<int> vec)
{
    std::ostringstream oss;

    for (auto num : vec)
    {
        oss << num;
    }

    return oss.str();
}

std::vector<int> getRepeatingPattern(size_t patternRepeatLength, size_t patternLength)
{
    std::vector<int> repeatingPattern;

    for (size_t i = 0; i < patternLength; ++i)
    {
        int patternElement = BASE_PATTERN.at((PATTERN_OFFSET + i) / patternRepeatLength % BASE_PATTERN_SIZE);
        repeatingPattern.push_back(patternElement);
    }

    return repeatingPattern;
}

int multiplyElementwiseAndSummarize(const std::vector<int>& v1, const std::vector<int>& v2)
{
    assert(v1.size() == v2.size());

    std::vector<int> result;

    std::transform(v1.cbegin(), v1.cend(), v2.begin(), std::back_inserter(result), std::multiplies<int>());

    return std::accumulate(result.begin(), result.end(), 0);
}

std::vector<int> applyPhase(const std::vector<int>& digits)
{
    std::vector<int> result;

    for (size_t i = 0; i < digits.size(); ++i)
    {
        std::vector<int> repeatingPattern = getRepeatingPattern(i + 1, digits.size());

        int reductionResult = multiplyElementwiseAndSummarize(digits, repeatingPattern);

        result.push_back(std::abs(reductionResult) % 10);
    }

    return result;
}

std::vector<int> applyPhaseToShortSuffix(const std::vector<int>& digits)
{
    std::vector<int> result;

    // Calculate suffix sum
    std::partial_sum(digits.crbegin(), digits.crend(), std::back_inserter(result));
    std::reverse(result.begin(), result.end());

    for (auto& i : result)
    {
        i %= 10;
    }

    return result;
}

std::vector<int> extendToRealSignalRelevantPart(const std::string& signalString)
{
    std::vector<int> digits = convertToVectorOfDigits(signalString);
    std::vector<int> extendedDigits;

    for (size_t i = 0; i < MULTIPLIER_OF_REAL_SIGNAL; ++i)
    {
        std::copy(digits.begin(), digits.end(), std::back_inserter(extendedDigits));
    }

    std::vector<int> relevantDigits;
    int messageOffset = std::stoi(signalString.substr(0, 7));
    std::copy(extendedDigits.cbegin() + messageOffset, extendedDigits.cend(), std::back_inserter(relevantDigits));

    return relevantDigits;
}

std::string firstEightDigitsOfFinalOutput(const std::string& signalString)
{
    std::vector<int> digits = convertToVectorOfDigits(signalString);

    for (size_t i = 0; i < NUM_ITERATIONS; ++i)
    {
        digits = applyPhase(digits);
    }

    std::string digitsAsString = convertToString(digits);

    return digitsAsString.substr(0, 8);
}

std::string messageInFinalOutputForRealSignal(const std::string& signalString)
{
    std::vector<int> relevantDigits = extendToRealSignalRelevantPart(signalString);

    // Under this assumption, applying a phase becomes a simple prefix sum operation
    assert(relevantDigits.size() < signalString.size() / 2);

    for (size_t i = 0; i < NUM_ITERATIONS; ++i)
    {
        relevantDigits = applyPhaseToShortSuffix(relevantDigits);
    }

    std::string digitsAsString = convertToString(relevantDigits);

    return digitsAsString.substr(0, 8);
}

}
}
}