#include <iostream>
#include <vector>
#include <string>
#include <Eigen/Dense>
#include <boost/python.hpp>
#include <math.h>
#include <chrono>
#include <boost/python/numpy.hpp>

using namespace std;
using namespace boost::python;
using namespace Eigen;
namespace np = boost::python::numpy;

typedef vector<double> vec;
typedef vector<vec> mat;

#define PI 3.14159265

class Arm {
public:
  int numDOF;
  boost::python::list displacements;
  boost::python::list axes;
  boost::python::list rotOffsets;
  boost::python::tuple dispOffset;
  boost::python::list velocity_limits;
  boost::python::list joint_limits;
  char* name;
  Matrix3d rotX;
  Matrix3d rotY;
  Matrix3d rotZ;

  Arm(boost::python::list axes, boost::python::list displacements,
    boost::python::list rotOffsets, boost::python::tuple dispOffset, char* name) {
    Py_Initialize();
    np::initialize();
    this->axes = axes;
    this->displacements = displacements;
    this->rotOffsets = rotOffsets;
    this->dispOffset = dispOffset;
    this->numDOF = len(displacements);
    this->name = name;

    this->rotX = MatrixXd::Zero(3,3);
    this->rotY = MatrixXd::Zero(3,3);
    this->rotZ = MatrixXd::Zero(3,3);

    this->rotX(0,0) = 1.0;
    this->rotY(1,1) = 1.0;
    this->rotZ(2,2) = 1.0;
  }

  Matrix3d& rot3(char axis, double s, double c) {
    if (axis == 'z' || axis == 'Z') {
      this->rotZ(0,0) = c;
      this->rotZ(0,1) = -s;
      this->rotZ(1,0) = s;
      this->rotZ(1,1) = c;
      return this->rotZ;
    }
    else if(axis == 'y' || axis == 'Y') {
      this->rotY(0,0) = c;
      this->rotY(0,2) = s;
      this->rotY(2,0) = -s;
      this->rotY(2,2) = c;
      return this->rotY;
    }
    else if(axis == 'x' || axis == 'X') {
      this->rotX(1,1) = c;
      this->rotX(1,2) = -s;
      this->rotX(2,1) = s;
      this->rotX(2,2) = c;
      return this->rotX;
    }
    else {
      cout << "ERROR: not a valid axis label";
    }
  }

  boost::python::list getFrames(boost::python::list state) {
    boost::python::list ret;
    boost::python::list pts;
    Vector3d pt = this->array(this->dispOffset);
    pts.append(this->tolist(pt));
    boost::python::list frames;
    Matrix3d rot = MatrixXd::Zero(3,3);
    rot(0,0) = 1.0;
    rot(1,1) = 1.0;
    rot(2,2) = 1.0;
    frames.append(np::array(this->tolist(rot)));

    for(int i = 0; i < this->numDOF; i++) {
      double s = sin(extract<double>(state[i]));
      double c = cos(extract<double>(state[i]));

      //TODO: do rotation offsets

      char curr_axis = extract<char>(this->axes[i]);

      Matrix3d rmat = this->rot3(curr_axis,s,c);
      rot = rot*rmat;
      Vector3d disp;
      disp(0) = extract<double>((this->displacements[i])[0]);
      disp(1) = extract<double>((this->displacements[i])[1]);
      disp(2) = extract<double>((this->displacements[i])[2]);
      // cout << rot(1,1);
      // cout << "\n";
      // high_resolution_clock::time_point t1 = high_resolution_clock::now();
      pt = (rot*disp) + pt;
      // high_resolution_clock::time_point t2 = high_resolution_clock::now();
      // duration<double> time_span = duration_cast<duration<double>>(t2 - t1);
      // std::cout << "It took me " << time_span.count() << " seconds.\n";

      pts.append(np::array(this->tolist(pt)));
      frames.append(np::array(this->tolist(rot)));
    }

    ret.append(pts);
    ret.append(frames);
    return ret;
  }

private:

  Vector3d array(boost::python::list input) {
    Vector3d v;
    v(0) = extract<double>(input[0]);
    v(1) = extract<double>(input[1]);
    v(2) = extract<double>(input[2]);
    return v;
  }

  Vector3d array(boost::python::tuple input) {
    Vector3d v;
    v(0) = extract<double>(input[0]);
    v(1) = extract<double>(input[1]);
    v(2) = extract<double>(input[2]);
    return v;
  }

  boost::python::list tolist(Vector3d v) {
    boost::python::list l;
    l.append(v(0));
    l.append(v(1));
    l.append(v(2));
    return l;
  }

  mat tomat(Vector3d v) {
    mat ret;
    vec ve;
    ve.push_back(v(0));
    ve.push_back(v(1));
    ve.push_back(v(2));
    ret.push_back(ve);
    return ret;
  }

  mat tomat(Matrix3d m) {
    mat ret;
    vec row1;
    vec row2;
    vec row3;

    row1.push_back(m(0,0));
    row1.push_back(m(0,1));
    row1.push_back(m(0,2));

    row2.push_back(m(1,0));
    row2.push_back(m(1,1));
    row2.push_back(m(1,2));

    row3.push_back(m(2,0));
    row3.push_back(m(2,1));
    row3.push_back(m(2,2));

    ret.push_back(row1);
    ret.push_back(row2);
    ret.push_back(row3);

    return ret;
  }

  boost::python::list tolist(Matrix3d m) {
    boost::python::list ret;
    boost::python::list row1;
    boost::python::list row2;
    boost::python::list row3;

    row1.append(m(0,0));
    row1.append(m(0,1));
    row1.append(m(0,2));

    row2.append(m(1,0));
    row2.append(m(1,1));
    row2.append(m(1,2));

    row3.append(m(2,0));
    row3.append(m(2,1));
    row3.append(m(2,2));

    ret.append(row1);
    ret.append(row2);
    ret.append(row3);

    return ret;
  }

  np::ndarray convert_to_numpy(mat const & input) {
    u_int n_rows = input.size();
    u_int n_cols = input[0].size();
    boost::python::tuple shape = boost::python::make_tuple(n_rows, n_cols);
    boost::python::tuple stride = boost::python::make_tuple(sizeof(double));
    np::dtype dtype = np::dtype::get_builtin<double>();
    boost::python::object own;
    np::ndarray converted = np::zeros(shape, dtype);

    for (u_int i = 0; i < n_rows; i++)
    {
        shape = boost::python::make_tuple(n_cols);
        converted[i] = np::from_data(input[i].data(), dtype, shape, stride, own);
    }
    return converted;
  }

};

BOOST_PYTHON_MODULE(Arm_ext) {
    class_<Arm>("Arm", init<boost::python::list, boost::python::list,
    boost::python::list, boost::python::tuple, char*>())
        .def("getFrames", &Arm::getFrames)
        .def_readwrite("numDOF", &Arm::numDOF)
        .def_readwrite("name", &Arm::name)
        .def_readwrite("velocity_limits", &Arm::velocity_limits)
        .def_readwrite("rotOffsets", &Arm::rotOffsets)
        .def_readwrite("dispOffset", &Arm::dispOffset)
        .def_readwrite("joint_limits", &Arm::joint_limits)
        .def_readwrite("displacements", &Arm::displacements)
        .def_readwrite("axes", &Arm::axes)
    ;
}


/*
 * van_der_pol_stiff.cpp
 *
 * Created on: Dec 12, 2011
 *
 * Copyright 2011 Rajeev Singh
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <iostream>
#include <fstream>
#include <utility>

#include <boost/numeric/odeint.hpp>

#include <boost/phoenix/core.hpp>
#include <boost/phoenix/operator.hpp>

using namespace std;
using namespace boost::numeric::odeint;
namespace phoenix = boost::phoenix;

const double mu = 1000.0;

typedef boost::numeric::ublas::vector< double > vector_type;
typedef boost::numeric::ublas::matrix< double > matrix_type;

struct vdp_stiff
{
    void operator()( const vector_type &x , vector_type &dxdt , double t )
    {
        dxdt[0] = x[1];
        dxdt[1] = -x[0] - mu * x[1] * (x[0]*x[0]-1.0);
    }
};

struct vdp_stiff_jacobi
{
    void operator()( const vector_type &x , matrix_type &J , const double &t , vector_type &dfdt )
    {
        J(0, 0) = 0.0;
        J(0, 1) = 1.0;
        J(1, 0) = -1.0 - 2.0*mu * x[0] * x[1];
        J(1, 1) = -mu * ( x[0] * x[0] - 1.0);

        dfdt[0] = 0.0;
        dfdt[1] = 0.0;
    }
};

int main( int argc , char **argv )
{
    //[ integrate_stiff_system
    vector_type x( 2 );
    /* initialize random seed: */
    srand ( time(NULL) );

    // initial conditions
    for (int i=0; i<2; i++)
        x[i] = 1.0; //(1.0 * rand()) / RAND_MAX;

    size_t num_of_steps = integrate_const( make_dense_output< rosenbrock4< double > >( 1.0e-6 , 1.0e-6 ) ,
            make_pair( vdp_stiff() , vdp_stiff_jacobi() ) ,
            x , 0.0 , 1000.0 , 1.0
            , cout << phoenix::arg_names::arg2 << " " << phoenix::arg_names::arg1[0] << " " << phoenix::arg_names::arg1[1] << "\n"
            );
    //]
    clog << num_of_steps << endl;

    //[ integrate_stiff_system_alternative

    vector_type x2( 2 );
    // initial conditions
    for (int i=0; i<2; i++)
        x2[i] = 1.0; //(1.0 * rand()) / RAND_MAX;

    size_t num_of_steps2 = integrate_const( make_dense_output< runge_kutta_dopri5< vector_type > >( 1.0e-6 , 1.0e-6 ) ,
            vdp_stiff() , x2 , 0.0 , 1000.0 , 1.0
            , cout << phoenix::arg_names::arg2 << " " << phoenix::arg_names::arg1[0] << " " << phoenix::arg_names::arg1[1] << "\n"
            );
    //]
    clog << num_of_steps2 << endl;

    return 0;
}


#include <string>
#include <algorithm>
#include <vector>
#include <array>
#include <memory>
#include <map>
#include <cassert>
#include <fstream>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include "DatasetAR.h"
#include "GaussSeq.h"
#include "ZeroSeq.h"
#include "utils.h"
using utils::my_float;
using boost::random::uniform_real_distribution;

/*
 *  Member variables
 */
boost::random::mt19937 DatasetAR::gen {};

/*
 *  Helper functions
 *
 *  All "noise setters" require the target sequence be defined.
 */
void DatasetAR::noise_zero() {
    for (auto i = 0; i < utils::FEAT_COUNT; i++) {
        feats.push_back(std::make_unique<ZeroSeq>());
    }
    feats[utils::SALIENT_IND] = std::make_unique<ARSeq>(this->target);
}
void DatasetAR::noise_one() {
    for (auto i = 0; i < utils::FEAT_COUNT; i++) {
        feats.push_back(std::make_unique<GaussSeq>());
    }
    feats[utils::SALIENT_IND] = std::make_unique<ARSeq>(this->target);
}
void DatasetAR::noise_two() {
    uniform_real_distribution<my_float> u_mean(-10, 10);
    uniform_real_distribution<my_float> u_std(0.1, 5);

    for (auto i = 0; i < utils::FEAT_COUNT; i++) {
        feats.push_back(std::make_unique<GaussSeq>(u_mean(gen), u_std(gen)));
    }
    feats[utils::SALIENT_IND] = std::make_unique<ARSeq>(this->target);
}
void DatasetAR::noise_three() {
    uniform_real_distribution<my_float> u_const(0, 1);
    uniform_real_distribution<my_float> u_coeff(-0.9, 0.9);
    uniform_real_distribution<my_float> u_seed(-1, 1);
    std::vector<my_float> seed_vect {u_seed(gen),};
    std::map<int, my_float> coeff_map {
        {1, u_coeff(gen)},
    };

    for (auto i = 0; i < utils::FEAT_COUNT; i++) {
        // prep seq backing
        coeff_map[1] = u_coeff(gen);
        seed_vect[0] = u_seed(gen);

        ARSeq in_seq(coeff_map, u_const(gen));
        in_seq.seed_prev_vals(seed_vect);

        feats.push_back(std::make_unique<ARSeq>(in_seq));
    }
    feats[utils::SALIENT_IND] = std::make_unique<ARSeq>(this->target);
}
void DatasetAR::noise_four() {
    uniform_real_distribution<my_float> u_const(-1000000, -999999);
    uniform_real_distribution<my_float> u_coeff(1.1, 2);
    uniform_real_distribution<my_float> u_seed(0, 0.0000001);
    std::vector<my_float> seed_vect {u_seed(gen),};
    std::map<int, my_float> coeff_map {
        {1, u_coeff(gen)},
    };

    for (auto i = 0; i < utils::FEAT_COUNT; i++) {
        // prep seq backing
        coeff_map[1] = u_coeff(gen);
        seed_vect[0] = u_seed(gen);

        ARSeq in_seq(coeff_map, u_const(gen));
        in_seq.seed_prev_vals(seed_vect);

        feats.push_back(std::make_unique<ARSeq>(in_seq));
    }
    feats[utils::SALIENT_IND] = std::make_unique<ARSeq>(this->target);
}

/*
 *  Constructors and destructors
 */
DatasetAR::DatasetAR(std::string file_name, ARSeq target_seq):
    fname{file_name.append(".csv")},
    target{target_seq}
    {};

DatasetAR::DatasetAR(std::string file_name, ARSeq target_seq, unsigned char type):
    fname{file_name.append(".csv")},
    noise_type{type},
    target{target_seq} {
    set_noise(type);
}

/*
 *  Member functions
 */
void DatasetAR::set_noise(unsigned char type) {
    this->noise_type = type;

    switch (type) {
        case 0: this->noise_zero();
                break;
        case 1: this->noise_one();
                break;
        case 2: this->noise_two();
                break;
        case 3: this->noise_three();
                break;
        case 4: this->noise_four();
                break;
    }
    this->prev_targ_val = feats[utils::SALIENT_IND]->next();
}
std::array<std::array<my_float, utils::FEAT_COUNT + 1>, utils::TIME_STEP>
    DatasetAR::generate_normalized_dat() {
    namespace mp = boost::multiprecision;
    constexpr int T = utils::TIME_STEP;
    constexpr int F = utils::FEAT_COUNT;
    // observe storage[feat][time] indexing
    std::array<std::array<my_float, T + 1>, F> storage {};
    std::array<my_float, T + 1> targ_storage {};            // storage for target

    /*
     *  Prepare progress bar
     */

    // offset used to determine when to update progress bar
    int offset_mod = static_cast<int>(F / 10);
    int offset_counter {0};
    // print initial progress bar
    std::cout << "[";
    for (auto i = 0; i < 10; i++) {
        std::cout << " ";
    }
    std::cout << "]\r";

    /*
     *  Store non-normalized values
     */

    // Store time-delayed values
    // If noise type 3 or 4, then there is one previous value as a ARSeq.
    // Otherwise, simply invoke next()
    if (this->noise_type == 3 || this->noise_type == 4) {
        for (auto f = 0; f < F; f++) {
            if (f == utils::SALIENT_IND)
                targ_storage[0] = this->prev_targ_val;
            else
                storage[f][0] = dynamic_cast<ARSeq*>(feats[f].get())->get_prev_val()[0];
        }
    } else {
        for (auto f = 0; f < F; f++) {
            if (f == utils::SALIENT_IND)
                targ_storage[0] = this->prev_targ_val;
            else
                storage[f][0] = feats[f]->next();
        }
    }

    // Store current values (those non-negative)
    for (auto f = 0; f < F; f++) {
        // print progress bar
        if ((f - 1) % offset_mod == 0) {
            offset_counter++;

            std::cout << "[";
            for (auto i = 0; i < offset_counter; i++) {
                std::cout << "X";
            }
            for (auto i = 0; i < 10 - offset_counter; i++) {
                std::cout << " ";
            }
            std::cout << "]\r";
        }

        // write and record sequences
        for (int t = 1; t < T + 1; t++) {
            if (f != utils::SALIENT_IND)
                storage[f][t] = feats[f]->next();
            else {
                // target column
                this->prev_targ_val = feats[utils::SALIENT_IND]->next();
                targ_storage[t] = this->prev_targ_val;
            }
        }
    }

    /*
     *  Normalize array values according to utils::NORM_VAL via min-max norm
     */
    my_float feat_min {0};
    my_float feat_max {0};
    // observe normalized_feats[time][feat] indexing
    std::array<std::array<my_float, F + 1>, T> normalized_feats {};

    // set normalized non-targ vals
    for (auto f = 0; f < F; f++) {
        if (f != utils::SALIENT_IND) {
            // if we have zero values, no need to rescale
            if (this->noise_type == 0) {
                for (auto t = 0; t < T; t++) {
                    normalized_feats[t][f] = storage[f][t];
                }
            } else {
                feat_min = *std::min_element(storage[f].begin(), storage[f].end());
                feat_max = *std::max_element(storage[f].begin(), storage[f].end());

                for (auto t = 0; t < T; t++) {
                    if (feat_max - feat_min == 0)
                        normalized_feats[t][f] = (utils::NORM_VAL * 2 * (storage[f][t] - feat_min)) -
                            utils::NORM_VAL;
                    else
                        normalized_feats[t][f] = (utils::NORM_VAL * 2 * ((storage[f][t] - feat_min) /
                            (feat_max - feat_min))) - utils::NORM_VAL;
                }
            }
        } else {
            feat_min = *std::min_element(targ_storage.begin(), targ_storage.end());
            feat_max = *std::max_element(targ_storage.begin(), targ_storage.end());

            for (auto t = 0; t < T; t++) {
                if (feat_max - feat_min == 0) {
                    normalized_feats[t][utils::SALIENT_IND] = (utils::NORM_VAL * 2 * (targ_storage[t] - feat_min)) -
                        utils::NORM_VAL;
                    normalized_feats[t][F] = (utils::NORM_VAL * 2 * (targ_storage[t + 1] - feat_min)) -
                        utils::NORM_VAL;
                } else {
                    normalized_feats[t][utils::SALIENT_IND] = (utils::NORM_VAL * 2 * ((targ_storage[t] - feat_min) /
                        (feat_max - feat_min))) - utils::NORM_VAL;
                    normalized_feats[t][F] = (utils::NORM_VAL * 2 * ((targ_storage[t + 1] - feat_min) /
                        (feat_max - feat_min))) - utils::NORM_VAL;
                }
            }
        }
    }

    return normalized_feats;
}

void DatasetAR::write_csv() {
    // was the noise value assigned?
    assert(this->noise_type != 10);

    // storage for normalize values
    constexpr int T = utils::TIME_STEP;
    constexpr int F = utils::FEAT_COUNT;
    std::array<std::array<my_float, F + 1>, T> storage = generate_normalized_dat();

    // initialize csv
    std::ofstream synth_data {this->fname};
    synth_data << std::setprecision(25);

    // prepare labels
    for (auto i = 0; i < utils::FEAT_COUNT + 2; i++) {
        if (i == 0)
            synth_data << "Time,";
        else if (i == utils::FEAT_COUNT + 1)
            synth_data << "Target\n";
        else
            synth_data << "Feature_" << i << ",";
    }

    // write values to csv
    for (auto t = 0; t < T; t++) {
        for (auto f = 0; f < F + 2; f++) {
            // time column
            if (f == 0)
                synth_data << t << ",";
            // target column
            else if (f == utils::FEAT_COUNT + 1)
                synth_data << storage[t][f - 1] << "\n";
            // data column
            else
                synth_data << storage[t][f - 1] << ",";
        }
    }
    std::cout << "\nFinished writing " << this->fname << "!\n";
    synth_data.close();
}


//
// Created by jk on 2020/3/8.
//
// #define R_BUILD
#ifdef R_BUILD
#include <Rcpp.h>
#include <RcppEigen.h>
// [[Rcpp::depends(RcppEigen)]]
using namespace Rcpp;
#else
#include <Eigen/Eigen>
#include "List.h"
#endif
#include <algorithm>
#include <vector>
#include <iostream>

using namespace std;

void Normalize(Eigen::MatrixXd &X, Eigen::VectorXd &y, Eigen::VectorXd &weights, Eigen::VectorXd &meanx, double &meany, Eigen::VectorXd &normx)
{
    int n = X.rows();
    int p = X.cols();
    Eigen::VectorXd tmp(n);
    for (int i = 0; i < p; i++)
    {
        meanx(i) = weights.dot(X.col(i)) / double(n);
    }
    meany = (y.dot(weights)) / double(n);
    for (int i = 0; i < p; i++)
    {
        X.col(i) = X.col(i).array() - meanx(i);
    }
    y = y.array() - meany;

    for (int i = 0; i < p; i++)
    {
        tmp = X.col(i);
        tmp = tmp.array().square();
        normx(i) = sqrt(weights.dot(tmp));
    }
    for (int i = 0; i < p; i++)
    {
        X.col(i) = sqrt(double(n)) * X.col(i) / normx(i);
    }
}

void Normalize(Eigen::MatrixXd &X, Eigen::MatrixXd &y, Eigen::VectorXd &weights, Eigen::VectorXd &meanx, Eigen::VectorXd &meany, Eigen::VectorXd &normx)
{
    // cout << "multigaussian normal" << endl;
    int n = X.rows();
    int p = X.cols();
    Eigen::VectorXd tmp(n);
    for (int i = 0; i < p; i++)
    {
        meanx(i) = weights.dot(X.col(i)) / double(n);
    }
    meany = y.transpose() * weights / double(n);
    // cout << "meany: " << meany << endl;
    for (int i = 0; i < p; i++)
    {
        X.col(i) = X.col(i).array() - meanx(i);
    }

    for (int i = 0; i < n; i++)
    {
        y.row(i) = y.row(i) - meany;
    }
    // y = y.array() - meany;

    for (int i = 0; i < p; i++)
    {
        tmp = X.col(i);
        tmp = tmp.array().square();
        normx(i) = sqrt(weights.dot(tmp));
    }
    for (int i = 0; i < p; i++)
    {
        X.col(i) = sqrt(double(n)) * X.col(i) / normx(i);
    }
}

void Normalize3(Eigen::MatrixXd &X, Eigen::VectorXd &weights, Eigen::VectorXd &meanx, Eigen::VectorXd &normx)
{
    int n = X.rows();
    int p = X.cols();
    Eigen::VectorXd tmp(n);
    for (int i = 0; i < p; i++)
    {
        meanx(i) = weights.dot(X.col(i)) / double(n);
    }
    for (int i = 0; i < p; i++)
    {
        X.col(i) = X.col(i).array() - meanx(i);
    }
    for (int i = 0; i < p; i++)
    {
        tmp = X.col(i);
        tmp = tmp.array().square();
        normx(i) = sqrt(weights.dot(tmp));
    }
    for (int i = 0; i < p; i++)
    {
        X.col(i) = sqrt(double(n)) * X.col(i) / normx(i);
    }
}

void Normalize4(Eigen::MatrixXd &X, Eigen::VectorXd &weights, Eigen::VectorXd &normx)
{
    int n = X.rows();
    int p = X.cols();
    Eigen::VectorXd tmp(n);
    for (int i = 0; i < p; i++)
    {
        tmp = X.col(i);
        tmp = tmp.array().square();
        normx(i) = sqrt(weights.dot(tmp));
    }
    for (int i = 0; i < p; i++)
    {
        X.col(i) = sqrt(double(n)) * X.col(i) / normx(i);
    }
}

void Normalize(Eigen::SparseMatrix<double> &X, Eigen::VectorXd &y, Eigen::VectorXd &weights, Eigen::VectorXd &meanx, double &meany, Eigen::VectorXd &normx) { return; }
void Normalize(Eigen::SparseMatrix<double> &X, Eigen::MatrixXd &y, Eigen::VectorXd &weights, Eigen::VectorXd &meanx, Eigen::VectorXd &meany, Eigen::VectorXd &normx) { return; }
void Normalize3(Eigen::SparseMatrix<double> &X, Eigen::VectorXd &y, Eigen::VectorXd &meanx, Eigen::VectorXd &normx) { return; }
void Normalize4(Eigen::SparseMatrix<double> &X, Eigen::VectorXd &y, Eigen::VectorXd &normx) { return; }


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

#ifndef ITL_FSM_INCLUDE
#define ITL_FSM_INCLUDE

#include <boost/numeric/mtl/operation/two_norm.hpp>

namespace itl {

/// Folded spectrum method
/** Computed and named as in http://en.wikipedia.org/wiki/Folded_spectrum_method **/
template < typename LinearOperator, typename VectorSpace, typename EigenValue,
	   typename Damping, typename Iteration >
int fsm(const LinearOperator& H, VectorSpace& phi, EigenValue eps, Damping alpha, Iteration& iter)
{
    VectorSpace v1(H * phi - eps * phi);
    for (; !iter.finished(v1); ++iter) {
	VectorSpace v2(H * v1 - eps * v1);
	phi-= alpha * v2;
	phi/= two_norm(phi);
	v1= H * phi - eps * phi;
    }
    return iter;
}

} // namespace itl

#endif // ITL_FSM_INCLUDE