//
// Created by christoph on 18.12.18.
//

#include <iostream>
#include <fstream>
#include <boost/algorithm/string/predicate.hpp>

#define GLM_ENABLE_EXPERIMENTAL
#include <glm/gtx/quaternion.hpp>
#include <glm/gtx/euler_angles.hpp>
#include <glm/gtx/norm.hpp>

#include <Utils/Events/Stream/Stream.hpp>
#include <Utils/File/Logfile.hpp>

#include "CameraPath.hpp"
#include "Math/Math.hpp"

//#define ROTATION_AND_ZOOMING_MODE
//#define ROTATION_MODE
//#define ZOOMING_MODE
#define VIDEO_MODE

ControlPoint::ControlPoint(float time, float tx, float ty, float tz, float yaw, float pitch)
{
    this->time = time;
    this->position = glm::vec3(tx, ty, tz);

    /*glm::vec3 cameraFront, cameraRight, cameraUp;
    glm::vec3 globalUp(0.0f, 1.0f, 0.0f);
    cameraFront.x = cos(yaw) * cos(pitch);
    cameraFront.y = sin(pitch);
    cameraFront.z = sin(yaw) * cos(pitch);

    cameraFront = glm::normalize(cameraFront);
    cameraRight = glm::normalize(glm::cross(cameraFront, globalUp));
    cameraUp    = glm::normalize(glm::cross(cameraRight, cameraFront));

    this->orientation = glm::toQuat(glm::lookAt(glm::vec3(0.0f), cameraFront, cameraUp));*/
////    this->orientation = glm::toQuat(glm::lookAt(glm::vec3(0.0f), cameraFront, cameraUp));*/
//    this->orientation = glm::quat(glm::vec3(pitch, yaw, 0));
////    this->orientation = glm::toQuat(glm::orientate3(glm::vec3(yaw, pitch, 0)));
//
//    float yaw2 = glm::yaw(this->orientation);
//    float pitch2 = glm::pitch(this->orientation);
//    float test = 0;

    this->orientation = glm::angleAxis(-pitch, glm::vec3(1, 0, 0)) * glm::angleAxis(yaw + sgl::PI / 2.0f, glm::vec3(0, 1, 0));
}

void CameraPath::fromCirclePath(sgl::AABB3 &sceneBoundingBox, const std::string &modelFilenamePure)
{
    const size_t NUM_CIRCLE_POINTS = 64;
    controlPoints.clear();

    glm::vec3 centerOffset(0.0f, 0.0f, 0.0f);
    if (boost::starts_with(modelFilenamePure, "Data/Hair/ponytail")) {
        centerOffset.y += 0.1f;
    }
    float startAngle = 0.0f;
    float standardZoom = 1.0f;
    float rotateFactor = 1.0f;
    float yaw = 0;

    bool isConvectionRolls = boost::starts_with(modelFilenamePure, "Data/ConvectionRolls/output");
    bool isRings = boost::starts_with(modelFilenamePure, "Data/Rings");
    bool isHair = boost::starts_with(modelFilenamePure, "Data/Hair");
    bool isUCLA = boost::starts_with(modelFilenamePure, "Data/UCLA");

    if (isConvectionRolls)
    {
        sceneBoundingBox = sgl::AABB3();
        sceneBoundingBox.combine(glm::vec3(-1, -0.5, -1));
        sceneBoundingBox.combine(glm::vec3( 1, 0.5, 1));
    }

#ifdef VIDEO_MODE
    float pulseFactor = 2.0f;

    // Zooming and pulsing
    if (boost::starts_with(modelFilenamePure, "Data/Trajectories/9213_streamlines")) {
        startAngle += 1.2f;
        standardZoom = 1.0f;
        pulseFactor = 1.4f;
//        pulseFactor = 2.0f; // for depth complexity
        centerOffset.x -= 0.1f;
    }
    if (boost::starts_with(modelFilenamePure, "Data/ConvectionRolls/turbulence8000")) {
        pulseFactor = 4.0f;
//        pulseFactor = 5.0f; // for depth complexity
        standardZoom = 1.8f;
    }
    if (isConvectionRolls) {
        pulseFactor = 1.5f;
        standardZoom = 0.8f;
        startAngle += 0.0f;
        centerOffset.y += 0.15f;
//        centerOffset.z -= 1.5;
//        centerOffset.x += 0.3;
        yaw = -0.50769;

        // for zooming: 0, 1.04763, 1.76833, 0.806672, -1.56452, -1.61031
        // for rotation: 1, 1.1138, 0.395518, 2.17143, -1.62076, -0.60769
    }
    if (isUCLA)
    {
        standardZoom = 1.5f;
        pulseFactor = 3.0f;
        yaw = -0.25;
        centerOffset.y += 0.115f;
        startAngle += 0.0f;
    }

//    float pulseFactor = 0.0f;
//
//    if (boost::starts_with(modelFilenamePure, "Data/Trajectories/9213_streamlines")) {
//        startAngle += 1.2f;
//        standardZoom = 1.15f;
//        centerOffset.x -= 0.1f;
//    }
//    else if (boost::starts_with(modelFilenamePure, "Data/ConvectionRolls/turbulence8000")) {
//        standardZoom = 1.5f;
//    }
//    else if (boost::starts_with(modelFilenamePure, "Data/ConvectionRolls/turbulence2000")) {
//        standardZoom = 2.0f;
//        startAngle += 1.58f;
//    }
//    else if (boost::starts_with(modelFilenamePure, "Data/WCB")) {
//        centerOffset.y -= 0.05f;
//        standardZoom = 1.3f;
//    }
//    else if (isConvectionRolls) {
//        standardZoom = 0.9f;
//        startAngle += 1.58f;
//        centerOffset.y += 0.5f;
////        centerOffset.z -= 1.5;
////        centerOffset.x += 0.3;
//        yaw = -0.62769;
//
//        // for zooming: 0, 1.04763, 1.76833, 0.806672, -1.56452, -1.61031
//        // for rotation: 1, 1.1138, 0.395518, 2.17143, -1.62076, -0.60769
//    }

    else if (isRings)
    {
        standardZoom = 1.7;
    }

    else if (isHair)
    {
        standardZoom = 0.8;
    }
#endif

#ifdef ROTATION_AND_ZOOMING_MODE
    float pulseFactor = 2.0f;

    // Zooming and pulsing
    if (boost::starts_with(modelFilenamePure, "Data/Trajectories/9213_streamlines")) {
        startAngle += 1.2f;
        standardZoom = 1.0f;
        pulseFactor = 1.1f;
//        pulseFactor = 2.0f; // for depth complexity
        centerOffset.x -= 0.1f;
    }
    if (boost::starts_with(modelFilenamePure, "Data/ConvectionRolls/turbulence8000")) {
        pulseFactor = 4.0f;
//        pulseFactor = 5.0f; // for depth complexity
        standardZoom = 1.8f;
    }
    if (isConvectionRolls) {
        pulseFactor = 1.5f;
        standardZoom = 0.8f;
        startAngle += 0.0f;
        centerOffset.y += 0.15f;
//        centerOffset.z -= 1.5;
//        centerOffset.x += 0.3;
        yaw = -0.50769;

        // for zooming: 0, 1.04763, 1.76833, 0.806672, -1.56452, -1.61031
        // for rotation: 1, 1.1138, 0.395518, 2.17143, -1.62076, -0.60769
    }
    if (isUCLA)
    {
        standardZoom = 1.5f;
        pulseFactor = 3.0f;
        yaw = -0.25;
        centerOffset.y += 0.115f;
        startAngle += 0.0f;
    }

//    if (boost::starts_with(modelFilenamePure, "Data/WCB")) {
//        centerOffset.y -= 0.05f;
//        pulseFactor = 2.0f;
//        standardZoom = 1.3f;
//    }
#endif

#ifdef ROTATION_MODE
    float pulseFactor = 0.0f;

    if (boost::starts_with(modelFilenamePure, "Data/Trajectories/9213_streamlines")) {
        startAngle += 1.2f;
        standardZoom = 1.1f;
        centerOffset.x -= 0.1f;
    }
    else if (boost::starts_with(modelFilenamePure, "Data/ConvectionRolls/turbulence8000")) {
        standardZoom = 1.3f;
    }
    else if (boost::starts_with(modelFilenamePure, "Data/ConvectionRolls/turbulence2000")) {
        standardZoom = 2.0f;
        startAngle += 1.58f;
    }
    else if (boost::starts_with(modelFilenamePure, "Data/WCB")) {
        centerOffset.y -= 0.05f;
        standardZoom = 1.3f;
    }
    else if (isConvectionRolls) {
        standardZoom = 0.8f;
        startAngle += 1.58f;
        centerOffset.y += 0.4f;
//        centerOffset.z -= 1.5;
//        centerOffset.x += 0.3;
        yaw = -0.60769;

        // for zooming: 0, 1.04763, 1.76833, 0.806672, -1.56452, -1.61031
        // for rotation: 1, 1.1138, 0.395518, 2.17143, -1.62076, -0.60769
    }
    else if (isUCLA)
    {
        standardZoom = 1.0f;
        yaw = -0.50;
        centerOffset.y += 0.205f;
    }

    else if (isRings)
    {
        standardZoom = 1.3;
    }

    else if (isHair)
    {
        standardZoom = 0.8;
    }

#endif

#ifdef ZOOMING_MODE
    float pulseFactor = 0.0f;
    rotateFactor = 0.0f;

    if (boost::starts_with(modelFilenamePure, "Data/Trajectories/9213_streamlines")) {
        startAngle += 1.2f;
        standardZoom = 0.6f;
        centerOffset.x -= 0.1f;
        pulseFactor = 2.5f;
    }
    if (boost::starts_with(modelFilenamePure, "Data/ConvectionRolls/turbulence8000")) {
        standardZoom = 1.2f;
        pulseFactor = 2.5f;
    }
    if (boost::starts_with(modelFilenamePure, "Data/ConvectionRolls/turbulence2000")) {
        standardZoom = 1.0f;
        startAngle += 1.58f;
        pulseFactor = 2.5f;
    }
    if (boost::starts_with(modelFilenamePure, "Data/WCB")) {
        centerOffset.y -= 0.05f;
        standardZoom = 0.8f;
        pulseFactor = 2.5f;
    }

    standardZoom += pulseFactor / 2.0f;

#endif

    std::ofstream testFile("Data/ControlPoints.txt");

    for (size_t i = 0; i <= NUM_CIRCLE_POINTS; i++) {
        float time = float(i) * 0.5f;//float(i)/NUM_CIRCLE_POINTS;
        float angleTrans = float(i)/NUM_CIRCLE_POINTS*sgl::TWO_PI*rotateFactor;

        float angle = angleTrans + startAngle;
//        float pulseRadius = (cos(2.0f*angle)-1.0f)/(8.0f/pulseFactor)+standardZoom;
#ifdef ZOOMING_MODE
        float pulseRadius = cos(float(i) / NUM_CIRCLE_POINTS * sgl::PI * 2) * pulseFactor / 2.0 + standardZoom;
#else
        float pulseRadius = (cos(2.0f*angle)-1.0f)/(8.0f/pulseFactor)+standardZoom;
#endif

//                (pulseFactor * float(i)/ NUM_CIRCLE_POINTS) / 2.0f - pulseFactor + standardZoom;
        glm::vec3 cameraPos = glm::vec3(cos(angle)*pulseRadius, 0.0f, sin(angle)*pulseRadius)
                * glm::length(sceneBoundingBox.getExtent()) + sceneBoundingBox.getCenter() + centerOffset;
        controlPoints.push_back(ControlPoint(time, cameraPos.x, cameraPos.y, cameraPos.z, sgl::PI + angle, yaw));

        testFile << "time: " << time << "(" << cameraPos.x << "," << cameraPos.y << "," << cameraPos.z << ")\n" << std::endl;
    }
    testFile.close();

    update(0.0f);
}

void CameraPath::fromControlPoints(const std::vector<ControlPoint> &controlPoints)
{
    this->controlPoints = controlPoints;
    update(0.0f);
}

const uint32_t CAMERA_PATH_FORMAT_VERSION = 1u;

bool CameraPath::fromBinaryFile(const std::string &filename)
{
    std::ifstream file(filename.c_str(), std::ifstream::binary);
    if (!file.is_open()) {
        sgl::Logfile::get()->writeError(std::string() + "Error in CameraPath::fromBinaryFile: File \""
                + filename + "\" not found.");
        return false;
    }

    file.seekg(0, file.end);
    size_t size = file.tellg();
    file.seekg(0);
    char *buffer = new char[size];
    file.read(buffer, size);
    file.close();

    sgl::BinaryReadStream stream(buffer, size);
    uint32_t version;
    stream.read(version);
    if (version != CAMERA_PATH_FORMAT_VERSION) {
        sgl::Logfile::get()->writeError(std::string() + "Error in CameraPath::fromBinaryFile: "
                + "Invalid version in file \"" + filename + "\".");
        return false;
    }

    controlPoints.clear();
    stream.readArray(controlPoints);
    update(0.0f);

    return true;
}

bool CameraPath::saveToBinaryFile(const std::string &filename)
{
    std::ofstream file(filename.c_str(), std::ofstream::binary);
    if (!file.is_open()) {
        sgl::Logfile::get()->writeError(std::string() + "Error in CameraPath::saveToBinaryFile: File \""
                + filename + "\" not found.");
        return false;
    }

    sgl::BinaryWriteStream stream;
    stream.write((uint32_t)CAMERA_PATH_FORMAT_VERSION);
    stream.writeArray(controlPoints);
    file.write((const char*)stream.getBuffer(), stream.getSize());
    file.close();

    return true;
}

void CameraPath::normalizeToTotalTime(float totalTime)
{
    float factor = totalTime / getEndTime();
    int n = (int)controlPoints.size();
    for (int i = 0; i < n; i++) {
        controlPoints.at(i).time *= factor;
    }
}

void CameraPath::update(float currentTime)
{
    time = fmod(currentTime, controlPoints.back().time);

    // Find either exact control point or two to interpolate between
    int n = (int)controlPoints.size();
    for (int i = 0; i < n; i++) {
        // Perfect match?
        ControlPoint &currentCtrlPoint = controlPoints.at(i);
        if (i == n-1 || glm::abs(time - currentCtrlPoint.time) < 0.0001f) {
            currentTransform = toTransform(currentCtrlPoint.position, currentCtrlPoint.orientation);
            break;
        }
        ControlPoint &nextCtrlPoint = controlPoints.at(i+1);
        if (time > currentCtrlPoint.time && time < nextCtrlPoint.time) {
            float percentage = (time - currentCtrlPoint.time) / (nextCtrlPoint.time - currentCtrlPoint.time);
            glm::vec3 interpPos = glm::mix(currentCtrlPoint.position, nextCtrlPoint.position, percentage);
            glm::quat interpQuat = glm::slerp(currentCtrlPoint.orientation, nextCtrlPoint.orientation, percentage);
            currentTransform = toTransform(interpPos, interpQuat);
            break;
        }
    }
}

glm::mat4x4 CameraPath::toTransform(const glm::vec3 &position, const glm::quat &orientation)
{
    return glm::toMat4(orientation) * sgl::matrixTranslation(-position);
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

template <typename PC>
void test(const PC& pc, bool expected)
{
    if (is_identity(pc) != expected) {
	std::cerr << "is_identity should be " << (expected ? "true" : "false") << '\n';
	throw "wrong result";
    }
}

int main()
{
    // For a more realistic example set sz to 1000 or larger
    const int size = 2, N = size * size;

    typedef mtl::compressed2D<double>  matrix_type;
    mtl::compressed2D<double>          A(N, N), dia(N, N);
    laplacian_setup(A, size, size);

    itl::pc::identity<matrix_type>     identity(A);
    itl::pc::ic_0<matrix_type>         ic_0(A);
    itl::pc::ilu_0<matrix_type>        ilu_0(A);
    itl::pc::diagonal<matrix_type>     diagonal(A);

    test(identity, true);
    test(ic_0, false);
    test(ilu_0, false);
    test(diagonal, false);

    return 0;
}


//==================================================================================================
/*
  Copyright 2017 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/algorithm/transform_reduce.hpp>
#include <boost/align/aligned_allocator.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/plus.hpp>
#include <numeric>
#include <vector>
#include <simd_test.hpp>

using namespace boost::simd;
using namespace boost::alignment;

STF_CASE_TPL( "Check simd::transform_reduce(f,l,i)", STF_NUMERIC_TYPES )
{
  namespace bs =  boost::simd;
  static const int N = pack<T>::static_size;

  std::vector<T,aligned_allocator<T,pack<T>::alignment>> d(2*N+1);
  std::iota(d.begin(), d.end(),T(1));

  auto s1  = bs::transform_reduce( d.data(), d.data()+2*N, bs::sqr,T(0), bs::plus);
  auto s1s = std::inner_product( d.data(), d.data()+2*N, d.data(), T(0));
  STF_EQUAL(s1, s1s);
  auto s2 =  bs::transform_reduce( d.data()+1, d.data()+2*N+1, bs::sqr, T(1), bs::plus);
  auto s2s = std::inner_product( d.data()+1, d.data()+2*N+1, d.data()+1, T(1));
  STF_EQUAL(s2, s2s);

  auto s3  = bs::transform_reduce( d.data()+1, d.data()+2*N+1, bs::sqr, T(0), bs::plus);
  auto s3s = std::inner_product( d.data()+1, d.data()+2*N+1, d.data()+1, T(0));
  STF_EQUAL(s3, s3s);

}


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

#include "get_valtonenornhag_arxiv_2020b.hpp"
#include <Eigen/Geometry>
#include <cmath>  // max
#include <vector>
#include "solver_valtonenornhag_arxiv_2020b_fHf.hpp"
#include "normalize2dpts.hpp"
#include "posedata.hpp"

namespace HomLib {
namespace ValtonenOrnhagArxiv2020B {
    inline Eigen::Vector4d construct_hvector(double w, const Eigen::VectorXd input);

    std::vector<HomLib::PoseData> get_fHf(
        const Eigen::MatrixXd &p1,
        const Eigen::MatrixXd &p2,
        const Eigen::Matrix3d &R1,
        const Eigen::Matrix3d &R2
    ) {
        // This is a 2-point method
        int nbr_pts = 2;

        // We expect inhomogenous input data, i.e. p1 and p2 are 2x3 matrices
        assert(p1.rows() == 2);
        assert(p2.rows() == 2);
        assert(p1.cols() == nbr_pts);
        assert(p2.cols() == nbr_pts);
        int nbr_coeffs = 26;

        // Save copies of the inverse rotation
        Eigen::Matrix3d R1T = R1.transpose();
        Eigen::Matrix3d R2T = R2.transpose();

        // Compute normalization matrix
        double scale1 = normalize2dpts(p1);
        double scale2 = normalize2dpts(p2);
        double scale = std::max(scale1, scale2);
        Eigen::Vector3d s;
        s << scale, scale, 1.0;
        Eigen::DiagonalMatrix<double, 3> S = s.asDiagonal();

        // Normalize data
        Eigen::MatrixXd x1(3, 2);
        Eigen::MatrixXd x2(3, 2);
        x1 = p1.colwise().homogeneous();
        x2 = p2.colwise().homogeneous();

        x1 = S * x1;
        x2 = S * x2;

        Eigen::Matrix2d x1t;
        x1t << x1.colwise().hnormalized();
        Eigen::Matrix2d x2t;
        x2t << x2.colwise().hnormalized();

        // Wrap input data to expected format
        Eigen::VectorXd input(nbr_coeffs);
        input << x1t.col(0),
                 x2t.col(0),
                 x1t.col(1),
                 x2t.col(1),
                 Eigen::Map<Eigen::VectorXd>(R1T.data(), 9),
                 Eigen::Map<Eigen::VectorXd>(R2T.data(), 9);

        // Extract w
        Eigen::VectorXcd w = HomLib::ValtonenOrnhagArxiv2020B::solver_fHf(input);

        // Pre-processing: Remove complex-valued solutions
        double thresh = 1e-5;
        Eigen::ArrayXd real_w = w.imag().array().abs();

        // This is a 2 pt solver
        std::vector<HomLib::PoseData> posedata;
        HomLib::PoseData tmp_pose;
        double w_tmp;
        Eigen::Vector4d hvec;
        Eigen::Matrix3d K, Ki, Htmp;

        double tol = 1e-13;

        for (int i = 0; i < real_w.size(); i++) {
            if (real_w(i) <= thresh) {
                // Compute algebraic error, and compare to other solutions.
                w_tmp = w(i).real();

                // Compute h vector
                hvec = construct_hvector(w_tmp, input);

                // Two spurious solutions were added, corresponding to last element
                // equal to zero.
                if (std::abs(hvec(3)) > tol) {
                    Htmp = Eigen::Matrix3d::Identity(3, 3);
                    Htmp.col(1) = hvec.hnormalized();

                    K = Eigen::Vector3d(w_tmp, w_tmp, 1).asDiagonal();
                    Ki = Eigen::Vector3d(1, 1, w_tmp).asDiagonal();
                    Htmp = S.inverse() * K * R2 * Htmp * R1T * Ki * S;

                    // Package output
                    tmp_pose.homography = Htmp;
                    tmp_pose.focal_length = w_tmp / scale;
                    posedata.push_back(tmp_pose);
                }
            }
        }

        return posedata;
    }

    inline Eigen::Vector4d construct_hvector(double w, const Eigen::VectorXd input) {
        Eigen::VectorXd d(27);
        d << w, input;

        double s1 = d[0]*d[25];
        double s2 = d[0]*d[16];
        double t1 = s1 + d[7]*d[19] + d[8]*d[22];
        double t2 = d[3]*d[19] + s1 + d[4]*d[22];
        double t3 = d[5]*d[10] + s2 + d[6]*d[13];
        double t4 = d[1]*d[10] + d[2]*d[13] + s2;
        double u1 = d[0]*d[24];
        double u2 = d[0]*d[15];
        double u3 = d[0]*d[26];

        Eigen::Matrix<double, 4, 4> M;
        M << 0, -t4*(d[3]*d[20] + u3 + d[4]*d[23]), t4*t2, (d[1]*d[11] + d[2]*d[14] + d[0]*d[17])*t2,  // NOLINT
             -t4*t2,  t4*(d[3]*d[18] + u1 + d[4]*d[21]), 0, -(d[1]*d[9] + d[2]*d[12] + u2)*t2,  // NOLINT
             0, -t3*(u3 + d[7]*d[20] + d[8]*d[23]), t3*t1,  (d[5]*d[11] + d[0]*d[17] + d[6]*d[14])*t1,  // NOLINT
             -t3*t1,  t3*(u1 + d[7]*d[18] + d[8]*d[21]), 0, -(d[5]*d[9] + u2 + d[6]*d[12])*t1;  // NOLINT

        // Perform SVD
        Eigen::JacobiSVD<Eigen::Matrix<double, 4, 4>> svd(M, Eigen::ComputeFullV);

        // Extract hvector
        Eigen::Vector4d h = svd.matrixV().col(3);

        return h;
    }
}  // namespace ValtonenOrnhagArxiv2020B
}  // namespace HomLib


//
//  expr/detail/expr.hpp
//  solver
//
//  Created by Alexandre Martin on 28/04/2016.
//  Copyright © 2016 scalexm. All rights reserved.
//

#ifndef DETAIL_EXPR_HPP
#define DETAIL_EXPR_HPP

#include <ostream>
#include <boost/variant.hpp>

namespace expr { namespace detail {
    /*
        tags for the template struct binary: each tag identifies a binary logical operator
    */
    struct and_tag {
        constexpr static const char * text = "/\\";
    };

    struct or_tag {
        constexpr static const char * text = "\\/";
    };

    struct xor_tag {
        constexpr static const char * text = "X";
    };

    struct impl_tag {
        constexpr static const char * text = "=>";
    };

    struct equiv_tag {
        constexpr static const char * text = "<=>";
    };

    struct none_ { };
    template<class Atom> struct not_;
    template<class Tag, class Atom> struct binary_;

    template<class Atom>
    using and_ = binary_<detail::and_tag, Atom>;

    template<class Atom>
    using or_ = binary_<detail::or_tag, Atom>;

    template<class Atom>
    using xor_ = binary_<detail::xor_tag, Atom>;

    template<class Atom>
    using impl_ = binary_<detail::impl_tag, Atom>;

    template<class Atom>
    using equiv_ = binary_<detail::equiv_tag, Atom>;

    template<class Atom>
    using expr_ = boost::variant<
        none_, // none in first position for default ctor
        Atom,
        boost::recursive_wrapper<not_<Atom>>,
        boost::recursive_wrapper<and_<Atom>>,
        boost::recursive_wrapper<or_<Atom>>,
        boost::recursive_wrapper<xor_<Atom>>,
        boost::recursive_wrapper<impl_<Atom>>,
        boost::recursive_wrapper<equiv_<Atom>>
    >;

    template<class Tag, class Atom>
    struct binary_ {
        expr_<Atom> op_left, op_right;
    };

    template<class Atom>
    struct not_ {
        expr_<Atom> op;
    };

    inline bool operator ==(const none_ &, const none_ &) {
        return true;
    }

    template<class Atom>
    inline bool operator ==(const not_<Atom> & a, const not_<Atom> & b) {
        return a.op == b.op;
    }

    template<class Tag, class Atom>
    inline bool operator ==(const binary_<Tag, Atom> & a, const binary_<Tag, Atom> & b) {
        return a.op_left == b.op_left && a.op_right == b.op_right;
    }

    inline std::ostream & operator <<(std::ostream & stream, const none_ &) {
        return stream << "[none]";
    }

    template<class Atom>
    inline std::ostream & operator <<(std::ostream & stream, const not_<Atom> & exp) {
        return stream << "~(" << exp.op << ")";
    }

    template<class Tag, class Atom>
    inline std::ostream & operator <<(std::ostream & stream, const binary_<Tag, Atom> & exp) {
        return stream << "(" << exp.op_left << " " << Tag::text << " " << exp.op_right << ")";
    }
} }

#endif


/*
 * This file is part of the statismo library.
 *
 * Author: Marcel Luethi (marcel.luethi@unibas.ch)
 *
 * Copyright (c) 2011 University of Basel
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution.
 *
 * Neither the name of the project's author nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
 * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#ifndef __PCAModelBuilder_TXX
#define __PCAModelBuilder_TXX

#include "PCAModelBuilder.h"

#include <iostream>

#include <Eigen/SVD>
#include <Eigen/Eigenvalues>

#include "CommonTypes.h"
#include "Exceptions.h"

namespace statismo {

template <typename T>
PCAModelBuilder<T>::PCAModelBuilder()
    : Superclass() {
}

template <typename T>
typename PCAModelBuilder<T>::StatisticalModelType*
PCAModelBuilder<T>::BuildNewModel(const DataItemListType& sampleDataList, double noiseVariance, bool computeScores, EigenValueMethod method) const {

    unsigned n = sampleDataList.size();
    if (n <= 0) {
        throw StatisticalModelException("Provided empty sample set. Cannot build the sample matrix");
    }

    unsigned p = sampleDataList.front()->GetSampleVector().rows();
    const Representer<T>* representer = sampleDataList.front()->GetRepresenter();

    // Compute the mean vector mu
    VectorType mu = VectorType::Zero(p);

    for (typename DataItemListType::const_iterator it = sampleDataList.begin();
            it != sampleDataList.end();  ++it) {
        assert((*it)->GetSampleVector().rows() == p); // all samples must have same number of rows
        assert((*it)->GetRepresenter() == representer); // all samples have the same representer
        mu += (*it)->GetSampleVector();
    }
    mu /= n;

    // Build the mean free sample matrix X0
    MatrixType X0(n, p);
    unsigned i = 0;
    for (typename DataItemListType::const_iterator it = sampleDataList.begin();
            it != sampleDataList.end(); ++it) {
        X0.row(i++) = (*it)->GetSampleVector() - mu;
    }

    // build the model
    StatisticalModelType* model = BuildNewModelInternal(representer, X0, mu, noiseVariance, method);

    // compute the scores if requested
    MatrixType scores;
    if (computeScores) {
        scores = this->ComputeScores(sampleDataList, model);
    }

    typename BuilderInfo::ParameterInfoList bi;
    bi.push_back(BuilderInfo::KeyValuePair("NoiseVariance ", Utils::toString(noiseVariance)));

    typename BuilderInfo::DataInfoList dataInfo;
    i = 0;
    for (typename DataItemListType::const_iterator it = sampleDataList.begin();
            it != sampleDataList.end();
            ++it, i++) {
        std::ostringstream os;
        os << "URI_" << i;
        dataInfo.push_back(BuilderInfo::KeyValuePair(os.str().c_str(),(*it)->GetDatasetURI()));
    }

    // finally add meta data to the model info
    BuilderInfo builderInfo("PCAModelBuilder", dataInfo, bi);

    ModelInfo::BuilderInfoList biList;
    biList.push_back(builderInfo);

    ModelInfo info(scores, biList);
    model->SetModelInfo(info);

    return model;
}

template <typename T>
typename PCAModelBuilder<T>::StatisticalModelType*
PCAModelBuilder<T>::BuildNewModelInternal(const Representer<T>* representer, const MatrixType& X0, const VectorType& mu,
        double noiseVariance, EigenValueMethod method) const {

    unsigned n = X0.rows();
    unsigned p = X0.cols();

    switch(method) {
    case JacobiSVD:

        typedef Eigen::JacobiSVD<MatrixType> SVDType;
        typedef Eigen::JacobiSVD<MatrixTypeDoublePrecision> SVDDoublePrecisionType;

        // We destinguish the case where we have more variables than samples and
        // the case where we have more samples than variable.
        // In the first case we compute the (smaller) inner product matrix instead of the full covariance matrix.
        // It is known that this has the same non-zero singular values as the covariance matrix.
        // Furthermore, it is possible to compute the corresponding eigenvectors of the covariance matrix from the
        // decomposition.

        if (n < p) {
            // we compute the eigenvectors of the covariance matrix by computing an SVD of the
            // n x n inner product matrix 1/(n-1) X0X0^T
            MatrixType Cov = X0 * X0.transpose() * 1.0/(n-1);
            SVDDoublePrecisionType SVD(Cov.cast<double>(), Eigen::ComputeThinV);
            VectorType singularValues = SVD.singularValues().cast<ScalarType>();
            MatrixType V = SVD.matrixV().cast<ScalarType>();

            unsigned numComponentsAboveTolerance = ((singularValues.array() - noiseVariance - Superclass::TOLERANCE) > 0).count();

            // there can be at most n-1 nonzero singular values in this case. Everything else must be due to numerical inaccuracies
            unsigned numComponentsToKeep = std::min(numComponentsAboveTolerance, n - 1);
            // compute the pseudo inverse of the square root of the singular values
            // which is then needed to recompute the PCA basis
            VectorType singSqrt = singularValues.array().sqrt();
            VectorType singSqrtInv = VectorType::Zero(singSqrt.rows());
            for (unsigned i = 0; i < numComponentsToKeep; i++) {
                assert(singSqrt(i) > Superclass::TOLERANCE);
                singSqrtInv(i) = 1.0 / singSqrt(i);
            }

            if (numComponentsToKeep == 0) {
                throw StatisticalModelException("All the eigenvalues are below the given tolerance. Model cannot be built.");
            }

            // we recover the eigenvectors U of the full covariance matrix from the eigenvectors V of the inner product matrix.
            // We use the fact that if we decompose X as X=UDV^T, then we get X^TX = UD^2U^T and XX^T = VD^2V^T (exploiting the orthogonormality
            // of the matrix U and V from the SVD). The additional factor sqrt(n-1) is to compensate for the 1/sqrt(n-1) in the formula
            // for the covariance matrix.

            MatrixType pcaBasis = X0.transpose() * V * singSqrtInv.asDiagonal();
            pcaBasis /= sqrt(n - 1.0);
            pcaBasis.conservativeResize(Eigen::NoChange, numComponentsToKeep);

            VectorType sampleVarianceVector = singularValues.topRows(numComponentsToKeep);
            VectorType pcaVariance = (sampleVarianceVector - VectorType::Ones(numComponentsToKeep) * noiseVariance);

            StatisticalModelType* model = StatisticalModelType::Create(representer, mu, pcaBasis, pcaVariance, noiseVariance);

            return model;
        } else {
            // we compute an SVD of the full p x p  covariance matrix 1/(n-1) X0^TX0 directly
            SVDType SVD(X0.transpose() * X0, Eigen::ComputeThinU);
            VectorType singularValues = SVD.singularValues();
            singularValues /= (n - 1.0);
            unsigned numComponentsToKeep = ((singularValues.array() - noiseVariance - Superclass::TOLERANCE) > 0).count();
            MatrixType pcaBasis = SVD.matrixU();

            pcaBasis.conservativeResize(Eigen::NoChange, numComponentsToKeep);

            if (numComponentsToKeep == 0) {
                throw StatisticalModelException("All the eigenvalues are below the given tolerance. Model cannot be built.");
            }

            VectorType sampleVarianceVector = singularValues.topRows(numComponentsToKeep);
            VectorType pcaVariance = (sampleVarianceVector - VectorType::Ones(numComponentsToKeep) * noiseVariance);
            StatisticalModelType* model = StatisticalModelType::Create(representer, mu, pcaBasis, pcaVariance, noiseVariance);
            return model;
        }
        break;

    case SelfAdjointEigenSolver: {
        // we compute the eigenvalues/eigenvectors of the full p x p  covariance matrix 1/(n-1) X0^TX0 directly

        typedef Eigen::SelfAdjointEigenSolver<MatrixType> SelfAdjointEigenSolver;
        SelfAdjointEigenSolver es;
        es.compute(X0.transpose() * X0);
        VectorType eigenValues = es.eigenvalues().reverse(); // SelfAdjointEigenSolver orders the eigenvalues in increasing order
        eigenValues /= (n -1.0);

        unsigned numComponentsToKeep = ((eigenValues.array() - noiseVariance - Superclass::TOLERANCE) > 0).count();
        MatrixType pcaBasis = es.eigenvectors().rowwise().reverse();
        pcaBasis.conservativeResize(Eigen::NoChange, numComponentsToKeep);

        if (numComponentsToKeep == 0) {
            throw StatisticalModelException("All the eigenvalues are below the given tolerance. Model cannot be built.");
        }

        VectorType sampleVarianceVector = eigenValues.topRows(numComponentsToKeep);
        VectorType pcaVariance = (sampleVarianceVector - VectorType::Ones(numComponentsToKeep) * noiseVariance);
        StatisticalModelType* model = StatisticalModelType::Create(representer, mu, pcaBasis, pcaVariance, noiseVariance);
        return model;
    }
    break;

    default:
        throw StatisticalModelException("Unrecognized decomposition/eigenvalue solver method.");
        return 0;
        break;
    }
}

} // namespace statismo

#endif


#include <boost/simd/include/functions/shuffle.hpp>
#include <boost/simd/sdk/simd/pack.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <iostream>

using boost::simd::pack;
using boost::simd::shuffle;

int main()
{
  pack<float,4> f(1,2,3,4), g(10,20,30,40);

  // Generates [0 1 10 0]
  std::cout << shuffle<-1,0,4,-1>(f,g) << "\n";
}


//
// Copyright 2005-2007 Adobe Systems Incorporated
// Copyright 2018 Mateusz Loskot <mateusz at loskot dot net>
//
// Distribtted 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/channel.hpp>
#include <boost/gil/channel_algorithm.hpp>
#include <boost/gil/typedefs.hpp>
#include <cstdint>
#include <limits>

#define BOOST_TEST_MODULE test_scoped_channel_value
#include <gil_test_common.hpp>

namespace gil = boost::gil;

BOOST_AUTO_TEST_CASE(scoped_channel_float32_t)
{
    auto const tolerance = btt::tolerance(std::numeric_limits<float>::epsilon());
    // min
    BOOST_TEST(gil::float_point_zero<float>::apply() == 0.0, tolerance);
    BOOST_TEST(gil::channel_traits<gil::float32_t>::min_value() == 0.0);
    // max
    BOOST_TEST(gil::float_point_one<float>::apply() == 1.0, tolerance);
    BOOST_TEST(gil::channel_traits<gil::float32_t>::max_value() == 1.0);
}

BOOST_AUTO_TEST_CASE(scoped_channel_float64_t)
{
    auto const tolerance = btt::tolerance(std::numeric_limits<double>::epsilon());
    // min
    BOOST_TEST(gil::float_point_zero<double>::apply() == 0.0, tolerance);
    BOOST_TEST(gil::channel_traits<gil::float64_t>::min_value() == 0.0, tolerance);
    // max
    BOOST_TEST(gil::float_point_one<double>::apply() == 1.0, tolerance);
    BOOST_TEST(gil::channel_traits<gil::float64_t>::max_value() == 1.0, tolerance);
}

BOOST_AUTO_TEST_CASE(scoped_channel_halfs)
{
    // Create a double channel with range [-0.5 .. 0.5]
    struct minus_half { static double apply() { return -0.5; } };
    struct plus_half { static double apply() { return 0.5; } };
    using halfs = gil::scoped_channel_value<double, minus_half, plus_half>;

    auto const tolerance = btt::tolerance(std::numeric_limits<double>::epsilon());
    BOOST_TEST(gil::channel_traits<halfs>::min_value() == minus_half::apply(), tolerance);
    BOOST_TEST(gil::channel_traits<halfs>::max_value() == plus_half::apply(), tolerance);
    // scoped channel maximum should map to the maximum
    BOOST_TEST(gil::channel_convert<std::uint16_t>(
        gil::channel_traits<halfs>::max_value()) == 65535, tolerance);
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/arithmetic/include/functions/fast_rec.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/two.hpp>
#include <boost/simd/include/constants/half.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/ten.hpp>
#include <boost/simd/include/constants/three.hpp>
#include <boost/simd/include/constants/third.hpp>
#include <boost/simd/include/constants/oneo_10.hpp>

NT2_TEST_CASE_TPL ( fast_rec_real,  BOOST_SIMD_REAL_TYPES)
{
  using boost::simd::fast_rec;
  using boost::simd::tag::fast_rec_;
  typedef typename boost::dispatch::meta::call<fast_rec_(T)>::type r_t;

  NT2_TEST_TYPE_IS(T,r_t);

  NT2_TEST_ULP_EQUAL(fast_rec(boost::simd::Two<T>()), boost::simd::Half<r_t>(), 16);
  NT2_TEST_ULP_EQUAL(fast_rec(boost::simd::Mone<T>()), boost::simd::Mone<r_t>(), 16);
  NT2_TEST_ULP_EQUAL(fast_rec(boost::simd::One<T>()), boost::simd::One<r_t>(), 16);
  NT2_TEST_ULP_EQUAL(fast_rec(boost::simd::Ten<T>()), boost::simd::Oneo_10<r_t>(), 16);
  NT2_TEST_ULP_EQUAL(fast_rec(boost::simd::Three<T>()), boost::simd::Third<r_t>(), 16);
}


/*  _______________________________________________________________________

    DAKOTA: Design Analysis Kit for Optimization and Terascale Applications
    Copyright 2014 Sandia Corporation.
    This software is distributed under the GNU Lesser General Public License.
    For more information, see the README file in the top Dakota directory.
    _______________________________________________________________________ */

//- Description:  This file contains code related to data utilities that should
//-               be compiled, rather than inlined in data_util.hpp.
//-
//- Owner:        Mike Eldred
//- Version: $Id: dakota_data_util.cpp 7024 2010-10-16 01:24:42Z mseldre $

#include "dakota_data_util.hpp"
#include <boost/math/special_functions/round.hpp>

namespace Dakota {

// ------------
// == operators
// ------------

bool nearby(const RealVector& rv1, const RealVector& rv2, Real rel_tol)
{
  // Check for equality in array lengths
  size_t len = rv1.length();
  if ( rv2.length() != len )
    return false;

  // Check each value (labels are ignored!)
  Real abs_tol = DBL_MIN; // ~ 2.2e-308
  for (size_t i=0; i<len; i++)
    // prevent division by 0
    if (std::abs(rv1[i]) < abs_tol) { //(rv1[i] == 0.)
      if (std::abs(rv2[i]) > abs_tol) //(rv2[i] != 0.)
	return false;
    }
    else if ( std::abs(1. - rv2[i]/rv1[i]) > rel_tol ) // DBL_EPSILON
      return false;

  return true;
}

bool operator==(const ShortArray& dsa1, const ShortArray& dsa2)
{
  // Check for equality in array lengths
  size_t len = dsa1.size();
  if ( dsa2.size() != len )
    return false;

  // Check each value
  for (size_t i=0; i<len; ++i)
    if ( dsa2[i] != dsa1[i] )
      return false;

  return true;
}

bool operator==(const StringArray& dsa1, const StringArray& dsa2)
{
  // Check for equality in array lengths
  size_t len = dsa1.size();
  if ( dsa2.size() != len )
    return false;

  // Check each string
  for (size_t i=0; i<len; ++i)
    if ( dsa2[i] != dsa1[i] )
      return false;

  return true;
}

// ---------------------------------
// miscellaneous numerical utilities
// ---------------------------------

Real rel_change_L2(const RealVector& curr_rv, const RealVector& prev_rv)
{
  size_t i, rv_len = prev_rv.length();
  Real norm = 0.;

  // check previous vector for zeros
  bool zero_prev = false, zero_curr = false;
  for (i=0; i<rv_len; ++i)
    if (std::abs(prev_rv[i]) < Pecos::SMALL_NUMBER)
      { zero_prev = true; break; }
  // check current vector for zeros
  if (zero_prev)
    for (i=0; i<rv_len; ++i)
      if (std::abs(curr_rv[i]) < Pecos::SMALL_NUMBER)
	{ zero_curr = true; break; }

  // Compute norm of relative change one of three ways
  if (!zero_prev) { // change relative to previous
    for (i=0; i<rv_len; ++i)
      norm += std::pow(curr_rv[i] / prev_rv[i] - 1., 2.);
    return std::sqrt(norm);
  }
  else if (!zero_curr) { // change relative to current
    for (i=0; i<rv_len; ++i)
      norm += std::pow(prev_rv[i] / curr_rv[i] - 1., 2.);
    return std::sqrt(norm);
  }
  else { // absolute change scaled by norm of previous
    Real scaling = 0.;
    for (i=0; i<rv_len; ++i) {
      norm    += std::pow(curr_rv[i] - prev_rv[i], 2.);
      scaling += std::pow(prev_rv[i], 2.);
    }
    return (scaling > Pecos::SMALL_NUMBER) ?
      std::sqrt(norm / scaling) : std::sqrt(norm);
  }
}

Real rel_change_L2(const RealVector& curr_rv1, const RealVector& prev_rv1,
		   const IntVector&  curr_iv,  const IntVector&  prev_iv,
		   const RealVector& curr_rv2, const RealVector& prev_rv2)
{
  size_t i, rv1_len = prev_rv1.length(), iv_len = prev_iv.length(),
    rv2_len = prev_rv2.length();
  Real norm = 0.;

  // check previous vectors for zeros
  bool zero_prev = false, zero_curr = false;
  for (i=0; i<rv1_len; ++i)
    if (std::abs(prev_rv1[i]) < Pecos::SMALL_NUMBER)
      { zero_prev = true; break; }
  if (!zero_prev)
    for (i=0; i<iv_len; ++i)
      if (std::abs(prev_iv[i]))
	{ zero_prev = true; break; }
  if (!zero_prev)
    for (i=0; i<rv2_len; ++i)
      if (std::abs(prev_rv2[i]) < Pecos::SMALL_NUMBER)
	{ zero_prev = true; break; }
  // check current vectors for zeros
  if (zero_prev) {
    for (i=0; i<rv1_len; ++i)
      if (std::abs(curr_rv1[i]) < Pecos::SMALL_NUMBER)
	{ zero_curr = true; break; }
    if (!zero_prev)
      for (i=0; i<iv_len; ++i)
	if (std::abs(curr_iv[i]))
	  { zero_curr = true; break; }
    if (!zero_prev)
      for (i=0; i<rv2_len; ++i)
	if (std::abs(curr_rv2[i]) < Pecos::SMALL_NUMBER)
	  { zero_curr = true; break; }
  }

  // Compute norm of relative change one of three ways
  if (!zero_prev) { // change relative to previous
    for (i=0; i<rv1_len; ++i)
      norm += std::pow(curr_rv1[i] / prev_rv1[i] - 1., 2.);
    for (i=0; i<iv_len; ++i)
      norm += std::pow(curr_iv[i]  / prev_iv[i]  - 1., 2.);
    for (i=0; i<rv2_len; ++i)
      norm += std::pow(curr_rv2[i] / prev_rv2[i] - 1., 2.);
    return std::sqrt(norm);
  }
  else if (!zero_curr) { // change relative to current
    for (i=0; i<rv1_len; ++i)
      norm += std::pow(prev_rv1[i] / curr_rv1[i] - 1., 2.);
    for (i=0; i<iv_len; ++i)
      norm += std::pow(prev_iv[i]  / curr_iv[i]  - 1., 2.);
    for (i=0; i<rv2_len; ++i)
      norm += std::pow(prev_rv2[i] / curr_rv2[i] - 1., 2.);
    return std::sqrt(norm);
  }
  else { // absolute change scaled by norm of previous
    Real scaling = 0.;
    for (i=0; i<rv1_len; ++i) {
      norm    += std::pow(curr_rv1[i] - prev_rv1[i], 2.);
      scaling += prev_rv1[i] * prev_rv1[i];
    }
    for (i=0; i<iv_len; ++i) {
      norm    += std::pow(curr_iv[i] - prev_iv[i], 2.);
      scaling += prev_iv[i] * prev_iv[i];
    }
    for (i=0; i<rv2_len; ++i) {
      norm    += std::pow(curr_rv2[i] - prev_rv2[i], 2.);
      scaling += prev_rv2[i] * prev_rv2[i];
    }
    return (scaling > Pecos::SMALL_NUMBER) ?
      std::sqrt(norm / scaling) : std::sqrt(norm);
  }
}

void remove_column(RealMatrix& matrix, int index)
{
  int num_cols = matrix.numCols();
  RealMatrix matrix_new(matrix.numRows(), num_cols-1);
  for (int i = 0; i<num_cols; ++i){
      const RealVector& col_vec = Teuchos::getCol(Teuchos::View, matrix, i);
    if (i < index){
      Teuchos::setCol(col_vec, i, matrix_new);
    }
    if (i > index){
      Teuchos::setCol(col_vec, i-1, matrix_new);
    }
  }
  matrix.reshape(matrix.numRows(), num_cols-1);
  matrix = matrix_new;
}

void iround(const RealVector& input_vec, IntVector& rounded_vec)
{
  int len = input_vec.length();
  if (rounded_vec.length() != len)
    rounded_vec.resize(len);
  for (int i=0; i<len; ++i)
    rounded_vec[i] = boost::math::iround(input_vec[i]);
}

} // namespace Dakota


#include <fstream>
#include <iostream>
#include <boost/thread/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/bind.hpp>

extern "C"
{
	#include "mtrand.h"
	#include "cvodesim.h"
	#include "ssa.h"
	#include "testing.h"
}

#include "ParameterMC.h"

static int NUM_THREADS = 4;
static boost::mutex mutex;

static double ** SUM_XiXj = 0;
static double * SUM_Xi = 0;

void setNumThreads(int n)
{
	if (n > 0)
		NUM_THREADS = n;
}

static void sampleParameterSpaceSingleRun(int numSamples, int numParameters, RandomParameterFunc getRandParams, TargetFunc isGood, int * count)
{
	int i=0,j=0,k=0,n=0,accept=0;
	double * params = (double*)malloc(numParameters * sizeof(double));

	//run n times
	for (k=0; k < numSamples; ++k)
	{
		//get a random set of parameter
		getRandParams(params);

		//check if it is acceptable (the time consuming step, usually)
		accept = isGood(params);

		if (accept)
		{
			//if acceptable, store it
			mutex.lock();
			++(*count);
			for (i=0; i < numParameters; ++i)
			{
				//std::cout << params[i] << "\t";
				SUM_Xi[i] += params[i];
				for (j=i; j < numParameters; ++j)
					SUM_XiXj[i][j] += params[i] * params[j];
			}
			//std::cout << std::endl;
			mutex.unlock();
		}
	}

	delete params;
}

void sampleParameterSpace(int numSamples, int numParameters, RandomParameterFunc getRandParams, TargetFunc isGood, double * Mu, double ** Sigma)
{
	int i = 0, j = 0, n = 0, n_last = 0, num_threads = 0, count = 0;
	boost::thread ** threads = 0;
	boost::thread * thread = 0;

	/**** global arrays for storing covariance and means ****/
	//SUM_XiXj = new double*[numParameters];
	//SUM_Xi = new double[numParameters];

	SUM_XiXj = Sigma;
	SUM_Xi = Mu;

	for (i=0; i < numParameters; ++i)
	{
		//SUM_XiXj[i] = new double[numParameters];
		SUM_Xi[i] = 0.0;

		for (j=0; j < numParameters; ++j)
			SUM_XiXj[i][j] = 0.0;
	}

	/**** divide the task amongst n threads ****/

	if (numSamples < NUM_THREADS)
		numSamples = NUM_THREADS;

	n = (int)(numSamples/NUM_THREADS); //each thread is allocated n runs
	num_threads = NUM_THREADS;

	n_last = numSamples - (n * NUM_THREADS);  //last thread is given n_last runs
	if (n_last > 0)
		++num_threads;

	threads = new boost::thread*[num_threads];

	/**** make Boost threads ****/

	for (i=0; i < num_threads; ++i)
	{
		if ((n_last > 0) && (i == (num_threads-1)))
			n = n_last;

		thread = new boost::thread(
			boost::bind(
			&sampleParameterSpaceSingleRun,
			n,
			numParameters,
			getRandParams,
			isGood,
			&count));

		threads[i] = thread;
	}

	for (i=0; i < num_threads; ++i)
		threads[i]->join();

	for (i=0; i < num_threads; ++i)
		delete threads[i];

	delete threads;

	--count; //unbiased mean and cov

	for (i = 0; i < numParameters; ++i)
		SUM_Xi[i] /= (double)count;

	for (i = 0; i < numParameters; ++i)
	{
		for (j = i; j < numParameters; ++j)
		{
			SUM_XiXj[i][j] = SUM_XiXj[i][j]/(double)count - SUM_Xi[i]*SUM_Xi[j];
			SUM_XiXj[j][i] = SUM_XiXj[i][j];
		}
	}
}

//testing

int isgood(double * p)
{
	int numPeaks = 0;
	int i,k,sz;
	double dt,dx,mu;
	double * y = 0;

	assignParameters(p);
	TCinitialize();

	//y = ODEsim2(TCvars, TCreactions, TCstoic, &TCpropensity, TCinit, 0, 100, 0.1, 0,0,0,0);
	//sz = 1000;
	y = SSA(TCvars, TCreactions, TCstoic, &TCpropensity, TCinit, 0, 100, 100000, &sz, 0,0,0,0) ;
	if (y)
	{
		mu = 0.0;
		for (i = sz/2; i < sz; ++i)
			mu += getValue(y,TCvars+1,i,8);

		mu = 2.0*mu/sz;
		dt = sz/100;

		for (i=1,k=0; i < sz; ++i)
		{
			dx = mu - getValue(y,TCvars+1,i,8);
			if ((dx*dx) < 1)
			{
				if (k==0 || (i > k+dt))
					++numPeaks;
				k = i;
			}
		}
		free(y);
	}

	return (int)( sz > 100 && numPeaks > 10 );
}

void randomParams(double * p)
{
	int i;
	for (i=0; i < TCparams; ++i)
		p[i] = mtrand() * pow(2.0,5 * mtrand());
	p[6] = 4.0*mtrand();
	p[8] = 4.0*mtrand();
	p[16] = 4.0*mtrand();
	p[18] = 4.0*mtrand();
}

int main(int argc, char* argv[])
{
	double * Mu = new double[TCparams];
	double ** Sigma = new double*[TCparams];

	for (int i=0; i < TCparams; ++i)
		Sigma[i] = new double[TCparams];

	FILE * fout = fopen( "params.tab", "w" );

	sampleParameterSpace(1000000, TCparams ,&randomParams, &isgood, Mu, Sigma);

	fprintf(fout, "\n\nMu\n\n");

	for (int i=0; i < TCparams; ++i)
	{
		fprintf(fout, "%lf\t",Mu[i]);
	}

	fprintf(fout, "\n\nSigma\n\n");

	for (int i=0; i < TCparams; ++i)
	{
		for (int j=0; j < TCparams; ++j)
			fprintf(fout, "%lf\t",Sigma[i][j]);
		fprintf(fout, "\n");
	}

	fclose(fout);

	for (int i=0; i < TCparams; ++i)
		delete Sigma[i];

	delete Sigma;
	delete Mu;

	return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SCALAR_ILOGB_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SCALAR_ILOGB_HPP_INCLUDED
#include <boost/simd/toolbox/ieee/functions/ilogb.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/include/functions/scalar/is_gtz.hpp>
#include <boost/simd/include/functions/scalar/exponent.hpp>
#include <boost/simd/sdk/math.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::ilogb_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, signed>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename boost::dispatch::meta::as_floating<A0>::type type;
      return boost::simd::ilogb(type(a0));
    }
  };

#ifdef BOOST_SIMD_HAS_ILOGB
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::ilogb_, tag::cpu_
                            , (A0)
                            , (scalar_< double_<A0> >)
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, signed>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return is_gtz(a0)? ::ilogb(a0) : Zero<result_type>();
    }
  };
#endif

#ifdef BOOST_SIMD_HAS_ILOGBF
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::ilogb_, tag::cpu_
                            , (A0)
                            , (scalar_< single_<A0> >)
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, signed>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return is_gtz(a0)? ::ilogbf(a0) : Zero<result_type>();
    }
  };
#endif

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::ilogb_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {

    typedef typename dispatch::meta::as_integer<A0, signed>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return is_gtz(a0)? boost::simd::exponent(a0) : Zero<result_type>();
    }
  };
} } }

#endif


#pragma once

#include <boost/python/list.hpp>
#include <vector>

namespace optimization
{
enum class function
{
    bent_cigar,
    rosenbrock,
    rastrigin,
    zakharov
};

double bent_cigar(const std::vector<double> &x);
double bent_cigar_pywrapper(const boost::python::list &x);

double rosenbrock(const std::vector<double> &x);
double rosenbrock_pywrapper(const boost::python::list &x);

double rastrigin(const std::vector<double> &x);
double rastrigin_pywrapper(const boost::python::list &x);

double zakharov(const std::vector<double> &x);
double zakharov_pywrapper(const boost::python::list &x);

double objective_function(function f, const std::vector<double> &x);
} // optimization


/* Author: Toby D. Young, Polish Academy of Sciences,             */
/*         Wolfgang Bangerth, Texas A&M University                */
/*    $Id: step-36.cc 28237 2013-02-05 16:53:54Z heister $*/
/*                                                                */
/*    Copyright (C) 2009, 2011-2012 by the deal.II authors */
/*                                                                */
/*    This file is subject to QPL and may not be  distributed     */
/*    without copyright and license information. Please refer     */
/*    to the file deal.II/doc/license.html for the  text  and     */
/*    further information on this license.                        */

// @sect3{Include files}

// As mentioned in the introduction, this program is essentially only a
// slightly revised version of step-4. As a consequence, most of the following
// include files are as used there, or at least as used already in previous
// tutorial programs:
#include <deal.II/base/logstream.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/function_parser.h>
#include <deal.II/base/parameter_handler.h>
#include <deal.II/base/utilities.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/lac/full_matrix.h>

// PETSc appears here because SLEPc depends on this library:
#include <deal.II/lac/petsc_sparse_matrix.h>
#include <deal.II/lac/petsc_vector.h>

// And then we need to actually import the interfaces for solvers that SLEPc
// provides:
#include <deal.II/lac/slepc_solver.h>

// We also need some standard C++:
#include <fstream>
#include <iostream>

// Finally, as in previous programs, we import all the deal.II class and
// function names into the namespace into which everything in this program
// will go:
namespace Step36
{
  using namespace dealii;

  // @sect3{The EigenvalueProblem class template}

  // Following is the class declaration for the main class template. It looks
  // pretty much exactly like what has already been shown in step-4:
  template <int dim>
  class EigenvalueProblem
  {
  public:
    EigenvalueProblem (const std::string &prm_file);
    void run ();

  private:
    void make_grid_and_dofs ();
    void assemble_system ();
    unsigned int solve ();
    void output_results () const;

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

    // With these exceptions: For our eigenvalue problem, we need both a
    // stiffness matrix for the left hand side as well as a mass matrix for
    // the right hand side. We also need not just one solution function, but a
    // whole set of these for the eigenfunctions we want to compute, along
    // with the corresponding eigenvalues:
    PETScWrappers::SparseMatrix        stiffness_matrix, mass_matrix;
    std::vector<PETScWrappers::Vector> eigenfunctions;
    std::vector<double>                eigenvalues;

    // And then we need an object that will store several run-time parameters
    // that we will specify in an input file:
    ParameterHandler parameters;

    // Finally, we will have an object that contains "constraints" on our
    // degrees of freedom. This could include hanging node constraints if we
    // had adaptively refined meshes (which we don't have in the current
    // program). Here, we will store the constraints for boundary nodes
    // $U_i=0$.
    ConstraintMatrix constraints;
  };

  // @sect3{Implementation of the EigenvalueProblem class}

  // @sect4{EigenvalueProblem::EigenvalueProblem}

  // First up, the constructor. The main new part is handling the run-time
  // input parameters. We need to declare their existence first, and then read
  // their values from the input file whose name is specified as an argument
  // to this function:
  template <int dim>
  EigenvalueProblem<dim>::EigenvalueProblem (const std::string &prm_file)
    :
    fe (1),
    dof_handler (triangulation)
  {
//TODO investigate why the minimum number of refinement steps required to obtain the correct eigenvalue degeneracies is 6
    parameters.declare_entry ("Global mesh refinement steps", "5",
                              Patterns::Integer (0, 20),
                              "The number of times the 1-cell coarse mesh should "
                              "be refined globally for our computations.");
    parameters.declare_entry ("Number of eigenvalues/eigenfunctions", "5",
                              Patterns::Integer (0, 100),
                              "The number of eigenvalues/eigenfunctions "
                              "to be computed.");
    parameters.declare_entry ("Potential", "0",
                              Patterns::Anything(),
                              "A functional description of the potential.");

    parameters.read_input (prm_file);
  }

  // @sect4{EigenvalueProblem::make_grid_and_dofs}

  // The next function creates a mesh on the domain $[-1,1]^d$, refines it as
  // many times as the input file calls for, and then attaches a DoFHandler to
  // it and initializes the matrices and vectors to their correct sizes. We
  // also build the constraints that correspond to the boundary values
  // $u|_{\partial\Omega}=0$.
  //
  // For the matrices, we use the PETSc wrappers. These have the ability to
  // allocate memory as necessary as non-zero entries are added. This seems
  // inefficient: we could as well first compute the sparsity pattern,
  // initialize the matrices with it, and as we then insert entries we can be
  // sure that we do not need to re-allocate memory and free the one used
  // previously. One way to do that would be to use code like this:
  // @code
  //   CompressedSimpleSparsityPattern
  //      csp (dof_handler.n_dofs(),
  //           dof_handler.n_dofs());
  //   DoFTools::make_sparsity_pattern (dof_handler, csp);
  //   csp.compress ();
  //   stiffness_matrix.reinit (csp);
  //   mass_matrix.reinit (csp);
  // @endcode
  // instead of the two reinit() calls for the
  // stiffness and mass matrices below.
  //
  // This doesn't quite work, unfortunately. The code above may lead to a few
  // entries in the non-zero pattern to which we only ever write zero entries;
  // most notably, this holds true for off-diagonal entries for those rows and
  // columns that belong to boundary nodes. This shouldn't be a problem, but
  // for whatever reason, PETSc's ILU preconditioner, which we use to solve
  // linear systems in the eigenvalue solver, doesn't like these extra entries
  // and aborts with an error message.
  //
  // In the absence of any obvious way to avoid this, we simply settle for the
  // second best option, which is have PETSc allocate memory as
  // necessary. That said, since this is not a time critical part, this whole
  // affair is of no further importance.
  template <int dim>
  void EigenvalueProblem<dim>::make_grid_and_dofs ()
  {
    GridGenerator::hyper_cube (triangulation, -1, 1);
    triangulation.refine_global (parameters.get_integer ("Global mesh refinement steps"));
    dof_handler.distribute_dofs (fe);

    DoFTools::make_zero_boundary_constraints (dof_handler, constraints);
    constraints.close ();

    stiffness_matrix.reinit (dof_handler.n_dofs(),
                             dof_handler.n_dofs(),
                             dof_handler.max_couplings_between_dofs());
    mass_matrix.reinit (dof_handler.n_dofs(),
                        dof_handler.n_dofs(),
                        dof_handler.max_couplings_between_dofs());

    // The next step is to take care of the eigenspectrum. In this case, the
    // outputs are eigenvalues and eigenfunctions, so we set the size of the
    // list of eigenfunctions and eigenvalues to be as large as we asked for
    // in the input file:
    eigenfunctions
    .resize (parameters.get_integer ("Number of eigenvalues/eigenfunctions"));
    for (unsigned int i=0; i<eigenfunctions.size (); ++i)
      eigenfunctions[i].reinit (dof_handler.n_dofs ());

    eigenvalues.resize (eigenfunctions.size ());
  }

  // @sect4{EigenvalueProblem::assemble_system}

  // Here, we assemble the global stiffness and mass matrices from local
  // contributions $A^K_{ij} = \int_K \nabla\varphi_i(\mathbf x) \cdot
  // \nabla\varphi_j(\mathbf x) + V(\mathbf x)\varphi_i(\mathbf
  // x)\varphi_j(\mathbf x)$ and $M^K_{ij} = \int_K \varphi_i(\mathbf
  // x)\varphi_j(\mathbf x)$ respectively. This function should be immediately
  // familiar if you've seen previous tutorial programs. The only thing new
  // would be setting up an object that described the potential $V(\mathbf x)$
  // using the expression that we got from the input file. We then need to
  // evaluate this object at the quadrature points on each cell. If you've
  // seen how to evaluate function objects (see, for example the coefficient
  // in step-5), the code here will also look rather familiar.
  template <int dim>
  void EigenvalueProblem<dim>::assemble_system ()
  {
    QGauss<dim>   quadrature_formula(2);

    FEValues<dim> fe_values (fe, quadrature_formula,
                             update_values | update_gradients |
                             update_quadrature_points | update_JxW_values);

    const unsigned int dofs_per_cell = fe.dofs_per_cell;
    const unsigned int n_q_points    = quadrature_formula.size();

    FullMatrix<double> cell_stiffness_matrix (dofs_per_cell, dofs_per_cell);
    FullMatrix<double> cell_mass_matrix (dofs_per_cell, dofs_per_cell);

    std::vector<unsigned int> local_dof_indices (dofs_per_cell);

    FunctionParser<dim> potential;
    potential.initialize (FunctionParser<dim>::default_variable_names (),
                          parameters.get ("Potential"),
                          typename FunctionParser<dim>::ConstMap());

    std::vector<double> potential_values (n_q_points);

    typename DoFHandler<dim>::active_cell_iterator
    cell = dof_handler.begin_active (),
    endc = dof_handler.end ();
    for (; cell!=endc; ++cell)
      {
        fe_values.reinit (cell);
        cell_stiffness_matrix = 0;
        cell_mass_matrix      = 0;

        potential.value_list (fe_values.get_quadrature_points(),
                              potential_values);

        for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
          for (unsigned int i=0; i<dofs_per_cell; ++i)
            for (unsigned int j=0; j<dofs_per_cell; ++j)
              {
                cell_stiffness_matrix (i, j)
                += (fe_values.shape_grad (i, q_point) *
                    fe_values.shape_grad (j, q_point)
                    +
                    potential_values[q_point] *
                    fe_values.shape_value (i, q_point) *
                    fe_values.shape_value (j, q_point)
                   ) * fe_values.JxW (q_point);

                cell_mass_matrix (i, j)
                += (fe_values.shape_value (i, q_point) *
                    fe_values.shape_value (j, q_point)
                   ) * fe_values.JxW (q_point);
              }

        // Now that we have the local matrix contributions, we transfer them
        // into the global objects and take care of zero boundary constraints:
        cell->get_dof_indices (local_dof_indices);

        constraints
        .distribute_local_to_global (cell_stiffness_matrix,
                                     local_dof_indices,
                                     stiffness_matrix);
        constraints
        .distribute_local_to_global (cell_mass_matrix,
                                     local_dof_indices,
                                     mass_matrix);
      }

    // At the end of the function, we tell PETSc that the matrices have now
    // been fully assembled and that the sparse matrix representation can now
    // be compressed as no more entries will be added:
    stiffness_matrix.compress ();
    mass_matrix.compress ();
  }

  // @sect4{EigenvalueProblem::solve}

  // This is the key new functionality of the program. Now that the system is
  // set up, here is a good time to actually solve the problem: As with other
  // examples this is done using a "solve" routine. Essentially, it works as
  // in other programs: you set up a SolverControl object that describes the
  // accuracy to which we want to solve the linear systems, and then we select
  // the kind of solver we want. Here we choose the Krylov-Schur solver of
  // SLEPc, a pretty fast and robust choice for this kind of problem:
  template <int dim>
  unsigned int EigenvalueProblem<dim>::solve ()
  {

    // We start here, as we normally do, by assigning convergence control we
    // want:
    SolverControl solver_control (dof_handler.n_dofs(), 1e-9);
    SLEPcWrappers::SolverKrylovSchur eigensolver (solver_control);

    // Before we actually solve for the eigenfunctions and -values, we have to
    // also select which set of eigenvalues to solve for. Lets select those
    // eigenvalues and corresponding eigenfunctions with the smallest real
    // part (in fact, the problem we solve here is symmetric and so the
    // eigenvalues are purely real). After that, we can actually let SLEPc do
    // its work:
    eigensolver.set_which_eigenpairs (EPS_SMALLEST_REAL);

    eigensolver.solve (stiffness_matrix, mass_matrix,
                       eigenvalues, eigenfunctions,
                       eigenfunctions.size());

    // The output of the call above is a set of vectors and values. In
    // eigenvalue problems, the eigenfunctions are only determined up to a
    // constant that can be fixed pretty arbitrarily. Knowing nothing about
    // the origin of the eigenvalue problem, SLEPc has no other choice than to
    // normalize the eigenvectors to one in the $l_2$ (vector)
    // norm. Unfortunately this norm has little to do with any norm we may be
    // interested from a eigenfunction perspective: the $L_2(\Omega)$ norm, or
    // maybe the $L_\infty(\Omega)$ norm.
    //
    // Let us choose the latter and rescale eigenfunctions so that they have
    // $\|\phi_i(\mathbf x)\|_{L^\infty(\Omega)}=1$ instead of
    // $\|\Phi\|_{l_2}=1$ (where $\phi_i$ is the $i$th eigenfunction
    // and $\Phi_i$ the corresponding vector of nodal values). For the $Q_1$
    // elements chosen here, we know that the maximum of the function
    // $\phi_i(\mathbf x)$ is attained at one of the nodes, so $\max_{\mathbf
    // x}\phi_i(\mathbf x)=\max_j (\Phi_i)_j$, making the normalization in the
    // $L_\infty$ norm trivial. Note that this doesn't work as easily if we
    // had chosen $Q_k$ elements with $k>1$: there, the maximum of a function
    // does not necessarily have to be attained at a node, and so
    // $\max_{\mathbf x}\phi_i(\mathbf x)\ge\max_j (\Phi_i)_j$ (although the
    // equality is usually nearly true).
    for (unsigned int i=0; i<eigenfunctions.size(); ++i)
      eigenfunctions[i] /= eigenfunctions[i].linfty_norm ();

    // Finally return the number of iterations it took to converge:
    return solver_control.last_step ();
  }

  // @sect4{EigenvalueProblem::output_results}

  // This is the last significant function of this program. It uses the
  // DataOut class to generate graphical output from the eigenfunctions for
  // later visualization. It works as in many of the other tutorial programs.
  //
  // The whole collection of functions is then output as a single VTK file.
  template <int dim>
  void EigenvalueProblem<dim>::output_results () const
  {
    DataOut<dim> data_out;

    data_out.attach_dof_handler (dof_handler);

    for (unsigned int i=0; i<eigenfunctions.size(); ++i)
      data_out.add_data_vector (eigenfunctions[i],
                                std::string("eigenfunction_") +
                                Utilities::int_to_string(i));

    // The only thing worth discussing may be that because the potential is
    // specified as a function expression in the input file, it would be nice
    // to also have it as a graphical representation along with the
    // eigenfunctions. The process to achieve this is relatively
    // straightforward: we build an object that represents $V(\mathbf x)$ and
    // then we interpolate this continuous function onto the finite element
    // space. The result we also attach to the DataOut object for
    // visualization.
    Vector<double> projected_potential (dof_handler.n_dofs());
    {
      FunctionParser<dim> potential;
      potential.initialize (FunctionParser<dim>::default_variable_names (),
                            parameters.get ("Potential"),
                            typename FunctionParser<dim>::ConstMap());
      VectorTools::interpolate (dof_handler, potential, projected_potential);
    }
    data_out.add_data_vector (projected_potential, "interpolated_potential");

    data_out.build_patches ();

    std::ofstream output ("eigenvectors.vtk");
    data_out.write_vtk (output);
  }

  // @sect4{EigenvalueProblem::run}

  // This is the function which has the top-level control over everything. It
  // is almost exactly the same as in step-4:
  template <int dim>
  void EigenvalueProblem<dim>::run ()
  {
    make_grid_and_dofs ();

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

    assemble_system ();

    const unsigned int n_iterations = solve ();
    std::cout << "   Solver converged in " << n_iterations
              << " iterations." << std::endl;

    output_results ();

    std::cout << std::endl;
    for (unsigned int i=0; i<eigenvalues.size(); ++i)
      std::cout << "      Eigenvalue " << i
                << " : " << eigenvalues[i]
                << std::endl;
  }
}

// @sect3{The main function}
int main (int argc, char **argv)
{
  try
    {

      // Here is another difference from other steps: We initialize the SLEPc
      // work space which inherently initializes the PETSc work space, then go
      // ahead run the whole program. After that is done, we finalize the
      // SLEPc-PETSc work.
      SlepcInitialize (&argc, &argv, 0, 0);

      {
        using namespace dealii;
        using namespace Step36;

        deallog.depth_console (0);

        EigenvalueProblem<2> problem ("step-36.prm");
        problem.run ();
      }

      SlepcFinalize ();
    }

  // All the while, we are watching out if any exceptions should have been
  // generated. If that is so, we panic...
  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  // If no exceptions are thrown, then we tell the program to stop monkeying
  // around and exit nicely:
  std::cout << std::endl
            << "   Job done."
            << std::endl;

  return 0;
}