/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file   testVerticalBlockMatrix.cpp
 * @brief  Unit tests for VerticalBlockMatrix class
 * @author Frank Dellaert
 * @date   February 15, 2014
 **/

#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/VerticalBlockMatrix.h>
#include <boost/assign/list_of.hpp>

using namespace std;
using namespace gtsam;
using boost::assign::list_of;

//*****************************************************************************
TEST(VerticalBlockMatrix, constructor) {
  VerticalBlockMatrix actual(list_of(3)(2)(1),
      (Matrix(6, 6) << 1, 2, 3, 4, 5, 6, //
      2, 8, 9, 10, 11, 12, //
      3, 9, 15, 16, 17, 18, //
      4, 10, 16, 22, 23, 24, //
      5, 11, 17, 23, 29, 30, //
      6, 12, 18, 24, 30, 36));
  EXPECT_LONGS_EQUAL(6,actual.rows());
  EXPECT_LONGS_EQUAL(6,actual.cols());
  EXPECT_LONGS_EQUAL(3,actual.nBlocks());
}

//*****************************************************************************
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
//*****************************************************************************


#include <iostream>
#include <Eigen/Dense>
#include <chrono>

#include "get_floor_fHf.h"
#include "get_floor_Hf.h"
#include "get_floor_f1Hf2.h"

using namespace Eigen;
using namespace std;

int main()
{
    /* Timing experiments */
    int nbr_iter = 10000;
    MatrixXcd out;

    // f1Hf2
    cout << "f1Hf2" << endl;
    auto start = chrono::steady_clock::now();
    for (int i=0; i < nbr_iter; i++) {
        const VectorXd data = VectorXd::Random(45);
        out = solver_floor_f1Hf2(data);
    }

    auto end = chrono::steady_clock::now();
    cout << "Elapsed time in microseconds : "
        << chrono::duration_cast<chrono::microseconds>(end - start).count() / nbr_iter
        << " µs" << endl;

    // fHf
    cout << "fHf" << endl;
    start = chrono::steady_clock::now();
    for (int i=0; i < nbr_iter; i++) {
        const VectorXd data = VectorXd::Random(3 * 2 * 2 + 9 * 2);
        out = solver_floor_fHf(data);
    }

    end = chrono::steady_clock::now();
    cout << "Elapsed time in microseconds : "
        << chrono::duration_cast<chrono::microseconds>(end - start).count() / nbr_iter
        << " µs" << endl;

    // Hf
    cout << "Hf" << endl;
    start = chrono::steady_clock::now();
    for (int i=0; i < nbr_iter; i++) {
        const VectorXd data = VectorXd::Random(3 * 2 * 2 + 9);
        out = solver_floor_Hf(data);
    }

    end = chrono::steady_clock::now();
    cout << "Elapsed time in microseconds : "
        << chrono::duration_cast<chrono::microseconds>(end - start).count() / nbr_iter
        << " µs" << endl;

    return 0;
}


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/div.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/function/ceil.hpp>

STF_CASE_TPL( "Check div behavior with floating", STF_IEEE_TYPES )
{
  namespace bs = boost::simd;
  using bs::div;
  using r_t = decltype(div(T(), T()));
  STF_TYPE_IS(r_t, T);

#ifndef STF_NO_INVALIDS
  STF_IEEE_EQUAL(div(bs::Inf<T>(),  bs::Inf<T>()), bs::Nan<r_t>());
  STF_IEEE_EQUAL(div(bs::Minf<T>(), bs::Minf<T>()), bs::Nan<r_t>());
  STF_IEEE_EQUAL(div(bs::Nan<T>(),  bs::Nan<T>()), bs::Nan<r_t>());
#endif
  STF_EQUAL(div(bs::One<T>(),bs::Zero<T>()), bs::Inf<r_t>());
  STF_IEEE_EQUAL(div(bs::Zero<T>(), bs::Zero<T>()), bs::Nan<r_t>());
  STF_EQUAL(div(bs::One<T>(), bs::One<T>()), bs::One<r_t>());
}

STF_CASE_TPL( "Check div behavior with options", STF_NUMERIC_TYPES )
{
  namespace bs = boost::simd;
  using bs::div;
  using r_t = decltype(div(T(), T()));
  STF_TYPE_IS(r_t, T);
  STF_EQUAL(div(bs::One<T>(), bs::Two<T>())          , bs::Half<r_t>());
  STF_EQUAL(div(bs::One<T>(), bs::Two<T>(), bs::ceil), bs::One<r_t>());
  STF_EQUAL(div(bs::One<T>(), bs::Two<T>(), bs::floor), bs::Zero<r_t>());
  STF_EQUAL(div(bs::One<T>(), bs::Two<T>(), bs::round), bs::One<r_t>());
  STF_EQUAL(div(bs::One<T>(), bs::Two<T>(), bs::round2even), bs::Zero<r_t>());
  STF_EQUAL(div(bs::One<T>(), bs::Two<T>(), bs::fix), bs::Zero<r_t>());
}


/*
 Copyright (C) 2019 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <qle/instruments/brlcdiswap.hpp>

#include <qle/cashflows/brlcdicouponpricer.hpp>
#include <qle/cashflows/couponpricer.hpp>
#include <ql/cashflows/overnightindexedcoupon.hpp>
#include <ql/cashflows/simplecashflow.hpp>
#include <ql/time/schedule.hpp>
#include <ql/time/daycounters/business252.hpp>
#include <boost/assign/list_of.hpp>

using namespace QuantLib;
using boost::assign::list_of;
using std::vector;
using std::pow;

namespace QuantExt {

// Reason for use of convert_to_container:
// https://stackoverflow.com/a/17805923/1771882
BRLCdiSwap::BRLCdiSwap(Type type, Real nominal, const Date& startDate, const Date& endDate, Rate fixedRate,
    const boost::shared_ptr<BRLCdi>& overnightIndex, Spread spread, bool telescopicValueDates)
    : OvernightIndexedSwap(type, nominal, Schedule(list_of(startDate)(endDate).convert_to_container<vector<Date> >(),
        NullCalendar(), QuantLib::Unadjusted, QuantLib::Unadjusted, 100 * Years), fixedRate,
        overnightIndex->dayCounter(), overnightIndex, spread, 0, ModifiedFollowing, overnightIndex->fixingCalendar(),
        telescopicValueDates), startDate_(startDate), endDate_(endDate), index_(overnightIndex) {

    // Need to overwrite the fixed leg with the correct fixed leg for a standard BRL CDI swap
    // Fixed leg is of the form: N [(1 + k) ^ \delta - 1]
    // where \delta is the number of BRL business days in the period divided by 252 i.e.
    // the day count fraction for the period on a Business252 basis.
    Time dcf = index_->dayCounter().yearFraction(startDate_, endDate_);
    Real fixedLegPayment = nominal * (pow(1.0 + fixedRate, dcf) - 1.0);
    Date paymentDate = legs_[0].back()->date();
    boost::shared_ptr<CashFlow> fixedCashflow = boost::make_shared<SimpleCashFlow>(fixedLegPayment, paymentDate);
    legs_[0].clear();
    legs_[0].push_back(fixedCashflow);
    registerWith(fixedCashflow);

    // Set the pricer on the BRL CDI coupon
    QL_REQUIRE(legs_[1].size() == 1, "BRLCdiSwap expected exactly one overnight coupon");
    boost::shared_ptr<OvernightIndexedCoupon> coupon = boost::dynamic_pointer_cast<OvernightIndexedCoupon>(legs_[1][0]);
    coupon->setPricer(boost::make_shared<BRLCdiCouponPricer>());
}

Real BRLCdiSwap::fixedLegBPS() const {

    calculate();

    static Spread basisPoint = 1.0e-4;
    if (!close(endDiscounts_[0], 0.0) && endDiscounts_[0] != Null<DiscountFactor>()) {
        DiscountFactor df = endDiscounts_[0];
        Time dcf = index_->dayCounter().yearFraction(startDate_, endDate_);
        legBPS_[0] = df * nominal() * (pow(1.0 + fixedRate() + basisPoint, dcf) - pow(1.0 + fixedRate(), dcf));
        return legBPS_[0];
    }

    QL_FAIL("BRLCdiSwap cannot calculate fixed leg BPS because end discount is not populated");
}

Real BRLCdiSwap::fairRate() const {

    calculate();

    if (!close(endDiscounts_[0], 0.0) && endDiscounts_[0] != Null<DiscountFactor>()) {
        DiscountFactor df = endDiscounts_[0];
        Time dcf = index_->dayCounter().yearFraction(startDate_, endDate_);
        return pow(overnightLegNPV() / (nominal() * df) + 1.0, 1.0 / dcf) - 1.0;
    }

    QL_FAIL("BRLCdiSwap cannot calculate fair rate because end discount is not populated");
}

}


#include <cstdio>
#include <cstdlib>

#if defined(USE_OPENGL)
    #include <GL/glut.h>
#endif

#if defined(USE_OPENMP)
    #include "omp.h"
#endif

#include "Color.h"
#include "Ray.h"
#include "Surface.h"
#include "SurfaceList.h"
#include "Sphere.h"
#include "Plane.h"
#include "Intersection.h"
#include "Scene.h"

#include <Eigen/Dense>

#include <easyppm.h>

constexpr int NX = 512;
constexpr int NY = 512;

Color* buffer;

PPM ppm;

#if defined(USE_OPENGL)
void gl_display() {
    glClearColor(0,0,0,1);
    glClear(GL_COLOR_BUFFER_BIT);

    float* float_buffer = new float[3*NX*NY];

    int k = 0;
    for (int i = 0; i < NX; i++) {
        for (int j = 0; j < NY; j++) {
            float_buffer[k++] = buffer[j*NY + i].r;
            float_buffer[k++] = buffer[j*NY + i].g;
            float_buffer[k++] = buffer[j*NY + i].b;
        }
    }

    glDrawPixels(NX, NY, GL_RGB, GL_FLOAT, float_buffer);

    glutSwapBuffers();

    delete[] float_buffer;
}

void gl_keyboard(unsigned char key, int x, int y) {
    switch (key) {
        // ESC
        case 27:
            exit(EXIT_SUCCESS);
    }
}
#endif

int clamp(int x, int min, int max) {
    return std::max(min, std::min(x, max));
}

int main(int argc, char* argv[]) {
    constexpr float l = -0.1f;
    constexpr float r =  0.1f;
    constexpr float b = -0.1f;
    constexpr float t =  0.1f;

    constexpr float dist = 0.1f;

    constexpr int SAMPLES   = 64;

    const int SAMPLEDIM = sqrt(SAMPLES);

#ifndef USE_OPENGL
    ppm = easyppm_create(NX, NY, IMAGETYPE_PPM);
#endif

    // Materials
    Material mp(Color(0.2f, 0.2f, 0.2f), Color(1.0f, 1.0f, 1.0f), Color(0.0f, 0.0f, 0.0f),  0.0f, 0.0f);
    Material m1(Color(0.2f, 0.0f, 0.0f), Color(1.0f, 0.0f, 0.0f), Color(0.0f, 0.0f, 0.0f),  0.0f, 0.0f);
    Material m2(Color(0.0f, 0.2f, 0.0f), Color(0.0f, 0.5f, 0.0f), Color(0.5f, 0.5f, 0.5f), 32.0f, 0.0f);
    Material m3(Color(0.0f, 0.0f, 0.2f), Color(0.0f, 0.0f, 1.0f), Color(0.0f, 0.0f, 0.0f),  0.0f, 0.0f);

    // Surfaces
    SurfaceList surfaces;
    surfaces.add(std::move(std::unique_ptr<Plane>(new Plane(Eigen::Vector3f(0, -2, 0), Eigen::Vector3f(0, 1, 0), mp))));
    surfaces.add(std::move(std::unique_ptr<Sphere>(new Sphere(Eigen::Vector3f(-4, 0, -7), 1, m1))));
    surfaces.add(std::move(std::unique_ptr<Sphere>(new Sphere(Eigen::Vector3f( 0, 0, -7), 2, m2))));
    surfaces.add(std::move(std::unique_ptr<Sphere>(new Sphere(Eigen::Vector3f( 4, 0, -7), 1, m3))));

    // Light
    Light light(Eigen::Vector3f(-4, 4, -3), 1);

    // Add surfaces and light to scene
    Scene scene(surfaces, light);

    // Fill pixel buffer
    buffer = new Color[NX*NY];
    #if defined(USE_OPENMP)
        #pragma omp parallel for
    #endif
    for (int k = 0; k < NX*NY; k++) {
        int i = k % NX;
        int j = k / NX;
        Color res(0,0,0);
        for (int si = 0; si < SAMPLEDIM; si++) {
            for (int sj = 0; sj < SAMPLEDIM; sj++) {
                // Uniform sampling
                float x = (i-0.5f) + (si)/(float)SAMPLEDIM;
                float y = (j-0.5f) + (sj)/(float)SAMPLEDIM;

                float u = l + ((r-l)*(x+0.5f)/NX);
                float v = b + ((t-b)*(y+0.5f)/NY);

                auto p = Eigen::Vector3f(0, 0, 0);
                auto d = Eigen::Vector3f(u, v, -dist);

                auto ray = Ray(p, d);

                auto hit = std::unique_ptr<Intersection>(scene.intersect(ray));
                if (hit)
                    res += scene.shade(ray, *hit, 2, true, false).correct(2.2f);
            }
        }
        #if defined(USE_OPENGL)
            buffer[i*NY + j] = res / SAMPLES;
        #else
            auto color = res / SAMPLES;
            auto r = clamp(color.r * 255, 0, 255);
            auto g = clamp(color.g * 255, 0, 255);
            auto b = clamp(color.b * 255, 0, 255);
            easyppm_set(&ppm, i, j, easyppm_rgb(r, g, b));
        #endif
    }

    #if defined(USE_OPENGL)
        // Write buffer to OpenGL window
        glutInit(&argc, argv);
        glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
        glutInitWindowSize(NX, NY);
        glutCreateWindow("Part 3");
        glutDisplayFunc(gl_display);
        glutKeyboardFunc(gl_keyboard);
        glutMainLoop();
    #else
        // Write buffer to image file
        const char* path = "images/part3.ppm";

        easyppm_invert_y(&ppm);
        easyppm_write(&ppm, path);
        easyppm_destroy(&ppm);

        printf("Image written to %s\n", path);
    #endif

    delete[] buffer;

    return 0;
}


//
// Copyright 2005-2007 Adobe Systems Incorporated
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
#include <boost/gil/image.hpp>
#include <boost/gil/typedefs.hpp>
#include <boost/gil/extension/io/jpeg_io.hpp>
#include <boost/gil/extension/numeric/sampler.hpp>
#include <boost/gil/extension/numeric/resample.hpp>

// Example for resize_view() in the numeric extension

int main()
{
    using namespace boost::gil;

    rgb8_image_t img;
    jpeg_read_image("test.jpg",img);

    // test resize_view
    // Scale the image to 100x100 pixels using bilinear resampling
    rgb8_image_t square100x100(100,100);
    resize_view(const_view(img), view(square100x100), bilinear_sampler());
    jpeg_write_view("out-resize.jpg",const_view(square100x100));

    return 0;
}


#include "Common/Common.h"
#include "Demos/Visualization/MiniGL.h"
#include "Demos/Visualization/Selection.h"
#include "GL/glut.h"
#include "Demos/Simulation/TimeManager.h"
#include <Eigen/Dense>
#include "Demos/Simulation/SimulationModel.h"
#include "Demos/Simulation/TimeStepController.h"
#include <iostream>
#include "Demos/Simulation/Constraints.h"
#include "Demos/Visualization/Visualization.h"
#include "Demos/Utils/OBJLoader.h"
#include "Demos/Utils/Utilities.h"
#include "Demos/Utils/Timing.h"

#define _USE_MATH_DEFINES
#include "math.h"

// Enable memory leak detection
#if defined(_DEBUG) && !defined(EIGEN_ALIGN)
	#define new DEBUG_NEW
#endif

using namespace PBD;
using namespace Eigen;
using namespace std;

void timeStep ();
void buildModel ();
void createRigidBodyModel();
void createClothMesh();
void render ();
void reset();
void cleanup();
void initShader();
void selection(const Eigen::Vector2i &start, const Eigen::Vector2i &end);
void TW_CALL setTimeStep(const void *value, void *clientData);
void TW_CALL getTimeStep(void *value, void *clientData);
void TW_CALL setVelocityUpdateMethod(const void *value, void *clientData);
void TW_CALL getVelocityUpdateMethod(void *value, void *clientData);
void TW_CALL setStiffness(const void *value, void *clientData);
void TW_CALL getStiffness(void *value, void *clientData);
void TW_CALL setXXStiffness(const void *value, void *clientData);
void TW_CALL getXXStiffness(void *value, void *clientData);
void TW_CALL setYYStiffness(const void *value, void *clientData);
void TW_CALL getYYStiffness(void *value, void *clientData);
void TW_CALL setXYStiffness(const void *value, void *clientData);
void TW_CALL getXYStiffness(void *value, void *clientData);
void TW_CALL setXYPoissonRatio(const void *value, void *clientData);
void TW_CALL getXYPoissonRatio(void *value, void *clientData);
void TW_CALL setYXPoissonRatio(const void *value, void *clientData);
void TW_CALL getYXPoissonRatio(void *value, void *clientData);
void TW_CALL setNormalizeStretch(const void *value, void *clientData);
void TW_CALL getNormalizeStretch(void *value, void *clientData);
void TW_CALL setNormalizeShear(const void *value, void *clientData);
void TW_CALL getNormalizeShear(void *value, void *clientData);
void TW_CALL setBendingStiffness(const void *value, void *clientData);
void TW_CALL getBendingStiffness(void *value, void *clientData);
void TW_CALL setBendingMethod(const void *value, void *clientData);
void TW_CALL getBendingMethod(void *value, void *clientData);
void TW_CALL setSimulationMethod(const void *value, void *clientData);
void TW_CALL getSimulationMethod(void *value, void *clientData);

SimulationModel model;
TimeStepController sim;

const int nRows = 20;
const int nCols = 20;
const Real clothWidth = 10.0;
const Real clothHeight = 10.0;
const Real width = 0.2;
const Real height = 2.0;
const Real depth = 0.2;
short simulationMethod = 2;
short bendingMethod = 2;
bool doPause = true;
std::vector<unsigned int> selectedBodies;
std::vector<unsigned int> selectedParticles;
Vector3r oldMousePos;
Shader *shader;
Shader *shaderTex;
string exePath;
string dataPath;
float jointColor[4] = { 0.0, 0.4f, 0.2f, 1.0 };
float dynamicBodyColor[4] = { 0.1f, 0.4f, 0.8f, 1 };
float staticBodyColor[4] = { 0.4f, 0.4f, 0.4f, 1.0 };

// main
int main( int argc, char **argv )
{
	REPORT_MEMORY_LEAKS

	exePath = Utilities::getFilePath(argv[0]);
	dataPath = exePath + "/" + std::string(PBD_DATA_PATH);

	// OpenGL
	MiniGL::init (argc, argv, 1024, 768, 0, 0, "Rigid body demo");
	MiniGL::initLights ();
	MiniGL::initTexture();
	MiniGL::setClientIdleFunc (50, timeStep);
	MiniGL::setKeyFunc(0, 'r', reset);
	MiniGL::setSelectionFunc(selection);
	initShader();

	buildModel ();

	MiniGL::setClientSceneFunc(render);
	MiniGL::setViewport (40.0, 0.1f, 500.0, Vector3r (0.0, 10.0, 30.0), Vector3r (0.0, 0.0, 0.0));

	TwAddVarRW(MiniGL::getTweakBar(), "Pause", TW_TYPE_BOOLCPP, &doPause, " label='Pause' group=Simulation key=SPACE ");
	TwAddVarCB(MiniGL::getTweakBar(), "TimeStepSize", TW_TYPE_REAL, setTimeStep, getTimeStep, 0, " label='Time step size'  min=0.0 max = 0.1 step=0.001 precision=4 group=Simulation ");
	TwType enumType = TwDefineEnum("VelocityUpdateMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "VelocityUpdateMethod", enumType, setVelocityUpdateMethod, getVelocityUpdateMethod, &sim, " label='Velocity update method' enum='0 {First Order Update}, 1 {Second Order Update}' group=Simulation");
	TwType enumType2 = TwDefineEnum("SimulationMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "SimulationMethod", enumType2, setSimulationMethod, getSimulationMethod, &simulationMethod, " label='Simulation method' enum='0 {None}, 1 {Distance constraints}, 2 {FEM based PBD}, 3 {Strain based dynamics}' group=Simulation");
	TwAddVarCB(MiniGL::getTweakBar(), "Stiffness", TW_TYPE_REAL, setStiffness, getStiffness, &model, " label='Stiffness'  min=0.0 step=0.1 precision=4 group='Distance constraints' ");
	TwAddVarCB(MiniGL::getTweakBar(), "XXStiffness", TW_TYPE_REAL, setXXStiffness, getXXStiffness, &model, " label='Stiffness XX'  min=0.0 step=0.1 precision=4 group='Strain based dynamics' ");
	TwAddVarCB(MiniGL::getTweakBar(), "YYStiffness", TW_TYPE_REAL, setYYStiffness, getYYStiffness, &model, " label='Stiffness YY'  min=0.0 step=0.1 precision=4 group='Strain based dynamics' ");
	TwAddVarCB(MiniGL::getTweakBar(), "XYStiffness", TW_TYPE_REAL, setXYStiffness, getXYStiffness, &model, " label='Stiffness XY'  min=0.0 step=0.1 precision=4 group='Strain based dynamics' ");
	TwAddVarCB(MiniGL::getTweakBar(), "XXStiffnessFEM", TW_TYPE_REAL, setXXStiffness, getXXStiffness, &model, " label='Youngs modulus XX'  min=0.0 step=0.1 precision=4 group='FEM based PBD' ");
	TwAddVarCB(MiniGL::getTweakBar(), "YYStiffnessFEM", TW_TYPE_REAL, setYYStiffness, getYYStiffness, &model, " label='Youngs modulus YY'  min=0.0 step=0.1 precision=4 group='FEM based PBD' ");
	TwAddVarCB(MiniGL::getTweakBar(), "XYStiffnessFEM", TW_TYPE_REAL, setXYStiffness, getXYStiffness, &model, " label='Youngs modulus XY'  min=0.0 step=0.1 precision=4 group='FEM based PBD' ");
	TwAddVarCB(MiniGL::getTweakBar(), "XYPoissonRatioFEM", TW_TYPE_REAL, setXYPoissonRatio, getXYPoissonRatio, &model, " label='Poisson ratio XY'  min=0.0 step=0.1 precision=4 group='FEM based PBD' ");
	TwAddVarCB(MiniGL::getTweakBar(), "YXPoissonRatioFEM", TW_TYPE_REAL, setYXPoissonRatio, getYXPoissonRatio, &model, " label='Poisson ratio YX'  min=0.0 step=0.1 precision=4 group='FEM based PBD' ");
	TwAddVarCB(MiniGL::getTweakBar(), "NormalizeStretch", TW_TYPE_BOOL32, setNormalizeStretch, getNormalizeStretch, &model, " label='Normalize stretch' group='Strain based dynamics' ");
	TwAddVarCB(MiniGL::getTweakBar(), "NormalizeShear", TW_TYPE_BOOL32, setNormalizeShear, getNormalizeShear, &model, " label='Normalize shear' group='Strain based dynamics' ");
	TwType enumType3 = TwDefineEnum("BendingMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "BendingMethod", enumType3, setBendingMethod, getBendingMethod, &bendingMethod, " label='Bending method' enum='0 {None}, 1 {Dihedral angle}, 2 {Isometric bending}' group=Bending");
	TwAddVarCB(MiniGL::getTweakBar(), "BendingStiffness", TW_TYPE_REAL, setBendingStiffness, getBendingStiffness, &model, " label='Bending stiffness'  min=0.0 step=0.01 precision=4 group=Bending ");

	glutMainLoop ();

	cleanup ();

	return 0;
}

void initShader()
{
	std::string vertFile = dataPath + "/shaders/vs_flat.glsl";
	std::string geomFile = dataPath + "/shaders/gs_flat.glsl";
	std::string fragFile = dataPath + "/shaders/fs_flat.glsl";
	shader = MiniGL::createShader(vertFile, geomFile, fragFile);

	if (shader == NULL)
		return;

	shader->begin();
	shader->addUniform("modelview_matrix");
	shader->addUniform("projection_matrix");
	shader->addUniform("surface_color");
	shader->addUniform("shininess");
	shader->addUniform("specular_factor");
	shader->end();

	vertFile = dataPath + "/shaders/vs_smoothTex.glsl";
	fragFile = dataPath + "/shaders/fs_smoothTex.glsl";
	shaderTex = MiniGL::createShader(vertFile, "", fragFile);

	if (shaderTex == NULL)
		return;

	shaderTex->begin();
	shaderTex->addUniform("modelview_matrix");
	shaderTex->addUniform("projection_matrix");
	shaderTex->addUniform("surface_color");
	shaderTex->addUniform("shininess");
	shaderTex->addUniform("specular_factor");
	shaderTex->end();
}

void cleanup()
{
	delete TimeManager::getCurrent();
	delete shader;
	delete shaderTex;
}

void reset()
{
	Timing::printAverageTimes();
	Timing::reset();

	model.reset();
	sim.reset();
	TimeManager::getCurrent()->setTime(0.0);

	model.cleanup();
	buildModel();
}

void mouseMove(int x, int y)
{
	Vector3r mousePos;
	MiniGL::unproject(x, y, mousePos);
	const Vector3r diff = mousePos - oldMousePos;

	TimeManager *tm = TimeManager::getCurrent();
	const Real h = tm->getTimeStepSize();

	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
	for (size_t j = 0; j < selectedBodies.size(); j++)
	{
		rb[selectedBodies[j]]->getVelocity() += 1.0 / h * diff;
	}
	ParticleData &pd = model.getParticles();
	for (unsigned int j = 0; j < selectedParticles.size(); j++)
	{
		pd.getVelocity(selectedParticles[j]) += 5.0*diff / h;
	}
	oldMousePos = mousePos;
}

void selection(const Eigen::Vector2i &start, const Eigen::Vector2i &end)
{
	std::vector<unsigned int> hits;

	selectedParticles.clear();
	ParticleData &pd = model.getParticles();
	Selection::selectRect(start, end, &pd.getPosition(0), &pd.getPosition(pd.size() - 1), selectedParticles);

	selectedBodies.clear();
	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
	std::vector<Vector3r, Eigen::aligned_allocator<Vector3r> > x;
	x.resize(rb.size());
	for (unsigned int i = 0; i < rb.size(); i++)
	{
		x[i] = rb[i]->getPosition();
	}

	Selection::selectRect(start, end, &x[0], &x[rb.size() - 1], selectedBodies);
	if ((selectedBodies.size() > 0) || (selectedParticles.size() > 0))
		MiniGL::setMouseMoveFunc(GLUT_MIDDLE_BUTTON, mouseMove);
	else
		MiniGL::setMouseMoveFunc(-1, NULL);

	MiniGL::unproject(end[0], end[1], oldMousePos);
}

void timeStep ()
{
	if (doPause)
		return;

	// Simulation code
	for (unsigned int i = 0; i < 8; i++)
		sim.step(model);

	for (unsigned int i = 0; i < model.getTriangleModels().size(); i++)
		model.getTriangleModels()[i]->updateMeshNormals(model.getParticles());
}

void buildModel ()
{
	TimeManager::getCurrent ()->setTimeStepSize (0.005);

	createClothMesh();
	createRigidBodyModel();
}

void renderBallJoint(BallJoint &bj)
{
	MiniGL::drawSphere(bj.m_jointInfo.col(2), 0.1f, jointColor);
}

void renderRigidBodyParticleBallJoint(RigidBodyParticleBallJoint &bj)
{
	MiniGL::drawSphere(bj.m_jointInfo.col(1), 0.1f, jointColor);
}

void renderRigidBodyModel()
{
	// Draw simulation model

	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
	SimulationModel::ConstraintVector &constraints = model.getConstraints();

	float selectionColor[4] = { 0.8f, 0.0f, 0.0f, 1 };
	float surfaceColor[4] = { 0.1f, 0.4f, 0.8f, 1 };

	if (shader)
	{
		shader->begin();
		glUniform1f(shader->getUniform("shininess"), 5.0);
		glUniform1f(shader->getUniform("specular_factor"), 0.2f);

		GLfloat matrix[16];
		glGetFloatv(GL_MODELVIEW_MATRIX, matrix);
		glUniformMatrix4fv(shader->getUniform("modelview_matrix"), 1, GL_FALSE, matrix);
		GLfloat pmatrix[16];
		glGetFloatv(GL_PROJECTION_MATRIX, pmatrix);
		glUniformMatrix4fv(shader->getUniform("projection_matrix"), 1, GL_FALSE, pmatrix);
	}

	for (size_t i = 0; i < rb.size(); i++)
	{
		bool selected = false;
		for (unsigned int j = 0; j < selectedBodies.size(); j++)
		{
			if (selectedBodies[j] == i)
				selected = true;
		}

		if (rb[i]->getMass() != 0.0)
		{

			const VertexData &vd = rb[i]->getGeometry().getVertexData();
			const IndexedFaceMesh &mesh = rb[i]->getGeometry().getMesh();
			if (!selected)
			{
				if (shader)
					glUniform3fv(shader->getUniform("surface_color"), 1, surfaceColor);
				Visualization::drawMesh(vd, mesh, 0, surfaceColor);
			}
			else
			{
				if (shader)
					glUniform3fv(shader->getUniform("surface_color"), 1, selectionColor);
				Visualization::drawMesh(vd, mesh, 0, selectionColor);
			}
		}
	}
	if (shader)
		shader->end();

	for (size_t i = 0; i < constraints.size(); i++)
	{
		if (constraints[i]->getTypeId() == BallJoint::TYPE_ID)
		{
			renderBallJoint(*(BallJoint*)constraints[i]);
		}
		else if (constraints[i]->getTypeId() == RigidBodyParticleBallJoint::TYPE_ID)
		{
			renderRigidBodyParticleBallJoint(*(RigidBodyParticleBallJoint*)constraints[i]);
		}
	}
}

void renderTriangleModels()
{
	// Draw simulation model

	const ParticleData &pd = model.getParticles();
	float surfaceColor[4] = { 0.2f, 0.5, 1.0, 1 };

	if (shaderTex)
	{
		shaderTex->begin();
		glUniform3fv(shaderTex->getUniform("surface_color"), 1, surfaceColor);
		glUniform1f(shaderTex->getUniform("shininess"), 5.0);
		glUniform1f(shaderTex->getUniform("specular_factor"), 0.2f);

		GLfloat matrix[16];
		glGetFloatv(GL_MODELVIEW_MATRIX, matrix);
		glUniformMatrix4fv(shaderTex->getUniform("modelview_matrix"), 1, GL_FALSE, matrix);
		GLfloat pmatrix[16];
		glGetFloatv(GL_PROJECTION_MATRIX, pmatrix);
		glUniformMatrix4fv(shaderTex->getUniform("projection_matrix"), 1, GL_FALSE, pmatrix);
	}

	for (unsigned int i = 0; i < model.getTriangleModels().size(); i++)
	{
		// mesh
		TriangleModel *triModel = model.getTriangleModels()[i];
		const IndexedFaceMesh &mesh = triModel->getParticleMesh();
		Visualization::drawTexturedMesh(pd, mesh, triModel->getIndexOffset(), surfaceColor);
	}
	if (shaderTex)
		shaderTex->end();

	float red[4] = { 0.8f, 0.0, 0.0, 1 };
	for (unsigned int j = 0; j < selectedParticles.size(); j++)
	{
		MiniGL::drawSphere(pd.getPosition(selectedParticles[j]), 0.08f, red);
	}
}

void render ()
{
	MiniGL::coordinateSystem();

	renderRigidBodyModel();
	renderTriangleModels();

	MiniGL::drawTime( TimeManager::getCurrent ()->getTime ());
}

// Compute diagonal inertia tensor
Vector3r computeInertiaTensorBox(const Real mass, const Real width, const Real height, const Real depth)
{
	const Real Ix = (mass / 12.0) * (height*height + depth*depth);
	const Real Iy = (mass / 12.0) * (width*width + depth*depth);
	const Real Iz = (mass / 12.0) * (width*width + height*height);
	return Vector3r(Ix, Iy, Iz);
}

/** Create the model
*/
void createRigidBodyModel()
{
	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
	SimulationModel::ConstraintVector &constraints = model.getConstraints();

	string fileName = Utilities::normalizePath(dataPath + "/models/cube.obj");
	IndexedFaceMesh mesh;
	VertexData vd;
	OBJLoader::loadObj(fileName, vd, mesh, Vector3r(width, height, depth));

	rb.resize(12);

	//////////////////////////////////////////////////////////////////////////
	// -5, -5
	//////////////////////////////////////////////////////////////////////////
	rb[0] = new RigidBody();
	rb[0]->initBody(0.0,
		Vector3r(-5.0, 0.0, -5.0),
		computeInertiaTensorBox(1.0, 0.5, 0.5, 0.5),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vd, mesh);

	// dynamic body
	rb[1] = new RigidBody();
	rb[1]->initBody(1.0,
		Vector3r(-5.0, 1.0, -5.0),
		computeInertiaTensorBox(1.0, width, height, depth),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vd, mesh);

	// dynamic body
	rb[2] = new RigidBody();
	rb[2]->initBody(1.0,
		Vector3r(-5.0, 3.0, -5.0),
		computeInertiaTensorBox(1.0, width, height, depth),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vd, mesh);

	model.addBallJoint(0, 1, Vector3r(-5.0, 0.0, -5.0));
	model.addBallJoint(1, 2, Vector3r(-5.0, 2.0, -5.0));

	//////////////////////////////////////////////////////////////////////////
	// 5, -5
	//////////////////////////////////////////////////////////////////////////
	rb[3] = new RigidBody();
	rb[3]->initBody(0.0,
		Vector3r(5.0, 0.0, -5.0),
		computeInertiaTensorBox(1.0, 0.5, 0.5, 0.5),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vd, mesh);

	// dynamic body
	rb[4] = new RigidBody();
	rb[4]->initBody(1.0,
		Vector3r(5.0, 1.0, -5.0),
		computeInertiaTensorBox(1.0, width, height, depth),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vd, mesh);

	// dynamic body
	rb[5] = new RigidBody();
	rb[5]->initBody(1.0,
		Vector3r(5.0, 3.0, -5.0),
		computeInertiaTensorBox(1.0, width, height, depth),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vd, mesh);

	model.addBallJoint(3, 4, Vector3r(5.0, 0.0, -5.0));
	model.addBallJoint(4, 5, Vector3r(5.0, 2.0, -5.0));

	//////////////////////////////////////////////////////////////////////////
	// 5, 5
	//////////////////////////////////////////////////////////////////////////
	rb[6] = new RigidBody();
	rb[6]->initBody(0.0,
		Vector3r(5.0, 0.0, 5.0),
		computeInertiaTensorBox(1.0, 0.5, 0.5, 0.5),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vd, mesh);

	// dynamic body
	rb[7] = new RigidBody();
	rb[7]->initBody(1.0,
		Vector3r(5.0, 1.0, 5.0),
		computeInertiaTensorBox(1.0, width, height, depth),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vd, mesh);

	// dynamic body
	rb[8] = new RigidBody();
	rb[8]->initBody(1.0,
		Vector3r(5.0, 3.0, 5.0),
		computeInertiaTensorBox(1.0, width, height, depth),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vd, mesh);

	model.addBallJoint(6, 7, Vector3r(5.0, 0.0, 5.0));
	model.addBallJoint(7, 8, Vector3r(5.0, 2.0, 5.0));

	//////////////////////////////////////////////////////////////////////////
	// -5, 5
	//////////////////////////////////////////////////////////////////////////
	rb[9] = new RigidBody();
	rb[9]->initBody(0.0,
		Vector3r(-5.0, 0.0, 5.0),
		computeInertiaTensorBox(1.0, 0.5, 0.5, 0.5),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vd, mesh);

	// dynamic body
	rb[10] = new RigidBody();
	rb[10]->initBody(1.0,
		Vector3r(-5.0, 1.0, 5.0),
		computeInertiaTensorBox(1.0, width, height, depth),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vd, mesh);

	// dynamic body
	rb[11] = new RigidBody();
	rb[11]->initBody(1.0,
		Vector3r(-5.0, 3.0, 5.0),
		computeInertiaTensorBox(1.0, width, height, depth),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vd, mesh);

	model.addBallJoint(9, 10, Vector3r(-5.0, 0.0, 5.0));
	model.addBallJoint(10, 11, Vector3r(-5.0, 2.0, 5.0));

	model.addRigidBodyParticleBallJoint(2, 0);
	model.addRigidBodyParticleBallJoint(5, (nRows - 1)*nCols);
	model.addRigidBodyParticleBallJoint(8, nRows*nCols - 1);
	model.addRigidBodyParticleBallJoint(11, nCols-1);

}

/** Create a particle model mesh
*/
void createClothMesh()
{
	TriangleModel::ParticleMesh::UVs uvs;
	uvs.resize(nRows*nCols);

	const Real dy = clothWidth / (Real)(nCols - 1);
	const Real dx = clothHeight / (Real)(nRows - 1);

	Vector3r points[nRows*nCols];
	for (int i = 0; i < nRows; i++)
	{
		for (int j = 0; j < nCols; j++)
		{
			const Real y = (Real)dy*j;
			const Real x = (Real)dx*i;
			points[i*nCols + j] = Vector3r(x - 5.0, 4.0, y - 5.0);

			uvs[i*nCols + j][0] = x / clothWidth;
			uvs[i*nCols + j][1] = y / clothHeight;
		}
	}
	const int nIndices = 6 * (nRows - 1)*(nCols - 1);

	TriangleModel::ParticleMesh::UVIndices uvIndices;
	uvIndices.resize(nIndices);

	unsigned int indices[nIndices];
	int index = 0;
	for (int i = 0; i < nRows - 1; i++)
	{
		for (int j = 0; j < nCols - 1; j++)
		{
			int helper = 0;
			if (i % 2 == j % 2)
				helper = 1;

			indices[index] = i*nCols + j;
			indices[index + 1] = i*nCols + j + 1;
			indices[index + 2] = (i + 1)*nCols + j + helper;

			uvIndices[index] = i*nCols + j;
			uvIndices[index + 1] = i*nCols + j + 1;
			uvIndices[index + 2] = (i + 1)*nCols + j + helper;
			index += 3;

			indices[index] = (i + 1)*nCols + j + 1;
			indices[index + 1] = (i + 1)*nCols + j;
			indices[index + 2] = i*nCols + j + 1 - helper;

			uvIndices[index] = (i + 1)*nCols + j + 1;
			uvIndices[index + 1] = (i + 1)*nCols + j;
			uvIndices[index + 2] = i*nCols + j + 1 - helper;
			index += 3;
		}
	}

	model.addTriangleModel(nRows*nCols, nIndices / 3, &points[0], &indices[0], uvIndices, uvs);

	ParticleData &pd = model.getParticles();
	for (unsigned int i = 0; i < pd.getNumberOfParticles(); i++)
	{
		pd.setMass(i, 1.0);
	}

	// init constraints
	for (unsigned int cm = 0; cm < model.getTriangleModels().size(); cm++)
	{
		if (simulationMethod == 1)
		{
			const unsigned int offset = model.getTriangleModels()[cm]->getIndexOffset();
			const unsigned int nEdges = model.getTriangleModels()[cm]->getParticleMesh().numEdges();
			const IndexedFaceMesh::Edge *edges = model.getTriangleModels()[cm]->getParticleMesh().getEdges().data();
			for (unsigned int i = 0; i < nEdges; i++)
			{
				const unsigned int v1 = edges[i].m_vert[0] + offset;
				const unsigned int v2 = edges[i].m_vert[1] + offset;

				model.addDistanceConstraint(v1, v2);
			}
		}
		else if (simulationMethod == 2)
		{
			const unsigned int offset = model.getTriangleModels()[cm]->getIndexOffset();
			TriangleModel::ParticleMesh &mesh = model.getTriangleModels()[cm]->getParticleMesh();
			const unsigned int *tris = mesh.getFaces().data();
			const unsigned int nFaces = mesh.numFaces();
			for (unsigned int i = 0; i < nFaces; i++)
			{
				const unsigned int v1 = tris[3 * i] + offset;
				const unsigned int v2 = tris[3 * i + 1] + offset;
				const unsigned int v3 = tris[3 * i + 2] + offset;
				model.addFEMTriangleConstraint(v1, v2, v3);
			}
		}
		else if (simulationMethod == 3)
		{
			const unsigned int offset = model.getTriangleModels()[cm]->getIndexOffset();
			TriangleModel::ParticleMesh &mesh = model.getTriangleModels()[cm]->getParticleMesh();
			const unsigned int *tris = mesh.getFaces().data();
			const unsigned int nFaces = mesh.numFaces();
			for (unsigned int i = 0; i < nFaces; i++)
			{
				const unsigned int v1 = tris[3 * i] + offset;
				const unsigned int v2 = tris[3 * i + 1] + offset;
				const unsigned int v3 = tris[3 * i + 2] + offset;
				model.addStrainTriangleConstraint(v1, v2, v3);
			}
		}
		if (bendingMethod != 0)
		{
			const unsigned int offset = model.getTriangleModels()[cm]->getIndexOffset();
			TriangleModel::ParticleMesh &mesh = model.getTriangleModels()[cm]->getParticleMesh();
			unsigned int nEdges = mesh.numEdges();
			const TriangleModel::ParticleMesh::Edge *edges = mesh.getEdges().data();
			const unsigned int *tris = mesh.getFaces().data();
			for (unsigned int i = 0; i < nEdges; i++)
			{
				const int tri1 = edges[i].m_face[0];
				const int tri2 = edges[i].m_face[1];
				if ((tri1 != 0xffffffff) && (tri2 != 0xffffffff))
				{
					// Find the triangle points which do not lie on the axis
					const int axisPoint1 = edges[i].m_vert[0];
					const int axisPoint2 = edges[i].m_vert[1];
					int point1 = -1;
					int point2 = -1;
					for (int j = 0; j < 3; j++)
					{
						if ((tris[3 * tri1 + j] != axisPoint1) && (tris[3 * tri1 + j] != axisPoint2))
						{
							point1 = tris[3 * tri1 + j];
							break;
						}
					}
					for (int j = 0; j < 3; j++)
					{
						if ((tris[3 * tri2 + j] != axisPoint1) && (tris[3 * tri2 + j] != axisPoint2))
						{
							point2 = tris[3 * tri2 + j];
							break;
						}
					}
					if ((point1 != -1) && (point2 != -1))
					{
						const unsigned int vertex1 = point1 + offset;
						const unsigned int vertex2 = point2 + offset;
						const unsigned int vertex3 = edges[i].m_vert[0] + offset;
						const unsigned int vertex4 = edges[i].m_vert[1] + offset;
						if (bendingMethod == 1)
							model.addDihedralConstraint(vertex1, vertex2, vertex3, vertex4);
						else if (bendingMethod == 2)
							model.addIsometricBendingConstraint(vertex1, vertex2, vertex3, vertex4);
					}
				}
			}
		}
	}

	std::cout << "Number of triangles: " << nIndices / 3 << "\n";
	std::cout << "Number of vertices: " << nRows*nCols << "\n";

}

void TW_CALL setTimeStep(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	TimeManager::getCurrent()->setTimeStepSize(val);
}

void TW_CALL getTimeStep(void *value, void *clientData)
{
	*(Real *)(value) = TimeManager::getCurrent()->getTimeStepSize();
}

void TW_CALL setVelocityUpdateMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	((TimeStepController*)clientData)->setVelocityUpdateMethod((unsigned int)val);
}

void TW_CALL getVelocityUpdateMethod(void *value, void *clientData)
{
	*(short *)(value) = (short)((TimeStepController*)clientData)->getVelocityUpdateMethod();
}

void TW_CALL setStiffness(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((SimulationModel*)clientData)->setClothStiffness(val);
}

void TW_CALL getStiffness(void *value, void *clientData)
{
	*(Real *)(value) = ((SimulationModel*)clientData)->getClothStiffness();
}

void TW_CALL setXXStiffness(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((SimulationModel*)clientData)->setClothXXStiffness(val);
}

void TW_CALL getXXStiffness(void *value, void *clientData)
{
	*(Real *)(value) = ((SimulationModel*)clientData)->getClothXXStiffness();
}

void TW_CALL setYYStiffness(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((SimulationModel*)clientData)->setClothYYStiffness(val);
}

void TW_CALL getYYStiffness(void *value, void *clientData)
{
	*(Real *)(value) = ((SimulationModel*)clientData)->getClothYYStiffness();
}

void TW_CALL setXYStiffness(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((SimulationModel*)clientData)->setClothXYStiffness(val);
}

void TW_CALL getXYStiffness(void *value, void *clientData)
{
	*(Real *)(value) = ((SimulationModel*)clientData)->getClothXYStiffness();
}

void TW_CALL setYXPoissonRatio(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((SimulationModel*)clientData)->setClothYXPoissonRatio(val);
}

void TW_CALL getYXPoissonRatio(void *value, void *clientData)
{
	*(Real *)(value) = ((SimulationModel*)clientData)->getClothYXPoissonRatio();
}

void TW_CALL setXYPoissonRatio(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((SimulationModel*)clientData)->setClothXYPoissonRatio(val);
}

void TW_CALL getXYPoissonRatio(void *value, void *clientData)
{
	*(Real *)(value) = ((SimulationModel*)clientData)->getClothXYPoissonRatio();
}

void TW_CALL setNormalizeStretch(const void *value, void *clientData)
{
	const bool val = *(const bool *)(value);
	((SimulationModel*)clientData)->setClothNormalizeStretch(val);
}

void TW_CALL getNormalizeStretch(void *value, void *clientData)
{
	*(bool *)(value) = ((SimulationModel*)clientData)->getClothNormalizeStretch();
}

void TW_CALL setNormalizeShear(const void *value, void *clientData)
{
	const bool val = *(const bool *)(value);
	((SimulationModel*)clientData)->setClothNormalizeShear(val);
}

void TW_CALL getNormalizeShear(void *value, void *clientData)
{
	*(bool *)(value) = ((SimulationModel*)clientData)->getClothNormalizeShear();
}

void TW_CALL setBendingStiffness(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((SimulationModel*)clientData)->setClothBendingStiffness(val);
}

void TW_CALL getBendingStiffness(void *value, void *clientData)
{
	*(Real *)(value) = ((SimulationModel*)clientData)->getClothBendingStiffness();
}

void TW_CALL setBendingMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	*((short*)clientData) = val;
	reset();
}

void TW_CALL getBendingMethod(void *value, void *clientData)
{
	*(short *)(value) = *((short*)clientData);
}

void TW_CALL setSimulationMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	*((short*)clientData) = val;
	reset();
}

void TW_CALL getSimulationMethod(void *value, void *clientData)
{
	*(short *)(value) = *((short*)clientData);
}


/****************************************************************************
 * Copyright (c) 2017-2021 by the ArborX authors                            *
 * All rights reserved.                                                     *
 *                                                                          *
 * This file is part of the ArborX library. ArborX is                       *
 * distributed under a BSD 3-clause license. For the licensing terms see    *
 * the LICENSE file in the top-level directory.                             *
 *                                                                          *
 * SPDX-License-Identifier: BSD-3-Clause                                    *
 ****************************************************************************/

#include "ArborXTest_StdVectorToKokkosView.hpp"
#include "ArborX_EnableDeviceTypes.hpp" // ARBORX_DEVICE_TYPES
#include "ArborX_EnableViewComparison.hpp"
#include <ArborX_DetailsKokkosExtArithmeticTraits.hpp>
#include <ArborX_DetailsMutualReachabilityDistance.hpp>
#include <ArborX_LinearBVH.hpp>

#include "BoostTest_CUDA_clang_workarounds.hpp"
#include <boost/test/unit_test.hpp>

namespace Test
{
using ArborXTest::toView;

template <class ExecutionSpace>
auto compute_core_distances(ExecutionSpace exec_space,
                            std::vector<ArborX::Point> const &points_host,
                            int k)
{
  auto points = toView<ExecutionSpace>(points_host, "Test::points");

  ARBORX_ASSERT(points.extent_int(0) >= k);
  using MemorySpace = typename ExecutionSpace::memory_space;
  ArborX::BVH<MemorySpace> bvh{exec_space, points};
  Kokkos::View<float *, MemorySpace> distances(
      Kokkos::view_alloc(Kokkos::WithoutInitializing, "Test::core_distances"),
      bvh.size());
  constexpr auto inf = KokkosExt::ArithmeticTraits::infinity<float>::value;
  Kokkos::deep_copy(exec_space, distances, -inf);
  bvh.query(exec_space, ArborX::Details::NearestK<decltype(points)>{points, k},
            ArborX::Details::MaxDistance<decltype(points), decltype(distances)>{
                points, distances});

  return Kokkos::create_mirror_view_and_copy(Kokkos::HostSpace{}, distances);
}

#define ARBORX_TEST_COMPUTE_CORE_DISTANCES(exec_space, points, k, ref)         \
  BOOST_TEST(Test::compute_core_distances(exec_space, points, k) == ref,       \
             boost::test_tools::per_element())

template <class ExecutionSpace>
auto compute_mutual_reachability_distances(
    ExecutionSpace exec_space, std::vector<float> const &core_distances_host,
    std::vector<Kokkos::pair<int, int>> const &edges_host,
    std::vector<float> const &distances_host)
    -> Kokkos::View<float *, Kokkos::HostSpace>
{
  auto core_distances =
      toView<ExecutionSpace>(core_distances_host, "Test::core_distances");
  auto edges = toView<ExecutionSpace>(edges_host, "Test::edges");
  auto distances = toView<ExecutionSpace>(distances_host, "Test::distances");

  auto const n = edges.size();
  Kokkos::View<float *, ExecutionSpace> mutual_reachability_distances(
      Kokkos::view_alloc(Kokkos::WithoutInitializing,
                         "Test::mutual_reachability_distances"),
      n);
  ArborX::Details::MutualReachability<decltype(core_distances)> const
      distance_mutual_reach{core_distances};
  Kokkos::parallel_for("Test::compute_mutual_reachability_distances",
                       Kokkos::RangePolicy<ExecutionSpace>(exec_space, 0, n),
                       KOKKOS_LAMBDA(int i) {
                         mutual_reachability_distances(i) =
                             distance_mutual_reach(
                                 edges(i).first, edges(i).second, distances(i));
                       });

  return Kokkos::create_mirror_view_and_copy(Kokkos::HostSpace{},
                                             mutual_reachability_distances);
}

#define ARBORX_TEST_COMPUTE_MUTUAL_REACHABILITY_DISTANCES(                     \
    exec_space, core_distances, edges, distances, ref)                         \
  BOOST_TEST(Test::compute_mutual_reachability_distances(                      \
                 exec_space, core_distances, edges, distances) == ref,         \
             boost::test_tools::per_element())

} // namespace Test

BOOST_AUTO_TEST_SUITE(MutualReachabilityDistance)

BOOST_AUTO_TEST_CASE_TEMPLATE(compute_core_distances, DeviceType,
                              ARBORX_DEVICE_TYPES)
{
  using ExecutionSpace = typename DeviceType::execution_space;
  ExecutionSpace exec_space;

  std::vector<ArborX::Point> points{
      {0, 0, 0}, {1, 0, 0}, {2, 0, 0}, {3, 0, 0}, {4, 0, 0},
  };
  ARBORX_TEST_COMPUTE_CORE_DISTANCES(exec_space, points, 1,
                                     (std::vector<float>{0, 0, 0, 0, 0}));
  ARBORX_TEST_COMPUTE_CORE_DISTANCES(exec_space, points, 2,
                                     (std::vector<float>{1, 1, 1, 1, 1}));
  ARBORX_TEST_COMPUTE_CORE_DISTANCES(exec_space, points, 3,
                                     (std::vector<float>{2, 1, 1, 1, 2}));
  ARBORX_TEST_COMPUTE_CORE_DISTANCES(exec_space, points, 4,
                                     (std::vector<float>{3, 2, 2, 2, 3}));
  ARBORX_TEST_COMPUTE_CORE_DISTANCES(exec_space, points, 5,
                                     (std::vector<float>{4, 3, 2, 3, 4}));

  std::vector<ArborX::Point> non_equidistant_points{
      {0, 0, 0}, {1, 0, 0}, {2, 0, 0}, {3, 0, 0}, {6, 0, 0}, {10, 0, 0},
  };
  ARBORX_TEST_COMPUTE_CORE_DISTANCES(exec_space, non_equidistant_points, 2,
                                     (std::vector<float>{1, 1, 1, 1, 3, 4}));
  ARBORX_TEST_COMPUTE_CORE_DISTANCES(exec_space, non_equidistant_points, 3,
                                     (std::vector<float>{2, 1, 1, 2, 4, 7}));
  ARBORX_TEST_COMPUTE_CORE_DISTANCES(exec_space, non_equidistant_points, 4,
                                     (std::vector<float>{3, 2, 2, 3, 4, 8}));
  ARBORX_TEST_COMPUTE_CORE_DISTANCES(exec_space, non_equidistant_points, 5,
                                     (std::vector<float>{6, 5, 4, 3, 5, 9}));
  ARBORX_TEST_COMPUTE_CORE_DISTANCES(exec_space, non_equidistant_points, 6,
                                     (std::vector<float>{10, 9, 8, 7, 6, 10}));
}
BOOST_AUTO_TEST_CASE_TEMPLATE(compute_mutual_reachability_distances, DeviceType,
                              ARBORX_DEVICE_TYPES)
{
  using ExecutionSpace = typename DeviceType::execution_space;
  ExecutionSpace exec_space;

  { // equidistant points
    // 0     1     2     3     4
    //[0]   [1]   [2]   [3]   [4]
    std::vector<Kokkos::pair<int, int>> edges{{0, 1}, {0, 2}, {0, 3}, {0, 4},
                                              {1, 2}, {1, 3}, {1, 4}, {2, 3},
                                              {2, 4}, {3, 4}};
    std::vector<float> distances{1, 2, 3, 4, 1, 2, 3, 1, 2, 1};

    ARBORX_TEST_COMPUTE_MUTUAL_REACHABILITY_DISTANCES(
        exec_space, (std::vector<float>{1, 1, 1, 1, 1}), edges, distances,
        (std::vector<float>{1, 2, 3, 4, 1, 2, 3, 1, 2, 1}));

    ARBORX_TEST_COMPUTE_MUTUAL_REACHABILITY_DISTANCES(
        exec_space, (std::vector<float>{2, 1, 1, 1, 2}), edges, distances,
        (std::vector<float>{2, 2, 3, 4, 1, 2, 3, 1, 2, 2}));

    ARBORX_TEST_COMPUTE_MUTUAL_REACHABILITY_DISTANCES(
        exec_space, (std::vector<float>{3, 2, 2, 2, 3}), edges, distances,
        (std::vector<float>{3, 3, 3, 4, 2, 2, 3, 2, 3, 3}));

    ARBORX_TEST_COMPUTE_MUTUAL_REACHABILITY_DISTANCES(
        exec_space, (std::vector<float>{4, 3, 2, 3, 4}), edges, distances,
        (std::vector<float>{4, 4, 4, 4, 3, 3, 4, 3, 4, 4}));
  }
  { // non-equidistant points
    // 0   1   2   3   4   5   6   7   8   9   10
    //[0] [1] [2] [3]         [4]             [5]
    std::vector<Kokkos::pair<int, int>> edges{
        {0, 1}, {0, 2}, {0, 3}, {0, 4}, {0, 5}, {1, 2}, {1, 3}, {1, 4},
        {1, 5}, {2, 3}, {2, 4}, {2, 5}, {3, 4}, {3, 5}, {4, 5}};
    std::vector<float> distances{1, 2, 3, 6, 10, 1, 2, 5, 9, 1, 4, 8, 3, 7, 4};

    ARBORX_TEST_COMPUTE_MUTUAL_REACHABILITY_DISTANCES(
        exec_space, (std::vector<float>{1, 1, 1, 1, 3, 4}), edges, distances,
        (std::vector<float>{1, 2, 3, 6, 10, 1, 2, 5, 9, 1, 4, 8, 3, 7, 4}));

    ARBORX_TEST_COMPUTE_MUTUAL_REACHABILITY_DISTANCES(
        exec_space, (std::vector<float>{2, 1, 1, 2, 4, 7}), edges, distances,
        (std::vector<float>{2, 2, 3, 6, 10, 1, 2, 5, 9, 2, 4, 8, 4, 7, 7}));

    ARBORX_TEST_COMPUTE_MUTUAL_REACHABILITY_DISTANCES(
        exec_space, (std::vector<float>{3, 2, 2, 3, 4, 8}), edges, distances,
        (std::vector<float>{3, 3, 3, 6, 10, 2, 3, 5, 9, 3, 4, 8, 4, 8, 8}));

    ARBORX_TEST_COMPUTE_MUTUAL_REACHABILITY_DISTANCES(
        exec_space, (std::vector<float>{6, 5, 4, 3, 5, 9}), edges, distances,
        (std::vector<float>{6, 6, 6, 6, 10, 5, 5, 5, 9, 4, 5, 9, 5, 9, 9}));

    ARBORX_TEST_COMPUTE_MUTUAL_REACHABILITY_DISTANCES(
        exec_space, (std::vector<float>{10, 9, 8, 7, 6, 10}), edges, distances,
        (std::vector<float>{10, 10, 10, 10, 10, 9, 9, 9, 10, 8, 8, 10, 7, 10,
                            10}));
  }
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/log_2olog_10.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check log_2olog_10 behavior for integral types"
            , (std::uint32_t)(std::uint64_t)
              (std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::log_2olog_10;
  using boost::simd::Log_2olog_10;
  T ref = T(0);
  STF_TYPE_IS(decltype(Log_2olog_10<T>()), T);
  STF_EQUAL(Log_2olog_10<T>(), ref);
  STF_EQUAL(log_2olog_10( as(T{}) ),ref);
}

STF_CASE_TPL( "Check log_2olog_10 behavior for double"
            , (double)(float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::log_2olog_10;
  using boost::simd::Log_2olog_10;
  T ref = T(0.3010299956639811952137388947244930267681898814621085);

  STF_TYPE_IS(decltype(Log_2olog_10<T>()), T);
  STF_IEEE_EQUAL(Log_2olog_10<T>(), ref);
  STF_IEEE_EQUAL(log_2olog_10( as(T{}) ), ref);
}


// Copyright 2015-2020 The ALMA Project Developers
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//   http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
// implied. See the License for the specific language governing
// permissions and limitations under the License.

/// @file
/// Test force constant loading.
///
/// The code in this file loads a BORN file for InAs and
/// performs some checks on the resulting variables.

#include <boost/filesystem.hpp>
#include "gtest/gtest.h"
#include <cmakevars.hpp>
#include <vasp_io.hpp>

TEST(born_test_case, born_test) {
    auto basedir = boost::filesystem::path(TEST_RESOURCE_DIR);
    auto inas_dir = basedir / boost::filesystem::path("InAs");
    auto born_path = inas_dir / boost::filesystem::path("BORN");

    auto born = alma::load_BORN(born_path.string().c_str());

    ASSERT_EQ(2u, born->born.size());

    for (auto i = 0; i < 3; ++i) {
        for (auto j = 0; j < 3; ++j) {
            if (i == j) {
                EXPECT_DOUBLE_EQ(19.643, born->epsilon(i, j));
                EXPECT_DOUBLE_EQ(2.6781, born->born[0](i, j));
                EXPECT_DOUBLE_EQ(-2.6781, born->born[1](i, j));
            }
            else {
                EXPECT_DOUBLE_EQ(0., born->epsilon(i, j));
                EXPECT_DOUBLE_EQ(0., born->born[0](i, j));
                EXPECT_DOUBLE_EQ(0., born->born[1](i, j));
            }
        }
    }
}


#include <cmath>
#include <fstream>
#include <string>

#include <boost/timer/timer.hpp>

#include "optimization.hh"
#include "adaptive_refinement.hh"
#include "linalg/linearsystem.hh"
#include "linalg/triplet.hh"
#include "./opt_aux/src/include/Fmin.h"
#include "modelCreator.hh"
#include "modelFunctions.hh"

#ifndef Cygwin
using std::to_string;
#else
std::string to_string(size_t n)
{
  std::stringstream s; s << n; return s.str();
}
#endif

namespace Kaskade
{
  OptimizationParameters::OptimizationParameters(double desiredAccuracy_, int maxSteps_) : desiredAccuracy(desiredAccuracy_), maxSteps(maxSteps_) {}

  void OptimizationParameters::setThetaAim(double theta) { ThetaAim = theta; ensureAdmissibleThetas(); }
  void OptimizationParameters::setThetaNormal(double theta) { ThetaNormal = theta; ensureAdmissibleThetas(); }
  void OptimizationParameters::setThetaMax(double theta) { ThetaMax = theta; ensureAdmissibleThetas(); }
  void OptimizationParameters::setThetas(double thetaNormal, double thetaAim, double thetaMax)
  {
    ThetaNormal = thetaNormal;
    ThetaAim = thetaAim;
    ThetaMax = thetaMax;
    ensureAdmissibleThetas();
  }
  void OptimizationParameters::setEps(double eps_)
  {
    eps = eps_;
    sqrtEps = sqrt(eps);
    thirdSqrtEps = pow(eps,1./3.);
  }

  double OptimizationParameters::getThetaAim() const { return ThetaAim; }
  double OptimizationParameters::getThetaNormal() const { return ThetaNormal; }
  double OptimizationParameters::getThetaMax() const { return ThetaMax; }
  double OptimizationParameters::getEps() const { return eps; }
  double OptimizationParameters::getSqrtEps() const { return sqrtEps; }
  double OptimizationParameters::getThirdSqrtEps() const { return thirdSqrtEps; }

  void OptimizationParameters::ensureAdmissibleThetas()
  {
    if( !(ThetaAim < ThetaMax) )
    {
      std::cout << "OPTIMIZATION PARAMETERS: Warning: Inconsistent algorithmic parameters. (ThetaMax <= ThetaAIm)" << std::endl;
      std::cout << "OPTIMIZATION PARAMETERS: Adjusting ThetaAim from " << ThetaAim << " to ";
      ThetaAim = 0.9*ThetaMax;
      std::cout << ThetaAim << std::endl;
    }
    if( !(ThetaNormal < ThetaAim) )
    {
      std::cout << "OPTIMIZATION PARAMETERS: Warning: Inconsistent algorithmic parameters. (ThetaAim <= ThetaNormal)" << std::endl;
      std::cout << "OPTIMIZATION PARAMETERS: Adjusting ThetaNormal from " << ThetaNormal << " to ";
      ThetaNormal = 0.9*ThetaAim;
      std::cout << ThetaNormal << std::endl;
    }
  }

  Optimization::~Optimization(){}

  Optimization::Optimization(AbstractScalarProduct& nL, AbstractScalarProduct& nC, AbstractChart const& chart_, OptimizationParameters const& p_,
                             AbstractCompositeStepErrorEstimator* errorEstimator_, int verbose_)
    : normL(nL), normC(nC), chart(chart_.clone()), p(p_), errorEstimator(errorEstimator_),
      verbose(verbose_),L(verbose), C(verbose)
  {}

  Optimization::Optimization(AbstractNormalDirection& normalSolver,
                             AbstractTangentialSpace& tangentSpace_,
                             AbstractScalarProduct& nL,
                             OptimizationParameters const& p_,
                             double omegaCinit, double omegaLinit, int verbose_):
    normL(nL),
    normC(nL),
    chart(new PrimalChart()),
    p(p_),
    normalDirection(&normalSolver),
    tangentSpace(&tangentSpace_),
    verbose(verbose_),
    L(verbose,omegaLinit), C(verbose,omegaCinit)
  {}

  Optimization::Optimization(AbstractNormalDirection& normalSolver,
                             AbstractScalarProduct& nL,
                             AbstractScalarProduct& nC,
                             AbstractChart const& chart_,
                             OptimizationParameters const& p_, int verbose_):
    normL(nL),
    normC(nC),
    chart(chart_.clone()),
    p(p_),
    normalDirection(&normalSolver),
    verbose(verbose_),
    L(verbose), C(verbose)
  {}

  /// Return true, if convergence is detected, false otherwise
  Convergence Optimization::convergenceTest(double nu, std::vector<double> const& tau, double normOfCorrection) const
  {
    if( verbose > 0 )
    {
      std::cout << csPre << "Desired accuracy: " << p.desiredAccuracy << ", ||x|| = " << normOfIterate << std::endl;
      std::cout << csPre << "||dx|| = " << normOfCorrection << std::endl;
    }

    //if( nu > 0.1 && normOfCorrection < p.getEps() ) return Convergence::Achieved;

    if( !tangentSpace->localConvergenceLikely() ) return Convergence::Missed;

    // no local convergence as long as at least one of the steps is damped
    if( !noDamping(nu) || !noDamping(tau) ) return Convergence::Missed;
    if( normOfCorrection < p.desiredAccuracy * normOfIterate ) return Convergence::Achieved;

    return Convergence::Missed;
  }

  bool Optimization::adaptiveMeshRefinement(LagrangeLinearization& lin, double nu, std::vector<double> tau, AbstractFunctionSpaceElement const& correction)
  {
    double safetyFactorForDesiredAccuracy = 0.9;

    if( !( noDamping(nu) && noDamping(tau) ) )
    {
      std::cout << csPre << "Damping factors != 1 -> Skipping error estimation." << std::endl;
      return false;
    }

    if(hbErrorEstimator->gridSize() > p.maxGridSize)
    {
      std::cout << csPre << "Grid size: " << hbErrorEstimator->gridSize() << " > " << p.maxGridSize << ". Skipping error estimation. " << std::endl;
      return false;
    }

      // estimate error
    (*hbErrorEstimator)(lin, *iterate, correction, step, *iterate);

    double absoluteError=hbErrorEstimator->estimatedAbsoluteError();
    double requiredAbsoluteError=std::max(p.requiredRelativeError*normOfLastCorrection,p.desiredAccuracy*safetyFactorForDesiredAccuracy*normL(*iterate));
    bool accuracyReached = absoluteError < requiredAbsoluteError;

    if(verbose > 0)
    {
      std::cout << csPre << "ERROR ESTIMATOR: err0=" << p.requiredRelativeError*normOfLastCorrection << ", err1=" << p.desiredAccuracy*safetyFactorForDesiredAccuracy << std::endl;
      std::cout << csPre << "ERROR ESTIMATOR: normOfAcceptedCorrection=" << normOfLastCorrection << ", p.desiredAccuracy=" << p.desiredAccuracy << std::endl;
      std::cout << csPre << "ERROR ESTIMATOR: absolute error: " << absoluteError << ", required error: " << requiredAbsoluteError << std::endl;
    }

    if(accuracyReached)
    {
      std::cout << "ERROR ESTIMATOR: Discretization accuracy reached!" << std::endl;
      return false;
    }

    hbErrorEstimator->refineGrid();
    return true;
  }

  int Optimization::runAlgorithm()
  {
    std::ofstream omegafile("omegas.log");
    std::ofstream coefffile("coeff.log");
    std::ofstream correctionfile("corrections.log");
    std::ofstream values("values.log");
    values.setf(std::ios::scientific,std::ios::floatfield);
    values.precision(16);
    std::ofstream steps("steps.log");

    double nu = 0;
    std::vector<double> tau(1,0);
    // Main loop
    for( step=1 ; step <= p.maxSteps; ++step )
    {
      if(verbose > 0) std::cout <<  "\n --------------- Optimization-Step: " << step << " ----------------------" << std::endl;

      /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
      /* * * * * * * * * * * * * * * * Normal Step * * * * * * * * * * * * * * * * */
      /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
      if( functionalN == nullptr ) std::cout << "no normal lineraization" << std::endl;
      normalLinearization = functionalN->getLinearization(*iterate);
      values << normalLinearization->eval() << std::endl;

      normC.setOrigin(*normalLinearization);
      normL.setOrigin(*normalLinearization);
      std::unique_ptr<AbstractFunctionSpaceElement> normalStepResidual(iterate->initZeroVector()), adjointResidual(iterate->initZeroVector()), undampedStep(iterate->initZeroVector());
      auto normalStepResult = computeNormalStep(normalStepResidual.get(),adjointResidual.get());

      std::unique_ptr<AbstractFunctionSpaceElement> normalStep(std::move(normalStepResult.first)), adjointCorrection(std::move(normalStepResult.second));
      undampedStep->axpy(1.0,*normalStep,"primal");
      double normOfUndampedCorrection = 0;

      double Lxdn_res = adjointResidual->applyAsDualTo(*normalStep,"primal")
                      - normalStepResidual->applyAsDualTo(*adjointCorrection,"primal")
                      + adjointResidual->applyAsDualTo(*normalStep,"dual");
      if( verbose > 1 ) std::cout << csPre << "Lxdn_res: " << Lxdn_res << std::endl;
      auto normNormal = normC( *normalStep );

      // Predict damping factor for normal step
      nu = (normNormal > 0) ? updateNormalStepDampingFactor(normNormal) : 1.;
      if( verbose > 0 ) printNormalStep(normNormal,nu);

      std::unique_ptr<AbstractFunctionSpaceElement> tmpIter(iterate->clone());
      tmpIter->axpy(nu,*normalStep,"primal");
      std::unique_ptr<AbstractLinearization> lin_x_nudn(functionalN->getLinearization(*tmpIter));

      std::unique_ptr<AbstractFunctionSpaceElement> dc_x0_dn ( iterate->initZeroVector() );
      normalLinearization->d2axpy(1.0,*dc_x0_dn, *normalStep, 2, 3, 0, 2);
      double p_dc_x0_dn = iterate->applyAsDualTo(*dc_x0_dn,"dual");
      double f_x1 = lin_x_nudn->eval();
      double f_x0 = normalLinearization->eval();

      double normalStepMonitor = ( ( std::fabs(f_x1 - f_x0) + std::fabs(p_dc_x0_dn) ) > 0 ) ? std::fabs( f_x1 - f_x0 + nu*p_dc_x0_dn - Lxdn_res ) / ( std::fabs(f_x1 - f_x0) + std::fabs(p_dc_x0_dn) ) : 0.;
      if( normNormal < p.getSqrtEps()*normOfIterate || step == 1 ) normalStepMonitor = 0;
      bool reliableQuadraticModel = true;//normalStepMonitor < 0.5;

      if( verbose > 1 )
      {
        std::cout << "f(x0) = " << f_x0 << ", f(x1) = " << f_x1 << ", nupc'(x0) = " << nu << "*" << p_dc_x0_dn << std::endl;
        std::cout << "f(x1) - f(x0) = " << f_x1 - f_x0 << std::endl;
      }
      if( verbose > 0 ) std::cout << csPre << "NORMAL STEP MONITOR: " << normalStepMonitor << ", is reliable: " << reliableQuadraticModel << std::endl;

      /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
      /* * * * * * * * * * * * * * * Tangential Step * * * * * * * * * * * * * * * */
      /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
      double normTangential = 0;
      normOfUndampedCorrection = normNormal;
      std::vector<std::shared_ptr<AbstractFunctionSpaceElement> > tangentialBasis(1,iterate->clone());

      std::unique_ptr<LagrangeLinearization> lagrangeLinearization(nullptr);

      if(reliableQuadraticModel)
      {
        if(functionalT!=nullptr) tangentialLinearization = functionalT->getLinearization(*iterate);
        else tangentialLinearization = nullptr;

        lagrangeLinearization.reset(new LagrangeLinearization(normalLinearization.get(),tangentialLinearization.get(),*iterate));

        tangentialBasis = computeTangentialStep(*lagrangeLinearization,*normalStep, nu, tau);
        for(size_t i=0; i<tangentialBasis.size(); ++i) undampedStep->axpy(1.0,*tangentialBasis[i],"primal");
        normTangential = normL(*tangentialBasis[0]);
        normOfUndampedCorrection = normC(*undampedStep);
      }
      else
      {
        *tangentialBasis[0] *= 0;
        tau[0] = 0;
      }
      if( verbose > 0 ) std::cout << csPre << "Tangential step length: " << normTangential << std::endl;
      /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
      /* * * * * * * * * * * Lipschitz constants and damping * * * * * * * * * * * */
      /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
      AcceptanceTest acceptanceTestResult = AcceptanceTest::Failed;
      std::unique_ptr<AbstractFunctionSpaceElement> trialIterate(iterate->initZeroVector()), secondOrderCorrected(iterate->clone()), correction(iterate->initZeroVector());
      double normCAtCorr = normNormal;
      while(acceptanceTestResult != AcceptanceTest::Passed)
      {
        double eta = 1;
        if( acceptanceTestResult == AcceptanceTest::LeftAdmissibleDomain ) nu *= 0.5;
        else nu = updateNormalStepDampingFactor(normNormal);
        if( verbose > 0 ) printNormalStep(normNormal,nu);
        if(!reliableQuadraticModel)
        {
          correction = createCorrection(nu,*normalStep, tau, tangentialBasis);
          normCAtCorr = normC(*correction);
          // Compute Trial Iterate: first candidate: trialIterate = chart_x(dx)
          *trialIterate *= 0;
          chart->addPerturbation(*trialIterate,*correction,*normalLinearization);

          std::unique_ptr<AbstractLinearization> lin_xplus(functionalN->getLinearization(*trialIterate));
          auto simplifiedNormalStep = computeSimplifiedNormalStep( *lin_xplus, normalStepResidual.get());
          simplifiedNormalStep->axpy( nu-1.0, *normalStep, "primal" );

          updateConstraintD1LipschitzConstant( normC( *simplifiedNormalStep ), normCAtCorr );
        }
        else
        {
          QuadraticModelCreator quadraticModelCreator(*normalStep,tangentialBasis,*lagrangeLinearization,Lxdn_res);
          NormModelCreator normModelCreatorL(*normalStep,tangentialBasis,normL);

          QuadraticFunction normLModel(normModelCreatorL.create(nu));
          QuadraticFunction quadraticModel(quadraticModelCreator.create(nu));
          L.setL_xx(quadraticModel.quadraticPart[0][0]);
          IsotropicCubicRegularization reg(normLModel,L.omega/6);
          RegularizedQuadraticFunction cubic(quadraticModel, reg);
          if( acceptanceTestResult == AcceptanceTest::LeftAdmissibleDomain ) tau[0] *= 0.5;
          else updateTangentialDampingFactor(nu, normNormal, normTangential, cubic, tau);

          // Compute Correction as a linear combination of normal step and basis vectors of tangent search space
          correction = createCorrection(nu,*normalStep,tau,tangentialBasis);
          auto normLAtCorr = sqrt(normLModel.d0(tau));
          normCAtCorr = normC(*correction);
          auto quadraticModelAtCorr = quadraticModel.d0(tau);
          // Compute Trial Iterate: first candidate: trialIterate = chart_x(dx)
          trialIterate = iterate->clone();
          chart->addPerturbation(*trialIterate,*correction,*lagrangeLinearization);
          if( !functionalN->inDomain(*trialIterate) || ( functionalT != nullptr && !functionalT->inDomain(*trialIterate) ) )
          {
            std::cout << csPre << "Iterate leaves admissible domain: rejecting step." << std::endl;
            acceptanceTestResult = AcceptanceTest::LeftAdmissibleDomain;
          }
          else
          {
            acceptanceTestResult = AcceptanceTest::Failed;
            std::unique_ptr<AbstractLinearization> lin_xplus(functionalN->getLinearization(*trialIterate));
            auto simplifiedNormalStep = computeSimplifiedNormalStep( *lin_xplus, normalStepResidual.get());
            simplifiedNormalStep->axpy( nu-1.0, *normalStep, "primal" );
            //for( size_t i = 0; i < tangentialBasis.size(); ++i ) simplifiedNormalStep->axpy( tau[i] - 1.0, *tangentialBasis[i], "primal" );

            updateConstraintD1LipschitzConstant( normC( *simplifiedNormalStep ), normCAtCorr );

            // Perform second order correction update secondOrderCorrected=chart_x(dx+ds)
            std::unique_ptr<AbstractFunctionSpaceElement> secondOrderCorrection(simplifiedNormalStep->clone());
            secondOrderCorrection->axpy(1.0,*correction,"primal");
            *secondOrderCorrected = *iterate->clone();
            chart->addPerturbation(*secondOrderCorrected,*secondOrderCorrection,*lagrangeLinearization);

            eta = updateLagrangianD2LipschitzConstant(*lagrangeLinearization, *secondOrderCorrected, normLAtCorr, quadraticModelAtCorr, cubic, nu, tau,*normalStep,*trialIterate,*simplifiedNormalStep,*correction,Lxdn_res);
          }
        }

        // Acceptance tests
        if( acceptanceTestResult != AcceptanceTest::LeftAdmissibleDomain ) acceptanceTestResult = acceptanceTest(eta,nu,tau,normCAtCorr);
        if(!regularityTest(nu,tau,reliableQuadraticModel)) return -1;

        if( reliableQuadraticModel && (acceptanceTestResult==AcceptanceTest::TangentialStepFailed) && L.omega < (1 + 0.25*(1 - p.etaMin))*L.oldOmega )
        {
          if( verbose > 0 ) std::cout << csPre << "Stagnating omegaL update. Accepting step." << std::endl;
          acceptanceTestResult = AcceptanceTest::Passed;
          if(eta < 0.1)
          {
            if( verbose > 0 ) std::cout << csPre << "Ignoring tangential step." << std::endl;
            for(size_t i=0; i<tangentialBasis.size(); ++i)
            {
              trialIterate->axpy(-tau[i],*tangentialBasis[i]);
              secondOrderCorrected->axpy(-tau[i],*tangentialBasis[i]);
            }
          }
        }

        if(acceptanceTestResult == AcceptanceTest::Passed)
        {
          normOfLastCorrection = normCAtCorr;
          normOfLastCorrection_Undamped = normOfUndampedCorrection;
          correctionfile << normOfLastCorrection << " " << normL(*normalStep) << " " << normL(*tangentialBasis[0]) << std::endl;
        }
      }

      // Error estimation
      bool refinedMesh = false;
      if( hbErrorEstimator != nullptr && tangentSpace->localConvergenceLikely()  && reliableQuadraticModel ) refinedMesh = adaptiveMeshRefinement(*lagrangeLinearization, nu,tau,*correction);

      // at this point, *trialIterate is the combined step
      // and *secondOrderCorrected is the second order corrected step
      // if Theta > 0.25, this might be worse w.r.t feasibility than the original step
      // use original step in this case
      if(normalDirection)
      {
        if(C.theta > 0.25 || !reliableQuadraticModel)
        {
          if(verbose > 1) std::cout << csPre << "Adding ordinary update" << std::endl;
        }
        else
        {
          *trialIterate = *secondOrderCorrected;
          if(verbose > 1) std::cout << csPre << "Adding second order correction" << std::endl;
        }
      }

      iterate->swap(*trialIterate);
      normOfIterate = normL(*iterate);
//      *trialIterate -= *iterate;

      /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
      /* * * * * * * * * * * * * * * * Write data  * * * * * * * * * * * * * * * * */
      /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
      if( verbose > 0)
      {
        std::cout << "\n" << csPre << "Accepted step" << std::endl;
        printNormalStep(normNormal, nu);
        printTangentialStep(normTangential, tau[0]);
      }

      if( verbose > 1 )
      {
        std::string name = std::string("iterate_") + to_string(step);
        iterate->writeToFile(name,false);
      }

      omegafile << " " << L.omega << " " << " " << C.omega << " " << std::endl;
      coefffile << nu << "  " << tau[0] << std::endl;

      if( !refinedMesh && convergenceTest(nu,tau,normL(*correction)) == Convergence::Achieved )
      {
        steps << step << std::endl;

        if( verbose > 1 )
        {
          std::cout << csPre << "overall normal step computation time: " << normalStepComputationTime << std::endl;
          std::cout << csPre << "overall tangential step computation time: " << tangentialStepComputationTime << std::endl;
        }
        return 1;
      }
    }
    return -2;
  }

  void Optimization::solve(AbstractFunctional &fN, AbstractFunctional& fT, AbstractFunctionSpaceElement &x) { solve(fN,&fT,x); }

  void Optimization::solve(AbstractFunctional& fN, AbstractFunctional* fT,AbstractFunctionSpaceElement& x)
  {
    functionalN = &fN;
    functionalT = fT;

    iterate = x.clone();
    normalStepComputationTime = tangentialStepComputationTime = 0;
    if( tangentSpace != nullptr ) tangentSpace->setEps(p.getEps());

    normOfLastCorrection = normOfLastCorrection_Undamped = -1;
    normOfIterate = 0;
    auto terminationFlag = algorithmWrapper();
    if(terminationFlag > 0 || terminationFlag == -2) x=*iterate;
  }

  void Optimization::solve(AbstractFunctional& fN, AbstractFunctional& fT, AbstractFunctionSpaceElement& x, AbstractHierarchicalErrorEstimator& hbErrorEstimator_)
  {
    hbErrorEstimator = &hbErrorEstimator_;
    solve(fN,fT,x);
  }

  void Optimization::solve(AbstractFunctional& f, AbstractFunctionSpaceElement& x, AbstractHierarchicalErrorEstimator& hbErrorEstimator_)
  {
    hbErrorEstimator = &hbErrorEstimator_;
    solve(f,nullptr,x);
  }

  bool Optimization::regularityTest(double nu, std::vector<double> const& tau, bool reliableQuadraticModel) const
  {
    bool passed = nu > p.minimalDampingFactor;
    if(reliableQuadraticModel) passed = passed && tau[0] > p.minimalDampingFactor;
    if(verbose > 0 && !passed) std::cout << csPre << "Regularity test failed!!!" << std::endl;
    return passed;
  }

  std::pair<std::unique_ptr<AbstractFunctionSpaceElement>,std::unique_ptr<AbstractFunctionSpaceElement> > Optimization::computeNormalStep(AbstractFunctionSpaceElement* normalStepResidual, AbstractFunctionSpaceElement* adjointResidual)
  {
    std::pair<std::unique_ptr<AbstractFunctionSpaceElement>,std::unique_ptr<AbstractFunctionSpaceElement> > result(iterate->clone(),iterate->clone());
    *(result.first) *= 0;
    *(result.second) *= 0;

    if(normalDirection) // constrained problem
    {
      // Compute normal step, compute update for Lagrange multiplier
      if( step > 1 && std::fabs(normOfLastCorrection_Undamped-normOfLastCorrection) < p.getEps() * normOfLastCorrection_Undamped) normalDirection->setRelativeAccuracy(std::max(std::min(p.minimalAccuracy, C.omega*normOfLastCorrection),p.desiredAccuracy));
      else normalDirection->setRelativeAccuracy(p.minimalAccuracy);
      boost::timer::cpu_timer timer;
      normalDirection->ordinaryAndAdjoint(*(result.first), *(result.second), *normalLinearization, normalStepResidual, adjointResidual);
      normalStepComputationTime += (double)timer.elapsed().wall*1e-9;

      if( verbose > 1 ) std::cout << "normal step computation time: " << boost::timer::format(timer.elapsed()) << std::endl;

      iterate->axpy(1.0,*(result.second),"dual");
    }
    else // unconstrained problem
      if(verbose > 0) std::cout << csPre << "Unconstrained Problem" << std::endl;

    return result;
  }

  std::vector<std::shared_ptr<AbstractFunctionSpaceElement> > Optimization::computeTangentialStep(LagrangeLinearization& lagrangeLinearization, AbstractFunctionSpaceElement const& normalStep, double nu, std::vector<double> const& tau)
  {
    std::vector<std::shared_ptr<AbstractFunctionSpaceElement> > tangentialBasis;
    tangentialBasis.resize(1,std::shared_ptr<AbstractFunctionSpaceElement>(iterate->clone())); *(tangentialBasis[0]) *= 0;

    if(!tangentSpace) return tangentialBasis;

    double relativeAccuracy = std::max(p.desiredAccuracy,std::min(p.minimalAccuracy, L.omega * std::fabs(normOfLastCorrection)));
    if(step > 1 && std::fabs(normOfLastCorrection_Undamped-normOfLastCorrection) < p.getEps() * normOfLastCorrection_Undamped && noDamping(nu))
    {
      if(tangentSpace) tangentSpace->setRelativeAccuracy(relativeAccuracy);
      if(verbose > 1)
      {
        std::cout << csPre << "desired contraction: " << p.getThetaAim() << std::endl;
      }
    }
    else tangentSpace->setRelativeAccuracy(p.minimalAccuracy);// * std::min(1.,normOfLastCorrection_Undamped));

    boost::timer::cpu_timer timer;
    tangentSpace->setLipschitzConstant(L.omega);
    tangentSpace->basis(tangentialBasis, lagrangeLinearization, normalStep, nu, nullptr);
    tangentialStepComputationTime += (double) timer.elapsed().wall * 1e-9;

    if( verbose > 1 ) std::cout << "tangential step computation time: " << boost::timer::format(timer.elapsed()) << std::endl;

    return tangentialBasis;
  }

  double Optimization::updateNormalStepDampingFactor(double normNormal) const
  {
    double nu = 1;
    if(normNormal > p.getEps() && std::fabs(normNormal*C.omega) > p.getEps()) nu = std::min(1.0,/*2.0*/p.getThetaNormal()/(C.omega*normNormal));
    if(noDamping(nu)) nu = 1;
    return nu;
  }

  void Optimization::updateTangentialDampingFactor(double nu, double normNormal, double normTangential, RegularizedQuadraticFunction const& cubic, std::vector<double> &tau) const
  {
    if( tau.size()==1 )
    {
      tau[0] = 1.0;

      if( normTangential < p.getSqrtEps() ) return;

      double maxTauMax = 1;
      double taumax = maxTauMax;
      if( C.omega != 0.0  )
      {
        if( ( pow(p.getThetaAim()/C.omega,2)-pow(nu*normNormal,2) ) > 0 && normTangential > 0) taumax = sqrt(pow(p.getThetaAim()/C.omega,2)/*-pow(nu*normNormal,2)*/)/normTangential;//std::min(1.,normTangential);

        if( taumax > maxTauMax && tangentSpace->localConvergenceLikely() )
        {
          if( verbose > 1 ) std::cout << csPre << "reducing taumax: " << taumax << " to " << maxTauMax << std::endl;
          taumax = maxTauMax;
        }
      }
      if( verbose > 1 ) std::cout << csPre << "taumax: " << taumax << std::endl;

      if( taumax > 0.0 )
      {
        CubicModel1dForFmin cubicmodel(cubic);
        double toleranceFmin = p.getThirdSqrtEps()*std::min(taumax,1.);
        double taumin = 0;
        tau[0] = Fmin(taumin,taumax,cubicmodel,toleranceFmin);
      }

      if( noDamping(tau) ) tau[0]=1.0;
    }
    else
    {
      if( tau.size() > 1 )
      {
        std::cout << csPre << "Multidimensional search space not implemented" << std::endl;
        exit(-1);
      }
    }

  }

  std::unique_ptr<AbstractFunctionSpaceElement> Optimization::computeSimplifiedNormalStep(AbstractLinearization const& lin_xplus, AbstractFunctionSpaceElement* normalStepResidual)
  {
    std::unique_ptr<AbstractFunctionSpaceElement> simplifiedNormalStep(iterate->initZeroVector());
    if(!normalDirection) return simplifiedNormalStep;

    normalDirection->simplified(*simplifiedNormalStep,lin_xplus,nullptr);
    return simplifiedNormalStep;
  }

  void Optimization::updateConstraintD1LipschitzConstant(double normSimplifiedNormal, double normCAtCorr)
  {
    if( verbose > 1 ) std::cout << csPre << "normCAtCorr = " << normCAtCorr << ", normSimplifiedNormal = " << normSimplifiedNormal << std::endl;
//    if( normCAtCorr > p.getEps() )
    {
      C.theta = normSimplifiedNormal/normCAtCorr; // contraction rate
      bool lock = (C.theta < 0.25 && (normCAtCorr < p.getSqrtEps()*normOfIterate || normSimplifiedNormal < p.getEps()*normOfIterate) );
      C.update(2*C.theta/normCAtCorr, lock);
    }
  }

  double Optimization::updateLagrangianD2LipschitzConstant(AbstractLinearization const& lin_x0, AbstractFunctionSpaceElement const& secondOrderCorrected, double normLAtCorr, double quadraticModelAtCorr, RegularizedQuadraticFunction const& cubic, double nu, std::vector<double> const& tau,
                                                           AbstractFunctionSpaceElement const& normalStep, AbstractFunctionSpaceElement const& trialIterate, AbstractFunctionSpaceElement const& sNormalStep, AbstractFunctionSpaceElement const& correction, double Lxdn_res)
  {
    double eta = 1;
    if( tangentSpace == nullptr ) return eta;

    std::unique_ptr<AbstractLinearization> lin_xplus(functionalN->getLinearization(trialIterate));
    auto f_x0 = lin_x0.eval();
    // compute: f(x_)-q(x)dx or alternative if high round-off is expected
    double deltaf(0.0);
    auto zero = tau;
    for(double& d : zero) d = 0;

//    if(normalDirection)
//    {
//      std::unique_ptr<AbstractFunctionSpaceElement> dc_x0_dn ( iterate->initZeroVector() ), lxxdn( iterate->initZeroVector() );
//      lin_x0.d2axpy(1.0, *lxxdn, normalStep, 0, 2, 0, 2);
//      lin_x0.d2axpy(1.0, *dc_x0_dn, normalStep, 2, 3, 0, 2);
//      double p_dc_x0_dn = iterate->applyAsDualTo(*dc_x0_dn,"dual");
//      double lxxdndn = normalStep.applyAsDualTo(*lxxdn);
      std::unique_ptr<AbstractLinearization> lin_xbar=functionalN->getLinearization(secondOrderCorrected);
      deltaf=lin_xbar->eval()-f_x0;
      double deltaf_cor = deltaf - cubic.d0(zero);
      if(std::fabs(cubic.d0(tau) - cubic.d0(zero)) > p.getSqrtEps()*normOfIterate/* && std::fabs(deltaf_cor) > p.getSqrtEps()*/) eta = deltaf_cor/( cubic.d0(tau) - cubic.d0(zero) ) ;
      else eta = 1;
      if( verbose > 1 )
      {
        std::cout << csPre << "predicted decrease: " << (cubic.d0(tau) - cubic.d0(zero)) << std::endl;
        std::cout << csPre << "actual decrease: " << deltaf_cor << std::endl;
        std::cout << csPre << " eta = " << eta << std::endl;
      }
      L.setFirstOrder( normLAtCorr, C.theta, deltaf - quadraticModelAtCorr );

//    }
//    else { assert("not implemented"); }

    if(L.highRoundOffError(true,p.getEps()*f_x0))
    {
      std::unique_ptr<AbstractFunctionSpaceElement> tmp1=iterate->clone();
      // tmp1 = chart_x(0)-chart_x(dx+ds)
      *tmp1 -= secondOrderCorrected;

      lin_xbar->evald(*tmp1);
      double p0_c_xbar=tmp1->applyAsDualTo(*iterate,"dual");  // p0 is dual component of *iterate

      double p0_c_x0(0.0);
      if(!noDamping(nu))
      {
        lin_x0.evald(*tmp1);
        p0_c_x0=tmp1->applyAsDualTo(*iterate,"dual");     // p0 is dual component of *iterate
      }

      // Lx_xplus_ds = L_x(x_+,p_0) ds
      double Lx_xplus_ds(0.0);
      lin_xplus->evald(*tmp1);
      Lx_xplus_ds=tmp1->applyAsDualTo(sNormalStep,"primal"); // delta s is primal component of *snormalStep

      // secondorderestimate [ (L_x(x_+,p0)-L_x(x0,p0)-L_xx(x0,p0)dx)dx ] + [ L_x(x_+,p0)ds ]-[ p0 c(xbar)-(1-nu)p0 c(x0) ]
      // each term in [...] is third order.
      // the summands in the first (...) are first order (close to solution), the difference is second order -> first order round-off error effects
      // close to a solution where nu = 1, the very last term vanishes -> then no other round-off error effects
      // if nu is very small, and c(x_0) approx c(xbar), then round-off error is expected (try alternative?)

      // tmp1 = L_x(x0,p0)+L_xx(x0,p0)dx
      lin_x0.evald(*tmp1);
      lin_x0.ddxpy(*tmp1,correction);

      // tmp2=L_x(x_+,p0)-L_x(x0,p0)-L_xx(x0,p0)dx
      std::unique_ptr<AbstractFunctionSpaceElement> tmp2=iterate->clone();
      lin_xplus->evald(*tmp2);
      (*tmp2)-=(*tmp1);

      L.setSecondOrder(0.5*tmp2->applyAsDualTo(correction,"primal")+0.5*Lx_xplus_ds-(p0_c_xbar-(1-nu)*(p0_c_x0-Lxdn_res)));
    }

    L.update( std::fabs( eta - 1. ) < (1.-p.etaLock) );

    if(verbose > 1)
    {
      std::cout << csPre << "f(x_0): " << f_x0 << " f(x): "  << f_x0+deltaf << "  |  Decrease: " << -deltaf << std::endl;
      std::cout << csPre << "Quadratic model q(x):" << f_x0+quadraticModelAtCorr << std::endl;
      std::cout << csPre << "q(x)-f(x_0): " << quadraticModelAtCorr << std::endl;
      std::cout << csPre << "q(x)-f(x): " << quadraticModelAtCorr-deltaf << std::endl;
    }

    return eta;
  }

  AcceptanceTest Optimization::acceptanceTest(double eta, double nu, std::vector<double> const& tau, double normOfCorrection) const
  {
    if( nu > 0.1 && normOfCorrection < p.getEps() ) return AcceptanceTest::Passed;
    if( noDamping(nu) && noDamping(tau) && (normOfCorrection) < p.getSqrtEps() ) return AcceptanceTest::Passed;

    if( eta < p.etaMin )
    {
      if( verbose > 0 ) std::cout << csPre << "Rejecting due to small eta: " << eta << " < " << p.etaMin << std::endl;
      return AcceptanceTest::TangentialStepFailed;
    }

    if(C.theta > p.getThetaMax())
    {
      if(verbose > 0) std::cout << csPre << "Insufficient contraction:" << C.theta << " > " << p.getThetaMax() << std::endl;
      return AcceptanceTest::NormalStepFailed;
    }

    return AcceptanceTest::Passed;
  }

  bool Optimization::noDamping(double d) const
  {
    return std::fabs(d-1.) < dampingFactorTolerance;
  }

  bool Optimization::noDamping(std::vector<double> const& tau) const
  {
    for(double d : tau) if(!noDamping(d)) return false;
    return true;
  }

  void Optimization::terminationMessage(int flag)
  {
    switch(flag)
    {
      case 1  : std::cout << csPre << "Desired accuracy reached!" << std::endl; break;
      case -1 : std::cout << csPre << "Regularity test failed!" << std::endl;; break;
      case -2 : std::cout << csPre << "Maximum number of iterations reached!" << std::endl; break;
      default : Algorithm::terminationMessage(flag);
    }
  }

  void Optimization::printNormalStep(double normNormal, double nu) const
  {
    std::cout << csPre << "Normal step: " << normNormal << std::endl;
    std::cout << csPre << "Damping factor: " << nu << std::endl << std::endl;
  }

  void Optimization::printTangentialStep(double normTangential, double tau) const
  {
    std::cout << csPre << "Tangential step: " << normTangential << std::endl;
    std::cout << csPre << "Damping factor: " << tau << std::endl << std::endl;

  }

  std::unique_ptr<AbstractFunctionSpaceElement> Optimization::createCorrection(double nu, const AbstractFunctionSpaceElement &normalStep, const std::vector<double> &tau, const std::vector<std::shared_ptr<AbstractFunctionSpaceElement> > &tangentialBasis) const
  {
    std::unique_ptr<AbstractFunctionSpaceElement> correction(normalStep.clone());
    *correction *= 0.0;
    correction->axpy(nu,normalStep,"primal");
    for(size_t i=0; i<tangentialBasis.size();++i) correction->axpy(tau[i],*tangentialBasis[i],"primal");
    return correction;
  }

  void Optimization::addCorrection(AbstractFunctionSpaceElement &v, double nu, const AbstractFunctionSpaceElement &normalStep, const std::vector<double> &tau, const std::vector<std::shared_ptr<AbstractFunctionSpaceElement> > &tangentialBasis) const
  {
    v.axpy(nu,normalStep,"primal");
    for(size_t i=0; i<tangentialBasis.size(); ++i) v.axpy(tau[i],*tangentialBasis[i],"primal");
  }
}  // namespace Kaskade


#include <test.hpp>
#include <kernel.hpp>
#include <Eigen/Core>

namespace Test
{
    double testfunction()
    {
        std::vector<Eigen::Vector3d> v1(10, Eigen::Vector3d{ 1.0, 1.0, 1.0 });
        std::vector<Eigen::Vector3d> v2(10, Eigen::Vector3d{ -1.0, 1.0, 1.0 });

        double x = Kernel::dot(v1,v2);

        return x;
    }

    void testEigenValueSolver()
    {
        Eigen::Matrix3f a = Eigen::Matrix3f::Random();

        std::vector<Eigen::Matrix3f> m(10, a + a.transpose());

        Kernel::run_eigen_solver(m);
    }
}