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

//-*****************************************************************************
// The basic architecture of these Waves is based on the TweakWaves application
// written by Chris Horvath for Tweak Films in 2001.  This, in turn, was based
// on the SIGGRAPH papers and courses by Jerry Tessendorf, and by the paper
// "A Simple Fluid Solver based on the FTT" by Jos Stam.
//
// The TMA, JONSWAP, and Pierson Moskowitz Wave Spectra, as well as the
// directional spreading functions are formulated based on the descriptions
// given in "Ocean Waves: The Stochastic Approach",
// by Michel K. Ochi, published by Cambridge Ocean Technology Series, 1998,2005.
//
// This library is written as a working implementation of the paper:
// Christopher J. Horvath. 2015.
// Empirical directional wave spectra for computer graphics.
// In Proceedings of the 2015 Symposium on Digital Production (DigiPro '15),
// Los Angeles, Aug. 8, 2015, pp. 29-39.
//-*****************************************************************************

#include <EncinoWaves/All.h>

#include <boost/program_options.hpp>

#include <iostream>
#include <stdio.h>
#include <stdlib.h>

namespace ewav = EncinoWaves;
namespace po   = boost::program_options;

//-*****************************************************************************
void doTest(const ewav::Parametersf& i_params) {
  ewav::InitialStatef istate(i_params);
  int w = istate.HSpectralPos.width();
  int h = istate.HSpectralPos.height();
  int N = h;
  std::cout << "A: Computed initial state." << std::endl
            << "Size: " << w << " by " << h << std::endl
            << "HspecPos: " << istate.HSpectralPos[N / 4][N / 4] << std::endl
            << "HspecNeg: " << istate.HSpectralNeg[N / 4][N / 4] << std::endl
            << "Omega: " << istate.Omega[N / 4][N / 4] << std::endl;
}

//-*****************************************************************************
int main(int argc, char* argv[]) {
  ewav::Parametersf params;

  int threads              = -1;
  int dispersion           = 2;
  int spectrum             = 2;
  int directionalSpreading = 0;
  int filter               = 0;
  int random               = 0;
  int seed                 = 54321;

  po::options_description desc("Tweak Waves 2013 : Initial State Test");
  desc.add_options()

    // clang-format off

    ( "help,h", "prints this help message" )

    ( "threads",
      po::value<int>( &threads )
      ->default_value( threads ),
      "Threads to use. Default of -1 means use all." )

    ( "resolution",
      po::value<int>( &params.resolutionPowerOfTwo )
      ->default_value( params.resolutionPowerOfTwo ),
      "Power of Two of the sim resolution" )

    ( "domain",
      po::value<float>( &params.domain )
      ->default_value( params.domain ),
      "Size, in meters, of largest wave" )

    ( "gravity",
      po::value<float>( &params.gravity )
      ->default_value( params.gravity ),
      "Gravitational constant, in meters per second squared" )

    ( "surfaceTension",
      po::value<float>( &params.surfaceTension )
      ->default_value( params.surfaceTension ),
      "Surface tension constant, in Newtons per meter" )

    ( "density",
      po::value<float>( &params.density )
      ->default_value( params.density ),
      "Water density, in kilograms per meter cubed" )

    ( "depth",
      po::value<float>( &params.depth )
      ->default_value( params.depth ),
      "Average depth of the ocean, in meters" )

    ( "windSpeed",
      po::value<float>( &params.windSpeed )
      ->default_value( params.windSpeed ),
      "Average wind speed, in meters per second" )

    ( "fetch",
      po::value<float>( &params.fetch )
      ->default_value( params.fetch ),
      "Wind fetch, in KILOMETERS" )

    ( "pinch",
      po::value<float>( &params.pinch )
      ->default_value( params.pinch ),
      "Lateral displacement, normalized" )

    ( "amplitudeGain",
      po::value<float>( &params.amplitudeGain )
      ->default_value( params.amplitudeGain ),
      "Gain on the wave height" )

    ( "dispersion",
      po::value<int>( &dispersion )
      ->default_value( dispersion ),
      "Dispersion: 0 for Deep, 1 for Finite Depth, 2 for Capillary" )

    ( "spectrum",
      po::value<int>( &spectrum )
      ->default_value( spectrum ),
      "Spectrum: 0 for Pierson-Moskowitz, 1 for JONSWAP, 2 for TMA" )

    ( "directionalSpreading",
      po::value<int>( &directionalSpreading )
      ->default_value( directionalSpreading ),
      "Directional Spreading: 0 for Balanced Cos2 Theta, "
      "1 for Mitsuyasu, 2 for Hasselmann, 3 for Donelan-Banner" )

    ( "swell",
      po::value<float>( &params.directionalSpreading.swell )
      ->default_value( params.directionalSpreading.swell ),
      "The mix between a wind-driven local sea and a swell caused by a "
      "distant storm." )

    ( "filter",
      po::value<int>( &filter )
      ->default_value( filter ),
      "Filter: 0 for nullptr, 1 for Smoothed Invertible Band-Pass" )

    ( "filterSoftWidth",
      po::value<float>( &params.filter.softWidth )
      ->default_value( params.filter.softWidth ),
      "Size in meters of the softness of wavelength filter falloff." )

    ( "filterSmall",
      po::value<float>( &params.filter.smallWavelength )
      ->default_value( params.filter.smallWavelength ),
      "Size in meters of the smallest kept wavelengths." )

    ( "filterBig",
      po::value<float>( &params.filter.bigWavelength )
      ->default_value( params.filter.bigWavelength ),
      "Size in meters of the biggest kept wavelengths." )

    ( "filterMin",
      po::value<float>( &params.filter.min )
      ->default_value( params.filter.min ),
      "Minimum value of filter." )

    ( "filterInvert", "Invert the filter" )

    ( "random",
      po::value<int>( &random )
      ->default_value( random ),
      "Random Distribution: 0 for Normal, 1 for Log-Normal" )

    ( "seed",
      po::value<int>( &params.random.seed )
      ->default_value( params.random.seed ),
      "Random Seed" )

    ;

  // clang-format on

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
  po::notify(vm);

  //-*************************************************************************
  if (vm.count("help")) {
    std::cout << desc << std::endl;
    return 0;
  }

  if (vm.count("filterInvert")) {
    params.filter.invert = true;
  }

  params.dispersion.type = (ewav::DispersionType)dispersion;
  params.spectrum.type = (ewav::SpectrumType)spectrum;
  params.directionalSpreading.type =
    (ewav::DirectionalSpreadingType)directionalSpreading;
  params.filter.type = (ewav::FilterType)filter;
  params.random.type = (ewav::RandomType)random;
  params.random.seed = seed;

  doTest(params);

  return 0;
}


#include <chrono>
#include <ctime>
#include <iostream>
#include <fstream>
#include <vector>
#include <Eigen/Geometry>
using namespace std;

//default input values
string inputFile = "trajBig.pdb";
string outputFile = "output.txt";
int spheresAllocationFrame = 0;
double RMSDThreshPercent = 50;

//additional parameters
double sphereRadius = 8;

int SPHERES;
int ATOMS;
int FRAMES;
double highestSize;

//Atoms[<frame>][<atom>][<coordinate>]
vector<vector<vector<double>>> A;

//RMSDValue[<frame>][<sphere>]
vector<vector<double>> C;

//Maps sphere to CA; CAAtomNumber[<sphere>]
vector<int> sphereCA;

//Numer of atoms in [<sphere>]
vector<int> sphereSize;

struct sphere
{
    int frame;
    int index;
    double RMSD;
};

vector<vector<int>> sphereAtoms;
vector<vector<int>> atomToSphere;
vector<vector<double>> highestRMSD;
vector<sphere> highestRMSDbase;

bool operator <(const sphere& lhs, const sphere& rhs)
{
    return lhs.RMSD < rhs.RMSD;
}

vector<string> result;

//--------------------------------------------------

//reading data from input pdb file.
void readFile(string filename)
{
    string line;
    ifstream myfile(filename);
    if (myfile.is_open())
    {
        int frame = 0;
        int atom;
        SPHERES = 0;
        FRAMES = 0;
        ATOMS = 0;
        A = {};
        sphereCA = {};
        sphereSize = {};
        while (getline(myfile, line))
        {
            if (line[0] == 'M')
            {
                frame = stoi(line.substr(9, 5));
                frame--;
                A.push_back({});
                C.push_back({});
                FRAMES++;
            }
            else if (line[0] == 'A')
            {
                atom = stoi(line.substr(6, 5));
                atom--;
                A[frame].push_back({});
                A[frame][atom].push_back(stod(line.substr(30, 8)));
                A[frame][atom].push_back(stod(line.substr(38, 8)));
                A[frame][atom].push_back(stod(line.substr(46, 8)));
                if (frame == 0)
                {
                    ATOMS++;
                    if (line[14] == 'A' and line[13] == 'C')
                    {
                        sphereCA.push_back(atom);
                        sphereSize.push_back(0);
                        SPHERES++;
                    }
                }
            }
        }
        myfile.close();
    }
    else
    {
        cout << "Nie odnaleziono pliku!" << endl;
    }
}

//calculating distance between 2 atoms, used when allocating atoms to spheres.
double atomsDistanceCalc(int atom1, int atom2)
{
    double result = sqrt(pow(A[spheresAllocationFrame][atom1][0] - A[spheresAllocationFrame][atom2][0], 2) +
        pow(A[spheresAllocationFrame][atom1][1] - A[spheresAllocationFrame][atom2][1], 2) +
        pow(A[spheresAllocationFrame][atom1][2] - A[spheresAllocationFrame][atom2][2], 2));
    return result;
}

//allocating atoms into spheres, based on sphereRadius
void atomsAllocation()
{
    auto timestamp = std::chrono::system_clock::now();
    std::time_t timestamp_print = std::chrono::system_clock::to_time_t(timestamp);
    int checkpoint = 0;
    sphereAtoms = {};
    atomToSphere = {};
    for (int i = 0; i < SPHERES; i++)
    {
        sphereAtoms.push_back({});
    }
    for (int i = 0; i < ATOMS; i++)
    {
        atomToSphere.push_back({});
        for (int j = 0; j < SPHERES; j++)
        {
            if (atomsDistanceCalc(i, sphereCA[j]) <= sphereRadius)
            {
                atomToSphere[i].push_back(j);
                sphereAtoms[j].push_back(i);
                sphereSize[j]++;
            }
        }
        if ((i - checkpoint) >= 0.1 * ATOMS)
        {
            checkpoint = i;
            timestamp = std::chrono::system_clock::now();
            timestamp_print = std::chrono::system_clock::to_time_t(timestamp);
            cout << "[RMSD] Atom number " << i + 1 << " of " << ATOMS << " allocated to spheres at " << std::ctime(&timestamp_print) << endl;
        }
    }
}

// Find3DAffineTransform is from oleg-alexandrov repository on github, available here https://github.com/oleg-alexandrov/projects/blob/master/eigen/Kabsch.cpp [as of 27.01.2022]
// Given two sets of 3D points, find the rotation + translation + scale
// which best maps the first set to the second.
// Source: http://en.wikipedia.org/wiki/Kabsch_algorithm

// The input 3D points are stored as columns.
Eigen::Affine3d Find3DAffineTransform(Eigen::Matrix3Xd in, Eigen::Matrix3Xd out) {

    // Default output
    Eigen::Affine3d A;
    A.linear() = Eigen::Matrix3d::Identity(3, 3);
    A.translation() = Eigen::Vector3d::Zero();

    if (in.cols() != out.cols())
        throw "Find3DAffineTransform(): input data mis-match";

    // First find the scale, by finding the ratio of sums of some distances,
    // then bring the datasets to the same scale.
    double dist_in = 0, dist_out = 0;
    for (int col = 0; col < in.cols() - 1; col++) {
        dist_in += (in.col(col + 1) - in.col(col)).norm();
        dist_out += (out.col(col + 1) - out.col(col)).norm();
    }
    if (dist_in <= 0 || dist_out <= 0)
        return A;
    double scale = dist_out / dist_in;
    out /= scale;

    // Find the centroids then shift to the origin
    Eigen::Vector3d in_ctr = Eigen::Vector3d::Zero();
    Eigen::Vector3d out_ctr = Eigen::Vector3d::Zero();
    for (int col = 0; col < in.cols(); col++) {
        in_ctr += in.col(col);
        out_ctr += out.col(col);
    }
    in_ctr /= in.cols();
    out_ctr /= out.cols();
    for (int col = 0; col < in.cols(); col++) {
        in.col(col) -= in_ctr;
        out.col(col) -= out_ctr;
    }

    // SVD
    Eigen::MatrixXd Cov = in * out.transpose();
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(Cov, Eigen::ComputeThinU | Eigen::ComputeThinV);

    // Find the rotation
    double d = (svd.matrixV() * svd.matrixU().transpose()).determinant();
    if (d > 0)
        d = 1.0;
    else
        d = -1.0;
    Eigen::Matrix3d I = Eigen::Matrix3d::Identity(3, 3);
    I(2, 2) = d;
    Eigen::Matrix3d R = svd.matrixV() * I * svd.matrixU().transpose();

    // The final transform
    A.linear() = scale * R;
    A.translation() = scale * (out_ctr - R * in_ctr);

    return A;
}

//superpose changes vector of atoms frame 2 to map atoms from frame 1 in the way to minimise RMSD between both frames
void superpose(const vector<vector<double>>& frame1, vector<vector<double>>& frame2)
{
    int atomsInSphere = frame1.size();
    Eigen::Matrix3Xd S1(3, atomsInSphere);
    Eigen::Matrix3Xd S2(3, atomsInSphere);
    for (int j = 0; j < atomsInSphere; j++)
    {
        for (int k = 0; k < 3; k++)
        {
            S1(k, j) = frame1[j][k];
            S2(k, j) = frame2[j][k];
        }
    }
    Eigen::Affine3d RT = Find3DAffineTransform(S2, S1);
    S2 = RT.linear() * S2;
    for (int j = 0; j < atomsInSphere; j++)
    {
        S2.block<3, 1>(0, j) += RT.translation();
    }
    for (int j = 0; j < atomsInSphere; j++)
    {
        for (int k = 0; k < 3; k++)
        {
            frame2[j][k] = S2(k, j);
        }
    }
}

//initializing vector for RMSD values
void initializeC()
{
    C = {};
    for (int i = 0; i < FRAMES; i++)
    {
        C.push_back({});
        for (int j = 0; j < SPHERES; j++)
            C[i].push_back(0);
    }
}

//calculating RMSD on spheres, on adjacent frames
void calculateRMSDSuperpose()
{
    initializeC();
    vector<vector<vector<double>>> sphereMatrix;
    vector<vector<double>> tempMatrix;
    double tempRMSD;
    auto timestamp = std::chrono::system_clock::now();
    std::time_t timestamp_print = std::chrono::system_clock::to_time_t(timestamp);
    int checkpoint = 0;
    for (int s = 0; s < SPHERES; s++)
    {
        int atomsInSphere = sphereAtoms[s].size();
        sphereMatrix = {};
        for (int i = 0; i < FRAMES; i++)
        {
            sphereMatrix.push_back({});
            for (int j = 0; j < atomsInSphere; j++)
            {
                sphereMatrix[i].push_back(A[i][sphereAtoms[s][j]]);
            }
            if (i > 0)
            {
                tempMatrix = sphereMatrix[i];
                superpose(sphereMatrix[i - 1], tempMatrix);
                for (int j = 0; j < atomsInSphere; j++)
                {
                    for (int k = 0; k < 3; k++)
                    {
                        tempRMSD = pow(tempMatrix[j][k] - sphereMatrix[i - 1][j][k], 2);
                        C[i - 1][s] += tempRMSD;
                    }
                }
                C[i - 1][s] /= ((long)atomsInSphere * (long)3);
                C[i - 1][s] = sqrt(C[i - 1][s]);
            }
        }
        if ((s - checkpoint) >= 0.1 * SPHERES)
        {
            checkpoint = s;
            timestamp = std::chrono::system_clock::now();
            timestamp_print = std::chrono::system_clock::to_time_t(timestamp);
            cout << "[RMSD] RMSD calculated for " << s + 1 << " of " << SPHERES << " spheres at " << std::ctime(&timestamp_print) << endl;
        }
    }
}

//initializing vector for storing information about spheres with highest RMSD
void initializeHighestRMSD()
{
    highestRMSD = {};
    highestSize = (double)SPHERES / 100;
    highestSize *= (long)(FRAMES - 1);
    highestSize *= RMSDThreshPercent;
    for (int i = 0; i < highestSize; i++)
    {
        highestRMSD.push_back({});
        for (int j = 0; j < 3; j++)
            highestRMSD[i].push_back(-1);
    }
}

//choosing frames and spheres with highest RMSD, they will be colored later
void chooseHighestRMSD()
{
    initializeHighestRMSD();
    for (int i = 0; i < FRAMES - 1; i++)
    {
        for (int j = 0; j < SPHERES; j++)
        {
            for (unsigned int k = 0; k < highestRMSD.size(); k++)
            {
                if (C[i][j] > highestRMSD[k][2])
                {
                    for (int l = highestRMSD.size() - 1; l > k; l--)
                    {
                        for (int m = 0; m < 3; m++)
                        {
                            highestRMSD[l][m] = highestRMSD[l - 1][m];
                        }
                    }
                    highestRMSD[k][0] = i;
                    highestRMSD[k][1] = j;
                    highestRMSD[k][2] = C[i][j];
                    break;
                }
            }
        }
    }
}

//choosing frames and spheres with highest RMSD, they will be colored later
void chooseHighestRMSDquick()
{
    auto timestamp = std::chrono::system_clock::now();
    std::time_t timestamp_print = std::chrono::system_clock::to_time_t(timestamp);
    initializeHighestRMSD();
    int counter = 0;
    for (int i = 0; i < FRAMES - 1; i++)
    {
        for (int j = 0; j < SPHERES; j++)
        {
            highestRMSDbase.push_back(sphere());
            highestRMSDbase[counter].frame = i;
            highestRMSDbase[counter].index = j;
            highestRMSDbase[counter].RMSD = C[i][j];
            counter++;
        }
    }
    timestamp = std::chrono::system_clock::now();
    timestamp_print = std::chrono::system_clock::to_time_t(timestamp);
    cout << "[RMSD] RMSD started being sorted at " << std::ctime(&timestamp_print) << endl;
    sort(highestRMSDbase.begin(), highestRMSDbase.end());
    timestamp = std::chrono::system_clock::now();
    timestamp_print = std::chrono::system_clock::to_time_t(timestamp);
    cout << "[RMSD] RMSD sorted at " << std::ctime(&timestamp_print) << endl;
    for (unsigned int k = 0; k < highestRMSD.size(); k++)
    {
        highestRMSD[k][0] = highestRMSDbase[k].frame;
        highestRMSD[k][1] = highestRMSDbase[k].index;
        highestRMSD[k][2] = highestRMSDbase[k].RMSD;
    }
}

//calculates result variable content, basen on highestRMSD
void calculateResult()
{
    result = {};
    int numberOfSpheresToColor = highestRMSD.size();
    vector<vector<int>> resultSpheres;
    for (int i = 0; i < FRAMES - 1; i++)
    {
        resultSpheres.push_back({});
    }
    for (int i = 0; i < numberOfSpheresToColor; i++)
    {
        resultSpheres[highestRMSD[i][0]].push_back(highestRMSD[i][1]);
    }
    string subResult;
    for (int i = 0; i < FRAMES - 1; i++)
    {
        subResult = "";
        for (int j = 0; j < numberOfSpheresToColor; j++)
        {
            if (highestRMSD[j][0] == i)
            {
                stringstream ss;
                string s;
                ss << sphereCA[highestRMSD[j][1]];
                ss >> s;
                subResult += s + " ";
            }
        }
        result.push_back(subResult);
    }
}

//saving results to an output file
void saveToFile(string filename)
{
    ofstream myfile(filename);
    if (myfile.is_open())
    {
        for (int i = 0; i < FRAMES - 1; i++)
        {
            if (!result[i].empty())
            {
                myfile << i << ' ' << result[i] << endl;
            }
        }
        myfile.close();
    }
}

//debug function for checking superpose results
void makePDB(string filename)
{
    vector<vector<vector<double>>> sphereMatrix;
    int atomsInSphere = sphereAtoms[0].size();
    sphereMatrix = {};
    for (int i = 0; i < 2; i++)
    {
        sphereMatrix.push_back({});
        for (int j = 0; j < atomsInSphere; j++)
        {
            sphereMatrix[i].push_back(A[i][sphereAtoms[0][j]]);
        }
        if (i > 0)
        {
            superpose(sphereMatrix[i - 1], sphereMatrix[i]);
        }
    }
    string line;
    ifstream myfilein(filename);
    filename += "out";
    ofstream myfileout("testBase.pdb");
    ofstream myfilesuper("testSuperpose.pdb");
    int counter = 0;
    if (myfilein.is_open() && myfileout.is_open())
    {
        int frame = 0;
        int atom;
        while (getline(myfilein, line))
        {
            if (line[0] == 'M')
            {
                myfileout << line << endl;
                myfilesuper << line << endl;
            }

            if (line[0] == 'E')
            {
                myfileout << line << endl;
                myfilesuper << line << endl;
                frame++;
                if (frame > 1)
                {
                    break;
                }
            }
            if (line[0] == 'A')
            {
                atom = stoi(line.substr(6, 5));
                atom--;
                for (unsigned int i = 0; i < sphereAtoms[0].size(); i++)
                {
                    if (sphereAtoms[0][i] == atom)
                    {
                        myfileout << line << endl;
                        if (frame == 1)
                        {
                            line.replace(32, 6, to_string(sphereMatrix[1][counter][0]), 0, 6);
                            line.replace(40, 6, to_string(sphereMatrix[1][counter][1]), 0, 6);
                            line.replace(48, 6, to_string(sphereMatrix[1][counter][2]), 0, 6);
                            myfilesuper << line << endl;
                            counter++;
                        }
                        else
                        {
                            myfilesuper << line << endl;
                        }
                    }
                }
            }
        }
        myfilein.close();
        myfileout.close();
        myfilesuper.close();
    }
}

int main(int argc, char* argv[])
{
    auto start = chrono::high_resolution_clock::now();
    if (argc == 5)
    {
        inputFile = string(argv[1]);
        outputFile = string(argv[2]);
        spheresAllocationFrame = stoi(argv[3]) - 1;
        RMSDThreshPercent = stod(argv[4]);
    }
    readFile(inputFile);
    if (spheresAllocationFrame < 0)
    {
        spheresAllocationFrame = 1;
    }
    else if (spheresAllocationFrame >= FRAMES)
    {
        spheresAllocationFrame = FRAMES - 1;
    }
    auto timestamp = std::chrono::system_clock::now();
    std::time_t timestamp_print = std::chrono::system_clock::to_time_t(timestamp);
    cout << "[RMSD] Input file: " << inputFile << " loaded at " << std::ctime(&timestamp_print) << endl;
    atomsAllocation();
    timestamp = std::chrono::system_clock::now();
    timestamp_print = std::chrono::system_clock::to_time_t(timestamp);
    cout << "[RMSD] Atoms allocated to spheres at " << std::ctime(&timestamp_print) << endl;
    calculateRMSDSuperpose();
    chooseHighestRMSDquick();
    calculateResult();
    saveToFile(outputFile);
    timestamp = std::chrono::system_clock::now();
    timestamp_print = std::chrono::system_clock::to_time_t(timestamp);
    cout << "[RMSD] Output file: " << outputFile << " saved at " << std::ctime(&timestamp_print) << endl;
    //makePDB(inputFile);
    auto stop = chrono::high_resolution_clock::now();
    chrono::duration<double> elapsed = stop - start;
    timestamp = std::chrono::system_clock::now();
    timestamp_print = std::chrono::system_clock::to_time_t(timestamp);
    cout << "[RMSD] Computation time: " << elapsed.count() << "s, ended at: " << std::ctime(&timestamp_print) << endl;
}


#include "ocean_model_interfaces/fvcom/FVCOMStructure.h"
#include "ocean_model_interfaces/fvcom/FVCOM.h"

#include <netcdf>
#include <memory>
#include <cmath>
#include <limits>
#include <algorithm>
#include <iterator>

#include <boost/filesystem.hpp>
namespace fs = boost::filesystem;

using namespace ocean_model_interfaces;

FVCOMStructure::FVCOMStructure() {}

FVCOMStructure::FVCOMStructure(const std::string filename, int xChunkSize, int yChunkSize, int siglayChunkSize, int timeChunkSize) :
    xChunkSize(xChunkSize),
    yChunkSize(yChunkSize),
    siglayChunkSize(siglayChunkSize),
    timeChunkSize(timeChunkSize),
    lastContainingTriangle(0)
{
    loadStructureData(filename);

    splitIntoChunks();
}

FVCOMStructure::Plane::Plane() {}

FVCOMStructure::Plane::Plane(FVCOMStructure::Point& p0, FVCOMStructure::Point& p1, FVCOMStructure::Point& p2)
{
    double ab[3];
    double ac[3];

    ab[0] = p1.x - p0.x;
    ab[1] = p1.y - p0.y;
    ab[2] = p1.z - p0.z;

    ac[0] = p2.x - p0.x;
    ac[1] = p2.y - p0.y;
    ac[2] = p2.z - p0.z;

    a = (ab[1] * ac[2]) - (ab[2] * ac[1]);
    b = (ab[2] * ac[0]) - (ab[0] * ac[2]);
    c = (ab[0] * ac[1]) - (ab[1] * ac[0]);

    double magnitude = sqrt(a * a +  b * b +  c * c);
    a /= magnitude;
    b /= magnitude;
    c /= magnitude;

    d = -(p0.x * a + p0.y * b + p0.z * c);
}

double FVCOMStructure::Plane::getHeight(Point& interpolatePoint)
{
    return (-d - a * interpolatePoint.x - b * interpolatePoint.y) / c;
}

std::vector<std::string> FVCOMStructure::traverseDataFiles(const std::string filename)
{
    std::vector<std::string> filenames;
    fs::path p1 = filename;
    if(fs::is_directory(p1))
    {
        for(auto& p: fs::directory_iterator(p1))
        {
            if(!fs::is_directory(p))
            {
                filenames.push_back(p.path().string());
            }
        }
    }
    else
    {
        filenames.push_back(filename);
    }
    return filenames;
}

void FVCOMStructure::loadStructureData(const std::string directory)
{
    unsigned int timeDim = 0;
    std::vector<std::string> filenames = traverseDataFiles(directory);

    //Set start times and time dimensions from files
    for(auto &filename : filenames)
    {
        netCDF::NcFile dataFile(filename, netCDF::NcFile::read);
        timeDim += dataFile.getDim("time").getSize();

        std::vector<float> tempTimes;
        tempTimes.resize(dataFile.getDim("time").getSize());
        netCDF::NcVar timeVar = dataFile.getVar("time");
        timeVar.getVar(tempTimes.data());
        ModelFile modelFile;
        modelFile.filename = filename;
        modelFile.startTime = tempTimes[0];
        modelFile.timeDim = dataFile.getDim("time").getSize();
        modelFiles.push_back(modelFile);
    }

    //sort filenames based on start ties
    std::sort(modelFiles.begin(), modelFiles.end());

    //load time variables into one vector
    times.resize(timeDim);
    unsigned int currentIndex = 0;
    for(auto &modelFile : modelFiles)
    {
        netCDF::NcFile dataFile(modelFile.filename, netCDF::NcFile::read);
        netCDF::NcVar timeVar = dataFile.getVar("time");

        //Set the start time index for this file
        modelFile.startTimeIndex = currentIndex;

        //Load times from this file
        timeVar.getVar(times.data() + currentIndex);

        //move current index for next files
        currentIndex += dataFile.getDim("time").getSize();
    }
    netCDF::NcFile dataFile(modelFiles[0].filename, netCDF::NcFile::read);

    //Get dimensions of structure elements
    unsigned int nodeDim = dataFile.getDim("node").getSize();
    unsigned int neleDim = dataFile.getDim("nele").getSize();
    siglayDim = dataFile.getDim("siglay").getSize();

    //Load all variables for the structure of the model
    netCDF::NcVar xVar = dataFile.getVar("x");
    netCDF::NcVar yVar = dataFile.getVar("y");
    netCDF::NcVar xcVar = dataFile.getVar("xc");
    netCDF::NcVar ycVar = dataFile.getVar("yc");
    netCDF::NcVar nvVar = dataFile.getVar("nv");
    netCDF::NcVar hVar = dataFile.getVar("h");
    netCDF::NcVar siglayVar = dataFile.getVar("siglay");
    netCDF::NcVar centerSiglayVar = dataFile.getVar("siglay_center");

    //Load in as floats as that is what they are in the files
    //but they will be converted to doubles to maintain precision during calculations
    std::vector<float> nodeX;
    std::vector<float> nodeY;
    std::vector<float> nodeH;

    std::vector<float> triangleX;
    std::vector<float> triangleY;

    nodeX.resize(nodeDim);
    nodeY.resize(nodeDim);
    triangleX.resize(neleDim);
    triangleY.resize(neleDim);
    nodeH.resize(nodeDim);

    //Resize siglay 2d vectors
    nodeSiglay.resize(nodeDim);

    for(unsigned int i = 0; i < nodeSiglay.size(); i++)
    {
        nodeSiglay[i].resize(siglayDim);
    }

    //resize for multidimensional array
    triangleToNodes.resize(neleDim);

    for(unsigned int i = 0; i < neleDim; i++)
    {
        triangleToNodes[i].resize(3);
    }

    //Assign all arrays for the structure variables
    xVar.getVar(nodeX.data());
    yVar.getVar(nodeY.data());
    xcVar.getVar(triangleX.data());
    ycVar.getVar(triangleY.data());
    hVar.getVar(nodeH.data());

    //load nvVar into a multidimensional vector
    for(unsigned int i = 0; i < neleDim; i++)
    {
        std::vector<size_t> start = {0, i};
        std::vector<size_t> count = {3, 1};
        nvVar.getVar(start, count, triangleToNodes[i].data());
    }

    //load node siglay into a multidimensional vector
    for(unsigned int i = 0; i < nodeSiglay.size(); i++)
    {
        std::vector<size_t> start = {0, i};
        std::vector<size_t> count = {siglayDim, 1};
        siglayVar.getVar(start, count, nodeSiglay[i].data());
    }

    //Convert to use point struct
    nodes.resize(nodeDim);
    triangles.resize(neleDim);
    for(unsigned int i = 0; i < nodeDim; i++)
    {
        nodes[i].x = nodeX[i];
        nodes[i].y = nodeY[i];
        nodes[i].z = nodeH[i];
    }

    for(unsigned int i = 0; i < neleDim; i++)
    {
        triangles[i].x = triangleX[i];
        triangles[i].y = triangleY[i];
        triangles[i].z = std::numeric_limits<double>::quiet_NaN();
        //Height data is not loaded for triangles so we set it to NaN.
    }

    //The nv variable from the netCDF indexes starting at 1
    //Convert this to 0 by subtracting 1 from every value
    for(unsigned int i = 0; i < neleDim; i++)
    {
        for(unsigned int j = 0; j < 3; j++)
        {
            triangleToNodes[i][j]--;
        }
    }

    //Pre Processes model to get node to triangle conversion
    nodeToTriangles.resize(nodeDim);

    for(unsigned int i = 0; i < neleDim; i++)
    {
        for(unsigned int j = 0; j < 3; j++)
        {
            int triangle = i;
            int node = triangleToNodes[i][j];

            nodeToTriangles[node].push_back(triangle);
        }
    }

}

void FVCOMStructure::splitIntoChunks()
{
    getModelExtent();

    siglayDimChunks = std::ceil(siglayDim / (double)siglayChunkSize);
    timeDimChunks = std::ceil(times.size() / (double)timeChunkSize);
    yDimChunks = std::ceil((maxY - minY) / (double)yChunkSize);
    xDimChunks = std::ceil((maxX - minX) / (double)xChunkSize);

    nodesInChunk.resize(xDimChunks * yDimChunks);
    trianglesInChunk.resize(xDimChunks * yDimChunks);
    for(unsigned int i = 0; i < nodes.size(); i++)
    {
        FVCOMStructure::ChunkInfo chunk = getChunkForNode(i, 0, 0);

        //This does not contain the siglay or time dimensions as the
        //node locations do not change with depth or time.
        int chunkId = chunk.yChunk + (chunk.xChunk * yDimChunks);

        nodesInChunk[chunkId].push_back(i);
    }

    for(unsigned int i = 0; i < triangles.size(); i++)
    {
        FVCOMStructure::ChunkInfo chunk = getChunkForTriangle(i, 0, 0);

        //This does not contain the siglay or time dimensions as the
        //node locations do not change with depth or time.
        int chunkId = chunk.yChunk + (chunk.xChunk * yDimChunks);

        trianglesInChunk[chunkId].push_back(i);
    }
}

void FVCOMStructure::getModelExtent()
{
    for(unsigned int i = 0; i < nodes.size(); i++)
    {
        Point& node = nodes[i];

        if(node.x > maxX)
        {
            maxX = node.x;
        }

        if(node.x < minX)
        {
            minX = node.x;
        }

        if(node.y > maxY)
        {
            maxY = node.y;
        }

        if(node.y < minY)
        {
            minY = node.y;
        }
    }
}

bool FVCOMStructure::pointInTriangle(Point testPoint, int triangle) const
{
    int p0Index = triangleToNodes[triangle][0];
    int p1Index = triangleToNodes[triangle][1];
    int p2Index = triangleToNodes[triangle][2];

    Point p0 = nodes[p0Index];
    Point p1 = nodes[p1Index];
    Point p2 = nodes[p2Index];

    //Calculate barycentric coordinates
    double alpha = ((p1.y - p2.y)*(testPoint.x - p2.x) + (p2.x - p1.x)*(testPoint.y - p2.y)) /
        ((p1.y - p2.y)*(p0.x - p2.x) + (p2.x - p1.x)*(p0.y - p2.y));

    double beta = ((p2.y - p0.y)*(testPoint.x - p2.x) + (p0.x - p2.x)*(testPoint.y - p2.y)) /
           ((p1.y - p2.y)*(p0.x - p2.x) + (p2.x - p1.x)*(p0.y - p2.y));

    double gamma = 1.0 - alpha - beta;

    //if all coordinates are none negative then the point is in the triangle
    return alpha >= 0 && beta >= 0 && gamma >= 0;
}

int FVCOMStructure::getContainingTriangle(Point testPoint, int closestNode)
{
    if(pointInTriangle(testPoint, lastContainingTriangle))
    {
        return lastContainingTriangle;
    }

    //Search all triangles that are connected to the closest node
    for(unsigned int i = 0; i < nodeToTriangles[closestNode].size(); i++)
    {
        //return the triangle for which the point is inside
        if(pointInTriangle(testPoint, nodeToTriangles[closestNode][i]))
        {
            lastContainingTriangle = nodeToTriangles[closestNode][i];
            return nodeToTriangles[closestNode][i];
        }
    }

    //if the point is not inside any of those triangles search all the triangles
    for(unsigned int i = 0; i < triangles.size(); i++)
    {
        if(pointInTriangle(testPoint, i))
        {
            lastContainingTriangle = i;
            return i;
        }
    }

    throw std::out_of_range("FVCOM request outside of model extent");
}

int FVCOMStructure::getContainingTriangle(Point testPoint)
{
    if(pointInTriangle(testPoint, lastContainingTriangle))
    {
        return lastContainingTriangle;
    }

    //Get the closest node to start the search for the containing triangle
    int closestNode = getClosestNode(testPoint);

    return getContainingTriangle(testPoint, closestNode);
}

const std::vector<int>& FVCOMStructure::getNodesInTriangle(int triangle) const
{
    return triangleToNodes[triangle];
}

const std::vector<FVCOMStructure::ModelFile> FVCOMStructure::getModelFiles() const
{
    return modelFiles;
}

int FVCOMStructure::getClosestNode(Point testPoint) const
{
    //Checks distance between testPoint and every node, this is slow and will probably need to be improved
    double closestDistance = std::numeric_limits<double>::max();
    int node = -1;
    for(unsigned int i = 0; i < nodes.size(); i++)
    {

        if(distanceSquared(testPoint, nodes[i]) < closestDistance)
        {
            closestDistance = distanceSquared(testPoint, nodes[i]);
            node = i;
        }
    }

    return node;
}

int FVCOMStructure::getClosestTime(double time) const
{
    auto lower = std::lower_bound(times.begin(), times.end(), time);

    int index1 = std::distance(times.begin(), lower);
    int index2 = index1 - 1;

    if(std::abs(times[index1] - time) > std::abs(times[index2] - time))
    {
        return index2;
    }
    else
    {
        return index1;
    }

}

int FVCOMStructure::getPreviousTimeIndex(double time) const
{
    auto lower = std::lower_bound(times.begin(), times.end(), time);
    int index = std::distance(times.begin(), lower);

    if(times[index] == time)
     {
         return index;
    }

    return index - 1;
}

float FVCOMStructure::getTime(int timeIndex) const
{
    return times[timeIndex];
}

FVCOMStructure::Plane FVCOMStructure::getTrianglePlane(int triangle) const
{
    const std::vector<int>& surroundingNodes = triangleToNodes[triangle];

    FVCOMStructure::Point p0 = getNodePointWithH(surroundingNodes[0]);
    FVCOMStructure::Point p1 = getNodePointWithH(surroundingNodes[1]);
    FVCOMStructure::Point p2 = getNodePointWithH(surroundingNodes[2]);

    FVCOMStructure::Plane plane(p0, p1, p2);

    return plane;
}

FVCOMStructure::Plane FVCOMStructure::getTriangleSiglayPlane(int triangle, unsigned int siglay) const
{
    const std::vector<int>& surroundingNodes = triangleToNodes[triangle];

    FVCOMStructure::Point p0 = getNodePointAtSiglay(surroundingNodes[0], siglay);
    FVCOMStructure::Point p1 = getNodePointAtSiglay(surroundingNodes[1], siglay);
    FVCOMStructure::Point p2 = getNodePointAtSiglay(surroundingNodes[2], siglay);

    FVCOMStructure::Plane plane(p0, p1, p2);

    return plane;
}

double FVCOMStructure::distance(Point p0, Point p1) const
{
    return std::sqrt((p0.x - p1.x)*(p0.x - p1.x) + (p0.y - p1.y)*(p0.y - p1.y) );
}

double FVCOMStructure::distanceSquared(Point p0, Point p1) const
{
    return (p0.x - p1.x)*(p0.x - p1.x) + (p0.y - p1.y)*(p0.y - p1.y);
}

const FVCOMStructure::Point FVCOMStructure::getNodePointWithH(int node) const
{
    return nodes[node];
}

const FVCOMStructure::Point FVCOMStructure::getNodePointAtSiglay(int node, int siglay) const
{
    FVCOMStructure::Point returnPoint = nodes[node];
    returnPoint.z = returnPoint.z * nodeSiglay[node][siglay];

    return returnPoint;
}

FVCOMStructure::ChunkInfo FVCOMStructure::getChunkForNode(int node, int siglay, int time) const
{
    //Chunk ids based on this ordering (x,y,sigma,time)
    FVCOMStructure::ChunkInfo chunk;

    double nodeX = nodes[node].x;
    double nodeY = nodes[node].y;

    //calculate the chunks for each individual dimension
    chunk.xChunk = (nodeX - minX) / xChunkSize;
    chunk.yChunk = (nodeY - minY) / yChunkSize;
    chunk.siglayChunk = siglay / siglayChunkSize;
    chunk.timeChunk = time / timeChunkSize;

    //Check to insure that the chunks are valid.  If not this node is in the last chunk dimension.
    //NOTE: This should only occur if the x/y extent is divisible by x/y chunk dimension.
    //In this case maxX and maxY will give a chunk# as 1 more than the last chunk index.
    //It seems like a waste to have the a chunk only be these single nodes so they are included
    //in the last chunk.
    if(chunk.xChunk >= xDimChunks)
    {
        chunk.xChunk = xDimChunks - 1;
    }

    if(chunk.yChunk >= yDimChunks)
    {
        chunk.yChunk = yDimChunks - 1;
    }

    chunk.id = chunk.timeChunk +
           (chunk.siglayChunk * timeDimChunks) +
           (chunk.yChunk * timeDimChunks * siglayDimChunks) +
           (chunk.xChunk * timeDimChunks * siglayDimChunks * yDimChunks);

    chunk.xStart = chunk.xChunk * xChunkSize - minX;
    chunk.yStart = chunk.yChunk * yChunkSize - minX;
    chunk.siglayStart = chunk.siglayChunk * siglayChunkSize;
    chunk.timeStart = chunk.timeChunk * timeChunkSize;

    chunk.xSize = xChunkSize;
    chunk.ySize = yChunkSize;

    unsigned int timeSize = times.size();
    chunk.siglaySize = std::min(siglayChunkSize, siglayDim - chunk.siglayStart);
    chunk.timeSize = std::min(timeChunkSize, timeSize - chunk.timeStart);

    return chunk;
}

FVCOMStructure::ChunkInfo FVCOMStructure::getChunkForTriangle(int triangle, int siglay, int time) const
{
    //Chunk ids based on this ordering (x,y,sigma,time)

    FVCOMStructure::ChunkInfo chunk;

    double triangleX = triangles[triangle].x;
    double triangleY = triangles[triangle].y;

    //calculate the chunks for each individual dimension

    chunk.xChunk = (triangleX - minX) / xChunkSize;
    chunk.yChunk = (triangleY - minY) / yChunkSize;
    chunk.siglayChunk = siglay / siglayChunkSize;
    chunk.timeChunk = time / timeChunkSize;

    //Check to insure that the chunks are valid.  If not this node is in the last chunk dimension.
    //NOTE: This should only occur if the x/y extent is divisible by x/y chunk dimension.
    //In this case maxX and maxY will give a chunk# as 1 more than the last chunk index.
    //It seems like a waste to have the a chunk only be these single nodes so they are included
    //in the last chunk.

    if(chunk.xChunk >= xDimChunks)
    {
        chunk.xChunk = xDimChunks - 1;
    }

    if(chunk.yChunk >= yDimChunks)
    {
        chunk.yChunk = yDimChunks - 1;
    }

    chunk.id = chunk.timeChunk +
           (chunk.siglayChunk * timeDimChunks) +
           (chunk.yChunk * timeDimChunks * siglayDimChunks) +
           (chunk.xChunk * timeDimChunks * siglayDimChunks * yDimChunks);

    chunk.xStart = chunk.xChunk * xChunkSize - minX;
    chunk.yStart = chunk.yChunk * yChunkSize - minX;
    chunk.siglayStart = chunk.siglayChunk * siglayChunkSize;
    chunk.timeStart = chunk.timeChunk * timeChunkSize;

    chunk.xSize = xChunkSize;
    chunk.ySize = yChunkSize;

    unsigned int timeSize = times.size();
    chunk.siglaySize = std::min(siglayChunkSize, siglayDim - chunk.siglayStart);
    chunk.timeSize = std::min(timeChunkSize, timeSize - chunk.timeStart);

    return chunk;
}

void FVCOMStructure::timeInterpolation(double time, int& time1Index, int& time2Index, double& time1Percent) const
{
    time1Index = getPreviousTimeIndex(time);
    double previousTime = getTime(time1Index);

    //Time is exactly on a time division, no interpolation needed.
    if(previousTime == time)
    {
        time2Index = time1Index;
        time1Percent = 1;
    }
    else //time is spilt between divisions so it needs interpolation
    {
        time2Index = time1Index + 1;
        double nextTime = getTime(time2Index);
        time1Percent = (nextTime - time) / (nextTime - previousTime);
    }
}

void FVCOMStructure::siglayInterpolation(FVCOMStructure::Point& interpolatePoint, int& siglay1Index, int& siglay2Index, double& siglay1Percent)
{
    int containingTriangle = getContainingTriangle(interpolatePoint);
    siglayInterpolation(interpolatePoint, siglay1Index, siglay2Index, siglay1Percent, containingTriangle);
}

void FVCOMStructure::siglayInterpolation(FVCOMStructure::Point& interpolatePoint, int& siglay1Index, int& siglay2Index, double& siglay1Percent, int containingTriangle)
{
    siglay1Index = siglay2Index = -1;

    double prevDot = 0;
    for(uint i = 0; i < getNumSiglays(); i++)
    {
        FVCOMStructure::Plane plane = getTriangleSiglayPlane(containingTriangle, i);

        //calculate the dot product with the plane and the point to determine which side of the plane it is on
        double dot = plane.a * interpolatePoint.x + plane.b * interpolatePoint.y + plane.c * interpolatePoint.z + plane.d;

        if(i != 0)
        {
            if(dot == 0) //point is in the plane, use current siglayIndex and no interpolation needed
            {
                siglay1Index = i;
                siglay2Index = i;
                break;
            }
            else if((dot > 0 && prevDot < 0) || //The sign of dot has changed so the siglay has been found
                    (dot < 0 && prevDot > 0))
            {
                siglay1Index = i - 1;
                siglay2Index = i;
                break;
            }
            else if((prevDot > 0 && prevDot < dot) || //The distance from the plane to the point is increasing so we have passed it.
                    (prevDot < 0 && prevDot > dot))   //If this occurs then it means the point is above the 0th siglay, use the 0th siglay
            {
                siglay1Index = 0;
                siglay2Index = 0;
                break;
            }
        }

        prevDot = dot;
    }

    //The dot product was always decreasing however it never changed sign.
    //Therefore the point is below the final siglay, so use the final siglay.
    if(siglay1Index == siglay2Index && siglay2Index == -1)
    {
        siglay1Index = getNumSiglays() -1;
        siglay2Index = getNumSiglays() -1;
    }

    if(siglay1Index == siglay2Index) //The point is above the 0th siglay so and there is no data there
    {
        siglay1Percent = 1.0;
    }
    else
    {
        FVCOMStructure::Plane upperPlane = getTriangleSiglayPlane(containingTriangle, siglay1Index);
        FVCOMStructure::Plane lowerPlane = getTriangleSiglayPlane(containingTriangle, siglay2Index);

        double upperH = (-upperPlane.d - upperPlane.a * interpolatePoint.x - upperPlane.b * interpolatePoint.y) / upperPlane.c;
        double lowerH = (-lowerPlane.d - lowerPlane.a * interpolatePoint.x - lowerPlane.b * interpolatePoint.y) / lowerPlane.c;

        siglay1Percent = (lowerH - interpolatePoint.z) / (lowerH - upperH);
    }
}

double FVCOMStructure::getDepthAtPoint(FVCOMStructure::Point& interpolatePoint, int containingTriangle)
{
    const std::vector<int>& surroundingNodes = getNodesInTriangle(containingTriangle);

    FVCOMStructure::Point p1 = getNodePointWithH(surroundingNodes[0]);
    FVCOMStructure::Point p2 = getNodePointWithH(surroundingNodes[1]);
    FVCOMStructure::Point p3 = getNodePointWithH(surroundingNodes[2]);

    FVCOMStructure::Plane groundPlane(p1,p2,p3);

    return groundPlane.getHeight(interpolatePoint);
}

double FVCOMStructure::getDepthAtPoint(FVCOMStructure::Point& interpolatePoint)
{
    int containingTriangle = getContainingTriangle(interpolatePoint);

    const std::vector<int>& surroundingNodes = getNodesInTriangle(containingTriangle);

    FVCOMStructure::Point p1 = getNodePointWithH(surroundingNodes[0]);
    FVCOMStructure::Point p2 = getNodePointWithH(surroundingNodes[1]);
    FVCOMStructure::Point p3 = getNodePointWithH(surroundingNodes[2]);

    FVCOMStructure::Plane groundPlane(p1,p2,p3);

    return groundPlane.getHeight(interpolatePoint);
}

const bool FVCOMStructure::pointInModel(Point p, double time)
{
    int containingTriangle = 0;
    try
    {
        containingTriangle = getContainingTriangle(p);
    }
    catch(const std::out_of_range& e)
    {
        return false;
    }

    Plane plane = getTrianglePlane(containingTriangle);
    double depth = plane.getHeight(p);
    return p.x >= minX && p.x <= maxX &&
           p.y >= minY && p.y <= maxY &&
           time >= times[0] && time <= times[times.size() - 1] &&
           p.z <= 0 && p.z >= -depth;
}

const bool FVCOMStructure::timeInModel(double time) const
{
    return time >= times[0] && time <= times[times.size() - 1];
}

const bool FVCOMStructure::depthInModel(Point p)
{
    int containingTriangle = 0;
    try
    {
        containingTriangle = getContainingTriangle(p);
    }
    catch(const std::out_of_range& e)
    {
        return false;
    }

    Plane plane = getTrianglePlane(containingTriangle);
    double depth = plane.getHeight(p);

    return p.z <= 0 && p.z >= -depth;
}

const bool FVCOMStructure::xyInModel(Point p) const
{
    return p.x >= minX && p.x <= maxX &&
           p.y >= minY && p.y <= maxY;
}

const std::vector<unsigned int>& FVCOMStructure::getNodesInChunk(FVCOMStructure::ChunkInfo chunk) const
{
    int chunkId = chunk.yChunk + (chunk.xChunk * yDimChunks);

    return nodesInChunk[chunkId];
}

const std::vector<unsigned int>& FVCOMStructure::getTrianglesInChunk(FVCOMStructure::ChunkInfo chunk) const
{
    int chunkId = chunk.yChunk + (chunk.xChunk * yDimChunks);

    return trianglesInChunk[chunkId];
}

const unsigned int FVCOMStructure::getNumSiglays() const
{
    return siglayDim;
}


////////////////////////////////////////////////////////////////////////////////
// msh_processor.cc
////////////////////////////////////////////////////////////////////////////////
/*! @file
//      Extracts and processes fields from a .msh file. The field processor is
//      essentually an RPN evaluator that maintains a stack of values and
//      applies the filters specified on the command line in order.
//
//      Component-wise Binary Operations:
//          add, sub, mul, div
//      Component-wise Unary Operations:
//          abs, scale, set
//      (Partial) reductions:
//          min, max, minMag, maxMag, norm, index, sum, mean
//      Reductions can be done in multiple wasy for multi-indexed objects
//      (e.g. vector fields, vector interpolants, fields of vector interpolants).
//      By default, reductions are done over the innermost index
//      ("pointwise" for fields/interpolants)
//          Field<NonScalar>       -> Field<Reduced>       (recursive)
//          Interpolant<NonScalar> -> Interpolant<Reduced> (recursive)
//          Field<Scalar>          -> Scalar
//          Interpolant<Scalar>    -> Scalar
//          PointValue             -> Scalar
//      When "outer reduction" mode is requested, reductions are done over the
//      outer index (per-component reduction for vector fields/interpolants).
//      Note: this is probably the more natural action for sum, mean of vector
//      fields.
//          Field<NonScalar>     -> Scalar
//          Field<Scalar>        -> Scalar
//          Interpolant<Scalar>  -> Scalar
//          Field<T>             -> T
//          Interpolant<T>       -> T
//          PointValue           -> Scalar
//      Warning: operations treat interpolants as vectors of nodal values, so
//      for interpolants with negative weights component-wise min/max won't
//      necessarily be the min/max over the simplex.
//
//      TODO: store element *index* on interpolant: binary operations can only
//      act on a pair of interpolants with matching element index.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Company:  New York University
//  Created:  08/16/2014 15:26:04
////////////////////////////////////////////////////////////////////////////////
#include "argparse.hh"
#include "Sampler.hh"
#include "Values.hh"
#include "MeshConnectivity.hh"
#include <MeshFEM/ExpressionVector.hh>
#include <MeshFEM/filters/remove_dangling_vertices.hh>
#include <MeshFEM/MSHFieldParser.hh>
#include <MeshFEM/MSHFieldWriter.hh>
#include <MeshFEM/SimplicialMesh.hh>
#include <MeshFEM/Types.hh>
#include <MeshFEM/VonMises.hh>
#include <boost/algorithm/string.hpp>
#include <iomanip>
#include <regex>
#include <vector>
#include <map>
#include <set>
#include <stdexcept>
#include <cmath>
#include <cctype>
#include <memory>
#include <functional>
#include <limits>
#include <sstream>

//using namespace MeshIO;
using namespace std;

// Global parsers used for 2 and 3D cases.
unique_ptr<MSHFieldParser<2>> g_parser2D;
unique_ptr<MSHFieldParser<3>> g_parser3D;

template<size_t N, typename... Args>
void parseMSH(Args&&... args) {
    static_assert(N == 2 || N == 3, "Invalid parser dimension");
    if (N == 2) g_parser2D = make_unique<MSHFieldParser<2>>(std::forward<Args>(args)...);
    else        g_parser3D = make_unique<MSHFieldParser<3>>(std::forward<Args>(args)...);
}

template<size_t N>
const MSHFieldParser<N> &getParser();
template<> const MSHFieldParser<2> &getParser<2>() { return *g_parser2D; }
template<> const MSHFieldParser<3> &getParser<3>() { return *g_parser3D; }

template<size_t N>
MSHFieldParser<N> &getMutableParser();
template<>       MSHFieldParser<2> &getMutableParser<2>() { return *g_parser2D; }
template<>       MSHFieldParser<3> &getMutableParser<3>() { return *g_parser3D; }

// Global lazily-constructed element samplers used for 2 and 3D cases.
unique_ptr<ElementSampler::Sampler<2>> g_sampler2D;
unique_ptr<ElementSampler::Sampler<3>> g_sampler3D;

template<size_t N>
const ElementSampler::Sampler<N> &getElementSampler();
template<> const ElementSampler::Sampler<2> &getElementSampler<2>() { if (g_sampler2D) return *g_sampler2D; g_sampler2D = make_unique<ElementSampler::Sampler<2>>(g_parser2D->vertices(), g_parser2D->elements()); return *g_sampler2D; }
template<> const ElementSampler::Sampler<3> &getElementSampler<3>() { if (g_sampler3D) return *g_sampler3D; g_sampler3D = make_unique<ElementSampler::Sampler<3>>(g_parser3D->vertices(), g_parser3D->elements()); return *g_sampler3D; }

// Global lazily-constructed mesh data structures for 2 and 3D cases.
unique_ptr<SimplicialMesh<2>> g_triMesh;
unique_ptr<SimplicialMesh<3>> g_tetMesh;

template<size_t N>
const SimplicialMesh<N> &getMeshDS();
template<> const SimplicialMesh<2> &getMeshDS<2>() { if (g_triMesh) return *g_triMesh; else g_triMesh = make_unique<SimplicialMesh<2>>(g_parser2D->elements(), g_parser2D->vertices().size()); return *g_triMesh; }
template<> const SimplicialMesh<3> &getMeshDS<3>() { if (g_tetMesh) return *g_tetMesh; else g_tetMesh = make_unique<SimplicialMesh<3>>(g_parser3D->elements(), g_parser3D->vertices().size()); return *g_tetMesh; }

////////////////////////////////////////////////////////////////////////////////
// Stack operations
////////////////////////////////////////////////////////////////////////////////
using Stack = vector<NamedValue>;

struct Modifiers {
    bool outerReduction = false;
    bool applyAll = false;
};

NamedValue &getValue(Stack &stack, size_t offset = 0) {
    if (stack.size() <= offset) throw std::runtime_error("Accessed out of stack bounds.");
    size_t idx = stack.size() - 1 - offset;
    return stack.at(idx);
}

NamedValue popValue(Stack &stack) {
    if (stack.empty()) throw std::runtime_error("Tried to pop from empty stack.");
    NamedValue val = std::move(getValue(stack));
    stack.pop_back();
    return val;
}

template<typename T>
TypedNamedValue<T> &getTypedValue(Stack &stack, size_t offset = 0) {
    return TypedNamedValue<T>(getValue(stack, offset));
}

template<typename T>
TypedNamedValue<T> popTypedValue(Stack &stack) {
    if (stack.empty()) throw std::runtime_error("Tried to pop from empty stack.");
    NamedValue val = std::move(stack.back());
    stack.pop_back();
    return val;
}

////////////////////////////////////////////////////////////////////////////////
// Filters - operate on the stack.
// These are all template functions with the signature:
// template<size_t N>
// size_t f(const string &op, const string &arg, Stack &stack, const Modifiers &m)
//
// Filters return the number of results they pushed onto the stack. Filters are
// considered to first pop all of their arguments then push "results" onto the
// stack. For example, the print operation that acts on the stack top but
// does not modify the stack is considered to have one result (it pops and
// pushes back the argument). The reverse operation, which acts on the whole
// stack, returns the stack size.
//
// Placed in their own namespace because their names overload those defined
// elsewhere, causing problems with the lookup table definition
////////////////////////////////////////////////////////////////////////////////
namespace Filter {
// Data source filters
// Extract field(s) matching the pattern in "arg", pushing them on the top of
// the stack.
size_t pushScalarField(Stack &stack, const string &name, const ScalarField<Real> &sf, const DomainType &dtype) {
    TypedNamedValue<FSValue> sfv(name, dtype, sf.domainSize());
    for (size_t i = 0; i < sf.domainSize(); ++i)
        sfv->value[i] = SValue(sf[i]);
    stack.push_back(std::move(sfv));
    return 1;
}

template<size_t N>
size_t pushVectorField(Stack &stack, const string &name, const VectorField<Real, N> &vf, const DomainType &dtype) {
    TypedNamedValue<FVValue> vfv(name, dtype, vf.domainSize());
    for (size_t i = 0; i < vf.domainSize(); ++i)
        vfv->value[i] = VValue(vf(i).eval());
    stack.push_back(std::move(vfv));
    return 1;
}

template<size_t N>
size_t pushSymmetricMatrixField(Stack &stack, const string &name, const SymmetricMatrixField<Real, N> &smf, const DomainType &dtype) {
    TypedNamedValue<FSMValue> smfv(name, dtype, smf.domainSize());
    for (size_t i = 0; i < smf.domainSize(); ++i)
        smfv->value[i] = SMValue(smf(i));
    stack.push_back(std::move(smfv));
    return 1;
}

template<class IFType, class RawType>
size_t pushInterpolantField(Stack &stack, const string &name, const RawType &raw_if, const DomainType &dtype) {
    TypedNamedValue<IFType> ifv(name, dtype, raw_if.size());
    for (size_t i = 0; i < raw_if.size(); ++i)
        ifv->value[i] = raw_if[i];
    stack.push_back(std::move(ifv));
    return 1;
}

template<size_t N>
size_t extract(const string &/*op*/, const string &arg, Stack &stack, const Modifiers &) {
    const auto &parser = getParser<N>();
    std::regex pattern(arg);
    size_t origSize = stack.size();
    DomainType dtype;
    for (const string &name : parser.scalarFieldNames()) {
        if (regex_match(name, pattern))
            pushScalarField(stack, name, parser.scalarField(name, DomainType::ANY, dtype), dtype);
    }
    for (const string &name : parser.vectorFieldNames()) {
        if (regex_match(name, pattern))
            pushVectorField(stack, name, parser.vectorField(name, DomainType::ANY, dtype), dtype);
    }
    for (const string &name : parser.symmetricMatrixFieldNames()) {
        if (regex_match(name, pattern))
            pushSymmetricMatrixField(stack, name, parser.symmetricMatrixField(name, DomainType::ANY, dtype), dtype);
    }
    for (const string &name : parser.scalarInterpolantFieldNames()) {
        if (regex_match(name, pattern))
            pushInterpolantField< FISValue>(stack, name, parser.         scalarInterpolantField(name, DomainType::ANY, dtype), dtype);
    }
    for (const string &name : parser.vectorInterpolantFieldNames()) {
        if (regex_match(name, pattern))
            pushInterpolantField< FIVValue>(stack, name, parser.         vectorInterpolantField(name, DomainType::ANY, dtype), dtype);
    }
    for (const string &name : parser.symmetricMatrixInterpolantFieldNames()) {
        if (regex_match(name, pattern))
            pushInterpolantField<FISMValue>(stack, name, parser.symmetricMatrixInterpolantField(name, DomainType::ANY, dtype), dtype);
    }

    assert(stack.size() >= origSize);
    if (stack.size() == origSize) throw runtime_error("No fields matched '" + arg + "'");
    return stack.size() - origSize;
}

template<size_t N>
size_t extractAll(const string &/*op*/, const string &/* arg */, Stack &stack, const Modifiers &) {
    size_t origSize = stack.size();
    const auto &parser = getParser<N>();
    DomainType dtype;
    for (const string &name : parser.scalarFieldNames())
        pushScalarField(stack, name, parser.scalarField(name, DomainType::ANY, dtype), dtype);
    for (const string &name : parser.vectorFieldNames())
        pushVectorField(stack, name, parser.vectorField(name, DomainType::ANY, dtype), dtype);
    for (const string &name : parser.symmetricMatrixFieldNames())
        pushSymmetricMatrixField(stack, name, parser.symmetricMatrixField(name, DomainType::ANY, dtype), dtype);

    assert(stack.size() >= origSize);
    return stack.size() - origSize;
}

template<size_t N>
size_t generate(const string &, const string &arg, Stack &stack, const Modifiers &) {
    const auto &parser = getParser<N>();
    const auto &vertices = parser.vertices();
    const auto &elements = parser.elements();
    if (arg == "x") {
        VectorField<Real, N> x(vertices.size());
        for (size_t i = 0; i < vertices.size(); ++i)
            x(i) = truncateFrom3D<VectorND<N>>(vertices[i].point);
        pushVectorField(stack, "x", x, DomainType::PER_NODE);
    }
    else if (arg == "volume") {
        const auto &sampler = getElementSampler<N>();
        ScalarField<Real> vol(elements.size());
        for (size_t i = 0; i < elements.size(); ++i)
            vol[i] = sampler.volume(i);
        pushScalarField(stack, "volume", vol, DomainType::PER_ELEMENT);
    }
    else if (arg == "barycenter") {
        VectorField<Real, N> c(elements.size());
        c.clear();
        for (size_t ei = 0; ei < elements.size(); ++ei) {
            const auto &e = elements[ei];
            for (size_t j : e)
                c(ei) += truncateFrom3D<VectorND<N>>(vertices.at(j).point);
            c(ei) *= 1.0 / e.size();
        }
        pushVectorField(stack, "barycenter", c, DomainType::PER_ELEMENT);
    }
    else throw std::runtime_error("Invalid mesh property name: " + arg);

    return 1;
}

template<size_t N>
size_t expression(const string &, const string &arg, Stack &stack, const Modifiers &) {
    const auto &parser = getParser<N>();
    const auto &vertices = parser.vertices();

    vector<string> components;
    boost::split(components, arg, boost::is_any_of(","));
    string name = "expr(" + arg + ")";

    ExpressionEnvironment env;
    BBox<VectorND<N>> bb(vertices);
    env.setVectorValue("mesh_size_", bb.dimensions());
    env.setVectorValue("mesh_min_",  bb.minCorner);
    env.setVectorValue("mesh_max_",  bb.maxCorner);

    if (components.size() == 1) {
        Expression expr(components[0]);
        ScalarField<Real> sf(vertices.size());
        for (size_t i = 0; i < vertices.size(); ++i) {
            env.setXYZ(vertices[i].point);
            sf[i] = expr.eval(env);
        }
        pushScalarField(stack, name, sf, DomainType::PER_NODE);
    }
    else if (components.size() == N) {
        ExpressionVector expr(components);
        VectorField<Real, N> vf(vertices.size());
        for (size_t i = 0; i < vertices.size(); ++i) {
            env.setXYZ(vertices[i].point);
            vf(i) = expr.eval<N>(env);
        }
        pushVectorField(stack, name, vf, DomainType::PER_NODE);
    }
    else throw std::runtime_error("Invalid number of components in vector-valued expression " + arg);

    return 1;
}

size_t     dup(const string &, const string &   , Stack &stack, const Modifiers &) { stack.emplace_back(getValue(stack)); return 2; } // Copies: NamedValue has value semantics
size_t     pop(const string &, const string &   , Stack &stack, const Modifiers &) { popValue(stack); return 0; }
size_t    push(const string &, const string &arg, Stack &stack, const Modifiers &) { double d = parseRealArg(arg); stack.push_back(TypedNamedValue<SValue>(to_string(d), d)); return 1; }
size_t reverse(const string &, const string &   , Stack &stack, const Modifiers &) { std::reverse(stack.begin(), stack.end()); return stack.size(); }
size_t pull(const string &, const string &arg, Stack &stack, const Modifiers &) {
    for (auto it = stack.begin(); it != stack.end(); ++it) {
        if ((*it).name == arg) {
            NamedValue val(std::move(*it));
            stack.erase(it);
            stack.emplace_back(std::move(val));
            return 1;
        }
    }
    throw runtime_error("Couldn't find '" + arg + "' for pull.");
}

////////////////////////////////////////////////////////////////////////////////
// Symmetric matrix operations
////////////////////////////////////////////////////////////////////////////////
struct Eigenvalues {
    // Returns a vector of eigenvalues.
    using value_type = VValue;
    static constexpr const char *name = "eigenvalues";
    static VValue apply(const SMValue &sm) { return VValue(sm.value.eigenvalues()); }
};

struct VonMises {
    // Returns a symmetric matrix
    using value_type = SMValue;
    static constexpr const char *name = "vonMises";
    static SMValue apply(const SMValue &sm) { return vonMises(sm.value); }
};

struct FrobeniusNorm {
    // Returns a scalar
    using value_type = SValue;
    static constexpr const char *name = "frobeniusNorm";
    static SValue apply(const SMValue &sm) { return sqrt(sm.value.frobeniusNormSq()); }
};

template<class Op>
size_t SMatrixOperation(const string &, const string &/* arg */, Stack &stack, const Modifiers &) {
    using VT = typename Op::value_type;
    auto val = popValue(stack);
    string name = std::string(Op::name) + "(" + val.name + ")";
    if (auto sm = dynamic_cast<SMValue *>(VPtr(val)))
        stack.push_back(TypedNamedValue<VT>(name, Op::apply(sm->value)));
    else if (auto ism = dynamic_cast<ISMValue *>(VPtr(val))) {
        auto result = make_unique<InterpolantValue<VT>>(ism->simplexDimension());
        for (size_t i = 0; i < result->dim(); ++i)
            (*result)[i] = VT(Op::apply((*ism)[i].value));
        stack.emplace_back(name, std::move(result));
    }
    else if (auto fsm = dynamic_cast<FSMValue *>(VPtr(val))) {
        auto result = make_unique<FieldValue<VT>>(fsm->size());
        result->domainType = fsm->domainType;
        for (size_t i = 0; i < result->size(); ++i) {
            (*result)[i] = VT(Op::apply((*fsm)[i].value));
        }
        stack.emplace_back(name, std::move(result));
    }
    else if (/*auto ifsm =*/ dynamic_cast<FISMValue *>(VPtr(val))) {
        throw std::runtime_error("Not yet implemented.");
    }
    else throw runtime_error("called on non-matrix type argument");

    return 1;
}

// // This matrix->vector operator unfortunately must be implemented manually in
// // the current framework...
// size_t eigenvaluesAndEigenvectors(const string &, const string &arg, Stack &stack, const Modifiers &) {
//     auto val = popValue(stack);
//     string name = "eigenvalues(" + val.name + ")";
//     if (auto sm = dynamic_cast<SMValue *>(VPtr(val)))
//         stack.push_back(TypedNamedValue<VValue>(name, sm->value.eigenvalues()));
//     else if (auto  ism = dynamic_cast<ISMValue *>(VPtr(val))) {
//         auto result = make_unique<IVValue>(ism->simplexDimension());
//         for (size_t i = 0; i < result->dim(); ++i) {
//             auto &sm = (*ism)[i];
//             (*result)[i] = VValue(sm.value.eigenvalues());
//         }
//         stack.emplace_back(name, std::move(result));
//     }
//     else if (auto  fsm = dynamic_cast<FSMValue *>(VPtr(val))) {
//         auto result = make_unique<FVValue>(fsm->size());
//         result->domainType = fsm->domainType;
//         for (size_t i = 0; i < result->size(); ++i) {
//             auto &sm = (*fsm)[i];
//             (*result)[i] = VValue(sm.value.eigenvalues());
//         }
//         stack.emplace_back(name, std::move(result));
//     }
//     else if (/*auto ifsm =*/ dynamic_cast<FISMValue *>(VPtr(val))) {
//         throw std::runtime_error("Not yet implemented.");
//     }
//     else throw runtime_error("called on non-matrix type argument");
//
//     return 1;
// }

template<size_t N>
size_t elementBarycenterFieldTransfer(const string &, const string &arg, Stack &stack, const Modifiers &) {
    auto &sampler = getElementSampler<N>();
    sampler.accelerate();

    MSHFieldParser<N> targetMesh(arg);

    PointND<N> center;
    std::vector<ElementSampler::Sample> samplePts; samplePts.reserve(targetMesh.elements().size());
    for (const auto &e : targetMesh.elements()) {
        center.setZero();
        for (size_t vi : e) center += truncateFrom3D<VectorND<N>>(targetMesh.vertices().at(vi));
        center *= 1.0 / e.size();
        samplePts.emplace_back(sampler(center));
    }

    auto &currentMesh = getMutableParser<N>();
    Stack xferStack;

    for (const auto &val : stack) {
        string name = "transfer(" + val.name + ")";
        xferStack.emplace_back(name, val->sample(samplePts,
                    currentMesh.meshDegree(), currentMesh.meshDimension(),
                    DomainType::PER_ELEMENT));
    }

    // Replace mesh/value stack with the new mesh, stack of transferred values.
    // Sampler is also invalidated
    stack = std::move(xferStack);
    currentMesh = std::move(targetMesh);
    g_sampler2D.reset();
    g_sampler3D.reset();

    return stack.size();
}

template<size_t N>
size_t loadNewMSH(const string &, const string &arg, Stack &, const Modifiers &) {
    auto &currentMesh = getMutableParser<N>();
    currentMesh = MSHFieldParser<N>(arg);
    g_sampler2D.reset();
    g_sampler3D.reset();
    return 0;
}

// Filter elements using an indicator scalar field.
// (elements with value > 0 are kept).
template<size_t N>
size_t filterElements(const string &, const string &, Stack &stack, const Modifiers &) {
    auto invalid = std::runtime_error("Invalid argument to filterElements. Argument must be a per-element scalar field.");
    try {
        auto &currentMesh = getMutableParser<N>();
        auto top = popTypedValue<FSValue>(stack);
        if (top->domainType != DomainType::PER_ELEMENT)
            throw invalid;
        auto verts = currentMesh.vertices();
        const auto &oldElems  = currentMesh.elements();
        std::vector<::MeshIO::IOElement> elems;
        for (size_t i = 0; i < oldElems.size(); ++i) {
            if (top->value[i].value > 0) elems.emplace_back(oldElems[i]);
        }
        remove_dangling_vertices(verts, elems);
        currentMesh.replaceMesh(elems, verts);
        stack.clear();
    }
    catch (...) {
        throw invalid;
    }
    return 0;
}

// Sample a field at the point(s) specified by vector list encoded in "arg".
// Nodal fields are interpolated using the mesh's finite element basis functions.
// Element fields are interpolated piecewise constant
// Interpolant fields are sampled at barycentric coordinates.
// Error is thrown for sample points outside the mesh (note: could happen on
// element boundaries if inside/outside check is not robust)
template<size_t N>
size_t sample(const string &, const string &arg, Stack &stack, const Modifiers &) {
    auto pts = parseVectorListArg<N>(arg);
    auto val = popValue(stack);
    // Determine element index, barycentric coordinates, and element node
    // indices of the containing element.
    const auto &sampler = getElementSampler<N>();
    const auto &parser = getParser<N>();

    for (const auto &p : pts) {
        stringstream ss;
        ss << p.format(Eigen::IOFormat(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ", "", "", "[", "]"));
        string name = "sample(" + val.name + ", " + ss.str() + ")";
        stack.emplace_back(name, val->sample(sampler(p), parser.meshDegree(),
                           parser.meshDimension()));
    }

    return pts.size();
}

// Average a field over each element.
template<size_t N>
size_t elementAverage(const string &, const string &/* arg */, Stack &stack, const Modifiers &) {
    auto val = popValue(stack);
    const auto &parser = getParser<N>();
    stack.emplace_back("elementAverage(" + val.name + ")",
                        val->elementAverage(parser.elements(), parser.meshDegree(), parser.meshDimension()));
    return 1;
}

// Create a per-element field by a volume-weighted averaging over each element's
// neighbors.
template<size_t N>
size_t smoothedElementField(const string &, const string &/* arg */, Stack &stack, const Modifiers &) {
    auto val = popValue(stack);
    const auto &parser = getParser<N>();

    // Compute element volumes
    const auto &sampler = getElementSampler<N>();
    size_t nelems = parser.elements().size();
    std::vector<Real> volumes(nelems);
    for (size_t i = 0; i < nelems; ++i)
        volumes[i] = sampler.volume(i);

    stack.emplace_back("smoothedElementField(" + val.name + ")",
                        val->smoothedElementField(parser.elements(), parser.meshDegree(), parser.meshDimension(),
                                                  volumes, MeshConnectivityImpl<SimplicialMesh<N>>(getMeshDS<N>())));
    return 1;
}

// Report filters
// List all fields parsed
template<size_t N>
size_t listNames(const string &, const string &/* arg */, Stack &, const Modifiers &) {
    const auto &parser = getParser<N>();
    for (const string &name : parser.         scalarFieldNames()) { cout << "s\t"  << name << endl; }
    for (const string &name : parser.         vectorFieldNames()) { cout << "v\t"  << name << endl; }
    for (const string &name : parser.symmetricMatrixFieldNames()) { cout << "sm\t" << name << endl; }

    for (const string &name : parser.         scalarInterpolantFieldNames()) { cout << "si\t"  << name << endl; }
    for (const string &name : parser.         vectorInterpolantFieldNames()) { cout << "vi\t"  << name << endl; }
    for (const string &name : parser.symmetricMatrixInterpolantFieldNames()) { cout << "smi\t" << name << endl; }
    return 0;
}

// Print the top of the stack.
size_t print    (const string &, const string &, Stack &stack, const Modifiers &) { getValue(stack)->print(); cout << endl; return 1; }
size_t printName(const string &, const string &, Stack &stack, const Modifiers &) { cout << getValue(stack).name << endl; return 1; }
size_t noprint  (const string &, const string &, Stack &     , const Modifiers &) { return 1; }

template<size_t N>
size_t importScalarField(const string &/* op */, const string &arg, Stack &stack, const Modifiers &) {
    const auto &parser = getParser<N>();
    const auto &vertices = parser.vertices();
    const auto &elements = parser.elements();
    ifstream inFile(arg);
    if (!inFile.is_open()) throw std::runtime_error("Couldn't open scalar field import file: " + arg);
    Real val;
    std::vector<Real> values;
    while (inFile >> val)
        values.push_back(val);
    if (values.size() == vertices.size())
        pushScalarField(stack, arg, values, DomainType::PER_NODE);
    else if (values.size() == elements.size())
        pushScalarField(stack, arg, values, DomainType::PER_ELEMENT);
    else throw std::runtime_error("Didn't recognize imported field size.");

    return 1;
}

// Import a flattened vector field (x0 y0 z0 x1 ...)
template<size_t N>
size_t importVectorField(const string &/* op */, const string &arg, Stack &stack, const Modifiers &) {
    const auto &parser = getParser<N>();
    const auto &vertices = parser.vertices();
    const auto &elements = parser.elements();
    ifstream inFile(arg);
    if (!inFile.is_open()) throw std::runtime_error("Couldn't open vector field import file: " + arg);
    Real val;
    std::vector<Real> values;
    while (inFile >> val)
        values.push_back(val);
    VectorField<Real, N> vfield(values);

    if (vfield.domainSize() == vertices.size())
        pushVectorField(stack, arg, vfield, DomainType::PER_NODE);
    else if (vfield.domainSize() == elements.size())
        pushVectorField(stack, arg, vfield, DomainType::PER_ELEMENT);
    else throw std::runtime_error("Didn't recognize imported field size.");

    return 1;
}

template<size_t N>
size_t outputMSH(const string &/* op */, const string &arg, Stack &stack, const Modifiers &) {
    const auto &parser = getParser<N>();
    // Note: there will be downsampling of interpolant fields if the output
    // mesh elements are linear. But in these cases, the original extracted
    // interpolant fields were linear as well.
    // TODO: rewrite as template code.
    MSHFieldWriter writer(arg, parser.vertices(), parser.elements(), parser.meshType());
    for (const auto &val : stack) {
        if (auto fs = dynamic_cast<const FSValue *>(CVPtr(val))) {
            ScalarField<Real> sf(fs->value.size());
            for (size_t i = 0; i < sf.domainSize(); ++i)
                sf[i] = fs->value[i].value;
            writer.addField(val.name, sf, fs->domainType);
        }
        else if (auto fv = dynamic_cast<const FVValue *>(CVPtr(val))) {
            VectorField<Real, N> vf(fv->value.size());
            for (size_t i = 0; i < vf.domainSize(); ++i)
                vf(i) = fv->value[i].value;
            writer.addField(val.name, vf, fv->domainType);
        }
        else if (auto fsm = dynamic_cast<const FSMValue *>(CVPtr(val))) {
            SymmetricMatrixField<Real, N> smf(fsm->value.size());
            for (size_t i = 0; i < smf.domainSize(); ++i)
                smf(i) = fsm->value[i].value;
            writer.addField(val.name, smf, fsm->domainType);
        }
        else if (auto fis = dynamic_cast<const FISValue *>(CVPtr(val))) {
            std::vector<typename InterpolantGetter<SValue, N>::storage_backed_type> isf(fis->value.size());
            for (size_t i = 0; i < fis->value.size(); ++i)
                isf[i] = InterpolantGetter<SValue, N>::get(fis->value[i]);
            writer.addField(val.name, isf, fis->domainType);
        }
        else if (auto fiv = dynamic_cast<const FIVValue *>(CVPtr(val))) {
            std::vector<typename InterpolantGetter<VValue, N>::storage_backed_type> ivf(fiv->value.size());
            for (size_t i = 0; i < fiv->value.size(); ++i)
                ivf[i] = InterpolantGetter<VValue, N>::get(fiv->value[i]);
            writer.addField(val.name, ivf, fiv->domainType);
        }
        else if (auto fism = dynamic_cast<const FISMValue *>(CVPtr(val))) {
            std::vector<typename InterpolantGetter<SMValue, N>::storage_backed_type> ismf(fism->value.size());
            for (size_t i = 0; i < fism->value.size(); ++i)
                ismf[i] = InterpolantGetter<SMValue, N>::get(fism->value[i]);
            writer.addField(val.name, ismf, fism->domainType);
        }
        else cout << "WARNING: ignored non-field value on stack: " << val.name << endl;
    }
    return stack.size();
}

size_t rename(const string &/* op */, const string &arg, Stack &stack, const Modifiers &) {
    vector<string> names;
    boost::split(names, arg, boost::is_any_of(","));
    if (names.size() > stack.size()) {
        throw runtime_error("Too many names provided to rename");
    }
    size_t pos = stack.size();
    for(auto &name : names)
        stack[--pos].name = std::move(name);
    return names.size();
}

template<size_t N>
size_t setNodePositions(const string &, const string &/* arg */, Stack &stack, const Modifiers &) {
    const auto &top = popTypedValue<FVValue>(stack);

    size_t nVals = top->size();
    std::vector<PointND<3>> newPositions(nVals);
    for (size_t i = 0; i < nVals; ++i)
        newPositions[i] = top->operator[](i).value;

    getMutableParser<N>().setNodePositions(newPositions);
    return 0;
}

template<class R>
size_t applyReduction(const string &op, const string &arg, Stack &stack, const Modifiers &m) {
    auto top = popValue(stack);
    string name = op + arg + "(" + top.name + ")";
    if (m.outerReduction) name = "outer_" + name;
    R r(arg);
    if (m.outerReduction) stack.emplace_back(name, top->outerReduction(r));
    else                  stack.emplace_back(name, top->innerReduction(r));
    return 1;
}

template<class UOp>
size_t applyUnaryOp(const string &op, const string &arg, Stack &stack, const Modifiers &) {
    auto top = popValue(stack);
    stack.emplace_back(op + arg + "(" + top.name + ")",
                       top->componentwiseUnaryOp(UOp(arg)));
    return 1;
}

template<class BOp>
size_t applyBinaryOp(const string &op, const string &arg, Stack &stack, const Modifiers &) {
    // Top of stack is the second operand, next in stack is the first
    auto b = popValue(stack);
    auto a = popValue(stack);
    if (arg.size() != 0) throw runtime_error("Did not expect binary op argument");
    stack.emplace_back(op + "(" + a.name + ", " + b.name + ")",
                       a->componentwiseBinaryOp(BOp(), b));
    return 1;
}

} // end namespace Filter

template<size_t N>
void execute(vector<FilterInvocation> &filters) {
    map<string, function<size_t(const string &, const string &, Stack &, const Modifiers &)>>
    filterImplementations = {
        // Reductions
        {"min",    Filter::applyReduction<ReductionMin   >},
        {"max",    Filter::applyReduction<ReductionMax   >},
        {"minMag", Filter::applyReduction<ReductionMinMag>},
        {"maxMag", Filter::applyReduction<ReductionMaxMag>},
        {"norm",   Filter::applyReduction<ReductionNorm  >},
        {"sum",    Filter::applyReduction<ReductionSum   >},
        {"mean",   Filter::applyReduction<ReductionMean  >},
        {"index",  Filter::applyReduction<ReductionIndex >},
        // Unary operations
        {"abs",    Filter::applyUnaryOp<AbsOp  >},
        {"scale",  Filter::applyUnaryOp<ScaleOp>},
        {"set",    Filter::applyUnaryOp<SetOp  >},
        // Binary operations
        {"add",    Filter::applyBinaryOp<AddOp>},
        {"sub",    Filter::applyBinaryOp<SubOp>},
        {"mul",    Filter::applyBinaryOp<MulOp>},
        {"div",    Filter::applyBinaryOp<DivOp>},
        // Custom value operations
        {"print",          Filter::print},
        {"noprint",        Filter::noprint},
        {"printName",      Filter::printName},
        {"eigenvalues",    Filter::SMatrixOperation<Filter::Eigenvalues>},
        {"vonMises",       Filter::SMatrixOperation<Filter::VonMises>},
        {"frobeniusNorm",  Filter::SMatrixOperation<Filter::FrobeniusNorm>},
        // {"eigs",           Filter::eigenvaluesAndEigenvectors},
        {"sample",         Filter::sample<N>},
        {"elementAverage", Filter::elementAverage<N>},
        {"smoothedElementField", Filter::smoothedElementField<N>},
        // Stack operations
        {"list",          Filter::listNames<N>},
        {"extract",       Filter::extract<N>},
        {"extractAll",    Filter::extractAll<N>},
        {"generate",      Filter::generate<N>},
        {"expression",    Filter::expression<N>},
        {"dup",           Filter::dup},
        {"pop",           Filter::pop},
        {"push",          Filter::push},
        {"pull",          Filter::pull},
        {"rename",        Filter::rename},
        {"import_sfield", Filter::importScalarField<N>},
        {"import_vfield", Filter::importVectorField<N>},
        {"reverse",       Filter::reverse},
        {"setNodePositions", Filter::setNodePositions<N>},
        {"outMSH",        Filter::outputMSH<N>},

        {"transferFieldsToPerElem", Filter::elementBarycenterFieldTransfer<N>},
        {"loadNewMSH", Filter::loadNewMSH<N>},
        {"filterElements", Filter::filterElements<N>}
    };

    // Classify the operations.
    set<string> reductions = { "min", "max", "minMag", "maxMag",
                               "norm", "sum", "mean", "index" };
    set<string> unaryOps  = { "abs", "scale", "set" };
    set<string> binaryOps = { "add", "sub", "mul", "div" };

    // The following commands suppress automatic output of stack at exit when performed last
    set<string> suppressImplicitPrint = { "noprint", "print", "outMSH", "list" };

    // Apply-all makes sense only for the following operations:
    set<string> acceptsApplyAll = { "print", "printName", "eigenvalues",
                                    "vonMises", "frobeniusNorm", "sample" };
    acceptsApplyAll.insert(reductions.begin(), reductions.end());
    acceptsApplyAll.insert(  unaryOps.begin(),   unaryOps.end());
    acceptsApplyAll.insert( binaryOps.begin(),  binaryOps.end());

    // Implicit list operation when filters are empty
    if (filters.size() == 0) filters.push_back({"list", ""});

    // Add an implicit print operation unless it is supressed
    if (!suppressImplicitPrint.count(filters.back().first)) filters.push_back({"print", ""});

    Stack stack;
    for (size_t fi = 0; fi < filters.size(); ++fi) {
        try {
            Modifiers m; // fresh modifier flags
            runtime_error missingOperation("Modifier specified without an operation.");
            // Note: applyAll should appear *first*
            if (filters[fi].first == "applyAll")       { m.applyAll       = true; ++fi; }
            if (fi >= filters.size()) throw missingOperation;
            if (filters[fi].first == "outerReduction") { m.outerReduction = true; ++fi; }
            if (fi >= filters.size()) throw missingOperation;
            const auto &f = filters[fi];

            // Validate modifiers.
            if (m.outerReduction && (reductions.count(f.first) == 0))
                throw runtime_error("--outerReduction must be followed by reduction");
            if (m.applyAll && (acceptsApplyAll.count(f.first) == 0))
                throw runtime_error("operation does not support apply all");

            // Perform the filter either once or once per stack value.
            if (m.applyAll) {
                Stack newStack;
                while (stack.size()) {
                    size_t n = filterImplementations.at(f.first)(f.first, f.second, stack, m);
                    // Move each result over to the new stack
                    for (size_t r = 0; r < n; ++r)
                        newStack.emplace_back(popValue(stack));
                }
                std::reverse(newStack.begin(), newStack.end());
                stack = std::move(newStack);
            }
            else filterImplementations.at(f.first)(f.first, f.second, stack, m);
        }
        catch (const exception &e) {
            if (fi < filters.size())
                cout << "Filter '" << filters[fi].first << "' failed: " << e.what() << endl;
            else
                cout << "Filter failed: " << e.what() << endl;
            exit(-1);
        }
    }
}

int main(int argc, char *argv[])
{
    cout << std::scientific << std::setprecision(16);
    MeshIO::MeshIO_MSH io;
    vector<MeshIO::IOVertex>  v;
    vector<MeshIO::IOElement> e;

    string mshFile;
    vector<FilterInvocation> filters;
    auto forcedDim = boost::make_optional(false, size_t()); // work around maybe-uninitialized GCC warning bug
    std::tie(mshFile, filters, forcedDim) = parseCmdLine(argc, argv);

    ifstream infile(mshFile);
    if (!infile.is_open()) throw runtime_error("Couldn't open " + mshFile);
    MeshIO::MeshType type = io.load(infile, v, e, MeshIO::MESH_GUESS);
    size_t meshDim = ::MeshIO::meshDimension(type);

    size_t dim = forcedDim ? *forcedDim : meshDim;
    if (dim < 2 || dim > 3) throw std::runtime_error("Unsupported dimension: " + std::to_string(dim));

    if (meshDim != dim)
        cerr << "Warning: some operations won't work properly on non-full-dimension meshes" << endl;

    if (dim == 3) parseMSH<3>(infile, type, std::move(e), std::move(v), io.binary(), meshDim != dim);
    else          parseMSH<2>(infile, type, std::move(e), std::move(v), io.binary(), meshDim != dim);

    if (dim == 3) execute<3>(filters);
    else          execute<2>(filters);

    return 0;
}


// Copyright Nick Thompson, 2019
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)
#ifndef BOOST_MATH_INTERPOLATORS_WHITAKKER_SHANNON_DETAIL_HPP
#define BOOST_MATH_INTERPOLATORS_WHITAKKER_SHANNON_DETAIL_HPP
#include <boost/assert.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/sin_pi.hpp>
#include <boost/math/special_functions/cos_pi.hpp>

namespace boost { namespace math { namespace interpolators { namespace detail {

template<class RandomAccessContainer>
class whittaker_shannon_detail {
public:

    using Real = typename RandomAccessContainer::value_type;
    whittaker_shannon_detail(RandomAccessContainer&& y, Real const & t0, Real const & h) : m_y{std::move(y)}, m_t0{t0}, m_h{h}
    {
        for (size_t i = 1; i < m_y.size(); i += 2)
        {
            m_y[i] = -m_y[i];
        }
    }

    inline Real operator()(Real t) const {
        using boost::math::constants::pi;
        using std::isfinite;
        using std::floor;
        Real y = 0;
        Real x = (t - m_t0)/m_h;
        Real z = x;
        auto it = m_y.begin();

        // For some reason, neither clang nor g++ will cache the address of m_y.end() in a register.
        // Hence make a copy of it:
        auto end = m_y.end();
        while(it != end)
        {

            y += *it++/z;
            z -= 1;
        }

        if (!isfinite(y))
        {
            BOOST_ASSERT_MSG(floor(x) == ceil(x), "Floor and ceiling should be equal.\n");
            size_t i = static_cast<size_t>(floor(x));
            if (i & 1)
            {
                return -m_y[i];
            }
            return m_y[i];
        }
        return y*boost::math::sin_pi(x)/pi<Real>();
    }

    Real prime(Real t) const {
        using boost::math::constants::pi;
        using std::isfinite;
        using std::floor;

        Real x = (t - m_t0)/m_h;
        if (ceil(x) == x) {
            Real s = 0;
            long j = static_cast<long>(x);
            long n = m_y.size();
            for (long i = 0; i < n; ++i)
            {
                if (j - i != 0)
                {
                    s += m_y[i]/(j-i);
                }
                // else derivative of sinc at zero is zero.
            }
            if (j & 1) {
                s /= -m_h;
            } else {
                s /= m_h;
            }
            return s;
        }
        Real z = x;
        auto it = m_y.begin();
        Real cospix = boost::math::cos_pi(x);
        Real sinpix_div_pi = boost::math::sin_pi(x)/pi<Real>();

        Real s = 0;
        auto end = m_y.end();
        while(it != end)
        {
            s += (*it++)*(z*cospix - sinpix_div_pi)/(z*z);
            z -= 1;
        }

        return s/m_h;
    }

    Real operator[](size_t i) const {
        if (i & 1)
        {
            return -m_y[i];
        }
        return m_y[i];
    }

    RandomAccessContainer&& return_data() {
        for (size_t i = 1; i < m_y.size(); i += 2)
        {
            m_y[i] = -m_y[i];
        }
        return std::move(m_y);
    }

private:
    RandomAccessContainer m_y;
    Real m_t0;
    Real m_h;
};
}}}}
#endif