#include "Trainer.h"
#include "HoughForests.h"
#include "LocalFeatureExtractor.h"

#include <numpy.hpp>

#include <boost/format.hpp>
#include <boost/tokenizer.hpp>

#include <algorithm>
#include <filesystem>
#include <iostream>
#include <numeric>
#include <random>

namespace nuisken {

void Trainer::extractTrainingFeatures(const std::string& positiveVideoDirectoryPath,
                                      const std::string& negativeVideoDirectoryPath,
                                      const std::string& labelFilePath,
                                      const std::string& dstDirectoryPath, int localWidth,
                                      int localHeight, int localDuration, int xBlockSize,
                                      int yBlockSize, int tBlockSize, int xStep, int yStep,
                                      int tStep, const std::vector<double>& negativeScales,
                                      int nPositiveSamplesPerStep,
                                      int nNegativeSamplesPerStep) const {
    extractPositiveFeatures(positiveVideoDirectoryPath, dstDirectoryPath, localWidth, localHeight,
                            localDuration, xBlockSize, yBlockSize, tBlockSize, xStep, yStep, tStep,
                            nPositiveSamplesPerStep);
    extractNegativeFeatures(negativeVideoDirectoryPath, labelFilePath, dstDirectoryPath, localWidth,
                            localHeight, localDuration, xBlockSize, yBlockSize, tBlockSize, xStep,
                            yStep, tStep, negativeScales, nNegativeSamplesPerStep);
}

void Trainer::extractPositiveFeatures(const std::string& videoDirectoryPath,
                                      const std::string& dstDirectoryPath, int localWidth,
                                      int localHeight, int localDuration, int xBlockSize,
                                      int yBlockSize, int tBlockSize, int xStep, int yStep,
                                      int tStep, int nSamplesPerStep) const {
    std::vector<double> scales = {1.0};
    int randomSeed = 1;
    std::mt19937 randomEngine(randomSeed);

    std::tr2::sys::path directory(videoDirectoryPath);
    std::tr2::sys::directory_iterator end;
    for (std::tr2::sys::directory_iterator itr(directory); itr != end; ++itr) {
        std::string filePath = itr->path().string();

        std::cout << "extract" << std::endl;
        houghforests::LocalFeatureExtractor extractor(filePath, scales, localWidth, localHeight,
                                                      localDuration, xBlockSize, yBlockSize,
                                                      tBlockSize, xStep, yStep, tStep);
        std::vector<cv::Vec3i> selectedPoints;
        std::vector<std::vector<float>> selectedDescriptors;
        while (true) {
            std::cout << "frame: " << extractor.getStoredFeatureBeginT() << std::endl;
            std::vector<std::vector<cv::Vec3i>> points;
            std::vector<std::vector<std::vector<float>>> descriptors;
            extractor.extractLocalFeatures(points, descriptors);
            if (extractor.isEnded()) {
                break;
            }

            std::vector<size_t> indices(points[0].size());
            std::iota(std::begin(indices), std::end(indices), 0);
            std::shuffle(std::begin(indices), std::end(indices), randomEngine);

            int n = 0;
            for (auto index : indices) {
                if (n++ >= nSamplesPerStep) {
                    break;
                }

                selectedPoints.push_back(points[0][index]);
                selectedDescriptors.push_back(descriptors[0][index]);
            }
        }

        std::cout << "output: " << selectedPoints.size() << std::endl;
        std::string dstFileName = itr->path().filename().stem().string();
        std::string dstPointsFilePath = dstDirectoryPath + dstFileName + "_pt.npy";
        std::vector<int> outputPoints;
        for (const auto& point : selectedPoints) {
            for (int i = 0; i < point.rows; ++i) {
                outputPoints.push_back(point(i));
            }
        }
        aoba::SaveArrayAsNumpy<int>(dstPointsFilePath, selectedPoints.size(),
                                    selectedPoints.front().rows, outputPoints.data());

        std::string dstDescriptorsFilePath = dstDirectoryPath + dstFileName + "_desc.npy";
        std::vector<float> outputDescriptors;
        for (const auto& desc : selectedDescriptors) {
            for (int i = 0; i < desc.size(); ++i) {
                outputDescriptors.push_back(desc[i]);
            }
        }
        aoba::SaveArrayAsNumpy<float>(dstDescriptorsFilePath, selectedDescriptors.size(),
                                      selectedDescriptors.front().size(), outputDescriptors.data());
    }
}

void Trainer::extractNegativeFeatures(const std::string& videoDirectoryPath,
                                      const std::string& labelFilePath,
                                      const std::string& dstDirectoryPath, int localWidth,
                                      int localHeight, int localDuration, int xBlockSize,
                                      int yBlockSize, int tBlockSize, int xStep, int yStep,
                                      int tStep, const std::vector<double>& scales,
                                      int nSamplesPerStep) const {
    int randomSeed = 1;
    std::mt19937 randomEngine(randomSeed);

    std::tr2::sys::path directory(videoDirectoryPath);
    std::tr2::sys::directory_iterator end;
    for (std::tr2::sys::directory_iterator itr(directory); itr != end; ++itr) {
        std::string filePath = itr->path().string();
        std::string fileName = itr->path().filename().string();
        int dataIndex = std::stoi(std::string{fileName[0]});

        std::vector<cv::Rect> boxes;
        std::vector<std::pair<int, int>> temporalRanges;
        std::cout << "read labels" << std::endl;
        readLabelsInfo(labelFilePath, dataIndex, std::vector<int>(), boxes, temporalRanges);

        std::cout << "extract" << std::endl;
        houghforests::LocalFeatureExtractor extractor(filePath, scales, localWidth, localHeight,
                                                      localDuration, xBlockSize, yBlockSize,
                                                      tBlockSize, xStep, yStep, tStep);
        std::vector<cv::Vec3i> selectedPoints;
        std::vector<std::vector<float>> selectedDescriptors;
        while (true) {
            std::cout << "frame: " << extractor.getStoredFeatureBeginT() << std::endl;
            std::vector<std::vector<cv::Vec3i>> points;
            std::vector<std::vector<std::vector<float>>> descriptors;
            extractor.extractLocalFeatures(points, descriptors);
            if (extractor.isEnded()) {
                break;
            }

            std::cout << "select" << std::endl;
            for (int scaleIndex = 0; scaleIndex < points.size(); ++scaleIndex) {
                std::vector<size_t> indices;
                int index = 0;
                for (const auto& point : points[scaleIndex]) {
                    cv::Vec3i scaledPoint(point);
                    scaledPoint(1) /= scales[scaleIndex];
                    scaledPoint(2) /= scales[scaleIndex];
                    if (!contains(boxes, temporalRanges, point)) {
                        indices.push_back(index);
                    }
                    ++index;
                }

                std::shuffle(std::begin(indices), std::end(indices), randomEngine);

                int n = 0;
                for (auto index : indices) {
                    if (n++ >= nSamplesPerStep) {
                        break;
                    }

                    selectedPoints.push_back(points[scaleIndex][index]);
                    selectedDescriptors.push_back(descriptors[scaleIndex][index]);
                }
            }
        }

        std::cout << "output: " << selectedPoints.size() << std::endl;
        std::string dstFileName = itr->path().filename().stem().string();
        std::string dstPointsFilePath = dstDirectoryPath + dstFileName + "_pt.npy";
        std::vector<int> outputPoints;
        for (const auto& point : selectedPoints) {
            for (int i = 0; i < point.rows; ++i) {
                outputPoints.push_back(point(i));
            }
        }
        aoba::SaveArrayAsNumpy<int>(dstPointsFilePath, selectedPoints.size(),
                                    selectedPoints.front().rows, outputPoints.data());

        std::string dstDescriptorsFilePath = dstDirectoryPath + dstFileName + "_desc.npy";
        std::vector<float> outputDescriptors;
        for (const auto& desc : selectedDescriptors) {
            for (int i = 0; i < desc.size(); ++i) {
                outputDescriptors.push_back(desc[i]);
            }
        }
        aoba::SaveArrayAsNumpy<float>(dstDescriptorsFilePath, selectedDescriptors.size(),
                                      selectedDescriptors.front().size(), outputDescriptors.data());
    }
}

void Trainer::readLabelsInfo(const std::string& labelFilePath, int dataIndex,
                             std::vector<int>& classLabels, std::vector<cv::Rect>& boxes,
                             std::vector<std::pair<int, int>>& temporalRanges) const {
    std::ifstream inputStream(labelFilePath);
    std::string line;
    while (std::getline(inputStream, line)) {
        boost::char_separator<char> commaSeparator(",");
        boost::tokenizer<boost::char_separator<char>> commaTokenizer(line, commaSeparator);
        std::vector<std::string> tokens;
        std::copy(std::begin(commaTokenizer), std::end(commaTokenizer), std::back_inserter(tokens));

        if (std::stoi(tokens.at(0)) == dataIndex) {
            auto label = std::stoi(tokens.at(1));
            classLabels.push_back(label);

            auto beginFrame = std::stoi(tokens.at(2));
            auto endFrame = std::stoi(tokens.at(3));
            auto topLeftX = std::stoi(tokens.at(4));
            auto topLeftY = std::stoi(tokens.at(5));
            auto bottomRightX = std::stoi(tokens.at(6));
            auto bottomRightY = std::stoi(tokens.at(7));
            boxes.emplace_back(cv::Point(topLeftX, topLeftY),
                               cv::Point(bottomRightX, bottomRightY));
            temporalRanges.emplace_back(beginFrame, endFrame);
        }
    }
}

bool Trainer::contains(const cv::Rect& box, const std::pair<int, int>& temporalRange,
                       const cv::Vec3i& point) const {
    bool space = box.contains(cv::Point(point(2), point(1)));
    bool time = (temporalRange.first <= point(0)) && (temporalRange.second < point(0));
    return space && time;
}

bool Trainer::contains(const std::vector<cv::Rect>& boxes,
                       const std::vector<std::pair<int, int>>& temporalRanges,
                       const cv::Vec3i& point) const {
    for (int i = 0; i < boxes.size(); ++i) {
        if (contains(boxes.at(i), temporalRanges.at(i), point)) {
            return true;
        }
    }
    return false;
}

void Trainer::train(const std::string& featureDirectoryPath, const std::string& labelFilePath,
                    const std::string& forestsDirectoryPath,
                    const std::vector<int> trainingDataIndices, int nClasses, int baseScale,
                    int nTrees, double bootstrapRatio, int maxDepth, int minData, int nSplits,
                    int nThresholds, bool isMaskUsed) {
    using namespace nuisken;
    using namespace nuisken::houghforests;
    using namespace nuisken::randomforests;
    using namespace nuisken::storage;

    const int N_CHANNELS = 4;

    std::vector<std::shared_ptr<STIPFeature>> trainingData;
    for (int dataIndex : trainingDataIndices) {
        std::cout << "read " << dataIndex << std::endl;

        std::vector<cv::Rect> boxes;
        std::vector<std::pair<int, int>> ranges;
        readLabelsInfo(labelFilePath, dataIndex, std::vector<int>(), boxes, ranges);

        std::vector<cv::Vec3i> positiveActionPositions(boxes.size());
        for (int labelIndex = 0; labelIndex < boxes.size(); ++labelIndex) {
            cv::Vec3i position;
            position(T) = (ranges.at(labelIndex).second - ranges.at(labelIndex).first) / 2;
            int width = boxes.at(labelIndex).width;
            int height = boxes.at(labelIndex).height;
            double aspectRatio = static_cast<double>(width) / height;
            position(Y) = baseScale / 2;
            position(X) = (baseScale * aspectRatio) / 2;
            positiveActionPositions.at(labelIndex) = position;
        }

        int negativeLabel = nClasses - 1;

        auto tmpData = readData(featureDirectoryPath, dataIndex, positiveActionPositions,
                                negativeLabel, isMaskUsed);
        std::copy(std::begin(tmpData), std::end(tmpData), std::back_inserter(trainingData));
    }

    auto type = TreeParameters::ALL_RATIO;
    bool hasNegatieClass = true;
    TreeParameters treeParameters(nClasses, nTrees, bootstrapRatio, maxDepth, minData, nSplits,
                                  nThresholds, type, hasNegatieClass);
    std::vector<int> numberOfFeatureDimensions(N_CHANNELS);
    for (auto i = 0; i < N_CHANNELS; ++i) {
        numberOfFeatureDimensions.at(i) = trainingData.front()->getNumberOfFeatureDimensions(i);
    }
    STIPNode stipNode(nClasses, N_CHANNELS, numberOfFeatureDimensions);
    HoughForestsParameters houghParameters;
    houghParameters.setTreeParameters(treeParameters);
    int nThreads = 6;
    HoughForests houghForests(stipNode, houghParameters, nThreads);
    houghForests.train(trainingData);

    std::tr2::sys::path directory(forestsDirectoryPath);
    if (!std::tr2::sys::exists(directory)) {
        std::tr2::sys::create_directory(directory);
    }

    houghForests.save(forestsDirectoryPath);
}

std::vector<Trainer::FeaturePtr> Trainer::readData(
        const std::string& directoryPath, int dataIndex,
        const std::vector<cv::Vec3i>& positiveActionPositions, int negativeLabel,
        bool isMaskUsed) const {
    std::tr2::sys::path directory(directoryPath);
    std::tr2::sys::directory_iterator end;
    std::vector<int> usedLabelIndices;
    bool isNegativeRead = false;
    std::vector<FeaturePtr> trainingData;
    for (std::tr2::sys::directory_iterator itr(directory); itr != end; ++itr) {
        std::string filePath = itr->path().string();
        std::string fileName = itr->path().filename().string();

        boost::char_separator<char> separator("_");
        boost::tokenizer<boost::char_separator<char>> tokenizer(fileName, separator);
        std::vector<std::string> tokens;
        std::copy(std::begin(tokenizer), std::end(tokenizer), std::back_inserter(tokens));
        int fileDataIndex = std::stoi(tokens.at(0));
        if (fileDataIndex != dataIndex) {
            continue;
        }

        if (tokens.size() == 2) {
            if (isNegativeRead) {
                continue;
            }
            auto negatives = readNegativeData(directoryPath, dataIndex, negativeLabel);
            std::copy(std::begin(negatives), std::end(negatives), std::back_inserter(trainingData));

            isNegativeRead = true;
        } else if (tokens.size() == 4) {
            int labelIndex = std::stoi(tokens.at(1));
            if (std::find(std::begin(usedLabelIndices), std::end(usedLabelIndices), labelIndex) !=
                std::end(usedLabelIndices)) {
                continue;
            }

            int classLabel = std::stoi(tokens.at(2));
            auto positives = readPositiveData(directoryPath, dataIndex, labelIndex, classLabel,
                                              positiveActionPositions.at(labelIndex), isMaskUsed);
            std::copy(std::begin(positives), std::end(positives), std::back_inserter(trainingData));

            usedLabelIndices.push_back(labelIndex);
        }
    }

    return trainingData;
}

std::vector<Trainer::FeaturePtr> Trainer::readPositiveData(const std::string& directoryPath,
                                                           int dataIndex, int labelIndex,
                                                           int classLabel,
                                                           const cv::Vec3i& actionPosition,
                                                           bool isMaskUsed) const {
    std::string pointFilePath = (boost::format("%s%d_%d_%d_pt.npy") % directoryPath % dataIndex %
                                 labelIndex % classLabel)
                                        .str();
    std::string descriptorFilePath = (boost::format("%s%d_%d_%d_desc.npy") % directoryPath %
                                      dataIndex % labelIndex % classLabel)
                                             .str();
    std::string foregroundFilePath = (boost::format("%s%d_%d_%d_fgd.npy") % directoryPath %
                                      dataIndex % labelIndex % classLabel)
                                             .str();
    std::vector<Trainer::FeaturePtr> tmpFeatures;
    if (isMaskUsed) {
        tmpFeatures = readLocalFeatures(pointFilePath, descriptorFilePath, foregroundFilePath,
                                        classLabel, actionPosition);
    } else {
        tmpFeatures =
                readLocalFeatures(pointFilePath, descriptorFilePath, classLabel, actionPosition);
    }

    std::string flippedPointFilePath = (boost::format("%s%d_%d_%d_flip_pt.npy") % directoryPath %
                                        dataIndex % labelIndex % classLabel)
                                               .str();
    std::string flippedDescriptorFilePath = (boost::format("%s%d_%d_%d_flip_desc.npy") %
                                             directoryPath % dataIndex % labelIndex % classLabel)
                                                    .str();
    std::string flippedForegroundFilePath = (boost::format("%s%d_%d_%d_flip_fgd.npy") %
                                             directoryPath % dataIndex % labelIndex % classLabel)
                                                    .str();
    std::vector<Trainer::FeaturePtr> tmpFlippedFeatures;
    if (isMaskUsed) {
        tmpFlippedFeatures =
                readLocalFeatures(flippedPointFilePath, flippedDescriptorFilePath,
                                  flippedForegroundFilePath, classLabel, actionPosition);
    } else {
        tmpFlippedFeatures = readLocalFeatures(flippedPointFilePath, flippedDescriptorFilePath,
                                               classLabel, actionPosition);
    }

    std::vector<FeaturePtr> trainingData;
    std::copy(std::begin(tmpFeatures), std::end(tmpFeatures), std::back_inserter(trainingData));
    std::copy(std::begin(tmpFlippedFeatures), std::end(tmpFlippedFeatures),
              std::back_inserter(trainingData));
    return trainingData;
}

std::vector<Trainer::FeaturePtr> Trainer::readNegativeData(const std::string& directoryPath,
                                                           int dataIndex, int negativeLabel) const {
    std::string pointFilePath = (boost::format("%s%d_pt.npy") % directoryPath % dataIndex).str();
    std::string descriptorFilePath =
            (boost::format("%s%d_desc.npy") % directoryPath % dataIndex).str();
    return readLocalFeatures(pointFilePath, descriptorFilePath, negativeLabel, cv::Vec3i());
}

std::vector<Trainer::FeaturePtr> Trainer::readLocalFeatures(const std::string& pointFilePath,
                                                            const std::string& descriptorFilePath,
                                                            int classLabel,
                                                            const cv::Vec3i& actionPosition) const {
    using namespace storage;
    using namespace houghforests;

    const int N_CHANNELS = LocalFeatureExtractor::N_CHANNELS_;

    std::vector<int> pointShape;
    std::vector<int> points;
    aoba::LoadArrayFromNumpy<int>(pointFilePath, pointShape, points);

    std::vector<int> descShape;
    std::vector<float> descriptors;
    aoba::LoadArrayFromNumpy<float>(descriptorFilePath, descShape, descriptors);

    int nChannelFeatures = descShape[1] / N_CHANNELS;
    std::vector<FeaturePtr> localFeatures;
    for (int localIndex = 0; localIndex < pointShape[0]; ++localIndex) {
        int pointIndex = localIndex * 3;
        cv::Vec3i point(points[pointIndex], points[pointIndex + 1], points[pointIndex + 2]);
        std::vector<Eigen::MatrixXf> features(N_CHANNELS);
        for (int channelIndex = 0; channelIndex < N_CHANNELS; ++channelIndex) {
            Eigen::MatrixXf feature(1, nChannelFeatures);
            for (int featureIndex = 0; featureIndex < nChannelFeatures; ++featureIndex) {
                int index =
                        localIndex * descShape[1] + channelIndex * nChannelFeatures + featureIndex;
                feature.coeffRef(0, featureIndex) = descriptors[index];
            }
            features.at(channelIndex) = feature;
        }

        cv::Vec3i offset = actionPosition - point;
        auto data = std::make_shared<STIPFeature>(features, point, offset, std::make_pair(0.0, 0.0),
                                                  classLabel);
        data->setIndex(-1);
        localFeatures.push_back(data);
    }
    return localFeatures;
}

std::vector<Trainer::FeaturePtr> Trainer::readLocalFeatures(const std::string& pointFilePath,
                                                            const std::string& descriptorFilePath,
                                                            const std::string& foregroundFilePath,
                                                            int classLabel,
                                                            const cv::Vec3i& actionPosition) const {
    using namespace storage;
    using namespace houghforests;

    const int N_CHANNELS = LocalFeatureExtractor::N_CHANNELS_;

    std::vector<int> pointShape;
    std::vector<int> points;
    aoba::LoadArrayFromNumpy<int>(pointFilePath, pointShape, points);

    std::vector<int> descShape;
    std::vector<float> descriptors;
    aoba::LoadArrayFromNumpy<float>(descriptorFilePath, descShape, descriptors);

    std::vector<int> fgdShape;
    std::vector<unsigned char> foregrounds;
    aoba::LoadArrayFromNumpy<unsigned char>(foregroundFilePath, fgdShape, foregrounds);
    cv::Mat1b foregroundMat(fgdShape.size(), fgdShape.data());
    foregroundMat.data = foregrounds.data();

    int nChannelFeatures = descShape[1] / N_CHANNELS;
    std::vector<FeaturePtr> localFeatures;
    for (int localIndex = 0; localIndex < pointShape[0]; ++localIndex) {
        int pointIndex = localIndex * 3;
        cv::Vec3i point(points[pointIndex], points[pointIndex + 1], points[pointIndex + 2]);
        if (foregroundMat(point) == 0) {
            continue;
        }

        std::vector<Eigen::MatrixXf> features(N_CHANNELS);
        for (int channelIndex = 0; channelIndex < N_CHANNELS; ++channelIndex) {
            Eigen::MatrixXf feature(1, nChannelFeatures);
            for (int featureIndex = 0; featureIndex < nChannelFeatures; ++featureIndex) {
                int index =
                        localIndex * descShape[1] + channelIndex * nChannelFeatures + featureIndex;
                feature.coeffRef(0, featureIndex) = descriptors[index];
            }
            features.at(channelIndex) = feature;
        }

        cv::Vec3i offset = actionPosition - point;
        auto data = std::make_shared<STIPFeature>(features, point, offset, std::make_pair(0.0, 0.0),
                                                  classLabel);
        data->setIndex(-1);
        localFeatures.push_back(data);
    }
    return localFeatures;
}
}


#ifndef AMGCL_SOLVER_EIGEN_HPP
#define AMGCL_SOLVER_EIGEN_HPP

/*
The MIT License

Copyright (c) 2012-2020 Denis Demidov <dennis.demidov@gmail.com>

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.
*/

/**
\file   amgcl/solver/eigen.hpp
\author Denis Demidov <dennis.demidov@gmail.com>
\brief  Wrapper around eigen direct solvers.
*/

#include <Eigen/Dense>
#include <Eigen/SparseCore>

#include <type_traits>

#include <amgcl/backend/builtin.hpp>
#include <amgcl/util.hpp>

namespace amgcl {
namespace solver {

template < class Solver >
class EigenSolver {
    public:
        typedef typename Solver::MatrixType MatrixType;
        typedef typename Solver::Scalar     value_type;

        typedef amgcl::detail::empty_params params;

        static size_t coarse_enough() {
            return 3000 / math::static_rows<value_type>::value;
        }

        template <class Matrix>
        EigenSolver(const Matrix &A, const params& = params())
            : n( backend::rows(A) )
        {
            typedef
                typename std::remove_const<
                    typename std::remove_pointer<
                        typename backend::col_data_impl<Matrix>::type
                        >::type
                    >::type
                col_type;

            typedef
                typename std::remove_const<
                    typename std::remove_pointer<
                        typename backend::ptr_data_impl<Matrix>::type
                        >::type
                    >::type
                ptr_type;

            S.compute(
                    MatrixType(
                        Eigen::MappedSparseMatrix<value_type, Eigen::RowMajor, ptrdiff_t>(
                            backend::rows(A), backend::cols(A), backend::nonzeros(A),
                            const_cast<ptr_type*>(backend::ptr_data(A)),
                            const_cast<col_type*>(backend::col_data(A)),
                            const_cast<value_type*>(backend::val_data(A))
                            )
                        )
                    );
        }

        template <class Vec1, class Vec2>
        void operator()(const Vec1 &rhs, Vec2 &x) const {
            Eigen::Map< Eigen::Matrix<value_type, Eigen::Dynamic, 1> >
                RHS(const_cast<value_type*>(&rhs[0]), n), X(&x[0], n);

            X = S.solve(RHS);
        }

        friend std::ostream& operator<<(std::ostream &os, const EigenSolver &s) {
            return os << "eigen: " << s.n << " unknowns";
        }
    private:
        ptrdiff_t n;
        Solver S;
};

} // namespace solver
} // namespace amgcl

#endif


// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2013.
//
// This software is released under a three-clause BSD license:
//  * 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 any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// 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 ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS 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.
//
// --------------------------------------------------------------------------
// $Maintainer: Alexandra Zerck $
// $Authors: $
// --------------------------------------------------------------------------
//

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

#include <OpenMS/TRANSFORMATIONS/RAW2PEAK/OptimizePeakDeconvolution.h>
#include <OpenMS/MATH/MISC/MathFunctions.h>
#include <OpenMS/DATASTRUCTURES/ListUtils.h>
#include <limits>
#include <boost/math/special_functions/fpclassify.hpp>

#ifdef DEBUG_DECONV
#include <iostream>
#include <fstream>
#endif

namespace OpenMS
{

  const DoubleReal OptimizePeakDeconvolution::dist_ = 1.003;
  namespace OptimizationFunctions
  {
    // Evaluation of the target function for nonlinear optimization.
    Int residualDC(const gsl_vector * x, void * params, gsl_vector * f)
    {
      // According to the gsl conventions, x contains the parameters to be optimized.
      // The first two entries are the left and right width, respectively.They are equal
      // for all peaks. Then the height and position of all peaks are stored.
      //
      // Params might contain any additional parameters. We handle these using class members
      // instead.
      // The vector f is supposed to contain the result when we return from this function.
      // Note: GSL wants the values for each data point i as one component of the results vector
      std::vector<DoubleReal> & signal = static_cast<OptimizePeakDeconvolution::Data *>(params)->signal;
      std::vector<DoubleReal> & positions = static_cast<OptimizePeakDeconvolution::Data *>(params)->positions;
      std::vector<PeakShape> & peaks = static_cast<OptimizePeakDeconvolution::Data *>(params)->peaks;
      OptimizationFunctions::PenaltyFactorsIntensity & penalties = static_cast<OptimizePeakDeconvolution::Data *>(params)->penalties;
      Int charge = static_cast<OptimizePeakDeconvolution::Data *>(params)->charge;
      DoubleReal leftwidth = gsl_vector_get(x, 0);
      DoubleReal rightwidth = gsl_vector_get(x, 1);
      //DoubleReal posP1 = gsl_vector_get(x,2);

      // iterate over all points of the signal
      for (Size current_point = 0; current_point < positions.size(); current_point++)
      {
        DoubleReal computed_signal     = 0.;
        DoubleReal current_position    = positions[current_point];
        DoubleReal experimental_signal = signal[current_point];

        //iterate over all peaks
        for (Size current_peak = 0; current_peak < peaks.size(); current_peak++)
        {
          //Store the current parameters for this peak
          DoubleReal p_height        = gsl_vector_get(x, 2 + 2 * current_peak);
          DoubleReal p_position    = gsl_vector_get(x, 2 + 2 * current_peak + 1);
          DoubleReal p_width         = (current_position <= p_position) ? leftwidth : rightwidth;

          //is it a Lorentz or a Sech - Peak?
          if (peaks[current_peak].type == PeakShape::LORENTZ_PEAK)
          {
            computed_signal += p_height / (1. + pow(p_width * (current_position - p_position), 2));
          }
          else                   // It's a Sech - Peak
          {
            computed_signal += p_height / pow(cosh(p_width * (current_position - p_position)), 2);
          }
        }
        gsl_vector_set(f, current_point, computed_signal - experimental_signal);
      }

      // penalties : especially negative heights have to be penalised
      DoubleReal penalty = 0.;

      DoubleReal penalty_pos    = penalties.pos;
      DoubleReal penalty_lwidth = penalties.lWidth;
      DoubleReal penalty_rwidth = penalties.rWidth;
      DoubleReal penalty_intensity = penalties.height;

      //iterate over all peaks again to compute the penalties
      for (Size current_peak = 0; current_peak < peaks.size(); current_peak++)
      {
        DoubleReal p_position = gsl_vector_get(x, 2 + 2 * current_peak + 1);
        if (current_peak < peaks.size() - 1)
        {

          DoubleReal next_p_position  = gsl_vector_get(x, 2 + 2 * current_peak + 3);
          // if distance between peaks does not match the peptide mass rule
          if (fabs(fabs(p_position - next_p_position) - 1.003 / charge) > 0.05)
          {
            // penalize it
            penalty +=  penalty_pos * 10000
                       * pow(fabs(fabs(p_position - next_p_position) - 1.003 / charge), 2);
          }
        }
        DoubleReal old_position   = peaks[current_peak].mz_position;
        DoubleReal old_width_l  = peaks[current_peak].left_width;
        DoubleReal old_width_r    = peaks[current_peak].right_width;
        DoubleReal old_height    = peaks[current_peak].height;

        DoubleReal p_width_l    = gsl_vector_get(x, 0);
        DoubleReal p_width_r    = gsl_vector_get(x, 1);
        DoubleReal p_height     = gsl_vector_get(x, 2 + 2 * current_peak);

        if (p_height <  1)
        {
          penalty += 100000 * penalty_intensity * pow(fabs(p_height - old_height), 2);

        }
        if (p_width_l < 0)
        {
          penalty += penalty_lwidth * peaks.size() * 10000 * pow(fabs(p_width_l - old_width_l), 2);
        }
        else if (p_width_l < 1.5)
          penalty += 10000 * pow(fabs(p_width_l - old_width_l), 2);
        if (p_width_r < 0)
        {
          penalty += penalty_rwidth * peaks.size() * 10000 * pow(fabs(p_width_r - old_width_r), 2);
        }
        else if (p_width_r < 1.5)
          penalty += 10000 * pow(fabs(p_width_r - old_width_r), 2);
        if (fabs(old_position - p_position) > 0.1)
        {
          penalty += 10000 * penalty_pos * pow(fabs(old_position - p_position), 2);
        }

      }
      gsl_vector_set(f, f->size - 1, penalty);
      return GSL_SUCCESS;
    }

    /** Compute the Jacobian of the residual, where each row of the matrix corresponds to a
     *  point in the data.
     */
    Int jacobianDC(const gsl_vector * x, void * params, gsl_matrix * J)
    {
      // For the conventions on x and params c.f. the commentary in residual()
      //
      // The matrix J is supposed to contain the result when we return from this function.
      // Note: GSL expects the Jacobian as follows:
      //                    - each row corresponds to one data point
      //                    - each column corresponds to one parameter

      std::vector<DoubleReal> & positions = static_cast<OptimizePeakDeconvolution::Data *>(params)->positions;
      std::vector<PeakShape> & peaks = static_cast<OptimizePeakDeconvolution::Data *>(params)->peaks;
      OptimizationFunctions::PenaltyFactorsIntensity & penalties = static_cast<OptimizePeakDeconvolution::Data *>(params)->penalties;
      Int charge = static_cast<OptimizePeakDeconvolution::Data *>(params)->charge;

      DoubleReal leftwidth = gsl_vector_get(x, 0);
      DoubleReal rightwidth = gsl_vector_get(x, 1);

      gsl_matrix_set_zero(J);

      // iterate over all points of the signal
      for (Size current_point = 0; current_point < positions.size(); current_point++)
      {
        DoubleReal current_position    = positions[current_point];

        // iterate over all peaks
        for (Size current_peak = 0; current_peak < peaks.size(); current_peak++)
        {

          //Store the current parameters for this peak
          DoubleReal p_height        = gsl_vector_get(x, 2 + 2 * current_peak);
          DoubleReal p_position    = gsl_vector_get(x, 2 + 2 * current_peak + 1);
          DoubleReal p_width          = (current_position <= p_position) ? leftwidth : rightwidth;

          //is it a Lorentz or a Sech - Peak?
          if (peaks[current_peak].type == PeakShape::LORENTZ_PEAK)
          {
            DoubleReal diff      = current_position - p_position;
            DoubleReal denom_inv = 1. / (1. + pow(p_width * diff, 2));

            DoubleReal ddl_left  = (current_position <= p_position)
                                   ? -2 * p_height * pow(diff, 2) * p_width * pow(denom_inv, 2) :
                                     0;

            DoubleReal ddl_right = (current_position  > p_position)
                                   ? -2 * p_height * pow(diff, 2) * p_width * pow(denom_inv, 2) :
                                     0;

            // left and right width are the same for all peaks,
            // the sums of the derivations over all peaks are stored in the first two columns
            gsl_matrix_set(J, current_point, 0, gsl_matrix_get(J, current_point, 0) + ddl_left);
            gsl_matrix_set(J, current_point, 1, gsl_matrix_get(J, current_point, 1) + ddl_right);

            DoubleReal ddx0    = 2 * p_height * pow(p_width, 2) * diff * pow(denom_inv, 2);

            // partial derivation with respect to intensity
            gsl_matrix_set(J, current_point, 2 + 2 * current_peak, denom_inv);

            // partial derivation with respect to the mz-position
            gsl_matrix_set(J, current_point, 2 + 2 * current_peak + 1, ddx0);
          }
          else                   // It's a Sech - Peak
          {
            DoubleReal diff      = current_position - p_position;
            DoubleReal denom_inv = 1. / cosh(p_width * diff);

            // The remaining computations are not stable if denom_inv == 0. In that case, we are far away from the peak
            // and can assume that all derivatives vanish
            DoubleReal sinh_term = (fabs(denom_inv) < 1e-6) ? 0.0 : sinh(p_width * diff);

            DoubleReal ddl_left  = (current_position <= p_position)
                                   ? -2 * p_height * sinh_term * diff * pow(denom_inv, 3) :
                                     0;
            DoubleReal ddl_right = (current_position  > p_position)
                                   ? -2 * p_height * sinh_term * diff * pow(denom_inv, 3) :
                                     0;

            gsl_matrix_set(J, current_point, 0, gsl_matrix_get(J, current_point, 0) + ddl_left);
            gsl_matrix_set(J, current_point, 1, gsl_matrix_get(J, current_point, 1) + ddl_right);

            DoubleReal ddx0      = 2 * p_height * p_width * sinh_term * pow(denom_inv, 3);

            gsl_matrix_set(J, current_point, 2 + 2 * current_peak, pow(denom_inv, 2));
            gsl_matrix_set(J, current_point, 2 + 2 * current_peak + 1, ddx0);
          }
        }
      }

      /** Now iterate over all peaks again to compute the
       *  penalties.
       */

      for (Size current_peak = 0; current_peak < peaks.size(); current_peak++)
      {

        DoubleReal penalty_p = 0;
        DoubleReal p_position = gsl_vector_get(x, 2 + 2 * current_peak + 1);
        if (current_peak < peaks.size() - 1)
        {

          DoubleReal next_p_position  = gsl_vector_get(x, 2 + 2 * current_peak + 3);
          // if distance between peaks does not match the peptide mass rule
          if (fabs(fabs(p_position - next_p_position) - 1.003 / charge) > 0.05)
          {
            // penalize it
            penalty_p += penalties.pos * 20000
                         * fabs(fabs(p_position - next_p_position) - 1.003 / charge);

          }
        }
        //  std::cout << "penalty_p "<<penalty_p<<std::endl;
        DoubleReal p_width_left = gsl_vector_get(x, 0);
        DoubleReal p_width_right = gsl_vector_get(x, 1);
        DoubleReal p_height   = gsl_vector_get(x, 2 + 2 * current_peak);

        DoubleReal old_position    = peaks[current_peak].mz_position;
        DoubleReal old_width_left  = peaks[current_peak].left_width;
        DoubleReal old_width_right = peaks[current_peak].right_width;
        DoubleReal old_height      = peaks[current_peak].height;

        DoubleReal penalty_h = 0., penalty_l = 0., penalty_r = 0.;
        if (p_height < 1)
        {
          penalty_h += 100000 * 2 * penalties.height * (fabs(p_height) - fabs(old_height));
        }

        if (p_width_left < 0)
        {
          penalty_l += peaks.size() * 2 * penalties.lWidth * 10000 * (fabs(p_width_left - old_width_left));
        }
        else if (p_width_left < 1.5)
          penalty_l += 2 * penalties.lWidth * 10000 * pow(fabs(p_width_left - old_width_left), 2);
        if (p_width_right < 0)
        {
          penalty_r += peaks.size() * 2 * penalties.rWidth * 10000 * (fabs(p_width_right - old_width_right));
        }
        else if (p_width_right < 1.5)
          penalty_r += 2 * penalties.rWidth * 10000 * pow(fabs(p_width_right - old_width_right), 2);
        if (fabs(old_position - p_position) > 0.1)
        {
          penalty_p += 10000 * penalties.pos * 2 * fabs(old_position - p_position);
        }

        gsl_matrix_set(J, positions.size(), 2 + 2 * current_peak, 100 * penalty_h);
        gsl_matrix_set(J, positions.size(), 0, 100 * penalty_l);
        gsl_matrix_set(J, positions.size(), 1, 100 * penalty_r);
        gsl_matrix_set(J, positions.size(), 2 + 2 * current_peak + 1, 100 * penalty_p);
      }

      return GSL_SUCCESS;
    }

    // Driver function for the evaluation of function and jacobian.
    Int evaluateDC(const gsl_vector * x, void * params, gsl_vector * f, gsl_matrix * J)
    {
      residualDC(x, params, f);
      jacobianDC(x, params, J);

      return GSL_SUCCESS;
    }

  } // namespace OptimizationFunctions

  OptimizePeakDeconvolution::OptimizePeakDeconvolution() :
    DefaultParamHandler("OptimizePeakDeconvolution"), charge_(1)
  {

    defaults_.setValue("max_iteration", 10, "maximal number of iterations for the fitting step");
    defaults_.setValue("eps_abs", 1e-04, "if the absolute error gets smaller than this value the fitting is stopped", ListUtils::create<String>("advanced"));
    defaults_.setValue("eps_rel", 1e-04, "if the relative error gets smaller than this value the fitting is stopped", ListUtils::create<String>("advanced"));

    defaults_.setValue("penalties:left_width", 0.0, "penalty term for the fitting of the left width:" \
                                                    "If the left width gets too broad or negative during the fitting it can be penalized.");
    defaults_.setValue("penalties:right_width", 0.0, "penalty term for the fitting of the right width:" \
                                                     "If the right width gets too broad or negative during the fitting it can be penalized.");
    defaults_.setValue("penalties:height", 0.0, "penalty term for the fitting of the intensity:" \
                                                "If it gets negative during the fitting it can be penalized.");
    defaults_.setValue("penalties:position", 0.0, "penalty term for the fitting of the peak position:" \
                                                  "If the position changes more than 0.5Da during the fitting it can be penalized as well as " \
                                                  "discrepancies of the peptide mass rule.");

    defaults_.setValue("fwhm_threshold", 1.0, "If a peaks is broader than fwhm_threshold, it is assumed that it contains another peaks and an additional peak is added.");

    defaultsToParam_();
  }

  void OptimizePeakDeconvolution::updateMembers_()
  {
    penalties_.rWidth = (float)param_.getValue("penalties:right_width");
    penalties_.lWidth = (float)param_.getValue("penalties:left_width");
    penalties_.height = (float)param_.getValue("penalties:height");
    penalties_.pos    = (float)param_.getValue("penalties:position");

  }

  bool OptimizePeakDeconvolution::optimize(std::vector<PeakShape> & peaks, Data & data)
  {

    if (peaks.empty())
      return true;

#ifdef DEBUG_DECONV
    std::cout << "peaksanzahl:" << peaks.size();
    std::cout << "\tpeaks[0].mz_position:" << peaks[0].mz_position << std::endl;

    for (Size j = 0; j < peaks.size(); ++j)
    {
      std::cout << "\tpeaks[j].mz_position:" << peaks[j].mz_position;
      std::cout << "\tpeaks[j].height:" << peaks[j].height << std::endl;
      std::cout << "\tpeaks[j].left_width:" << peaks[j].left_width;
      std::cout << "\tpeaks[j].right_width:" << peaks[j].right_width << std::endl << std::endl;
    }

    for (Size j = 0; j < data.positions.size(); ++j)
    {
      std::cout << "positions[" << j << "]=" << data.positions[j] << std::endl;
    }

#endif

    // the input peaks are stored in a temporary vector
    std::vector<PeakShape> temp_shapes = peaks;

    Size global_peak_number = 0;

    DoubleReal min(std::numeric_limits<double>::max());
    Int best_charge;
    Size num_peaks;
    Size best_num_peaks;
    gsl_vector * best_result = gsl_vector_alloc((Int)(2 + 2 * data.peaks.size()));

    // try three different charge states : charge-1, charge, charge +1
    // take the best solution
    Int l = (charge_ - 1 > 1) ? charge_ - 1 : charge_;
    Int start_l = l;
#ifdef DEBUG_DECONV
    std::cout << "charge " << l << " max_charge" << charge_ + 1
              << "\tpeaks.size() " << peaks.size() << std::endl;
#endif
    best_charge = l;
    best_num_peaks = peaks.size();
    num_peaks = peaks.size();
    for (; l < charge_ + 2; ++l)
    {

      num_peaks = getNumberOfPeaks_(l, temp_shapes, data);
#ifdef DEBUG_DECONV
      std::cout << "charge " << l << " #peaks " << num_peaks << "\tpeaks.size() "
                << data.peaks.size() << std::endl;
#endif
      gsl_vector * start_value;
      // the vector storing the start values for the parameters has to be filled
      // differently depending on the usage of the peptide mass rule
      start_value = gsl_vector_alloc(2 + 2 * data.peaks.size());
      for (Size i = 0; i < data.peaks.size(); i++)
      {
        gsl_vector_set(start_value, 2 + 2 * i, data.peaks[i].height);
        gsl_vector_set(start_value, 3 + 2 * i, data.peaks[i].mz_position);
      }

      // Initialize the parameters for the optimization

      // all peaks shall have the same width
      DoubleReal wl = data.peaks[0].left_width;
      DoubleReal wr = data.peaks[0].right_width;
      if (boost::math::isnan(wl))
      {
        for (Size i = 0; i < data.peaks.size(); ++i)
        {
          data.peaks[i].left_width = 1;
        }
        wl = 1.;
      }
      if (boost::math::isnan(wr))
      {
        for (Size i = 0; i < data.peaks.size(); ++i)
        {
          data.peaks[i].right_width = 1;
        }
        wr = 1.;
      }

      gsl_vector_set(start_value, 0, wl);
      gsl_vector_set(start_value, 1, wr);

      // The gsl algorithms require us to provide function pointers for the evaluation of
      // the target function.
      gsl_multifit_function_fdf fit_function;

      fit_function.f      = OptimizationFunctions::residualDC;
      fit_function.df     = OptimizationFunctions::jacobianDC;
      fit_function.fdf    = OptimizationFunctions::evaluateDC;

      fit_function.n      = std::max(data.positions.size() + 1,
                                     2 + 2 * data.peaks.size());

      fit_function.p    = 2 + 2 * data.peaks.size();
      data.penalties = penalties_;
// fit_function.params = &penalties_;
      data.charge = l;
      fit_function.params = &data;
#ifdef DEBUG_DECONV
      std::cout << "fit_function.p " << fit_function.p << "\t fit_function.n " << fit_function.n << std::endl;
      std::cout << "peaks.size() " << data.peaks.size() << std::endl;
#endif
      const gsl_multifit_fdfsolver_type * type = gsl_multifit_fdfsolver_lmsder;
      gsl_multifit_fdfsolver * fit;
      fit = gsl_multifit_fdfsolver_alloc(type,
                                         std::max(data.positions.size() + 1,
                                                  2 + 2 * data.peaks.size()),
                                         2 + 2 * data.peaks.size());

      gsl_multifit_fdfsolver_set(fit, &fit_function, start_value);

#ifdef DEBUG_DECONV
      // initial norm
      std::cout << "Before optimization: ||f|| = " << gsl_blas_dnrm2(fit->f) << std::endl;
#endif
      // Iteration
      Int iteration = 0;
      Int status;

      do
      {
        iteration++;
        status = gsl_multifit_fdfsolver_iterate(fit);

#ifdef DEBUG_DECONV
        std::cout << "Iteration " << iteration << "; Status " << gsl_strerror(status) << "; " << std::endl;
        std::cout << "||f|| = " << gsl_blas_dnrm2(fit->f) << std::endl;
        std::cout << "Number of parms: " << data.peaks.size() + 3 << std::endl;
        std::cout << "Delta: " << gsl_blas_dnrm2(fit->dx) << std::endl;
#endif
        if (boost::math::isnan(gsl_blas_dnrm2(fit->dx)))
        {
#ifdef DEBUG_DECONV
          std::cout << "norm is not a number" << std::endl;
#endif
          break;
        }
        status = gsl_multifit_test_delta(fit->dx, fit->x, (float)param_.getValue("eps_abs"),
                                         (float)param_.getValue("eps_rel"));

        if (status != GSL_CONTINUE)
        {
#ifdef DEBUG_DECONV
          std::cout << "gsl status != GSL_CONTINUE" << std::endl;
#endif
          break;
        }
        //      if(!checkFWHM_(peaks,fit) && failure <1)
//                      {
// #ifdef DEBUG_DECONV
//                              std::cout << "fwhm differ"<<std::endl;
// #endif
//                              return false;
//                      }
      }
      while (status == GSL_CONTINUE && iteration < (Int)param_.getValue("max_iteration"));

      DoubleReal chi = gsl_blas_dnrm2(fit->f);
#ifdef DEBUG_DECONV
      std::cout << "Finished! Charge " << l << "\tIterations: " << iteration << std::endl;
      std::cout << "Delta: " << gsl_blas_dnrm2(fit->dx) << std::endl;

      std::cout << "chisq/dof = " << pow(chi, 2.0) / (data.positions.size()
                                                      - (3 + data.peaks.size()));
      std::cout << "\nAfter optimization: ||f|| = " << gsl_blas_dnrm2(fit->f) << std::endl;
#endif
      if ((l == start_l) || (chi < min))
      {
        if (l != start_l)
          gsl_vector_free(best_result);
        best_result = gsl_vector_alloc(2 + 2 * data.peaks.size());
        gsl_vector_memcpy(best_result, fit->x);
        min = chi;
        best_charge = l;
        best_num_peaks = data.peaks.size();
      }
      iteration = 0;

      gsl_multifit_fdfsolver_free(fit);
      gsl_vector_free(start_value);

    }
    global_peak_number += best_num_peaks;
    // iterate over all peaks and store the optimized values in peaks
    if (best_num_peaks > 0)
    {
      peaks.resize(best_num_peaks);
      for (Size current_peak = 0; current_peak < best_num_peaks; current_peak++)
      {

        // Store the current parameters for this peak

        peaks[current_peak].left_width  = gsl_vector_get(best_result, 0);
        peaks[current_peak].right_width = gsl_vector_get(best_result, 1);

        peaks[current_peak].height      = gsl_vector_get(best_result, 2 + 2 * current_peak);
        peaks[current_peak].mz_position = gsl_vector_get(best_result, 2 + 2 * current_peak + 1);

        // compute the area
        // is it a Lorentz or a Sech - Peak?
        if (peaks[current_peak].type == PeakShape::LORENTZ_PEAK)
        {
          PeakShape p = peaks[current_peak];
          DoubleReal x_left_endpoint = p.mz_position + 1 / p.left_width * sqrt(p.height / 1 - 1);
          DoubleReal x_right_endpoint = p.mz_position + 1 / p.right_width * sqrt(p.height / 1 - 1);
#ifdef DEBUG_DECONV
          std::cout << "x_left_endpoint " << x_left_endpoint << " x_right_endpoint " << x_right_endpoint << std::endl;
          std::cout << "p.height" << p.height << std::endl;
#endif
          DoubleReal area_left = -p.height / p.left_width * atan(p.left_width * (x_left_endpoint - p.mz_position));
          DoubleReal area_right = -p.height / p.right_width * atan(p.right_width * (p.mz_position - x_right_endpoint));
          peaks[current_peak].area = area_left + area_right;

        }
        else                  //It's a Sech - Peak
        {
          PeakShape p = peaks[current_peak];
          DoubleReal x_left_endpoint = p.mz_position + 1 / p.left_width * boost::math::acosh(sqrt(p.height / 0.001));
          DoubleReal x_right_endpoint = p.mz_position + 1 / p.right_width * boost::math::acosh(sqrt(p.height / 0.001));
#ifdef DEBUG_DECONV
          std::cout << "x_left_endpoint " << x_left_endpoint << " x_right_endpoint " << x_right_endpoint << std::endl;
          std::cout << "p.height" << p.height << std::endl;
#endif
          DoubleReal area_left = -p.height / p.left_width * (sinh(p.left_width * (p.mz_position - x_left_endpoint))
                                                             / cosh(p.left_width * (p.mz_position - x_left_endpoint)));
          DoubleReal area_right = -p.height / p.right_width * (sinh(p.right_width * (p.mz_position - x_right_endpoint))
                                                               / cosh(p.right_width * (p.mz_position - x_right_endpoint)));
          peaks[current_peak].area = area_left + area_right;

        }

      }
    }
    charge_ = best_charge;
    gsl_vector_free(best_result);

    return true;
  }

  bool OptimizePeakDeconvolution::checkFWHM_(std::vector<PeakShape> & peaks, gsl_multifit_fdfsolver * & fit)
  {
    DoubleReal fwhm_threshold = (DoubleReal)param_.getValue("fwhm_threshold");

    PeakShape p;
    for (Size current_peak = 0; current_peak < peaks.size(); current_peak++)
    {
      p.left_width  = gsl_vector_get(fit->x, 0);
      p.right_width = gsl_vector_get(fit->x, 1);
      p.type        = peaks[current_peak].type;
#ifdef DEBUG_DECONV
      std::cout << "fwhm: " << p.getFWHM() << " > " << fwhm_threshold << " ?" << std::endl;
#endif
      if (p.getFWHM() > fwhm_threshold)
        return false;
    }

    return true;
  }

  Size OptimizePeakDeconvolution::getNumberOfPeaks_(Int charge, std::vector<PeakShape> & temp_shapes, Data & data)
  {
    DoubleReal dist = dist_ / charge;

    data.peaks.clear();

    Size shape = 0;
#ifdef DEBUG_DECONV
    std::cout << "temp_shapes[0].mz_position " << temp_shapes[0].mz_position
              << "\t dist " << dist << "\tp_index " << shape << std::endl;
#endif
    // while the peak's position is smaller than the last considered position
    // take the peak for optimization
    while ((temp_shapes[0].mz_position + shape * dist <
            data.positions[data.positions.size() - 1]) &&
           (shape < temp_shapes.size()))
    {
      data.peaks.push_back(temp_shapes[shape]);
#ifdef DEBUG_DECONV
      std::cout << "temp_shapes[0].mz_position + p_index*dist = " << temp_shapes[0].mz_position + shape * dist << std::endl;
#endif
      ++shape;
    }

    return shape;

  }

}


//
// A module to create simple stereo hits out of StrawHits. StrawHit selection is done by flagging in an upstream module
//
//
//  Original Author: David Brown, LBNL
//

#include "canvas/Persistency/Common/Ptr.h"
#include "art/Framework/Core/EDProducer.h"
#include "art/Framework/Core/ModuleMacros.h"
#include "art/Framework/Principal/Event.h"
#include "art/Framework/Principal/Handle.h"
#include "fhiclcpp/ParameterSet.h"
#include "messagefacility/MessageLogger/MessageLogger.h"
#include "art_root_io/TFileService.h"

#include "Offline/GeometryService/inc/GeometryService.hh"
#include "Offline/GeometryService/inc/GeomHandle.hh"
#include "Offline/Mu2eUtilities/inc/TwoLinePCA_XYZ.hh"
#include "Offline/TrackerGeom/inc/Tracker.hh"
#include "Offline/RecoDataProducts/inc/StrawHit.hh"
#include "Offline/RecoDataProducts/inc/ComboHit.hh"
#include "Offline/RecoDataProducts/inc/StrawHitFlag.hh"
#include "Offline/Mu2eUtilities/inc/MVATools.hh"
// boost
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/weighted_variance.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/min.hpp>
using namespace boost::accumulators;

#include <iostream>
#include <float.h>
using namespace std;

namespace {

  struct StereoMVA
  {
    StereoMVA() : _pars(4,0.0),_dt(_pars[0]),_chisq(_pars[1]),_rho(_pars[2]),_ndof(_pars[3]){}

    std::vector <float> _pars;
    float& _dt;
    float& _chisq;
    float& _rho;
    float& _ndof;
  };
}

namespace mu2e {
  class MakeStereoHits : public art::EDProducer {
    public:
      explicit MakeStereoHits(fhicl::ParameterSet const& pset);
      void produce( art::Event& e);
      virtual void beginJob();
      virtual void beginRun(art::Run & run);
    private:
      typedef std::vector<uint16_t> ComboHits;

      int            _debug;
      art::InputTag  _shTag;
      art::InputTag  _chTag;
      art::InputTag  _shfTag;
      ComboHitCollection const* _chcol;

      StrawHitFlag   _shsel;      // flag selection
      StrawHitFlag   _shmask;     // flag anti-selection
      float         _maxDt;      // maximum time separation between hits
      bool          _useTOT;     // use TOT to estimate drift time
      float         _maxDPerp;   // maximum transverse separation
      float         _minDdot;    // minimum dot product of straw directions
      float _minR2, _maxR2; // transverse radius (squared)
      float         _maxChisq;   // maximum chisquared to allow making stereo hits
      float         _minMVA;     // minimum MVA output
      float         _wfac;       // resolution factor along the wire
      float         _tfac;       // resolution transverse to the wire
      bool           _doMVA;      // do MVA eval or simply use chi2 cut
      unsigned      _maxfsep;	  // max face separation
      bool	    _testflag; // test the flag or not
      StrawIdMask _smask; // define matches inside a station

      MVATools _mvatool;
      StereoMVA _vmva;

      std::array<std::vector<StrawId>,StrawId::_nupanels > _panelOverlap;   // which panels overlap each other
      void genMap();
      void finalize(ComboHit& combohit);
  };

  MakeStereoHits::MakeStereoHits(fhicl::ParameterSet const& pset) :
    art::EDProducer{pset},
    _debug(pset.get<int>(           "debugLevel",0)),
    _chTag(pset.get<art::InputTag>("ComboHitCollection")),
    _shsel(pset.get<std::vector<std::string> >("StrawHitSelectionBits",std::vector<std::string>{"EnergySelection","TimeSelection"} )),
    _shmask(pset.get<std::vector<std::string> >("StrawHitMask",std::vector<std::string>{} )),
    _maxDt(pset.get<float>(   "maxDt",40.0)), // nsec //FIXME tune with TOT
    _useTOT(pset.get<bool>("UseTOT",false)), // use TOT to estimate drift time
    _maxDPerp(pset.get<float>("maxDPerp",500.)), // mm, maximum perpendicular distance between time-division points
    _minDdot(pset.get<float>( "minDdot",0.6)), // minimum angle between straws
    _maxChisq(pset.get<float>("maxChisquared",5.0)), // position matching
    _minMVA(pset.get<float>(  "minMVA",0.6)), // MVA cut
    _wfac(pset.get<float>(    "ZErrorFactor",0.3)), // error component due to z separation
    _tfac(pset.get<float>(    "ZErrorFactor",1.0)), // error component due to z separation
    _doMVA(pset.get<bool>(  "doMVA",false)),
    _maxfsep(pset.get<unsigned>("MaxFaceSeparation",3)), // max separation between faces in a station
    _testflag(pset.get<bool>("TestFlag")),
    _smask("uniquepanel"),  // define the mask to select hits in the same unique panel

    _mvatool(pset.get<fhicl::ParameterSet>("MVATool",fhicl::ParameterSet()))
    {
      float minR = pset.get<float>("minimumRadius",395); // mm
      _minR2 = minR*minR;
      float maxR = pset.get<float>("maximumRadius",650); // mm
      _maxR2 = maxR*maxR;
      produces<ComboHitCollection>();
    }

  void MakeStereoHits::beginJob()
  {
    if(_doMVA){
      _mvatool.initMVA();
      if (_debug > 0) std::cout << "MakeStereoHits MVA parameters: " << std::endl;
      if (_debug > 0) _mvatool.showMVA();
    }
  }

  void MakeStereoHits::beginRun(art::Run & run)
  {
    genMap();
  }

  void MakeStereoHits::produce(art::Event& event) {
// find input: I have to get a Handle, not ValidHandle, to get the productID
    art::Handle<ComboHitCollection> chH;
    if(!event.getByLabel(_chTag, chH))
      throw cet::exception("RECO")<<"mu2e::MakeStereoHits: No ComboHit collection found for tag" <<  _chTag << endl;
    _chcol = chH.product();
    // setup output
    std::unique_ptr<ComboHitCollection> chcol(new ComboHitCollection());
    chcol->reserve(_chcol->size());
    // reference the parent in the new collection
    chcol->setParent(chH);
    // sort hits by unique panel.  This should be built in by construction upstream FIXME!!
    std::array<std::vector<uint16_t>,StrawId::_nupanels> phits;
    size_t nch = _chcol->size();
    if(_debug > 1)cout << "MakeStereoHits found " << nch << " Input hits" << endl;
    std::vector<bool> used(nch,false);
    for(uint16_t ihit=0;ihit<nch;++ihit){
      ComboHit const& ch = (*_chcol)[ihit];
      // select hits based on flag
      if( (!_testflag) ||( ch.flag().hasAllProperties(_shsel) && (!ch.flag().hasAnyProperty(_shmask))) ){
	phits[ch.strawId().uniquePanel()].push_back(ihit);
      }
    }
    if(_debug > 2){
      for (unsigned ipan=0; ipan < StrawId::_nupanels; ++ipan) {
	if(phits[ipan].size() > 0 ){
	  cout << "Panel " << ipan << " has " << phits[ipan].size() << " hits "<< endl;
	}
      }
    }
    //  Loop over all hits.  Every one must appear somewhere in the output
    for (size_t ihit=0;ihit<nch;++ihit) {
      if(used[ihit])continue;
      used[ihit] = true;
      // create an output combo hit for every hit; initialize it with this hit
      ComboHit const& ch1 = (*_chcol)[ihit];
      ComboHit combohit;
      combohit.init(ch1,ihit);
      // zero values that accumulate in pairs
      combohit._qual = 0.0;
      combohit._pos = XYZVectorF(0.0,0.0,0.0);
      // loop over the panels which overlap this hit's panel
      for (auto sid : _panelOverlap[ch1.strawId().uniquePanel()]) {
      // loop over hits in the overlapping panel
	for (auto jhit : phits[sid.uniquePanel()]) {
	  const ComboHit& ch2 = (*_chcol)[jhit];
	  if(_debug > 3) cout << " comparing hits " << ch1.strawId().uniquePanel() << " and " << ch2.strawId().uniquePanel();
	  if (!used[jhit] ){
            float dt;
            if (_useTOT)
	      dt = fabs(ch1.correctedTime()-ch2.correctedTime());
            else
	      dt = fabs(ch1.time()-ch2.time());
	    if(_debug > 3) cout << " dt = " << dt;
	    if (dt < _maxDt){
	      float ddot = ch1.wdir().Dot(ch2.wdir());
	      XYZVectorF dp = ch1.pos()-ch2.pos();
	      float dperp = sqrt(dp.perp2());
	      // negative crosings are in opposite quadrants and longitudinal separation isn't too big
	      if(_debug > 3) cout << " ddot = " << ddot << " dperp = " << dperp;
	      if (ddot > _minDdot && dperp < _maxDPerp ) {
		// solve for the POCA.
		TwoLinePCA_XYZ pca(ch1.pos(),ch1.wdir(),ch2.pos(),ch2.wdir());
		if(pca.closeToParallel()){
		  cet::exception("RECO")<<"mu2e::StereoHit: parallel wires" << std::endl;
		}
		// check the points are inside the tracker active volume; these are all the same as the
		float rho2 = pca.point1().Perp2();
		if(_debug > 3) cout << " rho2 = " << rho2;
		if(rho2 < _maxR2 && rho2 > _minR2 ){
		  // compute chisquared; include error for particle angle
		  // should be a cumulative linear regression FIXME!
		  float terr = _tfac*fabs(ch1.pos().z()-ch2.pos().z());
		  float terr2 = terr*terr;
		  float dw1 = pca.s1();
		  float dw2 = pca.s2();
		  float chisq = dw1*dw1/(ch1.wireErr2()+terr2) + dw2*dw2/(ch2.wireErr2()+terr2);
		  if(_debug > 3) cout << " chisq = " << chisq;
		  if (chisq < _maxChisq){
		    if(_debug > 3) cout << " added ";
		    // if we get to here, try to add the hit
		    // accumulate the chisquared
		    if(combohit.addIndex(jhit)) {
		      // average z
		      combohit._qual += chisq;
		      combohit._pos += XYZVectorF(pca.point1().x(),pca.point1().y(),0.5*(pca.point1().z()+pca.point2().z()));
		    } else
		      std::cout << "MakeStereoHits can't add hit" << std::endl;
		    used[jhit] = true;
		  }
		}
	      }
	    }
	  }
	  if(_debug > 3) cout << endl;
	}
      }
      finalize(combohit);
      chcol->push_back(std::move(combohit));
    }
    event.put(std::move(chcol));
  }

  void MakeStereoHits::finalize(ComboHit& combohit) {
    combohit._mask = _smask;
    if(combohit.nCombo() > 1){
      combohit._flag.merge(StrawHitFlag::stereo);
      combohit._flag.merge(StrawHitFlag::radsel);
      accumulator_set<float, stats<tag::weighted_mean>, unsigned > eacc;
      accumulator_set<float, stats<tag::weighted_mean>, unsigned > tacc;
      accumulator_set<float, stats<tag::weighted_mean>, unsigned > dtacc;
      accumulator_set<float, stats<tag::weighted_mean>, unsigned > placc;
      accumulator_set<float, stats<tag::min > > zmin;
      accumulator_set<float, stats<tag::max > > zmax;
      combohit._nsh = 0;
      for(size_t ich = 0; ich < combohit.nCombo(); ++ich){
	size_t index = combohit.index(ich);
	ComboHit const& ch = (*_chcol)[index];
	combohit._flag.merge(ch.flag());
	eacc(ch.energyDep(),weight=ch.nStrawHits());
	tacc(ch.time(),weight=ch.nStrawHits());
	dtacc(ch.driftTime(),weight=ch.nStrawHits());
	placc(ch.pathLength(),weight=ch.nStrawHits());
	zmin(ch.pos().z());
	zmax(ch.pos().z());
	combohit._nsh += ch.nStrawHits();
      }
      float maxz = extract_result<tag::max>(zmax);
      float minz = extract_result<tag::min>(zmin);
      combohit._time = extract_result<tag::weighted_mean>(tacc);
      combohit._dtime = extract_result<tag::weighted_mean>(dtacc);
      combohit._pathlength = extract_result<tag::weighted_mean>(placc);
      combohit._edep = extract_result<tag::weighted_mean>(eacc);
      float dz = (maxz-minz);
      combohit._wdist = dz;
      combohit._tres = dz*_tfac;
      combohit._wres = dz*_wfac;
      combohit._qual /= (combohit.nCombo()-1);// normalize by # of pairs
      combohit._pos /= (combohit.nCombo()-1);
      combohit._wdir = XYZVectorF(0.0,0.0,1.0);
    } else {
      size_t index = combohit.index(0);
      combohit._pos = (*_chcol)[index].pos();// put back original position
    }
  }

  // generate the overlap map
  void MakeStereoHits::genMap() {
    static bool init(false);
    if(!init){
      init = true;
      // initialize
      const Tracker& tt(*GeomHandle<Tracker>());
      // establihit the extent of a panel using the longest straw (0)
      Straw const& straw = tt.getStraw(StrawId(0,0,0));
      float phi0 = (straw.getMidPoint()-straw.halfLength()*straw.getDirection()).phi();
      float phi1 = (straw.getMidPoint()+straw.halfLength()*straw.getDirection()).phi();
      float lophi = std::min(phi0,phi1);
      float hiphi = std::max(phi0,phi1);
      float phiwidth = hiphi-lophi;
      if (phiwidth>M_PI) phiwidth = 2*M_PI-phiwidth;
      if(_debug > 0)std::cout << "Panel Phi width = " << phiwidth << std::endl;
      // loop over all unique panels
      for(size_t ipla = 0;ipla < StrawId::_nplanes; ++ipla) {
	for(int ipan=0;ipan<StrawId::_npanels;++ipan){
	  StrawId sid(ipla,ipan,0);
	  uint16_t upan = sid.uniquePanel();
	  Straw const& straw = tt.getStraw(StrawId(ipla,ipan,0));
	  float phi = straw.getMidPoint().phi();
	  if(_debug > 1)std::cout << "Plane " << ipla << " Panel " << ipan << " phi = " << phi << " z = " << straw.getMidPoint().z() << endl;
	  // loop over nearby panels and check for an overlap
	  size_t minpla = (size_t)std::max(0,(int)ipla-1);
	  size_t maxpla = (size_t)std::min(StrawId::_nplanes-1,(int)ipla+1);
	  for(size_t jpla = minpla; jpla <= maxpla;++jpla){
	    for(int jpan=0;jpan<StrawId::_npanels;++jpan){
	      StrawId osid(jpla,jpan,0);
	      Straw const& ostraw = tt.getStraw(StrawId(jpla,jpan,0));
	      if(_smask.equal(osid,sid) && osid.uniqueFace() != sid.uniqueFace() && (unsigned)abs(osid.uniqueFace() - sid.uniqueFace()) <= _maxfsep ) {
		float dphi = fabs(phi - ostraw.getMidPoint().phi());
		if (dphi > M_PI) dphi = 2*M_PI-dphi;
		if (dphi < phiwidth) _panelOverlap[upan].push_back(osid);
	      }
	    }
	  }
	}
      }
      if (_debug >0) {
	for(uint16_t ipan = 0; ipan < StrawId::_nupanels; ++ipan) {
	  std::cout << "Unique Panel " << ipan << " Overlaps with the panels: ";
	  for(auto sid : _panelOverlap[ipan])
	    std::cout << sid.uniquePanel() << ", ";
	  std::cout << std::endl;
	}
      }
    }
  }

}

using mu2e::MakeStereoHits;
DEFINE_ART_MODULE(MakeStereoHits)


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/lexical_cast.hpp>
using namespace std;
int main() {
    long long int n; cin >> n;
    long long int ans = 0;
    for (int i = 1; i < 1000000; i++) {
        string m = boost::lexical_cast<string>(i) + boost::lexical_cast<string>(i);
        long long int a = boost::lexical_cast<long long int>(m);
        if (a <= n) ans++;
        else break;
    }
    cout << ans << endl;
}


#include <iostream>
#include <franka/exception.h>
#include <franka/robot.h>
#include <thread>
#include <chrono>
#include <franka_typing_aid/examples_common.h>

#include <Eigen/Core>

using namespace std::chrono_literals;

int main(int argc, char **argv)
{
    if (argc != 2)
    {
        std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
        return -1;
    }
    try
    {
        franka::Robot robot(argv[1]);
        setDefaultBehavior(robot);
        size_t count = 0;
        robot.read([&count](const franka::RobotState &robot_state) {
            // Printing to std::cout adds a delay. This is acceptable for a read loop such as this, but
            // should not be done in a control loop.
            std::cout << "force/torques: ";
            printArray(robot_state.K_F_ext_hat_K.data(), robot_state.K_F_ext_hat_K.size());
            Eigen::Matrix<double, 3, 1> force_vec(robot_state.K_F_ext_hat_K.data());
            std::cout << "force norm: " << force_vec.norm() << std::endl;
            std::cout << "joint speeds: ";
            printArray(robot_state.dq.data(), robot_state.dq.size());
            Eigen::Matrix<double, 7, 1> q_dot(robot_state.dq.data());
            std::cout << "joint speed norm: " << q_dot.norm() << std::endl;
            std::this_thread::sleep_for(500ms);
            return count++ < 100;
        });
        std::cout << "Done." << std::endl;
    }
    catch (franka::Exception const &e)
    {
        std::cout << e.what() << std::endl;
        return -1;
    }
    return 0;
}


#include <boost/filesystem.hpp>
#include <fftw3.h>

#include "Simulation.h"

template <int dim>
Simulation<dim>::Simulation(
		json settings, double lc_thickness, unsigned int N_lc_steps,
		std::shared_ptr<CubicInterpolatedMapping<dim,3,double> > &n_field) :
	mat_properties(settings.at("Material properties")),
	N_lc_steps(N_lc_steps),
	lc_thickness(lc_thickness),
	n_field(n_field) {

	// We initialize the wavelength array
	auto &j = settings.at("Light source");
	if(j.find("Wavelengths") != j.end()) {
		wavelengths = j.at("Wavelengths").get<std::vector<double> >();
		N_wavelengths = int(wavelengths.size());
	}
	else {
		N_wavelengths = j.at("N wavelengths");
		double mean_lambda = j.at("Mean wavelength");
		double fwhm = j.at("Spectral FWHM");

		if(N_wavelengths==1)
			wavelengths.push_back(j.at("Mean wavelength"));
		else {
			for(int i=0; i<N_wavelengths; i++) {
				wavelengths.push_back(mean_lambda - 0.5*fwhm + fwhm*i/(N_wavelengths-1));
			}
		}
	}

	// We initialize the ODE for the isotropic layers
	lower_iso_thickness = std::accumulate(
		mat_properties.e_lower_layers.begin(),
		mat_properties.e_lower_layers.end(), 0.);
	upper_iso_thickness = std::accumulate(
		mat_properties.e_upper_layers.begin(),
		mat_properties.e_upper_layers.end(), 0.);

	lower_iso_odes.push_back(std::make_shared<IsotropicODEFunction<dim> >(
		std::pow(mat_properties.n_out, 2.),
		std::make_shared<SlabDomain<dim> >(
			BasisVector<dim>(dim-1),
			-lower_iso_thickness-lc_thickness*1.5,
			-lower_iso_thickness-lc_thickness/2.)));

	double z_interface = -lower_iso_thickness-lc_thickness/2.;
	for(int i=0; i<mat_properties.n_lower_layers.size(); i++) {
		lower_iso_odes.push_back(std::make_shared<IsotropicODEFunction<dim> >(
			std::pow(mat_properties.n_lower_layers[i],2.),
			std::make_shared<SlabDomain<dim> >(
				BasisVector<dim>(dim-1), z_interface,
				z_interface + mat_properties.e_lower_layers[i])));
		z_interface += mat_properties.e_lower_layers[i];
	}

	z_interface = lc_thickness/2;
	for(int i=0; i<mat_properties.e_upper_layers.size(); i++) {
		upper_iso_odes.push_back(std::make_shared<IsotropicODEFunction<dim> >(
			std::pow(mat_properties.n_upper_layers[i],2.),
			std::make_shared<SlabDomain<dim> >(
				BasisVector<dim>(dim-1), z_interface,
				z_interface + mat_properties.e_upper_layers[i])));
		z_interface += mat_properties.e_upper_layers[i];
	}
	upper_iso_odes.push_back(std::make_shared<IsotropicODEFunction<dim> >(
		std::pow(mat_properties.n_out,2.),
		std::make_shared<SlabDomain<dim> >(
			BasisVector<dim>(dim-1), z_interface,
			z_interface+lc_thickness)));

	// We initalize the other parameters
	N_refinement_cycles =
		settings.at("InverseScreenMap parameters").at("N refinement cycles");
	hc_parameters = settings.at("InverseScreenMap parameters");

	viz_settings = settings.at("Visualisation");
	numerical_aperture = viz_settings.at("Screen output").at("Numerical aperture");

	bulk_basename = viz_settings.at("Bulk output").at("Base name");
	screen_basename = viz_settings.at("Screen output").at("Base name");
	basedir = viz_settings.at("Results folder name");

	bulk_ray_output = viz_settings.at("Bulk output").at("Export ray data");
	bulk_fields_output = viz_settings.at("Bulk output").at("Export reconstructed fields");
	screen_ray_output = viz_settings.at("Screen output").at("Export ray data");
	screen_fields_output = viz_settings.at("Screen output").at("Export reconstructed fields");

	auto widths = parse_Vector<dim-1,double>(viz_settings, "Target output widths");
	auto dims = parse_Vector<dim-1,unsigned long>(viz_settings, "Target N pixels per dim");
	auto origin = -widths/2;

	Vector<dim-1,unsigned long> coarse_dims;
	for(int i=0; i<dim-1; i++) {
		coarse_dims(i) =
			std::floor(1+(dims(i)-1)/std::pow(2,N_refinement_cycles));
		dims(i) = 1+(coarse_dims(i)-1)*std::pow(2,N_refinement_cycles);
	}
	coarse_horiz_mesh = std::make_shared<CartesianMesh<dim-1> >(
		origin, widths, coarse_dims, false);
	full_horiz_mesh = std::make_shared<CartesianMesh<dim-1> >(
		origin, widths, dims, false);

	switch(dim) {
		case 2:
			typical_length = widths(0);
			break;
		case 3:
			typical_length = std::max(widths(0),widths(1));
			break;
	}

	double bulk_z_length = lc_thickness;
	if(settings.at("Geometry").at("Sample type")=="Droplet")
		bulk_z_length -= 2*settings.at("Geometry").at("Droplet sample parameters").at(
			"Distance from upper sample plate").get<double>();
	z_step = bulk_z_length / (N_lc_steps-1);

	double n_above_lc =
		mat_properties.n_upper_layers.size()>0 ?
		mat_properties.n_upper_layers[0] : mat_properties.n_out;
	z_foc_1 =
		bulk_z_length*(1-n_above_lc/mat_properties.n_lc)/2. +
		(lc_thickness-bulk_z_length)*(1-n_above_lc/mat_properties.n_host)/2.;
	z_foc_2 =
		bulk_z_length*(1-mat_properties.n_out/mat_properties.n_lc)/2. +
		(lc_thickness-bulk_z_length)*(1-mat_properties.n_out/mat_properties.n_host)/2.;
	for(int i=0; i<mat_properties.e_upper_layers.size(); i++)
		z_foc_2 +=
			mat_properties.e_upper_layers[i] *
			(1-mat_properties.n_out/mat_properties.n_upper_layers[i]);

	switch(dim) {
		case 2:
			N_horiz_pixels = dims(0);
			if(bulk_ray_output) {
				bulk_extra_data = std::make_shared<RayData>(
					1, dims(0), N_lc_steps,
					0., widths(0)/(dims(0)-1), z_step);
				bulk_ordi_data = std::make_shared<RayData>(
					1, dims(0), N_lc_steps,
					0., widths(0)/(dims(0)-1), z_step);
			}
			if(bulk_fields_output)
				bulk_fields_data = std::make_shared<FieldsData>(
					1, dims(0), N_lc_steps,
					0., widths(0)/(dims(0)-1), z_step,
					wavelengths);
			if(screen_ray_output) {
				screen_extra_data = std::make_shared<RayData>(
					1, dims(0), 1,
					0., widths(0)/(dims(0)-1), 0.);
				screen_ordi_data = std::make_shared<RayData>(
					1, dims(0), 1,
					0., widths(0)/(dims(0)-1), 0.);
			}
			if(screen_fields_output)
				screen_fields_data = std::make_shared<FieldsData>(
					1, dims(0), 1,
					0., widths(0)/(dims(0)-1), 0.,
					wavelengths);
			break;

		case 3:
			N_horiz_pixels = dims(0)*dims(1);
			if(bulk_ray_output) {
				bulk_extra_data = std::make_shared<RayData>(
					dims(0), dims(1), N_lc_steps,
					widths(0)/(dims(0)-1), widths(1)/(dims(1)-1), z_step);
				bulk_ordi_data = std::make_shared<RayData>(
					dims(0), dims(1), N_lc_steps,
					widths(0)/(dims(0)-1), widths(1)/(dims(1)-1), z_step);
			}
			if(bulk_fields_output)
				bulk_fields_data = std::make_shared<FieldsData>(
					dims(0), dims(1), N_lc_steps,
					widths(0)/(dims(0)-1), widths(1)/(dims(1)-1), z_step,
					wavelengths);
			if(screen_ray_output) {
				screen_extra_data = std::make_shared<RayData>(
					dims(0), dims(1), 1,
					widths(0)/(dims(0)-1), widths(1)/(dims(1)-1), 0.);
				screen_ordi_data = std::make_shared<RayData>(
					dims(0), dims(1), 1,
					widths(0)/(dims(0)-1), widths(1)/(dims(1)-1), 0.);
			}
			if(screen_fields_output)
				screen_fields_data = std::make_shared<FieldsData>(
					dims(0), dims(1), 1,
					widths(0)/(dims(0)-1), widths(1)/(dims(1)-1), 0.,
					wavelengths);
			break;
	}
}

template <int dim>
void Simulation<dim>::apply_fourier_iso_layer_filter() {

	int dims[3]; 		screen_fields_data->vti_data->GetDimensions(dims);
	double spacings[3];	screen_fields_data->vti_data->GetSpacing(spacings);

	fftw_complex *in = static_cast<fftw_complex*>(
		fftw_malloc(sizeof(fftw_complex) * dims[0]*dims[1]));
	fftw_complex *out = static_cast<fftw_complex*>(
		fftw_malloc(sizeof(fftw_complex) * dims[0]*dims[1]));
	fftw_plan forward_plan = (dim==3) ?
		fftw_plan_dft_2d(dims[1], dims[0], in, out, FFTW_FORWARD, FFTW_ESTIMATE) :
		fftw_plan_dft_1d(dims[1], in, out, FFTW_FORWARD, FFTW_ESTIMATE);
	fftw_plan backward_plan = (dim==3) ?
		fftw_plan_dft_2d(dims[1], dims[0], in, out, FFTW_BACKWARD, FFTW_ESTIMATE) :
		fftw_plan_dft_1d(dims[1], in, out, FFTW_BACKWARD, FFTW_ESTIMATE);

	#pragma omp parallel for
	for(int wave_idx=0; wave_idx<N_wavelengths; wave_idx++) {
		// First we assemble the filter
		auto iso_filter =
			std::make_shared<std::vector<std::complex<double> > >(dims[0]*dims[1]);
		double k0 = 2*PI/wavelengths[wave_idx];

		// transfer matrix in column-major order
		std::vector<std::complex<double> > tmat(4), prev_tmat(4);
		std::complex<double> t1, t2;

		for(int iy=0; iy<dims[1]; iy++) {
			for(int ix=0; ix<dims[0]; ix++) {
				double kx = (ix<double(dims[0]/2.)) ?
					2*PI*ix/(spacings[0]*(dims[0]-1)) :
					-2*PI*(dims[0]-ix)/(spacings[0]*(dims[0]-1));
				if(dim==2)
					kx = 0;
				double ky = (iy<double(dims[1]/2.)) ?
					2*PI*iy/(spacings[1]*(dims[1]-1)) :
					-2*PI*(dims[1]-iy)/(spacings[1]*(dims[1]-1));
				double k = std::sqrt(kx*kx+ky*ky);

				if(k<k0*numerical_aperture) {
					tmat[0] = 1;
					tmat[1] = 0;
					tmat[2] = 0;
					tmat[3] = 1;

					prev_tmat = tmat;

					double kN = std::sqrt(k0*k0-k*k);

					for(int p=0; p<mat_properties.e_upper_layers.size(); p++) {
						double np = mat_properties.n_upper_layers[p];
						double ep = mat_properties.e_upper_layers[p];
						double kp = std::sqrt(std::pow(k0*np,2.)-k*k);
						double next_kp = (p+1<mat_properties.e_upper_layers.size()) ?
							std::sqrt(std::pow(k0*mat_properties.n_upper_layers[p+1],2.)-k*k) :
							std::sqrt(std::pow(k0*mat_properties.n_out,2.)-k*k);

						t1 = 0.5*(1+kp/next_kp)*std::exp(std::complex<double>(0,(kp-kN)*ep));
						t2 = 0.5*(1-kp/next_kp)*std::exp(std::complex<double>(0,(kp-kN)*ep));

						tmat[0] = t1*prev_tmat[0] + std::conj(t2)*prev_tmat[1];
						tmat[1] = t2*prev_tmat[0] + std::conj(t1)*prev_tmat[1];
						tmat[2] = t1*prev_tmat[2] + std::conj(t2)*prev_tmat[3];
						tmat[3] = t2*prev_tmat[2] + std::conj(t1)*prev_tmat[3];

						prev_tmat = tmat;
					}
					iso_filter->at(ix+dims[0]*iy) =
						(tmat[0]-tmat[1]*tmat[2]/tmat[3]) *
						std::exp(std::complex<double>(0,kN*z_foc_2));
				}
				else
					iso_filter->at(ix+dims[0]*iy) = 0;
			}
		}

		// FFTW arrays for this thread
		fftw_complex *field = static_cast<fftw_complex*>(
			fftw_malloc(sizeof(fftw_complex) * dims[0]*dims[1]));
		fftw_complex *fft_field = static_cast<fftw_complex*>(
			fftw_malloc(sizeof(fftw_complex) * dims[0]*dims[1]));

		for(int pol_idx=0; pol_idx<2; pol_idx++) {
			for(int comp=0; comp<2; comp++) {
				// First, we fill the fftw input array with our data
				for(int i=0; i<dims[0]*dims[1]; i++) {
					field[i][0] =
						screen_fields_data->E_real[pol_idx][wave_idx]->GetComponent(i,comp);
					field[i][1] =
						screen_fields_data->E_imag[pol_idx][wave_idx]->GetComponent(i,comp);
				}

				// We switch to fourrier space and apply the iso filter
				fftw_execute_dft(forward_plan, field, fft_field);
				for(int i=0; i<dims[0]*dims[1]; i++) {
					auto val = std::complex<double>(fft_field[i][0], fft_field[i][1]);
					fft_field[i][0] = std::real(val * iso_filter->at(i));
					fft_field[i][1] = std::imag(val * iso_filter->at(i));
				}

				// We come back to real space and save the transformed
				// data
				fftw_execute_dft(backward_plan, fft_field, field);

				for(int i=0; i<dims[0]*dims[1]; i++) {
					screen_fields_data->E_real[pol_idx][wave_idx]->SetComponent(
						i, comp, field[i][0]/(dims[0]*dims[1]));
					screen_fields_data->E_imag[pol_idx][wave_idx]->SetComponent(
						i, comp, field[i][1]/(dims[0]*dims[1]));
				}
			}
		}

		// We free the FFTW pointers
		fftw_free(field);
		fftw_free(fft_field);
	}

	// We free the FFTW pointers
	fftw_destroy_plan(forward_plan);
	fftw_destroy_plan(backward_plan);
	fftw_free(in);
	fftw_free(out);
}

template class Simulation<2>;
template class Simulation<3>;


#ifndef INCOMPNS_PSOLVER_HPP
#define INCOMPNS_PSOLVER_HPP

#include "mfem.hpp"
using namespace mfem;

#include <Eigen/Core>

#include "../core/config.hpp"

#include "../mymfem/utilities.hpp"

#include "test_cases_factory.hpp"
#include "coefficients.hpp"
#include "pdiscretisation.hpp"
#include "pobserver.hpp"

class IncompNSParSolver
{
public:
    //! Constructors
    explicit IncompNSParSolver
    (MPI_Comm comm,
     const nlohmann::json config);

    explicit IncompNSParSolver
    (MPI_Comm comm,
     const nlohmann::json config,
     std::shared_ptr<IncompNSTestCases>& testCase);

    explicit IncompNSParSolver
    (MPI_Comm comm,
     const nlohmann::json config,
     std::shared_ptr<IncompNSTestCases>& testCase,
     const std::string mesh_dir,
     const int lx, const int Nt);

    //! Destructor
    virtual ~IncompNSParSolver ();

    //! Sets mesh and observer
    void set(const std::string mesh_dir, const int lx);

    //! Initializes and runs solver
    void operator() (std::unique_ptr<BlockVector>& U);

    //! Initializes and runs solver
    //! Computes the solution error if needed
    std::pair<double, Eigen::VectorXd> operator() (void);

    //! Initializes solver
    virtual void init ()
    {
        std::cout << "Init not implemented "
                     "for the class IncompNSParSolver"
                  << std::endl;
        abort();
    }

    //! Projects initial conditions
    void project_init_conditions
    (std::unique_ptr<BlockVector>& U,
     std::shared_ptr<ParGridFunction>& v,
     std::shared_ptr<ParGridFunction>& p) const;

    //! Runs initialized solver
    void run (std::unique_ptr<BlockVector>&,
              int sampleId=0) const;

    //! Deletes memory
    void clear ();

    //! Initializes linear system solver
    void init_linear_solver() const;

    //! Updates linear system solver
    void update_linear_solver() const;

    //! Initializes preconditioner
    void init_preconditioner () const;

    //! Updates preconditioner
    void update_preconditioner (const double dt) const;

    //! Solves one time step
    virtual void solve_one_step
    (const int, std::shared_ptr<ParGridFunction>&,
     BlockVector *, BlockVector *) const
    {
        std::cout << "Solve one step not implemented "
                     "for the class IncompNSParSolver"
                  << std::endl;
        abort();
    }

    //! Computes error for velocity and pressure
    Eigen::VectorXd compute_error
    (double t,
     std::shared_ptr<ParGridFunction> v,
     std::shared_ptr<ParGridFunction> p) const;

    //! Cleans divergence, if needed
    void clean_divergence
    (std::shared_ptr<ParGridFunction> v) const;

    //! Computes the time-step according to the
    //! cfl conditions.
    //! Assumes that all mesh elements have same geometry
    double compute_time_step
    (std::shared_ptr<ParGridFunction>& v) const;

    //! Visualizes solution
    void visualize
    (std::shared_ptr<ParGridFunction> v,
     std::shared_ptr<ParGridFunction> p) const;

    //! Returns test case
    inline std::shared_ptr<IncompNSTestCases>
    get_test_case() const {
        return m_testCase;
    }

    //! Returns mesh
    inline std::shared_ptr<ParMesh>
    get_mesh() const {
        return m_pmesh;
    }

    //! Returns discretisation
    inline std::shared_ptr<IncompNSParFEM>
    get_discr() const {
        return m_discr;
    }

    //! Returns observer
    inline std::shared_ptr<IncompNSParObserver>
    get_observer() const {
        return m_observer;
    }

    //! Evluates minimum and maximum mesh size
    inline std::pair<double, double>
    eval_hMinMax() const
    {
        double h_min, h_max, kappa_min, kappa_max;
        m_pmesh->GetCharacteristics(h_min, h_max,
                                    kappa_min, kappa_max);
        return {std::move(h_min), std::move(h_max)};
    }

    //! Returns minimum mesh size
    inline double
    get_hMin() const {
        return m_hMin;
    }

    //! Returns maximum mesh size
    inline double
    get_hMax() const {
        return m_hMax;
    }

protected:
    int m_myrank;
    MPI_Comm m_comm;

    const nlohmann::json m_config;
    int m_deg;

    int m_Nt;
    double m_tEnd;

    bool m_bool_read_mesh_from_file;
    int m_num_refinements;

    bool m_bool_error;
    bool m_bool_cleanDivg;

    std::string m_mesh_format;
    std::string m_mesh_elem_type;
    std::string m_mesh_dir;

    Array<int> m_block_offsets;
    Array<int> m_block_trueOffsets;

    std::shared_ptr<IncompNSTestCases> m_testCase;
    std::shared_ptr<ParMesh> m_pmesh;
    std::shared_ptr<IncompNSParFEM> m_discr;
    std::shared_ptr<IncompNSParObserver> m_observer;

    mutable HypreParMatrix *m_incompNSMat = nullptr;
    mutable BlockOperator *m_incompNSOp = nullptr;
    mutable Solver *m_incompNSPr = nullptr;

    mutable Solver *m_precond = nullptr;
    mutable Solver *m_solver = nullptr;

    bool m_bool_embedded_preconditioner;
    bool m_bool_mean_free_pressure;

    double m_hMin, m_hMax;
    double m_cfl_number = 0.3;
};

//! Backward Euler + FEM
class IncompNSBackwardEulerParSolver
        : public IncompNSParSolver
{
public:
    //! Constructor
    explicit IncompNSBackwardEulerParSolver
    (MPI_Comm comm,
     const nlohmann::json config,
     std::shared_ptr<IncompNSTestCases>& testCase,
     const std::string mesh_dir,
     const int lx, const int Nt);

    //! Destructor
    ~IncompNSBackwardEulerParSolver () override {}

    //! Initializes solver
    void init () override;

    //! Solves one time step
    void solve_one_step
    (const int, std::shared_ptr<ParGridFunction>&,
     BlockVector *, BlockVector *) const override;

private:
    //! Updates solver components
    void update (const double, const double,
                 std::shared_ptr<ParGridFunction>&,
                 BlockVector *, BlockVector *) const;

    //! Runs the appropriate linear system solvers
    void solve (BlockVector *, BlockVector *) const;
};

//! Semi-implicit/explicit Euler + FEM
class IncompNSImexEulerParSolver
        : public IncompNSParSolver
{
public:
    //! Constructor
    explicit IncompNSImexEulerParSolver
    (MPI_Comm comm,
     const nlohmann::json config,
     std::shared_ptr<IncompNSTestCases>& testCase,
     const std::string mesh_dir,
     const int lx, const int Nt);

    //! Destructor
    ~IncompNSImexEulerParSolver () override {}

    //! Initializes solver
    void init () override;

    //! Solves one time step
    void solve_one_step
    (const int, std::shared_ptr<ParGridFunction>&,
     BlockVector *, BlockVector *) const override;
};

//! Semi-implicit/explicit BDF2 + FEM
class IncompNSImexBdf2ParSolver
        : public IncompNSParSolver
{
public:
    //! Constructor
    explicit IncompNSImexBdf2ParSolver
    (MPI_Comm comm,
     const nlohmann::json config,
     std::shared_ptr<IncompNSTestCases>& testCase,
     const std::string mesh_dir,
     const int lx, const int Nt);

    //! Destructor
    ~IncompNSImexBdf2ParSolver () override {}

    //! Initializes solver
    void init () override;

    //! Solves one time step
    void solve_one_step
    (const int, std::shared_ptr<ParGridFunction>&,
     BlockVector *, BlockVector *) const override;
};

//! BDF1/RK2 + FEM
class IncompNSBdf1Rk2ParSolver
        : public IncompNSParSolver
{
public:
    //! Constructor
    explicit IncompNSBdf1Rk2ParSolver
    (MPI_Comm comm,
     const nlohmann::json config,
     std::shared_ptr<IncompNSTestCases>& testCase,
     const std::string mesh_dir,
     const int lx, const int Nt);

    //! Destructor
    ~IncompNSBdf1Rk2ParSolver () override {}

    //! Initializes solver
    void init () override;

    //! Solves one time step
    void solve_one_step
    (const int, std::shared_ptr<ParGridFunction>&,
     BlockVector *, BlockVector *) const override;

private:
    //! Runs propagation
    void propagate(double tn, Vector& Vs) const;

    //! Runs one time step of propagation
    //! Explicit RK2, mid-point method
    //! Applies matrix-free convection operator
    void propagate_one_step(double s,
                            double ds,
                            Vector& V) const;

    //! Runs one time step of propagation
    //! Explicit RK2, mid-point method
    //! Slow version assembles the convection matrix
    void propagate_one_step_slow(double s,
                                 double ds,
                                 Vector& V) const;

    //! Extrapolates solution from previous time steps
    void extrapolate(double ds, Vector& Vs) const;

private:
    double m_dt;
    HypreParMatrix *m_invMass = nullptr;

    mutable std::shared_ptr<ParGridFunction> m_vn;
    mutable std::shared_ptr<ParGridFunction> m_vs;

    mutable std::unique_ptr<Vector> m_Vs;
    mutable std::unique_ptr<Vector> m_buf;
    mutable std::unique_ptr<Vector> m_km;
};

#endif /// INCOMPNS_PSOLVER_HPP


#include <iostream>

#include <boost/program_options.hpp>

#include "jsonmodel.h"
#include "helpers.h"

using namespace mht;
using namespace helpers;

int main(int argc, char** argv) {
	namespace po = boost::program_options;

	std::string modelFilename;
	std::string outputFilename;
	std::string weightsFilename;

	// Declare the supported options.
	po::options_description description("Allowed options");
	description.add_options()
	    ("help", "produce help message")
	    ("model,m", po::value<std::string>(&modelFilename), "filename of model stored as Json file")
	    ("weights,w", po::value<std::string>(&weightsFilename), "filename of the weights stored as Json file")
	    ("output,o", po::value<std::string>(&outputFilename), "filename where the resulting tracking (as links) will be stored as Json file")
		("lp-relax", "run LP relaxation")
	;

	po::variables_map variableMap;
	po::store(po::parse_command_line(argc, argv, description), variableMap);
	po::notify(variableMap);

	if (variableMap.count("help"))
	{
	    std::cout << description << std::endl;
	    return 1;
	}

	if (!variableMap.count("model") || !variableMap.count("output") || !variableMap.count("weights"))
	{
	    std::cout << "Model, Weights and Output filenames have to be specified!" << std::endl;
	    std::cout << description << std::endl;
	}
	else
	{
		bool withIntegerConstraints = variableMap.count("lp-relax") == 0;
	    JsonModel model;
		model.readFromJson(modelFilename);
		std::vector<double> weights = readWeightsFromJson(weightsFilename);
		Solution solution = model.infer(weights, withIntegerConstraints);
		model.saveResultToJson(outputFilename, solution);
	}
}