/*
 * Copyright (c) 2020-2021, Marco Sánchez Beeckman
 * All rights reserved.
 *
 * This source code is licensed under the BSD-style license found in the
 * LICENSE file in the root directory of this source tree.
 */

#include <algorithm>
#include <cmath>
#include <fstream>
#include <iostream>
#include <numeric>

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/ml/ml.hpp>

#include "ImageUtils.h"
#include "ConsensusVoter.h"

namespace tfg {

ConsensusVoter::ConsensusVoter() {}
ConsensusVoter::ConsensusVoter(int estimateSpPerFrame, int numberOfFrames) {
    const int estimatedTotalSuperpixels = estimateSpPerFrame * numberOfFrames;
    this->superpixels.reserve(estimatedTotalSuperpixels);
    this->votes.reserve(estimatedTotalSuperpixels);
    this->saliencyScores.reserve(numberOfFrames);
    this->frameBeginningIndex.reserve(numberOfFrames);
}
ConsensusVoter::~ConsensusVoter() {}

/**
 * Initialize motion saliency scores for each image, using precomputed optical flows.
 * The scores are stored in OpenCV's Mat objects, and are the same size as the read flows.
 * As such, the saliency scores need not be the same size as the original images, and are resized
 * to the correct values when initializing the votes. This is done this way in case the flows are computed
 * on scaled down images to diminish the execution time.
 * @param flowFile A .txt file containing the absolute path to each image's optical flow .tiff files.
 * @param minimumPercentageValidity Minimum percentage of frames with dominant motion that should exist for the computation to be considered successful.
 * @return True if the majority (at least minimumPercentageValidity) of frames have a dominant motion, false otherwise.
 */
bool ConsensusVoter::initializeMotionSaliencyScores(std::ifstream &flowFile, float minimumPercentageValidity) {
    this->saliencyScores.clear();
    std::string line;

    std::getline(flowFile, line);
    const int NUMBER_OF_FRAMES = std::stoi(line); // The first line of the file tells the number of frames
    this->saliencyScores.reserve(NUMBER_OF_FRAMES);
    int framesWithDominantMotion = 0;

    // Each frame has a certain number of optical flows attributed to it, typically both forward and backward
    for(int f = 0; f < NUMBER_OF_FRAMES; f++) {
        std::getline(flowFile, line);
        const int NUMBER_OF_FLOWS = std::stoi(line); // The first line of a particular frame tells the number of flows

        std::vector<cv::Mat> motionSaliencies;
        // For each flow which the frame is the origin of, compute a motion saliency score
        for(int i = 0; i < NUMBER_OF_FLOWS; i++) {
            std::getline(flowFile, line);
            cv::Mat flowu = cv::imread(line, cv::IMREAD_ANYDEPTH);

            std::getline(flowFile, line);
            cv::Mat flowv = cv::imread(line, cv::IMREAD_ANYDEPTH);

            cv::Mat saliency;
            // Call an auxiliary method to compute motion saliency scores from an optical flow
            bool existsDominantMotion = computeMotionSaliency(flowu, flowv, saliency, 5, 1.0f, 0.55f, 10);
            // If a dominant motion exists, store temporarily the corresponding saliencies
            if(existsDominantMotion) {
                std::cout << " in flow " << i << " of frame " << f << '\n';
                motionSaliencies.push_back(saliency);
            }
        }

        // Extract the mean of the motion saliencies associated to the frame, and attribute it to it
        cv::Mat frameScore = cv::Mat::zeros(1, 1, CV_32FC1);
        if(motionSaliencies.size() > 0) {
            elementwiseMean(motionSaliencies, frameScore);
            framesWithDominantMotion += 1;
        } else {
            std::cout << "No dominant motion in frame " << f << ": saliency initialized to 0" << '\n';
        }

        this->saliencyScores.push_back(frameScore);
    }

    // The saliency score must be a number between 0 and 1, so dividing by the maximum value of the sequence (not per frame) is
    // the most straightforward way to normalize the sequence of images.
    normalizeByMaxOfSequence(this->saliencyScores);

    // If not enough frames have a dominant motion, consider the process a failure
    // In that case, visual saliency is the way to go
    return (framesWithDominantMotion > NUMBER_OF_FRAMES * minimumPercentageValidity);
}

/**
 * Compute motion saliency from two floating point matrices of optical flows (horizontal and vertical).
 * This method looks for two different kinds of motion: first, it tries to tell if the frame is static by comparing the median of the magnitude of the
 * flow and comparing to a low value; then, if the frame is not static, it searches for a dominant translational motion by computing a histogram of the phases
 * of the flow, and looking at the height of the largest bin.
 * The saliency is defined as the square of the deviation of the pixel value (magnitude of the flow in the static case, angle in the translational case) from the
 * dominant value (0 in the static case, the class mark of the largest bin in the translational case).
 * @param flowu A floating point matrix containing the horizontal optical flow.
 * @param flowv A floating point matrix containing the vertical optical flow.
 * @param saliency Output matrix containing the saliency scores for each pixel.
 * @param patchSize The patch size to smooth the saliencies.
 * @param staticTh The threshold value to compare with the median of the flow magnitude to tell if a frame is static or not.
 * @param transTh The threshold percentage to compare with the height of the largest bin to tell if a frame has a dominant translational motion.
 * @param orientationBins The number of bins for the histogram of flow angles.
 * @return True if the flow has either static or translational dominant motion, false otherwise.
 */
bool ConsensusVoter::computeMotionSaliency(const cv::Mat &flowu, const cv::Mat &flowv, cv::Mat &saliency, int patchSize, float staticTh, float transTh, int orientationBins) {

    // Compute the magnitude of the flow
    cv::Mat magnitude;
    cv::magnitude(flowu, flowv, magnitude);
    std::vector<float> magnitudeData;
    magnitude.reshape(0, 1).row(0).copyTo(magnitudeData);

    // Compute the median of the magnitude of the flow
    std::nth_element(magnitudeData.begin(), magnitudeData.begin() + magnitudeData.size()/2, magnitudeData.end());
    float median;
    if(magnitudeData.size()%2 == 1) {
        median = magnitudeData[magnitudeData.size()/2];
    } else {
        auto it = std::max_element(magnitudeData.begin(), magnitudeData.begin() + magnitudeData.size()/2);
        median = (*it + magnitudeData[magnitudeData.size()/2])/2.0f;
    }

    // If the median is low enough, the background is static
    // In that case, the saliency score is the square of each pixel's magnitude
    if(median < staticTh) {
        std::cout << "Static motion";
        cv::Mat magnitudeDeviation = magnitude.mul(magnitude);
        cv::Mat kernel = cv::Mat::ones(patchSize, patchSize, CV_32FC1) / (patchSize * patchSize);
        cv::filter2D(magnitudeDeviation, saliency, -1, kernel);
        return true;
    }

    // Reaching here means that the background is not static, so check for a dominant translational motion
    // To do so, compute an histogram of the orientations, and check if one bin has more than transTh*100% of the entries
    cv::Mat orientation;
    cv::phase(flowu, flowv, orientation);
    int onlyChannel[] = {0};
    int histSize[] = {orientationBins};
    float orientationRange[] = {0, 2*CV_PI};
    const float* ranges[] = {orientationRange};
    cv::Mat orientationHistogram;

    cv::calcHist(&orientation, 1, onlyChannel, cv::Mat(), orientationHistogram, 1, histSize, ranges);
    cv::Scalar sumHist = cv::sum(orientationHistogram);
    orientationHistogram /= sumHist(0) + (sumHist(0) == 0);

    double minVal, maxVal;
    int minPos[2], maxPos[2];
    cv::minMaxIdx(orientationHistogram, &minVal, &maxVal, &minPos[0], &maxPos[0]);

    // If the percentage of the largest bin is greater than a threshhold, we consider that the frame has dominant translational motion
    if(maxVal > transTh) {
        std::cout << "Translational motion";
        // Compute the deviation of the angles with respect to the class mark of the bin with the hightest value
        // Since an angle does not change by adding or substracting 2pi to it, the desired deviation is the elementwise minimum of
        // the deviations adding -2pi, 0, and 2pi.
        float classMark = (2*maxPos[0] + 1)*2*CV_PI / (2 * orientationBins);
        cv::Mat angleDeviation = (orientation - classMark).mul(orientation - classMark);
        cv::min(angleDeviation, (orientation - classMark + 2 * CV_PI).mul(orientation - classMark + 2 * CV_PI), angleDeviation);
        cv::min(angleDeviation, (orientation - classMark - 2 * CV_PI).mul(orientation - classMark - 2 * CV_PI), angleDeviation);

        cv::Mat kernel = cv::Mat::ones(patchSize, patchSize, CV_32FC1) / (patchSize * patchSize);
        cv::filter2D(angleDeviation, saliency, -1, kernel);
        return true;
    }

    return false;
}

/**
 * Compute the elementwise mean of a vector of matrices.
 * @param src A collection of OpenCV matrices.
 * @param dst An output matrix where each element contains the mean of the values in that position.
 */
void ConsensusVoter::elementwiseMean(const std::vector<cv::Mat> &src, cv::Mat &dst) {
    dst.release();

    const unsigned int VECTOR_SIZE = src.size();
    dst = cv::Mat::zeros(src[0].size(), CV_32FC1);
    for(unsigned int i = 0; i < VECTOR_SIZE; i++) {
        dst += src[i];
    }
    dst /= VECTOR_SIZE;
}

/**
 * Normalize a sequence of matrices so all the elements take values in the range [0, 1]
 * by dividing all the entries by the maximum of the whole sequence.
 * @param sequence A sequence of matrices.
 */
void ConsensusVoter::normalizeByMaxOfSequence(std::vector<cv::Mat> &sequence) {
    float max = 0;
    for(unsigned int i = 0; i < sequence.size(); i++) {
        double minVal, maxVal;
        cv::minMaxIdx(sequence[i], &minVal, &maxVal);
        max = maxVal > max ? maxVal : max;
    }

    if(max == 0) return;

    for(unsigned int i = 0; i < sequence.size(); i++) {
        sequence[i] = sequence[i] / max;
    }
}

/**
 * Save saliency scores to a sequence of grayscale images.
 * @param folder The folder where the new files will be stored.
 * @param fileName A prefix for the name of the new files.
 */
void ConsensusVoter::saveSaliencies(const std::string &folder, const std::string &fileName) {

    for(unsigned int f = 0; f < this->saliencyScores.size(); f++) {
        cv::Mat imageToBeSaved;
        this->saliencyScores[f].convertTo(imageToBeSaved, CV_8UC1, 255);
        std::stringstream ss;
        ss << folder << fileName << f << ".jpg";
        std::string saveAs = ss.str();
        cv::imwrite(saveAs, imageToBeSaved);
    }
}

void ConsensusVoter::mergeSaliencyWithImages(const std::vector<cv::Mat> &images, std::vector<cv::Mat> &mergedImages) {
    mergedImages.clear();
    mergedImages.resize(images.size());
    for(unsigned int f = 0; f < images.size(); f++) {
        cv::Mat imageLab;
        cv::cvtColor(images[f], imageLab, cv::COLOR_BGR2Lab);
        std::vector<cv::Mat> imageChannels;
        cv::split(imageLab, imageChannels);

        cv::Mat saliencyOfFrame;
        cv::resize(this->saliencyScores[f], saliencyOfFrame, images[f].size());
        cv::Mat fourthChannel;
        saliencyOfFrame.convertTo(fourthChannel, CV_8UC1, 255.0, 0.0);
        imageChannels.push_back(fourthChannel);

        cv::merge(imageChannels, mergedImages[f]);
    }
}

/**
 * Add a group of regions belonging to a single frame to the list of superpixels.
 * @param spInFrame A vector of Region objects containing information of superpixels in a frame.
 */
void ConsensusVoter::addRegionsByFrame(std::vector<tfg::Region> &spInFrame) {
    const int nextFrameIndex = this->frameBeginningIndex.size() == 0 ? 0 : this->frameBeginningIndex.back() + spInFrame.size();
    this->frameBeginningIndex.push_back(nextFrameIndex);
    std::move(spInFrame.begin(), spInFrame.end(), std::back_inserter(this->superpixels));
}

/**
 * Initialize votes from previously computed saliency scores, by attributing to each region the mean of the scores
 * of the pixels contained inside it.
 * This is done per-frame, and the saliency matrix of the frame is resized to be the same as the image in case the flows
 * were computed on a shrunken image. If the sizes do not match, a bilineal interpolation is performed on the saliencies.
 */
void ConsensusVoter::initializeVotesInFrame(int frame, const cv::Mat &pixelLabels, int numberOfSuperpixels) {
    cv::Mat saliencyOfFrame;
    cv::resize(this->saliencyScores[frame], saliencyOfFrame, pixelLabels.size());

    for(int i = 0; i < numberOfSuperpixels; i++) {
        cv::Mat superpixelMask(pixelLabels == i);
        cv::Scalar regionMeanVote = cv::mean(saliencyOfFrame, superpixelMask);
        const float vote = regionMeanVote(0);
        this->votes.push_back(vote);
    }
}

/**
 * Compute a random walk transition matrix. This is a sparse matrix that stores costs
 * between neighbouring regions, and is multiplied by a vector of region votes for them to
 * "reach consensus" (update the region votes with a weighted average of their neighbours').
 * The neighbours are computed using a KDTree-search, and a cost is attributed between two neighbours
 * based on the distance between their region descriptors.
 * @param F The number of surrounding frames forwards and backwards to search for nearest neighbours.
 * @param L A multiplier that indicates the number of possible good matches per frame to find nearest neighbours.
 * @param sigma2 The variance of the exponential variable, higher is less restrictive.
 */
void ConsensusVoter::computeTransitionMatrix(int F, int L, float sigma2) {
    computeDescriptors();

    const int NUMBER_OF_REGIONS = static_cast<int>(this->superpixels.size());
    const int NUMBER_OF_FRAMES = static_cast<int>(this->frameBeginningIndex.size());
    const int M = L*(2*F + 1);
    Eigen::SparseMatrix<float, Eigen::RowMajor> transM(NUMBER_OF_REGIONS, NUMBER_OF_REGIONS);
    std::vector<Eigen::Triplet<float>> transMEntries;
    transMEntries.reserve(M * NUMBER_OF_REGIONS);
    transMEntries.reserve(L*(2*F + 1) * NUMBER_OF_REGIONS);

    for(int f = 0; f < NUMBER_OF_FRAMES; f++) {
        // Extract the submatrix corresponding to the descriptors of the regions in the current frame
        cv::Rect currentFrameDelimiter;
        currentFrameDelimiter.y = this->frameBeginningIndex[f];
        currentFrameDelimiter.height = (f == (NUMBER_OF_FRAMES - 1)) ? (NUMBER_OF_REGIONS - this->frameBeginningIndex[f]) : (this->frameBeginningIndex[f + 1] - this->frameBeginningIndex[f]);
        currentFrameDelimiter.x = 0;
        currentFrameDelimiter.width = this->descriptors.cols;
        cv::Mat descriptorsInFrame(this->descriptors, currentFrameDelimiter);

        std::cout << "Extracted descriptors of superpixels in frame " << f << '\n';

        // Extract the submatrix corresponding to the descriptors of all the regions in the neighbouring frames
        cv::Rect searchWindow;
        const int yMinWindow = ((f - F) < 0) ? 0 : (f - F);
        const int yMaxWindow = ((f + F) > (NUMBER_OF_FRAMES - 1)) ? (NUMBER_OF_FRAMES - 1) : (f + F);
        searchWindow.y = this->frameBeginningIndex[yMinWindow];
        searchWindow.height = (yMaxWindow == (NUMBER_OF_FRAMES - 1)) ? (NUMBER_OF_REGIONS - this->frameBeginningIndex[yMinWindow]) : (this->frameBeginningIndex[yMaxWindow + 1] - this->frameBeginningIndex[yMinWindow]);
        searchWindow.x = 0;
        searchWindow.width = this->descriptors.cols;
        cv::Mat descriptorsInNeighbouringFrames(this->descriptors, searchWindow);

        std::cout << "Extracted descriptors of neighbours of frame " << f << '\n';

        // Keep track of the position of every neighbouring region
        // Put them in a Mat object so the KDTree returns the indices of the nearest neighbours
        std::vector<float> neighbourIndices(searchWindow.height);
        std::iota(neighbourIndices.begin(), neighbourIndices.end(), searchWindow.y);
        std::cout << "Frame " << f << " iota: from " << neighbourIndices[0] << " to " << neighbourIndices.back() << '\n';
        cv::Mat regionIndices(neighbourIndices.size(), 1, CV_32FC1, neighbourIndices.data());

        std::cout << "Created matrix with neighbour indices" << '\n';

        // Create a KDTree and train it using the descriptors in the neighbouring frames
        // Each sample belongs to a column, and its response is the index of the region
        cv::Ptr<cv::ml::KNearest> tree = cv::ml::KNearest::create();
        tree->train(descriptorsInNeighbouringFrames, cv::ml::ROW_SAMPLE, regionIndices);

        std::cout << "Created and trained KDTree " << f << '\n';

        // Search for the nearest M neighbours inside of the samples.
        // Since there can be more than one good match per frame, we search up to L regions per frame
        // for a total of M = L*(2*F + 1) nearest neighbours
        // const int M = L*(yMaxWindow - yMinWindow + 1);
        cv::Mat NNIndices;
        cv::Mat result; // unused for our purposes, but required for the function findNearest
        cv::Mat NNDistances2;
        tree->findNearest(descriptorsInFrame, M, result, NNIndices, NNDistances2);

        std::cout << "Found nearest neighbours of superpixels in frame " << f << '\n' << '\n';

        // Knowing who the nearest neighbours are and the distance to them, define a weight based on
        // the distance, and create a random walk transition matrix (which is sparse)
        for(int y = 0; y < currentFrameDelimiter.height; y++) {
            const int ownIndex = currentFrameDelimiter.y + y;
            for(int x = 0; x < M; x++) {
                const int neighbourIndex = static_cast<int>(NNIndices.at<float>(y, x));
                float weight = 1.0f;
                if(ownIndex != neighbourIndex) {
                    const float neighbourDistance2 = NNDistances2.at<float>(y, x);
                    weight = std::exp(-neighbourDistance2 / sigma2);
                }
                transMEntries.push_back(Eigen::Triplet<float>(ownIndex, neighbourIndex, weight));
            }
        }
    }
    transM.setFromTriplets(transMEntries.begin(), transMEntries.end());

    std::cout << "Created sparse transition matrix" << '\n';

    // Upon computing the transition matrix, normalize it by dividing each row by the sum of its elements
    Eigen::SparseMatrix<float, Eigen::RowMajor> normalizationMatrix(NUMBER_OF_REGIONS, NUMBER_OF_REGIONS);
    std::vector<Eigen::Triplet<float>> normalizationMatrixEntries;
    normalizationMatrixEntries.reserve(NUMBER_OF_REGIONS);
    for(int k = 0; k < transM.outerSize(); ++k) {
        float degree = 0.0f;
        for(Eigen::SparseMatrix<float, Eigen::RowMajor>::InnerIterator it(transM, k); it; ++it) {
            degree += it.value();
        }
        normalizationMatrixEntries.push_back(Eigen::Triplet<float>(k, k, 1 / degree));
    }
    normalizationMatrix.setFromTriplets(normalizationMatrixEntries.begin(), normalizationMatrixEntries.end());

    std::cout << "Created normalization matrix" << '\n';

    // TODO: measure efficiency difference between computing normalization matrix and multiplying transM by it (as it is now),
    // and looping through transM a second time and directly modifying the value through it.valueRef()
    this->transitionMatrix = normalizationMatrix * transM;

}

/**
 * Get matrix of descriptors. This basically concatenates the already computed descriptors of each region into a single matrix to be used for
 * the nearest neighbour search.
 */
void ConsensusVoter::computeDescriptors() {
    this->descriptors.release();

    std::vector<cv::Mat> descriptorsVector;
    descriptorsVector.reserve(this->superpixels.size());
    for(unsigned int r = 0; r < this->superpixels.size(); r++) {
        // cv::Mat singleRegionDescriptor = this->superpixels[r].getDescriptor();
        cv::Mat singleRegionDescriptor = this->superpixels[r].getDescriptor();
        descriptorsVector.push_back(singleRegionDescriptor);
    }
    cv::vconcat(descriptorsVector, this->descriptors);
}

/**
 * Update the votes by multiplying them by the transition matrix.
 * @param iterations The number of times the votes are to be multiplied by the matrix.
 */
void ConsensusVoter::reachConsensus(int iterations) {
    const int NUMBER_OF_FRAMES = static_cast<int>(this->frameBeginningIndex.size());
    const int NUMBER_OF_VOTES = static_cast<int>(this->votes.size());

    Eigen::Map<Eigen::VectorXf> updatedVotes(this->votes.data(), NUMBER_OF_VOTES);

    for(int t = 0; t < iterations; t++) {
        updatedVotes = transitionMatrix * updatedVotes;

        for(int f = 0; f < NUMBER_OF_FRAMES; f++) {
            const int superpixelsInFrame = (f == (NUMBER_OF_FRAMES - 1)) ? (NUMBER_OF_VOTES - frameBeginningIndex[f]) : (frameBeginningIndex[f + 1] - frameBeginningIndex[f]);
            const float maxVoteInFrame = updatedVotes.segment(frameBeginningIndex[f], superpixelsInFrame).maxCoeff();
            updatedVotes.segment(frameBeginningIndex[f], superpixelsInFrame) /= maxVoteInFrame + (maxVoteInFrame <= 0);
        }
    }
}

/**
 * Threshold the existing votes and get a sequence of masks from them.
 * @param masks The output vector of masks.
 * @param threshold The threshold value.
 * @param smallBlobsThreshold Relative size to the biggest object below which small non-connected blobs should be removed.
 */
void ConsensusVoter::getSegmentation(std::vector<cv::Mat> &masks, float threshold, float smallBlobsThreshold) {
    const int NUMBER_OF_FRAMES = static_cast<int>(this->frameBeginningIndex.size());
    const int NUMBER_OF_REGIONS = static_cast<int>(this->superpixels.size());
    masks.clear();
    masks.reserve(NUMBER_OF_FRAMES);
    for(int f = 0; f < NUMBER_OF_FRAMES; f++) {
        const int spBegin = this->frameBeginningIndex[f];
        const int spEnd = (f == (NUMBER_OF_FRAMES - 1)) ? (NUMBER_OF_REGIONS - 1) : (this->frameBeginningIndex[f + 1] - 1);
        // cv::Mat framePixelLabels = this->superpixels[spBegin].getFrameLabels();
        cv::Mat framePixelLabels = this->superpixels[spBegin].getFrameLabels();

        // Set the values of the mask (before thresholding) by superpixel: each pixel of a superpixel is attributed the same
        // value as the vote for the region
        cv::Mat maskNotThresholded(framePixelLabels.size(), CV_32FC1);
        for(int sp = spBegin; sp <= spEnd; sp++) {
            const int spLabelInFrame = sp - spBegin;
            cv::Mat spLocation(framePixelLabels == spLabelInFrame);
            maskNotThresholded.setTo(this->votes[sp], spLocation);
        }

        // The segmented images tend to have small non-connected blobs that are similar in color to the foreground,
        // but that are really background. This function eliminates the blobs that are much smaller than the biggest object
        // (that is assumed to be the real foreground)
        correctVotesForSmallBlobs(maskNotThresholded, spBegin, spEnd, framePixelLabels, threshold, smallBlobsThreshold);
        cv::Mat mask(maskNotThresholded > threshold);
        masks.push_back(mask);
    }
}

/**
 * Eliminate connected components of a mask whose size with respect to the biggest connected component is much smaller.
 * Then, correct the votes for the involved superpixels by setting them below the threshold.
 * @param matrix A non-thresholded mask whose pixels contain values between 0 and 1.
 * @param spBegin The index of the first superpixel of the frame the matrix corresponds to.
 * @param spEnd The index of the last superpixel of the frame the matrix corresponds to.
 * @param regionLabels A labeling of the image that separates the different superpixels in the frame.
 * @param threshold A threshold to separate background from foreground.
 * @param relativeSize The minimum relative size the blobs should at least have with respect to the biggest object in order no to be removed.
 */
void ConsensusVoter::correctVotesForSmallBlobs(cv::Mat &matrix, int spBegin, int spEnd, const cv::Mat &regionLabels, float threshold, float relativeSize) {
    tfg::removeSmallBlobs(matrix, threshold, relativeSize);

    // Correct values in votes vector
    for(int s = spBegin; s <= spEnd; s++) {
        const int superpixelLabel = s - spBegin;
        cv::Mat region(regionLabels == superpixelLabel);
        cv::Scalar regionMean = cv::mean(matrix, region);
        const float correctedVote = regionMean(0);
        this->votes[s] = correctedVote;
    }
}

} // namespace tfg


#ifndef STAN_MATH_PRIM_MAT_FUN_ELT_MULTIPLY_HPP
#define STAN_MATH_PRIM_MAT_FUN_ELT_MULTIPLY_HPP

#include <boost/math/tools/promotion.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/err/check_matching_dims.hpp>

namespace stan {
namespace math {

/**
 * Return the elementwise multiplication of the specified
 * matrices.
 *
 * @tparam T1 Type of scalars in first matrix.
 * @tparam T2 Type of scalars in second matrix.
 * @tparam R Row type of both matrices.
 * @tparam C Column type of both matrices.
 * @param m1 First matrix
 * @param m2 Second matrix
 * @return Elementwise product of matrices.
 */
template <typename T1, typename T2, int R, int C>
Eigen::Matrix<typename boost::math::tools::promote_args<T1, T2>::type, R, C>
elt_multiply(const Eigen::Matrix<T1, R, C>& m1,
             const Eigen::Matrix<T2, R, C>& m2) {
  check_matching_dims("elt_multiply", "m1", m1, "m2", m2);
  Eigen::Matrix<typename boost::math::tools::promote_args<T1, T2>::type, R, C>
      result(m1.rows(), m2.cols());
  for (int i = 0; i < m1.size(); ++i)
    result(i) = m1(i) * m2(i);
  return result;
}

}  // namespace math
}  // namespace stan
#endif


#include <math.h>
#include <fstream>
#include <iostream>
#include <iomanip>
#include <random>
#include <sstream>
#include <fftw3.h>
#include "parameters.hpp"
#include "utilities.hpp"
#include <boost/filesystem.hpp>
#include "equations.hpp"

//--------------------------------
// Directory & File Management
//--------------------------------

namespace fs = boost::filesystem;

void dir_manage(const std::string exist_dir, const std::string new_dir )
{

    //Path of the existing data directory
    const fs::path existpath("../" + exist_dir );

    //Tries to remove all files in there. If it fails, it throws an error.
    try {
        fs::remove_all(existpath);
    }
    catch (fs::filesystem_error& ex) {
        std::cout << ex.what() << std::endl;
        throw;
    }

    //Path of the newly set directory
    const fs::path newpath("../" + new_dir );

    //Tries to create a directory. If it fails, it throws an error.
    boost::system::error_code error;
    const bool result = fs::create_directory(newpath, error);
    if (!result || error) {
        std::cout << "failed to create directory" << std::endl;
    }

}

void file_manage(const std::string exist_file)
{
    //Path of the existing status file
    const fs::path statuspath("../" + exist_file);

    //Tries to remove the file. If it fails, it throws an error.
    try {
        fs::remove(statuspath);
    }
    catch (fs::filesystem_error& ex) {
        std::cout << ex.what() << std::endl;
        throw;
    }

}

//--------------------------------
// Double Inflation Subroutines
//--------------------------------

//subroutine for zeromode output
void zeromode_output(const std::string file, Vec_I_DP &xx, Mat_I_DP &yp, int timecount){
    static int output_timecount = 0; ++output_timecount;
    std::stringstream ss;
    std::ofstream zeromode_output;
    ss << "../" << file;

    if( output_timecount == 1 )
    {
        zeromode_output.open(ss.str().c_str(),std::ios::out);
    }else{
        zeromode_output.open(ss.str().c_str(),std::ios::app);
    }

    DP H,la,rho,rhop,w,a;
    Vec_DP tr(N_zero);
    int i,j;

    for (j=0;j<timecount;j++) {
    for (i=0;i<N_zero;i++) tr[i]=yp[i][j];
    la=xx[j];
    rho=rho_tot(tr[0],tr[1],tr[2],tr[3],tr[4],tr[5],tr[6]);
    rhop=rhoandp(tr[3],tr[4],tr[5],tr[6]);
    H=Fri(tr[0],tr[1],tr[2],tr[3],tr[4],tr[5],tr[6]);
    w=log10(H);
    a=exp(la);
    //output data
    zeromode_output << std::setw(6) << (la-Ini)*msigma/H << " " //t
    << std::setw(10) << la << " " //log(a)
    << std::setw(10) << tr[0] << " "  //buffer for yp_p[i][0] sigma
    << std::setw(20) << std::setprecision(20) << tr[1] << " " //buffer for yp_p[i][1] psi
    << std::setw(10) << tr[2] << " " //buffer for yp_p[i][1] phi
    << std::setw(10) << w << " "     //log10(H)
    << std::setw(10) << rho << " "
    << std::setw(10) << V_11(tr[0],tr[1],tr[2]) << " "
    << std::setw(10) << V_12(tr[0],tr[1],tr[2]) << " "
    << std::setw(10) << V_13(tr[0],tr[1],tr[2]) << " "
    << std::setw(10) << V_22(tr[0],tr[1],tr[2]) << " "
    << std::setw(10) << V_23(tr[0],tr[1],tr[2]) << " "
    << std::setw(10) << V_33(tr[0],tr[1],tr[2]) << " ";
        for(int loop = 0; loop < knum_zero.size(); loop++ ){
            if(loop ==  knum_zero.size()-1){
    zeromode_output << std::setw(10) << log10(UC::knum_to_kMPl(knum_zero[loop])/(a*H)) << "\n";
            }else{
                zeromode_output << std::setw(10) << log10(UC::knum_to_kMPl(knum_zero[loop])/(a*H)) << " ";
            };
        };

    };
}

//subroutine for k-analyze output
void kanalyze_output(const std::string dir, std::string file, Vec_I_DP &xx, Mat_I_DP &yp, int timecount, int knum, const DP k_comoving ){
    //output results
    static int output_timecount = 0;
    static int knum_static = knum;
    if(knum_static == knum){
        ++output_timecount;
    }else{
        knum_static = knum;
        output_timecount = 1;
    };

//    std::cout << knum_static << "\n";
//    std::cout << knum << "\n";
//    std::cout << output_timecount << "\n";

    std::stringstream ss;
    std::ofstream k_output;

    ss << "../" << dir << "/" << file << "_" << std::setw(4) << std::setfill('0') << knum <<".txt";

    if( output_timecount == 1 )
    {
        k_output.open(ss.str().c_str(),std::ios::out);
    }else{
        k_output.open(ss.str().c_str(),std::ios::app);
    }

    DP H,la,rho,rhop,w,a,Pzeta,Pzeta_raw,PPot,P_sigma,P_psi,P_phi;

    Vec_DP zeta(6);
    Vec_DP tr(N_pert);
    int i,j;
    for (j=0;j<timecount;j++) {
        for (i=0;i<N_pert;i++) tr[i]=yp[i][j];
        la=xx[j];
//        if(j==0){std::cout << la << "\n"; };
        rho=rho_tot(tr[0],tr[1],tr[2],tr[3],tr[4],tr[5],tr[6]);
        rhop=rhoandp(tr[3],tr[4],tr[5],tr[6]);
        H=Fri(tr[0],tr[1],tr[2],tr[3],tr[4],tr[5],tr[6]);
        w=log10(H);
        a=exp(la);
        for (i=0;i<3;i++) zeta[i]=2*rho*(tr[i+25] + tr[i+28]/H)/(rhop) + (1 + 2*k_comoving*k_comoving*rho/(9*a*a*H*H*rhop))*3*tr[i+25];
        for (i=0;i<3;i++) zeta[i+3]=2*rho*(tr[i+49] + tr[i+52]/H)/(rhop) + (1 + 2*k_comoving*k_comoving*rho/(9*a*a*H*H*rhop))*3*tr[i+49];

        Pzeta=0;
        for (i=0;i<6;i++) Pzeta = Pzeta + zeta[i]*zeta[i];
        Pzeta_raw = Pzeta;
        Pzeta = Pzeta/(2*M_PI*M_PI*9);
        Pzeta = log10(Pzeta) + 3*log10(k_comoving);

        PPot = 0;
        for (i=0;i<3;i++) PPot = PPot + tr[i+25]*tr[i+25];
        for (i=0;i<3;i++) PPot = PPot + tr[i+49]*tr[i+49];
        PPot = PPot/(2*M_PI*M_PI);
        PPot = log10(PPot) + 3*log10(k_comoving);

        P_sigma = 0;
        for (i=0;i<3;i++) P_sigma = P_sigma + tr[i+7]*tr[i+7];
        for (i=0;i<3;i++) P_sigma = P_sigma + tr[i+31]*tr[i+31];
        P_sigma = P_sigma/(2*M_PI*M_PI);
        P_sigma = log10(P_sigma) + 3*log10(k_comoving);

        P_psi = 0;
        for (i=0;i<3;i++) P_psi = P_psi + tr[i+10]*tr[i+10];
        for (i=0;i<3;i++) P_psi = P_psi + tr[i+34]*tr[i+34];
        P_psi = P_psi/(2*M_PI*M_PI);
        P_psi = log10(P_psi) + 3*log10(k_comoving);

        P_phi = 0;
        for (i=0;i<3;i++) P_phi = P_phi + tr[i+13]*tr[i+13];
        for (i=0;i<3;i++) P_phi = P_phi + tr[i+37]*tr[i+37];
        P_phi = P_phi/(2*M_PI*M_PI);
        P_phi = log10(P_phi) + 3*log10(k_comoving);

//        if(j==0){std::cout << la << "\n"; };
        k_output << std::setw(6) << la << " "               //log(a)
        << std::setw(10) << w << " "                        //log(H)
        << std::setw(10) << Pzeta << " "                    //log(Zeta)
        << std::setw(10) << PPot << " "                    //log(Gravitational Potential)
        << std::setw(10) << log10(rhop/rho) << " "        //log(p/rho)
        << std::setw(10) << log10(H/a) << " "            //log(H/a)
        << std::setw(10) << log10(k_comoving/(a*H)) << " "        //log(k/(a*H))
        << std::setw(10) << P_sigma << " "               //log(P_sigma)
        << std::setw(10) << P_psi << " "                 //log(P_psi)
        << std::setw(10) << P_phi << " "                 //log(P_phi)
        << std::setw(10) << zeta[0]*zeta[0]/Pzeta_raw << " "                 //
        << std::setw(10) << zeta[1]*zeta[1]/Pzeta_raw << " "                 //
        << std::setw(10) << zeta[2]*zeta[2]/Pzeta_raw << " "                 //
        << std::setw(10) << zeta[3]*zeta[3]/Pzeta_raw << " "                 //
        << std::setw(10) << zeta[4]*zeta[4]/Pzeta_raw << " "                 //
        << std::setw(10) << zeta[5]*zeta[5]/Pzeta_raw << " "                 //
        << "\n";
    };

}

//subroutine for spectrum output before oscillation period
void spectrum_bfosc_output(const std::string file, Vec_I_DP &xx, Mat_I_DP &yp, int timecount, int knum, const DP k_comoving){
    static int output_timecount = 0; ++output_timecount;
    std::stringstream ss;
    std::ofstream sp_output;
    ss << "../" << file;

    if( output_timecount == 1 )
    {
        sp_output.open(ss.str().c_str(),std::ios::out);
    }else{
        sp_output.open(ss.str().c_str(),std::ios::app);
    }

    DP H,a,la,rho,rhop,Pzeta,Pzeta_raw,PPot,P_sigma,P_psi,P_phi,PSTR;

    Vec_DP zeta(6);
    Vec_DP tr(N_pert);

    int i;

    la=xx[timecount-1];
    a=exp(la);

    for (i=0;i<N_pert;i++) tr[i] = yp[i][timecount-1];
    rho=rho_tot(tr[0],tr[1],tr[2],tr[3],tr[4],tr[5],tr[6]);
    rhop=rhoandp(tr[3],tr[4],tr[5],tr[6]);
    H=Fri(tr[0],tr[1],tr[2],tr[3],tr[4],tr[5],tr[6]);

    for (i=0;i<3;i++) zeta[i]=2*rho*(tr[i+25] + tr[i+28]/H)/(rhop) + (1 + 2*k_comoving*k_comoving*rho/(9*a*a*H*H*rhop))*3*tr[i+25];
    for (i=0;i<3;i++) zeta[i+3]=2*rho*(tr[i+49] + tr[i+52]/H)/(rhop) + (1 + 2*k_comoving*k_comoving*rho/(9*a*a*H*H*rhop))*3*tr[i+49];
    Pzeta=0;
    for (i=0;i<6;i++) Pzeta = Pzeta + zeta[i]*zeta[i];
    Pzeta = Pzeta/(2*M_PI*M_PI*9);
    Pzeta = log10(Pzeta) + 3*log10(k_comoving);
    PPot = 0;
    for (i=0;i<3;i++) PPot = PPot + tr[i+25]*tr[i+25];
    for (i=0;i<3;i++) PPot = PPot + tr[i+49]*tr[i+49];
    PSTR = PPot;
    PPot = PPot/(2*M_PI*M_PI);
    PPot = log10(PPot) + 3*log10(k_comoving);
    //output log(Gravitational Potential) and log(Zeta), along with k and knum.
    sp_output << std::setprecision (10) << std::setw(10) << k_comoving << " " << std::setw(5) << knum << " " << std::setw(10) << UC::knum_to_kMpc(knum) << " " << std::setw(10) << PPot << " " << std::setw(10) << Pzeta <<  " " << std::setw(10) << PSTR*sqrt(k_comoving*k_comoving*k_comoving) << "\n" << std::flush;
}

//subroutine for spectrum output
void spectrum_output(const std::string file, Vec_I_DP &xx, Mat_I_DP &yp, int timecount, int knum, const DP k_comoving){
    static int output_timecount = 0; ++output_timecount;
    std::stringstream ss;
    std::ofstream sp_output;
    ss << "../" << file;

    if( output_timecount == 1 )
    {
        sp_output.open(ss.str().c_str(),std::ios::out);
    }else{
        sp_output.open(ss.str().c_str(),std::ios::app);
    }

    DP H,a,la,rho,rhop,Pzeta,Pzeta_raw,PPot,P_sigma,P_psi,P_phi,PSTR;

    Vec_DP zeta(6);
    Vec_DP tr(N_pert);
    int i;

    la=xx[timecount-1];
    a=exp(la);

    for (i=0;i<N_pert;i++) tr[i] = yp[i][timecount-1];

    rho=rho_tot(tr[0],tr[1],tr[2],tr[3],tr[4],tr[5],tr[6]);
    rhop=rhoandp(tr[3],tr[4],tr[5],tr[6]);
    H=Fri(tr[0],tr[1],tr[2],tr[3],tr[4],tr[5],tr[6]);

    for (i=0;i<3;i++) zeta[i]=2*rho*(tr[i+25] + tr[i+28]/H)/(rhop) + (1 + 2*k_comoving*k_comoving*rho/(9*a*a*H*H*rhop))*3*tr[i+25];
    for (i=0;i<3;i++) zeta[i+3]=2*rho*(tr[i+49] + tr[i+52]/H)/(rhop) + (1 + 2*k_comoving*k_comoving*rho/(9*a*a*H*H*rhop))*3*tr[i+49];
    Pzeta=0;
    for (i=0;i<6;i++) Pzeta = Pzeta + zeta[i]*zeta[i];
    Pzeta = Pzeta/(2*M_PI*M_PI*9);
    Pzeta = log10(Pzeta) + 3*log10(k_comoving);
    PPot = 0;
    for (i=0;i<3;i++) PPot = PPot + tr[i+25]*tr[i+25];
    for (i=0;i<3;i++) PPot = PPot + tr[i+49]*tr[i+49];
    PSTR = PPot;
    PPot = PPot/(2*M_PI*M_PI);
    PPot = log10(PPot) + 3*log10(k_comoving);
    //output log(Gravitational Potential) and log(Zeta), along with k and knum.
    sp_output << std::setprecision (10) << std::setw(10) << k_comoving << " " << std::setw(5) << knum << " " << std::setw(10) << UC::knum_to_kMpc(knum) << " " << std::setw(10) << PPot << " " << std::setw(10) << Pzeta <<  " " << std::setw(10) << PSTR*sqrt(k_comoving*k_comoving*k_comoving) << "\n" << std::flush;
}
//--------------------------------
// Lattice Simulation Subroutines
//--------------------------------

double rand_uniform(void)
{
    std::random_device rnd;
    std::mt19937 mt( rnd() );
    std::uniform_real_distribution<> rand( 0, 1 );
 //   std::cout << rand(mt) << "\n";
    return (rand(mt));
}

void set_mode(double p2, double m2, double *field, double *deriv, int real)
{

    double phase, amplitude, rms_amplitude, omega;
    double re_f_left, im_f_left, re_f_right, im_f_right;
#if  dim==1
    static double norm = rescale_A*rescale_B*pow(L/(dx*dx),.5)/sqrt(4*M_PI);
#elif  dim==2
    static double norm =  rescale_A*rescale_B*pow(L/(dx*dx),1)/(sqrt(2*M_PI));
#elif  dim==3
    static double norm =  rescale_A*rescale_B*pow(L/(dx*dx),1.5)/sqrt(2);
#endif
    static int tachyonic = 0; //Avoid printing the same error repeatedly

    if(p2+m2>0)
        omega=sqrt(p2+m2);
    else
    {
        if(tachyonic==0)
            std::cout << "Warning: Tachyonic mode(s) may be initialized inaccurately" << std::endl;
        omega=sqrt(p2);
        tachyonic=1;
    }

    if(omega>0.)
        rms_amplitude=norm/sqrt(omega)*pow(p2,.75-(double)dim/4.);
    else
        rms_amplitude=0.;

        //Amplitude = RMS amplitude x Rayleigh distributed random number
        // The same amplitude is used for left and right moving waves to generate standing waves. The extra 1/sqrt(2) normalizes the initial occupation number correctly.

    amplitude = rms_amplitude/sqrt(2.)*sqrt(log(1./rand_uniform()));
    phase = 2*M_PI*rand_uniform();
  // std::cout << "phase1 " << phase/(2*M_PI) << std::endl;
        //Left moving component
        re_f_left = amplitude * cos( phase );
        im_f_left = amplitude * sin( phase );
        //Right moving component
    phase = 2*M_PI*rand_uniform();
  //  std::cout << "phase2 " << phase/(2*M_PI) << std::endl;
        re_f_right = amplitude * cos( phase );
        im_f_right = amplitude * sin( phase );

    field[0] = re_f_left + re_f_right;
    field[1] = im_f_left + im_f_right;

    deriv[0] = omega*(im_f_left - im_f_right);
    deriv[1] = -omega*(re_f_left - re_f_right);

    if(real==1)
    {
        field[1]=0;
        deriv[1]=0;
    }
    return;
   }

void DFT_c2rD1( double* f)
{
    fftw_plan p;
    double* out;
    fftw_complex *in;
    size_t in_size;

    in_size = sizeof(fftw_complex) * (N/2+1);
    in  = (fftw_complex*)fftw_malloc( in_size );
    out = new double [N]();
    p = fftw_plan_dft_c2r_1d( N, in, out, FFTW_ESTIMATE );

        for( int j = 0; j < N/2+1; ++j ){
            int idx = j;
            if(idx==0)
            {
                in[idx][0] = f[idx]  ;
                in[idx][1] = 0  ;

            }else if(idx==N/2){
                in[idx][0] = f[1]  ;
                in[idx][1] = 0  ;
            }else{
                in[idx][0] = f[2*idx]  ;
                in[idx][1] = f[2*idx+1]  ;
            }

        }

    fftw_execute(p);

    // Set output data
    for( int j = 0; j < N; ++j ){
        int idx = j;
        f[idx] = out[idx]/N;
    }

    if( p ) fftw_destroy_plan(p);
    if( in ) fftw_free(in);
    delete[] out;
}
/*
void DFT_c2rD2d( double* df,double* fdnyquist )
{
    fftw_plan p;
    double* out;
    fftw_complex *in;
    size_t in_size;

    in_size = sizeof(fftw_complex) * N * (N/2+1);
    in  = (fftw_complex*)fftw_malloc( in_size );
    out = new double [N*N]();
    p = fftw_plan_dft_c2r_2d( N, N, in, out, FFTW_ESTIMATE );

    for( int j = 0; j < N; ++j ){
        //#pragma omp parallel for schedule( static ) num_threads( num_threads )
        for( int k = 0; k < N/2+1; ++k ){
            int idx = (N/2+1)*j + k;
            // int idx = j*N + k;
            if(k == N/2){
                in[idx][0] = (double)fdnyquist[2*j] ;
                in[idx][1] = (double)fdnyquist[2*j+1] ;
               // std::cout << "in[" << idx << "][0] = " << in[idx][0] << std::endl;
               // std::cout << "in[" << idx << "][1] = " << in[idx][1] << std::endl;
            }else {
                in[idx][0] =  (double)df[j*N + 2*k] ;
                in[idx][1] = (double)df[j*N + 2*k+1] ;
               // std::cout << "in[" << idx << "][0] = " << in[idx][0] << std::endl;
               // std::cout << "in[" << idx << "][1] = " << in[idx][1] << std::endl;
            }
        }
    }
    fftw_execute(p);

    // Set output data

    for( int j = 0; j < N; ++j ){
        //#pragma omp parallel for schedule( static ) num_threads( num_threads )
        for( int k = 0; k < N; ++k ){
            int idx = j*N + k;
            std::cout << "out[" << idx << "] = " << out[idx] << std::endl;
            df[idx] = out[idx]/(N*N);
        }
    }

    if( p ) fftw_destroy_plan(p);
    if( in ) fftw_free(in);
    delete[] out;
}
*/
void DFT_c2rD2( double* f,double* fnyquist )
{
    fftw_plan p;
    double* out;
    fftw_complex *in;
    size_t in_size;

    in_size = sizeof(fftw_complex) * N * (N/2+1);
    in  = (fftw_complex*)fftw_malloc( in_size );
    out = new double [N*N]();
    p = fftw_plan_dft_c2r_2d( N, N, in, out, FFTW_ESTIMATE );

        for( int j = 0; j < N; ++j ){
            for( int k = 0; k < N/2+1; ++k ){
                int idx = (N/2+1)*j + k;
              //  int idx = j*N + k;
                if(k == N/2){
                    in[idx][0] =  fnyquist[2*j] ;
                    in[idx][1] =  fnyquist[2*j+1] ;
                }else {
                    in[idx][0] =  f[j*N + 2*k];
                    in[idx][1] =  f[j*N + 2*k+1] ;
                }
            }
        }
        fftw_execute(p);

        // Set output data

     for( int j = 0; j < N; ++j ){
        for( int k = 0; k < N; ++k ){
            int idx = j*N + k;
            f[idx] = out[idx]/(N*N);
        }
     }

    if( p ) fftw_destroy_plan(p);
    if( in ) fftw_free(in);
    delete[] out;
}

void DFT_c2rD3( double* f,double** fnyquist )
{
    fftw_plan p;
    double* out;
    fftw_complex *in;
    size_t in_size;

    in_size = sizeof(fftw_complex) * N * N * (N/2+1);
    in  = (fftw_complex*)fftw_malloc( in_size );
    out = new double [N*N*N]();
    p = fftw_plan_dft_c2r_3d( N, N, N, in, out, FFTW_ESTIMATE );

//#pragma omp parallel for simd collapse(3) schedule( static ) num_threads( num_threads )
        for( int j = 0; j < N; ++j ){
            for( int k = 0; k < N; ++k ){
                for( int l = 0; l < N/2+1; ++l ){
                    int idx = (j*N + k)*(N/2+1) + l;
                    if(l == N/2){
                        in[idx][0] = fnyquist[j][2*k] ;
                        in[idx][1] = fnyquist[j][2*k+1] ;
                    }
                    else{
                        in[idx][0] =  f[(j*N + k)*N + 2*l] ;
                        in[idx][1] =  f[(j*N + k)*N + 2*l+1] ;
                    }
                }
            }
        }
        fftw_execute(p);

        // Set output data

         for( int j = 0; j < N; ++j ){
        for( int k = 0; k < N; ++k ){
            for( int l = 0; l < N; ++l ){
                int idx = (j*N + k)*N + l;
                f[idx] = out[idx]/(N*N*N);
            }
        }
    }

    if( p ) fftw_destroy_plan(p);
    if( in ) fftw_free(in);
    delete[] out;
}

/*
void DFT_c2r( double** f,double* fnyquist )
{
	  fftw_plan p;
		double* out;
    fftw_complex *in;
		size_t in_size;

		switch( dim )
		{
			case 1:
				in_size = sizeof(fftw_complex) * (N/2+1);
				in  = (fftw_complex*)fftw_malloc( in_size );
				out = new double [N]();
				p = fftw_plan_dft_c2r_1d( N, in, out, FFTW_ESTIMATE );
				break;
			case 2:
				in_size = sizeof(fftw_complex) * N * (N/2+1);
				in  = (fftw_complex*)fftw_malloc( in_size );
				out = new double [N*N]();
				p = fftw_plan_dft_c2r_2d( N, N, in, out, FFTW_ESTIMATE );
				break;
			case 3:
				in_size = sizeof(fftw_complex) * N * N * (N/2+1);
				in  = (fftw_complex*)fftw_malloc( in_size );
				out = new double [N*N*N]();
				p = fftw_plan_dft_c2r_3d( N, N, N, in, out, FFTW_ESTIMATE );
				break;
		}

		for( int i = 0; i < num_fields; ++i )
		{

				// Create input data
				switch( dim ){
					case 1:
						#pragma omp parallel for schedule( static ) num_threads( num_threads )
						for( int j = 0; j < N/2+1; ++j ){
							int idx = j;
							if(idx==0)
                            {
                                in[idx][0] = f[i][idx]  ;
                                in[idx][1] = 0  ;

                            }else if(idx==N/2){
                                in[idx][0] = f[i][1]  ;
                                in[idx][1] = 0  ;
                            }else{
                                in[idx][0] = f[i][2*idx]  ;
                                in[idx][1] = f[i][2*idx+1]  ;
                            }

						}
						break;
					case 2:
						#pragma omp parallel for schedule( static ) num_threads( num_threads )
						for( int j = 0; j < N; ++j ){
							for( int k = 0; k < N/2+1; k=k+2 ){
									int idx = j*N + k;
                                if(k == N/2){
                                    in[idx][0] = fnyquist[2*j] ;
                                    in[idx][1] = fnyquist[2*j+1] ;
                                }else {
                                    in[idx][0] =  f[i][idx] ;
                                    in[idx][1] =  f[i][idx+1] ;
                                }
							}
						}
						break;
					case 3:
						#pragma omp parallel for schedule( static ) num_threads( num_threads )
						for( int j = 0; j < N; ++j ){
							for( int k = 0; k < N; ++k ){
									for( int l = 0; l < N/2+1; l=l+2 ){
											int idx = (j*N + k)*N + l;
                                        if(l == N/2){
                                        in[idx][0] = fnyquist[j][2*k] ;
                                        in[idx][1] = fnyquist[j][2*k+1] ;
                                        }
                                        else{
                                            in[idx][0] =  f[i][idx] ;
                                            in[idx][1] =  f[i][idx+1] ;
                                        }
                                        }
							}
						}
						break;
				}

				fftw_execute(p);

				// Set output data
        #pragma omp parallel for schedule( static ) num_threads( num_threads )
        for( int j = 0; j < N; ++j ){
            switch( dim ){
							case 1:
                int idx = j;
                f[i][idx] = out[idx]/N;
								break;
            	case 2:
                for( int k = 0; k < N; ++k ){
                    int idx = j*N + k;
                    f[i][idx] = out[idx]/(N*N);
                }
								break;
            	case 3:
                for( int k = 0; k < N; ++k ){
                    for( int l = 0; l < N; ++l ){
                        int idx = (j*N + k)*N + l;
                        f[i][idx] = out[idx]/(N*N*N);
                    }
                }
								break;
						}
        }
    }

		if( p ) fftw_destroy_plan(p);
		if( in ) fftw_free(in);
		delete[] out;
}
*/

void write_VTK_f( const std::string dir_f, double* f, std::string str, int loop )
{
   // double a = leapfrog->a();
	unsigned int size;
	std::stringstream ss;
	std::ofstream fout;

	if( dim == 1 ){
		ss << "../" << dir_f << "/" << str << "." << std::setw(4) << std::setfill('0') << loop+1 <<".txt";
    	fout.open( ss.str().c_str() );

    	for( int j = 0; j < N; j++ ){
			int idx = j;
			fout << idx*dx << " " << f[idx] << std::endl;
		}
    }else{
    	ss << "../" << dir_f << "/" << str << "." << std::setw(4) << std::setfill('0') << loop+1 <<".vti";
    	fout.open( ss.str().c_str() );

  	  	fout << "<?xml version=\"1.0\"?>" << std::endl;
  		fout << "<VTKFile type=\"ImageData\" version=\"1.0\" byte_order=\"LittleEndian\" header_type=\"UInt32\">" << std::endl <<std::endl;
    	switch( dim ){
    		case 2:
				size = sizeof(double) * pow(N, 2);//8byte*pow(N,2)
				fout << "<ImageData WholeExtent=\"0 " << N-1 << " 0 " << N-1 << " 0 " << " 0 " << "\" Origin=\"0 0 0\" Spacing=\"" << dx << " " << dx << " " << dx << "\">" << std::endl;
				fout << "<Piece Extent=\"0 " << N-1 << " 0 " << N-1 << " 0 " << " 0 " << "\">" << std::endl;
    			break;
			case 3:
				size = sizeof(double) * pow(N, 3);
				fout << "<ImageData WholeExtent=\"0 " << N-1 << " 0 " << N-1 <<" 0 " << N-1 << "\" Origin=\"0 0 0\" Spacing=\"" << dx << " " << dx << " " << dx << "\">" << std::endl;
				fout << "<Piece Extent=\"0 " << N-1 << " 0 " << N-1 << " 0 " << N-1 << "\">" << std::endl;
				break;
    	}
    	fout << "<PointData Scalars=\"field\">" << std::endl;
    	fout << "<DataArray type=\"Float64\" Name=\"field\" format=\"appended\" offset=\"0\" />" << std::endl;
    	fout << "</PointData>" << std::endl;
	    fout << "<CellData>" << std::endl;
	    fout << "</CellData>" << std::endl;
	    fout << "</Piece>" << std::endl << std::endl;

	    fout << "</ImageData>" << std::endl << std::endl;
	    fout << "<AppendedData encoding=\"raw\">" << std::endl;
	    fout << "_" ;
	    fout.close();

	    fout.open( ss.str().c_str(), std::ios::binary | std::ios::app);
	    fout.write( (char*) &size, sizeof(unsigned int) );//4byte

           fout.write( (char*) f, size );
	    fout.close();
	    fout.open( ss.str().c_str(), std::ios::app);
	    fout << std::endl << "</AppendedData>" << std::endl;
		fout << "</VTKFile>" ;
	}
	fout.close();
}

void write_VTK_ed( const std::string dir_ed, double* f, std::string str, int loop )
{
  //  double a = leapfrog->a();
   // std::cout << "aaa = " <<  a << std::endl;
    unsigned int size;
    std::stringstream ss;
    std::ofstream fout;

    if( dim == 1 ){
        ss << "../" << dir_ed << "/" << str << "." << std::setw(4) << std::setfill('0') << loop+1 <<".txt";
        fout.open( ss.str().c_str() );

        for( int j = 0; j < N; j++ ){
            int idx = j;
            fout << idx*dx << " " << f[idx] << std::endl;
        }
    }else{
        ss << "../" << dir_ed << "/" << str << "." << std::setw(4) << std::setfill('0') << loop+1 <<".vti";
        fout.open( ss.str().c_str() );

        fout << "<?xml version=\"1.0\"?>" << std::endl;
        fout << "<VTKFile type=\"ImageData\" version=\"1.0\" byte_order=\"LittleEndian\" header_type=\"UInt32\">" << std::endl <<std::endl;
        switch( dim ){
            case 2:
                size = sizeof(double) * pow(N, 2);//8byte*pow(N,2)
                fout << "<ImageData WholeExtent=\"0 " << N-1 << " 0 " << N-1 << " 0 " << " 0 " << "\" Origin=\"0 0 0\" Spacing=\"" << dx << " " << dx << " " << dx << "\">" << std::endl;
                fout << "<Piece Extent=\"0 " << N-1 << " 0 " << N-1 << " 0 " << " 0 " << "\">" << std::endl;
                break;
            case 3:
                size = sizeof(double) * pow(N, 3);
                fout << "<ImageData WholeExtent=\"0 " << N-1 << " 0 " << N-1 <<" 0 " << N-1 << "\" Origin=\"0 0 0\" Spacing=\"" << dx << " " << dx << " " << dx << "\">" << std::endl;
                fout << "<Piece Extent=\"0 " << N-1 << " 0 " << N-1 << " 0 " << N-1 << "\">" << std::endl;
                break;
        }
        fout << "<PointData Scalars=\"energy\">" << std::endl;
        fout << "<DataArray type=\"Float64\" Name=\"energy density\" format=\"appended\" offset=\"0\" />" << std::endl;
        fout << "</PointData>" << std::endl;
        fout << "<CellData>" << std::endl;
        fout << "</CellData>" << std::endl;
        fout << "</Piece>" << std::endl << std::endl;

        fout << "</ImageData>" << std::endl << std::endl;
        fout << "<AppendedData encoding=\"raw\">" << std::endl;
        fout << "_" ;
        fout.close();

        fout.open( ss.str().c_str(), std::ios::binary | std::ios::app);
        fout.write( (char*) &size, sizeof(unsigned int) );//4byte

            fout.write( (char*) f, size );

        fout.close();
        fout.open( ss.str().c_str(), std::ios::app);
        fout << std::endl << "</AppendedData>" << std::endl;
        fout << "</VTKFile>" ;
    }
    fout.close();
}

void write_status( const std::string status_file, Field* field, LeapFrog* leapfrog, Energy* energy, double** f, double t )
{
	double a = leapfrog->a();
	std::ofstream ofs;

	if( t == t0 )
	{
		ofs.open( "../" + status_file, std::ios::trunc );

		ofs << std::setw(3) << std::right << "  t ";
		if( expansion ) ofs << "  a ";
		for( int i = 0; i < num_fields; ++i ) ofs << "field_ave["  << i << "] ";
		for( int i = 0; i < num_fields; ++i ) ofs << "field_var["  << i << "] ";
        for( int i = 0; i < num_fields; ++i ) ofs << "field_deriv_ave["  << i << "] ";
        for( int i = 0; i < num_fields; ++i ) ofs << "field_deriv_var["  << i << "] ";
		for( int i = 0; i < num_fields; ++i ) ofs << "energy_ave[" << i << "] ";
        for( int i = 0; i < num_fields; ++i ) ofs << "energy_var[" << i << "] ";
        ofs << "total_energy_ave ";
         ofs << "time_deriv_ave ";
         ofs << "gradient_ave ";
        ofs << "potential_ave ";
        ofs << "hubble ";
        ofs << "adotdot ";
        ofs << "energy_max" << std::endl;
	}
	else ofs.open( "../" + status_file, std::ios::app );

	ofs << std::setw(3) << std::right << t << " ";
	if( expansion )
	{
		ofs << std::setw(3) << std::right << a << " ";
		for( int i = 0; i < num_fields; ++i ) ofs << std::showpos << std::scientific << std::setprecision(4) << field->f_average(f[i], i)/a << " "; //Reduced Plank units
		for( int i = 0; i < num_fields; ++i ) ofs << std::showpos << std::scientific << std::setprecision(4) << field->f_variance(f[i], i)/a << " ";//Reduced Plank units
        for( int i = 0; i < num_fields; ++i ) ofs << std::showpos << std::scientific << std::setprecision(4) << field->df_average(f[i], i) << " ";//Programming variable
        for( int i = 0; i < num_fields; ++i ) ofs << std::showpos << std::scientific << std::setprecision(4) << field->df_variance(f[i], i) << " ";//Programming variable
	}
	else
	{
		for( int i = 0; i < num_fields; ++i ) ofs << std::showpos << std::scientific << std::setprecision(4) << field->f_average(f[i], i) << " ";//Reduced Plank units
		for( int i = 0; i < num_fields; ++i ) ofs << std::showpos << std::scientific << std::setprecision(4) << field->f_variance(f[i], i) << " ";//Reduced Plank units
        for( int i = 0; i < num_fields; ++i ) ofs << std::showpos << std::scientific << std::setprecision(4) << field->df_average(f[i], i) << " ";//Programming variable
        for( int i = 0; i < num_fields; ++i ) ofs << std::showpos << std::scientific << std::setprecision(4) << field->df_variance(f[i], i) << " ";//Programming variable
	}
	for( int i = 0; i < num_fields; ++i ) ofs << std::showpos << std::scientific << std::setprecision(4) << energy->average(i) << " ";
    for( int i = 0; i < num_fields; ++i ) ofs << std::showpos << std::scientific << std::setprecision(4) << energy->variance(i) << " ";
	ofs << std::showpos << std::scientific << std::setprecision(4) << energy->total_average() << " ";
    ofs << std::showpos << std::scientific << std::setprecision(4) << energy->timederiv_average () << " ";
     ofs << std::showpos << std::scientific << std::setprecision(4) << energy->grad_average ()  << " ";
    ofs << std::showpos << std::scientific << std::setprecision(4) << energy->potential_average () << " ";
    ofs << std::showpos << std::scientific << std::setprecision(4) << leapfrog->hubble() << " ";
    ofs << std::showpos << std::scientific << std::setprecision(4) << leapfrog->adotdot() << " ";
    ofs << std::showpos << std::scientific << std::setprecision(4) << energy->energy_max() << std::endl;

}


//     Copyright (c) 2012 Vadym Kliuchnikov sqct(dot)software(at)gmail(dot)com, Dmitri Maslov, Michele Mosca
//
//     This file is part of SQCT.
//
//     SQCT is free software: you can redistribute it and/or modify
//     it under the terms of the GNU Lesser General Public License as published by
//     the Free Software Foundation, either version 3 of the License, or
//     (at your option) any later version.
//
//     SQCT is distributed in the hope that it will be useful,
//     but WITHOUT ANY WARRANTY; without even the implied warranty of
//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//     GNU Lesser General Public License for more details.
//
//     You should have received a copy of the GNU Lesser General Public License
//     along with SQCT.  If not, see <http://www.gnu.org/licenses/>.
//

#include "sk.h"
#include "exactdecomposer.h"
#include "gcommdecomposer.h"

#include "theoremverification.h"
#include "toptimalitytest.h"
#include "hoptimalitytest.h"

#include "netgenerator.h"

#include "output.h"

#include <fstream>
#include <sstream>
#include <chrono>

#include <boost/program_options.hpp>
#include <boost/timer/timer.hpp>
#include <cmath>

// command line parcing code based on http://www.boost.org/doc/libs/1_49_0/doc/html/program_options/tutorial.html#id2499896
// and BOOST_ROOT/libs/program_options/example/first.cpp
namespace po = boost::program_options;
namespace btm = boost::timer;

using namespace std;

//////////////////////////////////////////////////////////////////////////////

/// \brief Option of SK execution
struct SKOptions
{
    SKOptions() :
            math_fr( false ), write_dot_qc(false), angle(false) {};

    string in_filename; ///< input filename
    string out_filename; ///< output statistics filename
    bool math_fr; ///< should output be mathematica friendly
    int iterations_default; ///< number of SK iterations to perform by default
    bool write_dot_qc;///< should we output dot qc or not
    string out_dir;///< path where we should output dotqc files
    bool angle;///< how we interpret first element \b val of each input line ( Pi/\b val or just \b val)
    int max_sde;///< maximal value of sde to use during basic approximation step
};

//////////////////////////////////////////////////////////////////////////////

/// \brief Result of application of the SK algorithm
struct ApplicationResult
{
    circuit c;  ///< Circuit
    int hc;     ///< Hadamard counts
    int tc;     ///< T and inverse of T gates counts
    int pc;     ///< Phase and inverse of Phase gate counts
    int plc;    ///< Pauli matrices counts
    int total;  ///< Total cost using cost function defined by vector gateLibrary::cost
    double dst; ///< Trace distance to approximation
    double tappr; ///< Time required to do an approximation
    double tdecomp; ///< Time required to do a decomposition
    int nr; ///< Number of iterations performed
    int denom_reduction; ///< Difference between \f$ sde(|\cdot|^2) \f$
                         /// before and after conversion to canonical form
    int denom ; ///< \f$ sde(|\cdot|^2) \f$ of resulting unitary
    matrix2x2<mpz_class> exact_uni; /// exact unitary corresponding to the circuit c

    /// \brief Summarize gate statistics for Clifford + T library
    void updateForCliffordT()
    {
        typedef gateLibrary gl;
        total = c.cost();
        auto counts = gl::toCliffordT( c.count() );
        hc = counts[ gl::H ];
        tc = counts[ gl::T ];
        pc = counts[ gl::P ];
        plc =  counts[ gl::Z ] + counts[ gl::X ] + counts[ gl::Y ];
    }

    /// \brief Outputs results in CSV format
    void toCSVStream( std::ostream& out ) const
    {
        out.precision(5);
        out.setf( ios_base::scientific );
        out << nr << ","
            << tc << ","
            << hc << ","
            << pc << ","
            << plc << ","
            << dst << ","
            << tappr << ","
            << tdecomp << "," << denom_reduction << "," << denom ;
    }

    /// \brief Outputs results in Mathematica friendly format
    void toMathematicaStream( std::ostream& out ) const
    {
        out.precision(10);
        out.setf( ios_base::fixed ); // use fixed as mathematica does not understand scientific c++ format
        out << "AppResult["
            << nr << ","
            << tc << ","
            << hc << ","
            << pc << ","
            << plc << ",";
        out.precision(60);
        out << dst << "," ;
        out.precision(10);
        out << tappr << ","
            << tdecomp << "," << denom_reduction << "," << denom << "]";
    }

    /// \brief Outputs dot qc file with circuit and adds information about genration process into comments
    void generateDotQc( std::string filename, std::string circuit_name, std::string symbolic_form )
    {
        ofstream ofs(filename.c_str());
        if( !ofs )
        {
            cout << "Unable to open file:" << filename << " for output" <<endl;
            return;
        }

        ofs << "# Computed by SQCT, based on arXiv:1206.5236" << endl;
        ofs << "# Published at: http://code.google.com/p/sqct/" << endl;
        ofs << "# Symbolic form of unitary to approximate:" << symbolic_form << endl;
        ofs << "# Distance between unitary and approximation:" << dst << endl;
        ofs << "# (Formula (1) in http://arxiv.org/abs/quant-ph/0411206)" << endl;
        ofs << "# Total number of gates:" << tc + hc + pc + plc << endl;
        ofs << "# T and T^{Dagger} gates:" << tc << endl;
        ofs << "# Hadamard gates:" << hc << endl;
        ofs << "# Phase and Phase^{Dagger} gates:" << pc << endl;
        ofs << "# Pauli gates:" << plc << endl;
        ofs << "# Total cost: " << total << endl;
        ofs << "# Number of iterations:" << nr << endl;
        ofs << "# Approximation time(seconds):" << tappr << endl;
        ofs << "# Decomposition time(seconds):" << tdecomp << endl;
        ofs << "# Reduction:" << denom_reduction << endl;
        ofs << "# sde(abs(z)^2):" << denom << endl;
        ofs << "# Exact unitary:" << endl << exact_uni << endl;

        ofs << ".v 1" << endl;
        ofs << ".i 1" << endl;
        ofs << ".o 1" << endl;
        ofs << "BEGIN " << circuit_name <<"(1)" << endl;
        c.toStream(ofs);
        ofs << "END " << circuit_name << endl;
        ofs << "BEGIN" << endl;
        ofs << circuit_name << " 1" << endl;
        ofs << "END" << endl;
        ofs.close();
    }

};

///////////////////////////////////////////////////////////////////

/// \brief Process input file with inputs for SK in batch mode, outputs statistics etc
class SKApplication
{
public:
    SKApplication( SKOptions& skopt ) :
        opt( skopt ), skd(skopt.max_sde + 1)
    {}

    /// \brief Finds circuit and all supplementary information for unitary
    void calculate( const matrix2x2hpr& matrix, int recursion_level, ApplicationResult& ar )
    {
        boost::timer::cpu_times ct;
        boost::timer::cpu_times ct2;

        sk::Me res;
        {
            boost::timer::auto_cpu_timer t;
            skd.decompose(matrix,res,recursion_level);
            ct = t.elapsed();
        }

        int gde_before = res.min_gde_abs2();
        res.reduce();
        int gde_after = res.min_gde_abs2();
        ar.denom_reduction = gde_before - gde_after;
        ar.denom = res.max_sde_abs2();
        ar.exact_uni = res;

        {
            boost::timer::auto_cpu_timer t;
            ed.decompose(res,ar.c);
            ct2 = t.elapsed();
        }

        sk::Ma conv(res);

        ar.dst = trace_dist(matrix,conv);
        ar.tappr = (double) ct.wall * 1e-9;
        ar.tdecomp = (double) ct2.wall * 1e-9;
        ar.nr = recursion_level;
        ar.updateForCliffordT();

    }

    /// \brief Process input file defined by opt taking into account specified options
    void process()
    {
        static const double TwoPi = 2. * hprHelpers::toMachine( hprHelpers::pi() );
        ifstream ifs( opt.in_filename.c_str() );
        ofstream ofs( opt.out_filename.c_str() );
        Rotation r;
        int iter = 0;
        ApplicationResult ar;
        int line_number = 0;
        while( true )
        {
            string line;
            getline(ifs,line);
            iter = opt.iterations_default;
            istringstream ss(line,istringstream::in);
            if( ! ifs ) break;

            double val;
            ss >> val >> r.nx >> r.ny >> r.nz >> iter;
            if( ! opt.angle )
            {
                r.num = 1.0;
                r.den = val;
            }
            else
            {
                r.num = val;
                r.den = TwoPi;
            }

            calculate( r.matrix() , iter, ar );

            stringstream fname;
            fname <<  opt.out_dir << "/";
            if( r.isSpecial() != 'N' )
                fname << r.name() << "-" << iter;
            else
                fname << opt.out_filename << "." << line_number;
            fname << ".qc" ;

            if( opt.write_dot_qc )
                ar.generateDotQc( fname.str() ,
                               r.name(),
                               r.symbolic() );

            if( opt.math_fr )
            {
                ofs << "{" << r.Mathematica() << ",";
                ar.toMathematicaStream(ofs);
                ofs << "}" << endl;
            }
            else
            {
                ofs << r.CSV() << ",";
                ar.toCSVStream(ofs);
                ofs << endl;
            }

            line_number++;
        }
    }
private:
    const SKOptions& opt;
    sk skd;
    exactDecomposer ed;
};

///////////////////////////////////////////////////////////////////

bool print_help( const string& topic )
{
    map<string,string> hi;
    hi["in"] = "Each line of input file must have the followoing form:\n"
            "val,nx,ny,nz,[iter]\n"
            "val -- interpreted depending on the value of option --angle\n"
            "       if --angle is specified then val interpreted as rotation\n"
            "       angle phi. Otherwise rotation angle phi = 2 Pi / val.\n"
            "nx -- projection of rotation axis on, x\n"
            "ny -- projection of rotation axis on, y\n"
            "nz -- projection of rotation axis on, z\n"
            "iter -- optional parameter defining number of iterations used for approximation.\n"
            "The matrix that will be approximated is I*cos(phi/2) - \n"
            "i sin(phi/2) (nx * X + ny * Y + nz * Z). If the number of iterations is not specified,\n"
            "the default values is used. It can be set using --iterations option.\n"
            "\n"
            "Default output format is CSV and each line of it contains the following:\n"
            "num,den,nx,ny,nz,N,Tc,Hc,Pc,Plc,dst,tappr,tdecomp,denom_red,denom \n"
            "[num,den,nx,ny,nz] determines input rotation with phi = num * 2Pi / den\n"
            "N -- number of iterations of SK algorithm used"
            "Tc-- number of T gates in the resulting circuit\n"
            "Hc-- number of Hadamard gates in the circuit\n"
            "Pc-- number of Phase gates in the circuit\n"
            "Plc-- number of Pauli gates in the circuit\n"
            "dst-- distance to approximation, see \n"
            "(Formula (1) in http://arxiv.org/abs/quant-ph/0411206)\n"
            "tappr-- time required for approximation (seconds) \n"
            "tdecomp-- time required for exact decomposition (seconds)\n"
            "tdecomp-- time required for exact decomposition (seconds)\n"
            "denom_red -- change of the power of sqrt(2) in the denominator\n"
            "after conversion to canonical form.\n"
            "denom -- power of sqrt(2) in the denominator of the exact unitary in\n"
            "the canonical form. In the case of Mathematica output numbers are \n"
            "decorated with Rot and AppResult list headers.\n";
    if( hi.count(topic) )
    {
        cout << hi[topic] << endl;
        return true;
    }

    return false;
}

/////////////////////////////////////////////////////////////////////

struct ResynthOptions
{
    ResynthOptions() :
        reverse(false)
        {}
    vector<string> circuit_files;
    bool reverse;
    bool math_fr;
};

////////////////////////////////////////////////////////////////////

/// \brief Resyntheisizes circuits using exact decomposition algoritm
struct ResynthApplication
{
    ResynthApplication( const ResynthOptions& opt ) :
        ro(opt)
    {}

    void process()
    {
        for( const auto& fn : ro.circuit_files )
            process(fn);
    }

    void process( const string& filename )
    {
        const gateLibrary& gl = gateLibrary::instance();
        ifstream ifs(filename.c_str());
        ofstream ofs( (filename + ".out").c_str() );
        // read circuit first
        circuit in, out;
        in.fromStream(ifs,ro.reverse);
        ed.decompose(in,out);

        auto in_cost = in.count();
        int in_total = in.cost();
        in_cost = gl.toCliffordT(in_cost);

        auto out_cost = out.count();
        int out_total = out.cost();
        out_cost = gl.toCliffordT(out_cost);

        string cb = "# ";
        string ce = "";
        if( ro.math_fr )
            cb = "(* ",ce = " *)";

        ofs << cb << "Computed by SQCT, based on arXiv:1206.5236" << ce << endl;
        ofs << cb << "Software published at: http://code.google.com/p/sqct/"<< ce << endl;
        ofs << cb << "Gate counts for input circuit [T+Td,H,P+Pd,X,Z,Y] : ["
            << in_cost[gl.T] << "," << in_cost[gl.H] << ","
            << in_cost[gl.P] << "," << in_cost[gl.X] << ","
            << in_cost[gl.Z] << "," << in_cost[gl.Y] << "]" << ce << endl;
        ofs << cb << "Total cost of input:" << in_total << ce << endl;

        ofs << cb << "Gate counts for output circuit [T+Td,H,P+Pd,X,Z,Y] : ["
            << out_cost[gl.T] << "," << out_cost[gl.H] << ","
            << out_cost[gl.P] << "," << out_cost[gl.X] << ","
            << out_cost[gl.Z] << "," << out_cost[gl.Y] << "]" << ce << endl;
        ofs << cb << "Total cost of output:" << out_total << ce << endl;

        if( ro.math_fr )
            out.toMathStream(ofs);
        else
            out.toStreamSym(ofs);

        ofs.close();
        ifs.close();
    }

    exactDecomposer ed;
    const ResynthOptions& ro;
};

////////////////////////////////////////////////////////////////////

/// \brief Options for epsilon net generation
struct enetOptions
{
    /// \brief If one element specified -- upper bound for sde of epsilon net to be
    /// generated. If two elements specified -- interval of sde to be generated.
    vector<int> epsilon_net_layers;
};

////////////////////////////////////////////////////////////////////

struct enetApplication
{
    enetApplication( const enetOptions& options ) :
        m_options(options)
    {}

    /// \brief Checks if all layers generated on initial state are available
    void check_initial()
    {
        initial_ok = true;
        if( m_layers[0] == 0 )
            initial_ok = false;

        for( int i = 2; (i < initial_end) && initial_ok ; ++i )
            if( m_layers[i] == 0 ) initial_ok = false;
    }

    /// \brief Collects information about parts of epsilon net available
    void check_files()
    {
         m_layers.resize(100,0);
         // collect information about available layers
         for( int i = 0; i < 100; ++i )
         {
             string name = netGenerator::fileName(i);
             ifstream ifs(name);
             if( !ifs )
                 m_layers[i] = 0;
             else
                 m_layers[i] = 1;
         }
    }

    /// \brief Generates all layers with \f$ sde(|\cdot|^2) \f$ in the interval [st,end)
    void generate( int st, int end )
    {
        if( ! initial_ok && st < initial_end )
            m_ng.generateInitial();

        for( int i = max(initial_end,st) ; i < end; ++i )
        {
            if( m_layers[i] == 0 )
            {
                epsilonnet base_net;
                base_net.loadFromFile( netGenerator::fileName(i-1).c_str() );
                unique_ptr<epsilonnet> res( netGenerator::generate( base_net ) );
                res->saveToFile( netGenerator::fileName(i).c_str() );
            }
        }
    }

    /// \brief Do the work
    void process()
    {
        check_files();
        check_initial();

        int sz  = m_options.epsilon_net_layers.size();
        int b = m_options.epsilon_net_layers[0];
        switch( sz )
        {
        case 1:
            generate(0,b + 1);
            break;
        case 2:
            generate(b,m_options.epsilon_net_layers[1] + 1);
            break;
        default:
            std::cerr << "Wrong number of parameters. See help." << endl;
        }
    }

    void print_error_message()
    {
        cout << "Epsilon net files a not available, use --epsilon-net option" << endl
             << "to generate them." << endl;
    }

    bool check()
    {
        int st  = 0;
        int end = m_options.epsilon_net_layers[0] + 1;
        check_files();
        check_initial();

        if( ! initial_ok && st < initial_end )
        {
            print_error_message();
            return false;
        }

        for( int i = max(initial_end,st) ; i < end; ++i )
        {
            if( m_layers[i] == 0 )
            {
                print_error_message();
                return false;
            }
        }
        return true;
    }

    const enetOptions& m_options;       ///< Application options
    std::vector<int> m_layers;          ///< Ones for available layers, zeros for not availible layers
    static const int initial_end;       ///< Maximal layer generated on initial state
    bool initial_ok;                    ///< If all initial layers are availible
    netGenerator m_ng;                  ///< Epsilon net generator
};

const int enetApplication::initial_end = 21;

////////////////////////////////////////////////////////////////////

struct RandomRotationsOpts
{
    string filename;
    int samples;
};

////////////////////////////////////////////////////////////////////

struct RotationGeneratorApp
{
    RotationGeneratorApp( RandomRotationsOpts& opts) :
        m_opt(opts)
    {}
    /// \brief Generates file with rotations by random angle around random axis
    void process()
    {
        ofstream ofs( m_opt.filename.c_str() );
        unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
        std::default_random_engine generator (seed);
        static const double PI = 3.1415926535897932384626433832795028841971693993751;
        normal_distribution<double> nd(0.0,1.0);
        uniform_real_distribution<double> urd( 0.0, 2*PI );

        for( int i = 0; i < m_opt.samples; ++i )
        {
            double x = nd(generator);
            double y = nd(generator);
            double z = nd(generator);
            double norm = sqrt(x*x + y*y + z*z);
            x /= norm;
            y /= norm;
            z /= norm;
            double phi = urd(generator);
            ofs << phi << " " << x << " " << y << " " << z << endl;
        }
    }

    const RandomRotationsOpts& m_opt;
};

////////////////////////////////////////////////////////////////////
struct exactDecompositionOpts
{
  std::string filename;
};

////////////////////////////////////////////////////////////////////
struct exactDecompositionApplication
{
  exactDecompositionApplication( const exactDecompositionOpts& opts ) :
    m_opts(opts)
  {
  }

  void run()
  {
    ifstream ifs;
    ifs.exceptions(std::fstream::badbit | std::fstream::failbit);
    ifs.open(m_opts.filename);
    matrix2x2<mpz_class> exact_uni;
    circuit c;
    ifs >> exact_uni;
    cout << exact_uni << endl;
    ed.decompose(exact_uni,c);
    c.toStream(cout);
  }

  exactDecompositionOpts m_opts;
  exactDecomposer ed;
};

////////////////////////////////////////////////////////////////////

void print_about_message()
{
cout << "Copyright (c) 2012 Vadym Kliuchnikov sqct(dot)software(at)gmail(dot)com" << endl << endl;
cout << "SQCT is free software: you can redistribute it and/or modify" << endl;
cout << "it under the terms of the GNU Lesser General Public License as published by" << endl;
cout << "the Free Software Foundation, either version 3 of the License, or" << endl;
cout << "(at your option) any later version." << endl;
cout << "" << endl;
cout << "SQCT is distributed in the hope that it will be useful," << endl;
cout << "but WITHOUT ANY WARRANTY; without even the implied warranty of" << endl;
cout << "MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the" << endl;
cout << "GNU Lesser General Public License for more details." << endl;
cout << "" << endl;
cout << "You should have received a copy of the GNU Lesser General Public License" << endl;
cout << "along with SQCT.  If not, see <http://www.gnu.org/licenses/>." << endl;
cout << "" << endl;
cout << "The program code based on results of http://arxiv.org/abs/1206.5236." << endl;
cout << "It also implements the version of Solovay Kitaev algorithm described" << endl;
cout << "in http://arxiv.org/abs/quant-ph/0505030. " << endl;
cout << "" << endl;
cout << "The list of used libraries, corresponding licenses and authors:" << endl<< endl;
cout << "Boost | Boost Software License" << endl<< endl;
cout << "The GNU Multiple Precision Arithmetic Library | GNU Lesser General Public License v3" << endl;
cout << "(by Free Software Foundation, Inc.)" << endl<< endl;
cout << "The GNU MPFR Library the library | GNU Lesser General Public License v3" << endl;
cout << "(by Free Software Foundation, Inc.," << endl;
cout << " Contributed by the AriC and Caramel projects, INRIA. )" << endl << endl;
cout << "mpfr::real | GNU Lesser General Public License v3" << endl;
cout << "(by Christian Schneider <software(at)chschneider(dot)eu> )" << endl;
cout << "" << endl;
cout << "Source code of this program can be obtained from: " << endl;
}

////////////////////////////////////////////////////////////////////

int main(int ac, char* av[])
{
    // help
    string help_topic = "";
    // sk application parameters
    SKOptions sko;
    // epsilon net generation
    enetOptions eopt;
    // resynthesise
    ResynthOptions ro;
    // randomized rotations
    RandomRotationsOpts rro;
    // exact decomposition
    exactDecompositionOpts edo;

    try {

        po::options_description desc("Allowed options");
        desc.add_options()

            ("help,H", po::value< string >(&help_topic)->implicit_value(""),
             "Produce help message, see help <option name> for more details "
             "about specific option.")

            ("in,I", po::value< string >(&(sko.in_filename)),
             "File name with unitaries for approximation.")

            ("iterations,N", po::value< int >(&(sko.iterations_default))->default_value(3),
             "Default number of iteration used by the Solovay Kitaev algorithm, "
             "when not specified in input file.")

            ("angle,A", po::value< bool >(&(sko.angle))->zero_tokens(),
             "When true program interprets first entry in each line of input file as "
             "as rotation angle, and as denominator of Pi.")

            ("out,O", po::value< string >(&(sko.out_filename))->default_value("out.csv"),
             "File name with summary about "
             "unitary approximation results.")

            ("math-fr,F", po::value< bool >(&(sko.math_fr))->zero_tokens(),
             "Should output be Mathematica friendly. "
             "Default output format is CSV.")

            ("dotqc,C", po::value< bool >(&(sko.write_dot_qc))->zero_tokens(),
             "Should we output dotqc file for each generated circuit.")

            ("out-dir,D", po::value< string >(&(sko.out_dir))->default_value("out"),
             "Directory to output circuits.")

            ("max-sde,M", po::value< int >(&(sko.max_sde))->default_value(30),
             "Maximal value of sde to use during intial approximation step.")

            ("epsilon-net,E", po::value< vector<int> >(&(eopt.epsilon_net_layers))->multitoken(),
             "Generates epsilon net layers. If one values specified generates all layers "
             "with sde(|.|^2) less or equal than this value. If two values specified then sde(|.|^2) "
             "of result lies in interval [first,second] ")

            ("theory-topt",
             "Verifies conjecture about T optimality ")

            ("theory-hopt",
             "Performs brute force check necessary for proof of result in Appendix 2 "
             "in http://arxiv.org/abs/1206.5236")

            ("theory-correctness",
             "Performs brute force check described by Algoritm 2 "
             "in http://arxiv.org/abs/1206.5236 ( proves Lemma 3 ).")

            ("resynth,S", po::value< vector<string> >(&(ro.circuit_files) )->multitoken(),
             "Resynthesize circuit. Output may differ from original by global phase.")

            ("reverse", po::value< bool >(& (ro.reverse) )->zero_tokens(),
             "True if circuit file should be interpreted in matrix multiplication order.")

            ("random-rot", po::value< int >(&(rro.samples)),
             "Generates file with specified number of random rotations.")

            ("exact", po::value< string >(&(edo.filename)),
             "Performs an exact decomposition of a unitary over the ring.")

            ("about", "Information about the program.")
        ;

        po::variables_map vm;
        po::store(po::parse_command_line(ac, av, desc), vm);
        po::notify(vm);

        if( vm.count("resynth") ) {
            ro.math_fr = sko.math_fr;
            ResynthApplication ra(ro);
            ra.process();
            return 0;
        }

        if(vm.count("epsilon-net"))
        {
            enetApplication eapp(eopt);
            eapp.process();
            return 0;
        }

        if( vm.count("theory-correctness") ) {
            cout << is_theorem_true() << endl;
            return 0;
        }

        if( vm.count("theory-topt") ) {
            toptimalitytest tt;
            return 0;
        }

        if( vm.count("theory-hopt") ) {
            hoptimalitytest ht;
            return 0;
        }

        if ( vm.count("random-rot") ) {
            rro.filename = sko.out_filename;
            RotationGeneratorApp rga(rro);
            rga.process();
            return 0;
        }

        if( vm.count("about") ) {
            print_about_message();
            return 0;
        }

        if ( vm.count("in") ) {
            eopt.epsilon_net_layers.clear();
            eopt.epsilon_net_layers.push_back(sko.max_sde);
            enetApplication ea(eopt);
            if( ea.check() )
            {
                SKApplication app(sko);
                app.process();
            }
            return 0;
        }

        if( vm.count("exact") ) {
          exactDecompositionApplication eda(edo);
          eda.run();
          return 0;
        }

        if ( true ) {
            if( !print_help(help_topic) )
                cout << desc << endl;
            return 0;
        }

    }
    catch(exception& e) {
        cerr << "Error: " << e.what() << endl;
        return 1;
    }
    catch(...) {
        cerr << "Exception of unknown type!" << endl;
        return 1;
    }
}


//
// Copyright (c) 2004-2006
// Andreas Kloeckner
//
// Permission to use, copy, modify, distribute and sell this software
// and its documentation for any purpose is hereby granted without fee,
// provided that the above copyright notice appear in all copies and
// that both that copyright notice and this permission notice appear
// in supporting documentation.  The authors make no representations
// about the suitability of this software for any purpose.
// It is provided "as is" without express or implied warranty.
//

#ifndef HEADER_SEEN_PYUBLAS_ELEMENTWISE_OP_HPP
#define HEADER_SEEN_PYUBLAS_ELEMENTWISE_OP_HPP

#include <cmath>
#include <boost/numeric/ublas/functional.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>

namespace pyublas {
  template <class Functor>
  struct unary_op
  {
    template<class E>
    static
    typename boost::numeric::ublas::vector_unary_traits<E, Functor>::result_type
    apply(const boost::numeric::ublas::vector_expression<E> &e)
    {
      typedef typename boost::numeric::ublas::vector_unary_traits<E, Functor>
        ::expression_type expression_type;
      return expression_type(e());
    }

    template<class E>
    static
    typename boost::numeric::ublas::matrix_unary1_traits<E, Functor>::result_type
    apply(const boost::numeric::ublas::matrix_expression<E> &e)
    {
      typedef typename boost::numeric::ublas::matrix_unary1_traits<E, Functor>
        ::expression_type expression_type;
      return expression_type(e());
    }
  };

  template <class Functor>
  struct binary_op
  {
    template<class E1, class E2>
    static
    typename boost::numeric::ublas::vector_binary_traits<E1, E2, Functor>::result_type
    apply(
        const boost::numeric::ublas::vector_expression<E1> &e1,
        const boost::numeric::ublas::vector_expression<E2> &e2
        )
    {
      typedef typename boost::numeric::ublas::vector_binary_traits<E1, E2, Functor>
        ::expression_type expression_type;
      return expression_type(e1(), e2());
    }

    template<class E1, class E2>
    static
    typename boost::numeric::ublas::matrix_binary_traits<E1, E2, Functor>::result_type
    apply(
        const boost::numeric::ublas::matrix_expression<E1> &e1,
        const boost::numeric::ublas::matrix_expression<E2> &e2
        )
    {
      typedef typename boost::numeric::ublas::matrix_binary_traits<E1, E2, Functor>
        ::expression_type expression_type;
      return expression_type(e1(), e2());
    }
  };

  namespace unary_ops
  {
    class fabs : public boost::numeric::ublas::scalar_real_unary_functor<double>
    {
      private:
        typedef boost::numeric::ublas::scalar_unary_functor<double> super;

      public:
        static super::result_type apply(super::argument_type x)
        {
          return ::std::fabs(x);
        }
    };
  }

  namespace binary_ops
  {
    template<class T1, class T2=T1>
    class max : public boost::numeric::ublas::scalar_binary_functor<T1, T2>
    {
      private:
        typedef boost::numeric::ublas::scalar_binary_functor<T1, T2> super;

      public:
        static typename super::result_type apply(
            typename super::argument1_type t1,
            typename super::argument1_type t2)
        {
          if (t1 >= t2)
            return t1;
          else
            return t2;
        }
    };

    template<class T1, class T2=T1>
    class min : public boost::numeric::ublas::scalar_binary_functor<T1, T2>
    {
      private:
        typedef boost::numeric::ublas::scalar_binary_functor<T1, T2> super;

      public:
        static typename super::result_type apply(
            typename super::argument1_type t1,
            typename super::argument1_type t2)
        {
          if (t1 <= t2)
            return t1;
          else
            return t2;
        }
    };
  }

  // square sum ---------------------------------------------------------------
  template<class V>
  struct vector_square_sum :
    public boost::numeric::ublas::vector_scalar_real_unary_functor<V>
  {
    private:
      typedef boost::numeric::ublas::vector_scalar_real_unary_functor<V> super;

    public:
      typedef typename super::real_type real_type;
      typedef typename super::value_type value_type;
      typedef typename super::result_type result_type;

      template<class E>
      static result_type
      apply(const boost::numeric::ublas::vector_expression<E> &e)
      {
        using namespace boost::numeric::ublas;

        real_type t = real_type ();
        typedef typename E::size_type vector_size_type;
        vector_size_type size (e ().size ());
        for (vector_size_type i = 0; i < size; ++ i) {
          typename super::real_type u (
              type_traits<value_type>::norm_2 (e () (i)));
          t +=  u * u;
        }
        return t;
      }

      // Dense case
      template<class D, class I>
      static result_type apply(D size, I it)
      {
        using namespace boost::numeric::ublas;

        real_type t = real_type ();
        while (-- size >= 0) {
          real_type u (type_traits<value_type>::norm_2 (*it));
          t +=  u * u;
          ++ it;
        }
        return t;
      }

      // Sparse case
      template<class I>
      static result_type apply(I it, const I &it_end)
      {
        using namespace boost::numeric::ublas;

        real_type t = real_type ();
        while (it != it_end) {
          real_type u (type_traits<value_type>::norm_2 (*it));
          t +=  u * u;
          ++ it;
        }
        return t;
      }
  };

  template<class E>
  inline
  typename boost::numeric::ublas::vector_scalar_unary_traits<E, vector_square_sum<E> >::result_type
  square_sum(const boost::numeric::ublas::vector_expression<E> &e)
  {
    typedef typename boost::numeric::ublas::
      vector_scalar_unary_traits<E, vector_square_sum<E> >::expression_type
      expression_type;
    return expression_type(e());
  }
}

#endif


/*ckwg +29
 * Copyright 2015-2016 by Kitware, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 *  * Neither name of Kitware, Inc. nor the names of any contributors may be used
 *    to endorse or promote products derived from this software without specific
 *    prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS''
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/**
 * \file
 * \brief vxl estimate fundamental matrix implementation
 */

#include "estimate_fundamental_matrix.h"

#include <vital/vital_foreach.h>

#include <vital/types/feature.h>
#include <arrows/vxl/camera.h>
#include <arrows/core/epipolar_geometry.h>

#include <vgl/vgl_point_2d.h>
#include <Eigen/LU>

#include <vpgl/algo/vpgl_fm_compute_8_point.h>
#include <vpgl/algo/vpgl_fm_compute_7_point.h>

using namespace kwiver::vital;

namespace kwiver {
namespace arrows {
namespace vxl {

/// Private implementation class
class estimate_fundamental_matrix::priv
{
public:
  /// Constructor
  priv()
  : precondition(true),
    method(EST_8_POINT)
  {
  }

  enum method_t {EST_7_POINT, EST_8_POINT};

  bool precondition;
  method_t method;
};

/// Constructor
estimate_fundamental_matrix
::estimate_fundamental_matrix()
: d_(new priv)
{
}

/// Destructor
estimate_fundamental_matrix
::~estimate_fundamental_matrix()
{
}

/// Get this algorithm's \link vital::config_block configuration block \endlink
vital::config_block_sptr
estimate_fundamental_matrix
::get_configuration() const
{
  // get base config from base class
  vital::config_block_sptr config =
      vital::algo::estimate_fundamental_matrix::get_configuration();

  config->set_value("precondition", d_->precondition,
                    "If true, precondition the data before estimating the "
                    "fundamental matrix");

  std::string method_name = d_->method == priv::EST_8_POINT
                            ? "EST_8_POINT" : "EST_7_POINT";
  config->set_value("method", method_name,
                    "Fundamental matrix estimation method to use. "
                    "(Note: does not include RANSAC).  Choices are\n"
                    "  EST_7_POINT\n"
                    "  EST_8_POINT");

  return config;
}

/// Set this algorithm's properties via a config block
void
estimate_fundamental_matrix
::set_configuration(vital::config_block_sptr config)
{

  d_->precondition = config->get_value<bool>("precondition",
                                             d_->precondition);
  std::string method_name = config->get_value<std::string>("method");
  if( method_name == "EST_7_POINT" )
  {
    d_->method = priv::EST_7_POINT;
  }
  else
  {
    d_->method = priv::EST_8_POINT;
  }
}

/// Check that the algorithm's current configuration is valid
bool
estimate_fundamental_matrix
::check_configuration(vital::config_block_sptr config) const
{
  return true;
}

/// Estimate an essential matrix from corresponding points
fundamental_matrix_sptr
estimate_fundamental_matrix
::estimate(const std::vector<vector_2d>& pts1,
           const std::vector<vector_2d>& pts2,
           std::vector<bool>& inliers,
           double inlier_scale) const
{
  vcl_vector<vgl_homg_point_2d<double> > right_points, left_points;
  VITAL_FOREACH(const vector_2d& v, pts1)
  {
    right_points.push_back(vgl_homg_point_2d<double>(v.x(), v.y()));
  }
  VITAL_FOREACH(const vector_2d& v, pts2)
  {
    left_points.push_back(vgl_homg_point_2d<double>(v.x(), v.y()));
  }

  vpgl_fundamental_matrix<double> vfm;
  if( d_->method == priv::EST_8_POINT )
  {
    vpgl_fm_compute_8_point fm_compute(d_->precondition);
    fm_compute.compute(right_points, left_points, vfm);
  }
  else
  {
    std::vector< vpgl_fundamental_matrix<double>* > vfms;
    vpgl_fm_compute_7_point fm_compute(d_->precondition);
    fm_compute.compute(right_points, left_points, vfms);
    // TODO use the multiple solutions in a RANSAC framework
    // For now, only keep the first solution
    vfm = *vfms[0];
    VITAL_FOREACH(auto v, vfms)
    {
      delete v;
    }
  }

  matrix_3x3d F(vfm.get_matrix().data_block());
  F.transposeInPlace();

  fundamental_matrix_sptr fm(new fundamental_matrix_d(F));
  inliers = arrows::mark_fm_inliers(*fm, pts1, pts2, inlier_scale);
  return fm;
}

} // end namespace vxl
} // end namespace arrows
} // end namespace kwiver