#ifndef __Linear_regression__
#define __Linear_regression__
#include<iostream>
#include<Eigen/Dense>
#include<utility>
#include<string>
#include"generate_data.hpp"
#include<random>
#include <Eigen/Cholesky>
#include<cmath>

Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> shuffling_data(size_t n)
{
    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> shuffled(n);
    shuffled.setIdentity();
    std::random_shuffle(shuffled.indices().data(), shuffled.indices().data() + shuffled.indices().size());
    return shuffled;
}

float compute_mse(const Eigen::VectorXf& pred, const Eigen::VectorXf& y)
{
    float value = (1.0f/pred.rows()) * ((y - pred).transpose() * (y-pred)).sum();
    return value;

}
Eigen::VectorXf gradient_descent(const Eigen::MatrixXf& X, const Eigen::MatrixXf& y, float learning_rate, size_t num_rows)
{
    //I already assume that X is padded with 1's
    Eigen::VectorXf B = Eigen::VectorXf::Random(X.cols());
    B(0) = 0.0f;
    size_t blocks_size = (size_t) (X.rows()/num_rows);
    //in case if the num_rows was larger than the number of rows of X
    if(blocks_size == 0)
    {
        blocks_size = 1;
        num_rows = X.rows();
    }
    int counter = 0;
    Eigen::MatrixXf block_of_X = Eigen::MatrixXf::Zero(num_rows, X.cols());
    Eigen::VectorXf block_of_y = Eigen::VectorXf::Zero(num_rows);
    for(int iter = 0; iter < 100; iter++)
    {
        counter = 0;
        block_of_X = Eigen::MatrixXf(num_rows, X.cols());
        block_of_y = Eigen::VectorXf(num_rows);

        for(int block=0; block < blocks_size; block++)
        {
            //taking a slice from X, it appears that I don't have Eigen::seq
            for(int row=0; row < num_rows; row++)
            {
                block_of_X.row(row) = X.row(row + counter);
                block_of_y.row(row) = y.row(row + counter);
            }
            //updating B
            //std::cout << block_of_y << std::endl;
            // std::cout << "x\t" << block_of_X <<"\n" << std::endl;
            // std::cout << "xT\t" << block_of_X.transpose() <<"\n" << std::endl;

            B = B - learning_rate * 1.0f/(block_of_X.rows()) * block_of_X.transpose() * (block_of_X * B - block_of_y);
            Eigen::VectorXf pred = Eigen::VectorXf(block_of_X.rows());
            pred = block_of_X * B;
            float error = compute_mse(pred, block_of_y);
            std::cout << "Step: " << block + iter * blocks_size  << " error: " << error << std::endl;

            counter += num_rows;
        }
        if(counter < X.rows())
        {
            int rest_of_rows = X.rows() - counter;
            block_of_X = Eigen::MatrixXf(rest_of_rows, X.cols());
            block_of_y = Eigen::VectorXf(rest_of_rows);

            for(int row=0; row < rest_of_rows; row++)
            {
                block_of_X.row(row) = X.row(row + counter);
                block_of_y.row(row) = y.row(row + counter);
            }
            //updating B
            //std::cout << "rows: " << block_of_X.rows() << std::endl;
            B = B - learning_rate * block_of_X.transpose() * ( block_of_X * B - block_of_y );
            Eigen::VectorXf pred = Eigen::VectorXf(rest_of_rows);
            pred = block_of_X * B;
            float error = compute_mse(pred, block_of_y);
            std::cout << "Step: " << blocks_size + 1 + + iter * blocks_size << " error: " << error << std::endl;

        }
    }

    //std::cout << "B\n " << B << std::endl;

    return B;
}
float estimate_variance(const Eigen::MatrixXf& X, const Eigen::MatrixXf& y, const Eigen::VectorXf& B)
{
    //I will use the biased estimate in which I willn't divide by n-1
    float sigma = (1.0f/y.rows()) * ((y - X * B).transpose() * (y - X * B)).sum();
    return sigma;
}
void make_prediction(const Eigen::VectorXf& B, const Eigen::MatrixXf& x, float sigma, const float true_y);

//I will only deal with metric/conttinuous data
void Linear_regression_training(const std::string file_name, float lr, int num_rows)
{
    std::pair<Eigen::MatrixXf, Eigen::MatrixXf> data = load_csv_for_LinearR(file_name);
    Eigen::MatrixXf X = data.first;
    Eigen::MatrixXf X_with_pad = Eigen::MatrixXf(X.rows(), X.cols() + 1 );
    //pad X for the intercept term
    Eigen::VectorXf b0 = Eigen::VectorXf::Ones(X.rows());
    //shifting columns
    X_with_pad.col(0) = b0;
    for(int i=1; i < X_with_pad.cols(); i++)
    {
        X_with_pad.col(i) = X.col(i - 1);
    }
    Eigen::MatrixXf y = data.second;
    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> shuffled_indeces(shuffling_data(X.rows()));
    //shuffling data
    X_with_pad = shuffled_indeces * X_with_pad;
    y = shuffled_indeces * y;
    //standard normalize X
    // float* means = new float[X_with_pad.cols() - 1];
    // float* std_ = new float[X_with_pad.cols() - 1];
    // Eigen::VectorXf tmp = Eigen::VectorXf::Ones(X_with_pad.rows());

    // for(int i=1; i < X_with_pad.cols(); i++)
    // {
    //     means[i - 1] = X_with_pad.col(i).mean();
    //     X_with_pad.col(i) = X_with_pad.col(i) - tmp * means[i -1];
    //     std_[i - 1] = 1/X_with_pad.rows() * (X_with_pad.col(i).transpose() * X_with_pad.col(i)).sum();
    //     tmp = X_with_pad.col(i);
    //     std::cout << X_with_pad << std::endl;
    //     std::cout << "m " << means[i - 1] << " std: " << std_[i -1] << std::endl;
    //     tmp = tmp / std_[i-1];
    //     std::cout << tmp << std::endl;
    //     X_with_pad.col(i) = tmp;
    // }
    // float meany, std_y;
    // //standard normalize y
    // meany = y.mean();
    // y = y - tmp * meany;
    // std_y = 1/y.rows() * (y.transpose() * y).sum();
    // tmp = y;
    // y = tmp/std_y;
    //use meany, std_y, means and std_ for prediction stage.

    //Finding Value of B
    Eigen::VectorXf B = gradient_descent(X_with_pad, y, lr, num_rows);
    Eigen::VectorXf B_ols = (X_with_pad.transpose() * X_with_pad).inverse() * X_with_pad.transpose() * y;

    std::cout << "Mini-batch GD estimat of of B: \n" << B << std::endl;
    std::cout << "Error in prediction GD: " << compute_mse(X_with_pad * B, y) << std::endl;
    std::cout << "Sigma Estimate by GD: " << estimate_variance(X_with_pad, y, B) << std::endl;
    std::cout << "OLS of B: \n" << B_ols << std::endl;
    std::cout << "Error in prediction OLS: " << compute_mse(X_with_pad * B_ols, y) << std::endl;
    // std::cout << X_with_pad << std::endl;
    // std::cout << y << std::endl;
    make_prediction(B, X_with_pad.row(10).transpose(), estimate_variance(X_with_pad, y, B), y.row(10).sum());

}

void make_prediction(const Eigen::VectorXf& B, const Eigen::MatrixXf& x, float sigma, const float true_y)
{
    auto f = [](float pred, float true_v){

        return std::sqrt((pred - true_v) * (pred - true_v));
    };
    std::cout << "B: " << B.transpose().cols() << std::endl;
    std::cout << "x: " << x.rows() << std::endl;
    float y = (B.transpose() * x).sum();
    std::cout << "Prediciton: " << y << " true value: " << true_y << " rms: " << f(y, true_y) << std::endl;

}

#endif /*__Linear_regression__*/


//
//  main.cpp
//  Assignment 2 - Bearings
//
//  Created by - on 2016/09/27.
//  Copyright © 2016 Eddie Of the Ren. All rights reserved.
//
#include <iostream>
//#include "stdafx.h"
using namespace std;
#include <stdio.h>      /* printf */
#include <math.h>
//#include <boost/lexical_cast.hpp>
#include <string>

int cardinalNum1 = 0;
int cardinalNum2 = 0;

//This function converts the cardinal string to a number that can fit in an array
int cardinalStrToNum(string cardinalStr) {
    int cardinalNum = 0;
    if (cardinalStr== "N") {
        cardinalNum = 2;
    } else if (cardinalStr == "E") {
        cardinalNum = 3;
    } else if (cardinalStr == "S") {
        cardinalNum = 4;
    } else if (cardinalStr == "W") {
        cardinalNum = 5;
    }
    return cardinalNum;

}

//This checks if a string is a number that is composed only of "-.0123456789"
bool is_number(const std::string& s)
{
    return( strspn( s.c_str(), "-.0123456789" ) == s.size() );
}

//If the user enters a bearing that is not within 0 to 359, this converts it to within that limit
int getBearing(int x) {
    double customBearing = 0;
    //Turning negative values positive
    while (x < 0) {
        x = x + 360;
    }
    //Turning overweight values smaller
    while (x >= 360) {
        x = x - 360;
    }
    customBearing = x;

    return (customBearing);
}

//This function will accept a bearing direction and turn it into a compass direction
int bearingToCompass(double bearing, string gameChoice) {
    double choiceInt = 0;

    int quadrant = 0;
    bool CW = false;
    string cardinals [6] = {"W", "N", "E", "S", "W", "N"};

    //getBearing will convert the user entered value into a value between 1 and 360, discrimination is currently enforced to favor 0° over 360°

    //These two lines turn the bearing into a value between 0 and 359 and finds the inverse quadrant it resides in. The quadrants are counted clockwise, NOT counter-clockwise
    bearing = getBearing(bearing);
    quadrant = floor(bearing/90);

    //If this if statement is true, that means the degree applies in a clockwise direction relative to the first cardinal. Ex: N32E. the direciton is 32 east (clockwise) FROM north
    if ((bearing - 90*quadrant) <45) {
        CW = false;
        cout << cardinals[(quadrant + 1)] << (bearing - 90*quadrant) << cardinals[(quadrant+2)] <<endl;
        if (gameChoice == "1") {

            cout << "Primary compass direction:             " << cardinals[(quadrant + 1)] << endl;
            cout << "Compass angle relative to direction:   " << (bearing - 90*quadrant) << endl;
            cout << "Secondary compass direction:           " << cardinals[(quadrant+2)]<< endl;
        }

    }
    //If this if statement is true, that means the degree applies in a counter-clockwise direction relative to the first cardinal. Ex: N65E. The direction is 65 north (counterclockwise) FROM east. This is equivalent to E25N which is a better way of representing the direction.
    else if ((bearing - 90*quadrant) >45) {
        CW = true;
        cout << cardinals[(quadrant + 2)] << (90-(bearing - 90*quadrant)) << cardinals[(quadrant+1)] <<endl;

        if (gameChoice == "1") {
            cout << "Primary compass direction:             " << cardinals[(quadrant + 2)] << endl;
            //The line of code below ensures that the angle is between 1° and 44°
            cout << "Compass angle                          " << (90-(bearing - 90*quadrant)) << "°" <<     endl;
            cout << "Secondary compass direction:           "<< cardinals[(quadrant+1)]<< endl;
        }
    }
    //When the angle is 45° or exactly between two cardinals, the bearing is not displayed
    else if ((bearing - 90*quadrant) == 45) {
        cout << cardinals[(quadrant + 1)]  << cardinals[(quadrant+2)] <<endl;

        if (gameChoice == "1") {
            cout << "Primary compass direction:             " << cardinals[(quadrant + 1)] << endl;
            cout << "Compass angle relative to direction:   " << endl;
            cout << "Secondary compass direction:           " << cardinals[(quadrant+2)]<< endl;
        }
    }

    //cout << cardinals[(quadrant+1)] << endl;
    //cout << bearing<< endl;
    return 0;

}

//This functions checks the cardinals and angle the user entered are valid or not.
bool cardinalCheck (string primaryCardinal, string secondaryCardinal, string angleStr) {
    bool redo = false;
    if (primaryCardinal != "N"&& primaryCardinal!= "E" && primaryCardinal != "W" && primaryCardinal != "S") {
        redo = true;
        cout << "Your primary cardinal direciton is not one of N, E, S, W " << endl;
    } else if (secondaryCardinal != "N"&& secondaryCardinal!= "E" && secondaryCardinal != "W" && secondaryCardinal != "S") {
        redo = true;
        cout << "Your secondary cardinal direciton is not one of N, E, S, W " << endl;
    } else if (is_number(angleStr) == 0)  {
        redo = true;
        cout << "Your angle is not a number " << endl;
    } else {
        //following statements ensure that the user has not entered two identical or opposite cardinals, such as N20N or N20S
        cardinalNum1 = cardinalStrToNum(primaryCardinal);
        cardinalNum2 = cardinalStrToNum(secondaryCardinal);
        if (cardinalNum1 - cardinalNum2 >= 0) {
            if ((cardinalNum1-cardinalNum2)%2 == 0) {
                redo = true;
                cout<< "These coordinates are invalid"<< endl;
            }
        } else if (cardinalNum1 - cardinalNum2 < 0) {
            if ((-(cardinalNum1-cardinalNum2))%2 == 0) {
                redo = true;
                cout<< "These coordinates are invalid"<< endl;
            }
        }
    }
    return redo;
}

//This is the main function
int main() {

    //Getting user to choose which game mode to play
    cout << "You have been unfortunately chosen by the OCDSB to mark Eddie's compass project" << endl;
    cout << "" << endl;
    cout << "To begin choose whether you want to play in:" << endl;
    cout << "     ENTER 1 to play in BEARING mode (bearing to compass)" << endl;
    cout << "     ENTER 2 to play in COMPASS mode (compass to bearing, one step)" << endl;
    cout << "     ENTER 3 to play in COMPASS mode (compass to bearing, three step)" << endl;

    string gameChoice = "3";
    bool correctInput = false;
    cin >> gameChoice;

    do  {
        //This do while loop ensures that the user has entered a value between 1 and 3
        if ((gameChoice == "1") || (gameChoice == "2" || gameChoice == "3")) {
            correctInput = true;
        } else {
            cout << "You have not entered a valid input, please try again" << endl;
            cin >> gameChoice;
        }
        cout << correctInput << endl;
    } while (correctInput == false);

    if (gameChoice == "3") {
        //These are the critical variable inputs
        string primaryCardinal = "derp";
        string angleStr = "derp";
        string secondaryCardinal = "derp";

        double angle;
        double originalAngle = 0;
        bool redo = false;

        do {
            //Asks the user for necessary inputs and checks the inputs to see if they are valid
            cout <<"Please enter your compass direction: " << endl;
            cin >> primaryCardinal;
            cout <<"Please enter your angle: " << endl;
            cin >> angleStr;
            cout <<"Please enter your secondary direciton: " << endl;
            cin >> secondaryCardinal;

            redo = cardinalCheck(primaryCardinal, secondaryCardinal, angleStr);

        } while (redo == true);

        //Some necessary calculations
        angle = stod(angleStr);
        angle = getBearing(angle);
        originalAngle = angle;
        cout << angle << endl;
        string cardinals [6] = {"W", "N", "E", "S", "W", "N"};
        int cardinalNumSave = 0;

        //Turns the cardinals into bearing values
        if (secondaryCardinal == cardinals[cardinalNum1]) {
            angle = ((cardinalNum1-2)*90) + angle;
        } else if (secondaryCardinal == cardinals[cardinalNum1-2]) {
            angle = ((cardinalNum1-2)*90) - angle;
        } else {
            cout << "Something went here calculating the bearing angle" << endl;
        }
        //Makes sure the bearing values are between 0° and 359°
        angle = getBearing(angle);
        cout << "Your bearing is "<<angle<< "°C right fron North. Your direction is:  "<< bearingToCompass(angle, gameChoice)<< endl;

    } else if (gameChoice == "2") {

        cout << "You have been unfortunately chosen by the OCDSB to mark Eddie's compass project" << endl;
        bool redo = false;
        string directionStr = "bleBLERPBLERPrp";
        string primaryCardinal = "blerp";
        string secondaryCardinal = "blBLERPerp";
        string angleStr = "0";
        double angle = 0;

        do {
            //Gets input for the direction
            cout << "Please enter your direction in the form of N40E starting with a cardinal, bearing and secondary cardinal." << endl;
            cin >> directionStr;

            //Segments this direction into cardinals and angle
            primaryCardinal = directionStr[0];
            secondaryCardinal = directionStr[directionStr.length()-1];
            angleStr = directionStr.substr(1, directionStr.length()-2);

            //Checks if these are valid entries
            redo = cardinalCheck(primaryCardinal, secondaryCardinal, angleStr);
        } while (redo == true);

        angle = stod(angleStr);
        angle = getBearing(angle);
        double originalAngle = angle;
        cout << angle << endl;
        string cardinals [6] = {"W", "N", "E", "S", "W", "N"};
        int cardinalNumSave = 0;

        //Converts cardinals to proper angle measures which are added to the inputted angle
        if (secondaryCardinal == cardinals[cardinalNum1]) {
            angle = ((cardinalNum1-2)*90) + angle;
        } else if (secondaryCardinal == cardinals[cardinalNum1-2]) {
            angle = ((cardinalNum1-2)*90) - angle;
        } else {
            cout << "Something went here calculating the bearing angle" << endl;
        }
        //Makes sure the bearing is between 0° and 359°
        angle = getBearing(angle);

        //Outputs the bearing and a correct direction
        cout << "Your bearing is "<<angle<< "°C right fron North. Your direction is:  "<< bearingToCompass(angle, gameChoice)<< endl;

    } else if (gameChoice == "1") {
        cout << "Please enter your bearing in degrees"<< endl;
        double bearing = 0;
        string bearingStr = "hi";

        do {
            //Gets user to input a bearing and checks if it is a number
            cin >> bearingStr;
            if (is_number(bearingStr) == 0) {
                cout << "You have not entered a numeric value for the bearing, please try again my friend" << endl;
            }
        } while (is_number(bearingStr) == 0);
        std::string str = "3.14";
        double strVal;

        bearing = stod(bearingStr);
        cout << bearing << endl;
        //Converts bearing to compass direction
        bearingToCompass(bearing, gameChoice);

        //cin >> gameChoice;

        return 0;
    }
}

Assignment 2 - Bearings/PROJECT: BEARING/main.cpp


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

//
// Created by Phillip Keldenich on 11.11.19.
//

#pragma once

#include "ivarp/number.hpp"
#include <boost/iterator/iterator_facade.hpp>
#include <cassert>

namespace ivarp {
    /// Computes split intervals for a given range and a fixed number of subdivisions n.
    /// Valid indices are [0,n).
    template<typename IntervalType> class Splitter {
    public:
        using Interval = IntervalType;
        using Number = typename Interval::NumberType;

        static_assert(IsIntervalType<IntervalType>::value, "Splitter requires an interval type to work on!");

        IVARP_SUPPRESS_HD
        explicit IVARP_HD Splitter(const IntervalType& i, int n) :
            m_range(i), m_n(n), m_ind_width((i.ub() - i.lb()) / n)
        {}

        IVARP_SUPPRESS_HD
        IVARP_HD Number split_point(int i) const {
            if(i >= m_n) {
                return m_range.ub();
            }
            return m_range.lb() + i * m_ind_width;
        }

        IVARP_SUPPRESS_HD
        IVARP_HD IntervalType subrange(int i) const {
            return IntervalType{split_point(i), split_point(i+1)};
        }

        IVARP_HD int size() const noexcept {
            return m_n;
        }

        class Iterator :
            public boost::iterator_facade<Iterator, IntervalType, std::random_access_iterator_tag, IntervalType, int>
        {
        public:
            Iterator() noexcept : m_splitter(nullptr), i(0) {}
            Iterator(const Iterator&) noexcept = default;
            Iterator &operator=(const Iterator&) noexcept = default;

        private:
            explicit Iterator(const Splitter* s, int i) :
                m_splitter(s), i(i)
            {}

            friend class Splitter;
            friend class boost::iterator_core_access;

            IntervalType dereference() const {
                return m_splitter->subrange(i);
            }

            void increment() noexcept {
                ++i;
            }

            void decrement() noexcept {
                --i;
            }

            void advance(int n) noexcept {
                i += n;
            }

            int distance_to(const Iterator& o) const noexcept {
                return o.i - i;
            }

            bool equal(const Iterator& o) const noexcept {
                return i == o.i;
            }

            const Splitter* m_splitter;
            int i;
        };

        Iterator begin() const noexcept {
            return Iterator{this, 0};
        }

        Iterator end() const noexcept {
            return Iterator{this, m_n};
        }

    private:
        IntervalType m_range; ///< The outer range.
        int m_n; ///< The number of subintervals.
        Number m_ind_width; ///< The width of each individual subinterval.
    };
}


//  (C) Copyright 2006 Eric Niebler, Olivier Gygi.
//  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)

// Test case for weighted_kurtosis.hpp

#include <boost/random.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/numeric/functional/vector.hpp>
#include <boost/accumulators/numeric/functional/complex.hpp>
#include <boost/accumulators/numeric/functional/valarray.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/weighted_kurtosis.hpp>

using namespace boost;
using namespace unit_test;
using namespace boost::accumulators;

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    // tolerance in %
    // double epsilon = 1;

    accumulator_set<double, stats<tag::weighted_kurtosis>, double > acc1;
    accumulator_set<int, stats<tag::weighted_kurtosis>, int > acc2;

    // two random number generators
    boost::lagged_fibonacci607 rng;
    boost::normal_distribution<> mean_sigma(0,1);
    boost::variate_generator<boost::lagged_fibonacci607&, boost::normal_distribution<> > normal(rng, mean_sigma);

    for (std::size_t i=0; i<100000; ++i)
    {
        acc1(normal(), weight = rng());
    }

    // This check fails because epsilon is relative and not absolute
    // BOOST_CHECK_CLOSE( weighted_kurtosis(acc1), 0., epsilon );

    acc2(2, weight = 4);
    acc2(7, weight = 1);
    acc2(4, weight = 3);
    acc2(9, weight = 1);
    acc2(3, weight = 2);

    BOOST_CHECK_EQUAL( weighted_mean(acc2), 42./11. );
    BOOST_CHECK_EQUAL( accumulators::weighted_moment<2>(acc2), 212./11. );
    BOOST_CHECK_EQUAL( accumulators::weighted_moment<3>(acc2), 1350./11. );
    BOOST_CHECK_EQUAL( accumulators::weighted_moment<4>(acc2), 9956./11. );
    BOOST_CHECK_CLOSE( weighted_kurtosis(acc2), 0.58137026432, 1e-6 );
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("weighted_kurtosis test");

    test->add(BOOST_TEST_CASE(&test_stat));

    return test;
}


/**
 * @file Filter.hpp
 * @author Jorge Agüero Zamora
 * @brief Contains `Filter` class in charge of creating filters
 * @version 0.1
 * @date 2021-06-13
 *
 */
#ifndef FILTER_H
#define FILTER_H

#define DIRTY_IMAGE_SUFFIX "_dirty.png" //!< suffix used in saved noisy image filenames

#include <vector>
#include <string>
#include <armadillo>
#include <time.h>
#include <chrono>

#include "FilterInfo.hpp"
#include "VecImage.hpp"
#include "ImageLoader.hpp"
#include "libfrcima.hpp"

namespace AppLogic
{
    using arma::mat;
    using std::string;
    using std::vector;
    using std::chrono::duration;
    using std::chrono::duration_cast;
    using std::chrono::high_resolution_clock;
    using std::chrono::seconds;

    /**
     * @brief enum with the calculation methods for the filter
     *
     */
    typedef enum
    {
        RCIMA_METHOD,
        FAST_RCIMA_METHOD
    } calc_method_t;

    /**
     * @brief Allows to create filters based on training sets of images
     *
     */
    class Filter
    {
    private:
        FilterInfo calc_info; //!< filter calculation information
        vector<VecImage> working_images; //!< list of loaded images
        mat image_set; //!< training matrix of loaded images
        vector<string> loaded_file_paths; //!< paths to the loaded image files
        vector<string> loaded_file_names; //!< names of the loaded images
        noise_type_t last_used_noise_type; //!< last noise type used
        size_t last_used_noise_value; //!< last noise value used
        bool is_canceled; //!< indicates whether filter calculation has been canceled
        mat F_matrix; //!< created filter matrix

        /**
         * @brief generates training matrix out of the loaded images
         *
         * @return mat training matrix where each column is a vectorized form of each loaded image
         */
        mat getWorkingImagesMat();

        /**
         * @brief generates training matrix out of the loaded images applying last used noise on them
         *
         * @return mat training matrix where each column is a vectorized form of each loaded image with noise applied
         */
        mat getDirtyWorkingImagesMat();

    public:
        /**
         * @brief Construct a new Filter object
         *
         */
        Filter();

        /**
         * @brief applies selected noise to an image and returns a new image object
         *
         * @param image_id the index of the loaded image to apply noise to
         * @param noise_value percentage value (0-99) of amount of noise to apply
         * @param noise_type type of noise to apply to the image
         * @return VecImage new VecImage with the applied noise
         */
        VecImage applyNoiseToImage(size_t image_id, size_t noise_value, noise_type_t noise_type = noise_type_t::GAUSSIAN);

        /**
         * @brief sets the name of the filter
         *
         * @param name name of the filter
         */
        inline void setFilterName(string name)
        {
            this->calc_info.filter_name = name;
        }

        /**
         * @brief Get the Fmatrix object
         *
         * @return mat& filter matrix
         */
        inline mat &getFmatrix()
        {
            return this->F_matrix;
        };

        /**
         * @brief Get the Filter Info object
         *
         * @return FilterInfo* filter calculation information
         */
        inline FilterInfo *getFilterInfo()
        {
            return &this->calc_info;
        };

        /**
         * @brief removes all loaded image information
         *
         */
        inline void clearWorkingImages()
        {
            this->working_images.clear();
            this->loaded_file_paths.clear();
            this->loaded_file_names.clear();
        }

        /**
         * @brief loads images from specified full path to a folder
         *
         * This function will look for the first valid image in the folder and will load
         * all other valid images of the same resolution as that first loaded image, other
         * images are ignored.
         *
         * @param folder_path full path to the directory containing the images
         * @return vector<string> list of names of the images that were loaded
         */
        vector<string> loadImagesFromFolder(string folder_path);

        /**
         * @brief loads a single image and adds it to the working images
         *
         * @param file_path path of the image file
         * @return string name of the loaded image
         */
        string loadSingleImage(string folder_path);

        /**
         * @brief load images from a zip file
         *
         * This function will go through all files in a zip file. It  will look for the first
         * valid image in the folder and will load all other valid images of the same resolution
         * as the first loaded image, other images are ignored.
         *
         * @param file_path full path of a valid zip file
         * @return vector<string> list of names of the images that were loaded
         */
        vector<string> loadImagesFromZip(string file_path);

        /**
         * @brief returns the path of the loaded image specified by index
         *
         * @param index the index of the loaded image to get the path for
         * @return string path of the loaded image in the filesystem
         */
        string getImagePath(size_t index);

        /**
         * @brief returns a pointer to the loaded VecImage specified by index
         *
         * @param index the index of the loaded image to get
         * @return VecImage* pointer to the VecImage object of the loaded image
         */
        VecImage *getLoadedImage(const size_t index);

        /**
         * @brief calculates the filter
         *
         * Calculates the filter using the last noise type and value applied, rank, and selected calculation method.
         * Sets the calculation information in `calc_info`
         *
         * @param rank the rank to use for filter calculation. Must be a value between 1 and the minimun between the amount
         *              of images used and the size of the vectorized image.
         * @param calc_method calculation method to use. Either RCIMA or fast-RCIMA
         * @return true if calculation is successful
         * @return false if an error is encountered during calculation or calculation is canceled
         */
        bool calculateFilter(size_t rank, calc_method_t calc_method);

        /**
         * @brief saves filter matrix to file
         *
         * @param file_path full path of the file where the matrix will be saved
         * @return true if matrix is saved successfully
         * @return false if there is an error saving the matrix
         */
        bool saveToFile(string file_path = ImageLoader::FILTER_SAVE_LOCATION);

        /**
         * @brief loads filter matrix from file
         *
         * @param file_path full path of the file where the matrix is stored
         * @return true if matrix is loaded successfully
         * @return false if there is an error loading the matrix
         */
        bool loadFmatrixFromFile(string file_path);

        /**
         * @brief applies noise to all working images and saves them in the specified path
         *
         * Applies noise to all loaded images using the last noise type and value applied and
         * saves them to the folder path provided as PNG (.png) images. This operation does not
         * modify the loaded images.
         *
         * @param folder_path path where the images will be saved
         * @return true if images are saved correctly
         * @return false if an eror is encountered or saving process is canceled
         */
        bool saveAllDirtyImages(string folder_path);

        /**
         * @brief applies noise to image and saves it in the specified path
         *
         * Applies noise to the loaded image specified by the index image_id, using the
         * last noise type and value applied and saves it to the folder path provided
         * as a PNG (.png) image. This operation does not modify the loaded image.
         *
         * @param image_id index of the image to apply noise to and save
         * @param folder_path name of the file to save the dirty image
         * @return true if image is saved corectly
         * @return false if there is an error saving the image
         */
        bool saveDirtyImage(size_t image_id, string folder_path);

        /**
         * @brief saves filter calculation information in the app's data directory
         *
         * @param folder_path path where the filter_info is stored
         * @return true if filter information is saved correctly
         * @return false if there is an error saving the filter information
         */
        bool saveFilterInfo(string folder_path = ImageLoader::FILTER_SAVE_LOCATION);

         /**
         * @brief cancels the process of saving images with noise applied
         *
         */
        void cancelSaveDirtyImages();

        /**
         * @brief returns a string name for the calculation method
         *
         * @param calc_method calculation method
         * @return string name of the calculation method
         */
        static string getCalculationMethodName(calc_method_t calc_method);
    };

}

#endif


// Author: Steffen Urban (urbste@gmail.com)

#include <Eigen/Core>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <theia/theia.h>

#include <algorithm>
#include <memory>
#include <string>

#include "print_reconstruction_statistics.h"

DEFINE_string(reconstruction, "", "Reconstruction file");

int main(int argc, char* argv[]) {
  google::InitGoogleLogging(argv[0]);
  THEIA_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);

  // Load the SIFT descriptors into the cameras.
  std::unique_ptr<theia::Reconstruction> reconstruction(
      new theia::Reconstruction());
  CHECK(theia::ReadReconstruction(FLAGS_reconstruction, reconstruction.get()))
      << "Could not read reconstruction file.";

  std::cout << "\nNum views: " << reconstruction->NumViews()
            << "\nNum 3D points: " << reconstruction->NumTracks() << "\n";

  theia::BundleAdjustmentOptions options;
  for (int i = 0; i < reconstruction->TrackIds().size(); ++i) {
    Eigen::Matrix3d cov;
    double empirical_variance;
    theia::BundleAdjustmentSummary summary =
        theia::BundleAdjustTrack(options,
                                 reconstruction->TrackIds()[i],
                                 reconstruction.get(),
                                 &cov,
                                 &empirical_variance);

    std::cout << "FInal cost: " << summary.final_cost << "\n";
    std::cout << "standard deviations: "
              << cov.diagonal().array().sqrt().transpose() * 1000. << " [mm]\n";
  }
  return 0;
}


#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include <CGAL/Simple_cartesian.h>
#include <cassert>
#include <vector>
#include <algorithm>
#include <CGAL/point_generators_2.h>
#include <CGAL/algorithm.h>

using namespace CGAL;

typedef Simple_cartesian<int>         K;
typedef K::Point_2                    Point;
typedef Creator_uniform_2<int,Point>  Creator;

void
grid(int N, double eps)
{
  CGAL::Random rng;
  std::cout << N*N << std::endl;
  for(double i = 0; i < N; i++){
    for(double j = 0; j < N; j++){
      std::cout << i + rng.get_double(-eps,eps) << " "
                << j + rng.get_double(-eps,eps) << "\n";
    }
  }
}

int main(int argc, char* argv[])
{
  int N= 10;
  double eps = 0;
  try {

    po::options_description desc("Allowed options");
    desc.add_options()
      ("help", "Generator of perturbed points on a grid")
      ("N", po::value<int>(), "generate a grid with N x N points")
      ("eps", po::value<double>(), "perturb x and y of points by eps")
      ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
      std::cout << desc << "\n";
      return 1;
    }

    if (vm.count("N")) {
      N = vm["N"].as<int>();
    }

    if (vm.count("eps")) {
      eps = vm["eps"].as<double>();
    }
  }
  catch(std::exception& e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }

  grid(N,eps);
  return 0;
}


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

///
/// \file balanced_truncation.hpp
///
/// Balanced truncation method specialized for exponential sum function
///
#ifndef MXPFIT_BALANCED_TRUNCATION_HPP
#define MXPFIT_BALANCED_TRUNCATION_HPP
#include <cassert>

#include <Eigen/Core>
#include <Eigen/Eigenvalues>

#include <mxpfit/exponential_sum.hpp>
#include <mxpfit/quasi_cauchy_rrd.hpp>
#include <mxpfit/self_adjoint_coneigensolver.hpp>

namespace mxpfit
{

///
/// ### BalancedTruncation
///
/// \brief Find a truncated exponential sum function with smaller number of
///        terms by the modified balanced truncation method.
///
/// \tparam T  Scalar type of exponential sum function.
///
/// For a given exponential sum function,
///
/// \f[
///   f(t)=\sum_{j=1}^{n} c_{j}^{} e^{-a_{j}^{} t}, \quad
///   (\mathrm{Re}(a_{j}) > 0),
/// \f]
///
/// and prescribed accuracy \f$\epsilon > 0,\f$ this class calculates truncated
/// exponential \f$\hat{f}(t)\f$ sum such that
///
/// \f[
///   \hat{f}(t)=\sum_{j=1}^{k} \hat{c}_{j}^{}e^{-\hat{a}_{j}^{} t}, \quad
///   \left| f(t)-\hat{f}(t) \right| < \epsilon,
/// \f]
///
/// where \f$k \leq n.\f$ Let \f$F(s)\f$ and \f$\hat{F}(s)\f$ be the Laplace
/// transform of \f$f(t)\f$ and \f$\hat{f}(t),\f$ respectively. \f$F(s)\f$ can
/// be evaluated analytically as
///
/// \f[
///  F(s)=\sum_{j=1}^{n}\frac{c_{j}^{}}{s+a_{j}^{}}
/// \f]
///
/// and similar to \f$\hat{F}(s).\f$ Now, the problem can be rewritten as
/// finding optimal rational sum approximation \f$\hat{F}(s)\f$ such that \f$
/// \left|F(s)-\hat{F}(s)\right| < \epsilon.\f$
///
/// This class computes the truncated rational sum approximation
/// \f$\hat{F}(s)\f$ by the modified balanced truncation method combined with
/// the first and accurate con-eigensolver of a quasi-Cauchy matrix.
///
///
/// #### References
///
/// 1. K. Xu and S. Jiang, "A Bootstrap Method for Sum-of-Poles Approximations",
///    J. Sci. Comput. **55** (2013) 16-39.
///    [DOI: https://doi.org/10.1007/s10915-012-9620-9]
/// 2. T. S. Haut and G. Beylkin, "FAST AND ACCURATE CON-EIGENVALUE ALGORITHM
///    FOR OPTIMAL RATIONAL APPROXIMATIONS", SIAM J. Matrix Anal. Appl. **33**
///    (2012) 1101-1125.
///    [DOI: https://doi.org/10.1137/110821901]
/// 3. W. H. A. Schilders, H. A. van der Vorst, and J. Rommes, "Model Order
///    Reduction: Theory, Research Aspects and Applications", Springer (2008).
///    [DOI: https://doi.org/10.1007/978-3-540-78841-6]
///

template <typename T>
class BalancedTruncation
{
public:
    using Scalar        = T;
    using RealScalar    = typename Eigen::NumTraits<Scalar>::Real;
    using ComplexScalar = std::complex<RealScalar>;
    using Index         = Eigen::Index;

    using VectorType = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    using MatrixType = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    using ResultType = ExponentialSum<Scalar>;

    ///
    /// Compute truncated exponential sum \f$ \hat{f}(t) \f$
    ///
    /// \tparam DerivedF type of exponential sum inheriting ExponentialSumBase
    ///
    /// \param[in] orig original exponential sum function, \f$ f(t) \f$
    /// \param[in] threshold  prescribed accuracy \f$0 < \epsilon \ll 1\f$
    ///
    /// \return An instance of ExponentialSum represents \f$\hat{f}(t)\f$
    ///
    template <typename DerivedF>
    ResultType compute(const ExponentialSumBase<DerivedF>& orig,
                       RealScalar threshold);

private:
    enum
    {
        IsComplex = Eigen::NumTraits<Scalar>::IsComplex,
    };

    using EigenSolverType = typename Eigen::internal::conditional<
        IsComplex, Eigen::ComplexEigenSolver<MatrixType>,
        Eigen::SelfAdjointEigenSolver<MatrixType>>::type;
    using ConeigenSolverType = SelfAdjointConeigenSolver<Scalar>;
};

template <typename T>
template <typename DerivedF>
typename BalancedTruncation<T>::ResultType
BalancedTruncation<T>::compute(const ExponentialSumBase<DerivedF>& fn,
                               RealScalar threshold)
{
    //--------------------------------------------------------------------------
    //
    // The controllability Gramian matrix of the system is defined as
    //
    //   C(i, j) = sqrt(w[i] * conj(w[j])) / (p[i] + p[j]).
    //
    // C is a quasi-Cauchy matrix. Then compute partial Cholesky factorization
    // of matrix `C`
    //
    //   C = (P * L) * D^2 * (P * L)^H,
    //
    // where
    //
    //   - L: (n, m) matrix
    //   - D: (m, m) real diagonal matrix
    //   - P: (n, n) permutation matrix
    //
    // and `m = rank(C)`
    //
    //--------------------------------------------------------------------------
    static const RealScalar eps = Eigen::NumTraits<RealScalar>::epsilon();

    const Index n0 = fn.size();
    VectorType b(n0);
    b.array() = fn.weights().sqrt();

    const RealScalar rrd_threshold = threshold * eps * eps;
    SelfAdjointQuasiCauchyRRD<T> rrd;
    rrd.setThreshold(rrd_threshold);
    rrd.compute(b, fn.exponents());

    if (rrd.rank() == Index(0))
    {
        return ResultType();
    }

    //--------------------------------------------------------------------------
    //
    // Compute con-eigendecomposition of the controllability Gramian matrix
    //
    //   C = X D^2 X^H = U^C S U^T,
    //
    // where
    //
    //   - `X = P * L`: Cholesky factor (n, k)
    //   - `S`: (k, k) diagonal matrix. Diagonal elements `S(i,i)` are
    //          con-eigenvalues sorted in decreasing order.
    //   - `U`: (n, k) matrix. k-th column hold a con-eigenvector corresponding
    //          to k-th con-eigenvalue. The columns of `U` are orthogonal in the
    //          sense that `U^T * U = I`.
    //
    //---------------------------------------------------------------------------
    //
    // `diag` is overwritten by con-eigenvalues, and first k column of `matX` is
    // overwritten by con-eigenvectors `U`
    //
    ConeigenSolverType ceig;
    ceig.compute(rrd.matrixPL(), rrd.vectorD());
    //--------------------------------------------------------------------------
    //
    // Truncation
    //
    // Determines the order of reduced system, \f$ k \f$ from the error bound
    // computed from the Hankel singular values system. The Hankel singular
    // values are coincide with con-eigenvalues of the Gramian matrix.
    //
    // \f[
    //   \|\Sigma-\hat{\Sigma}\| \leq 2 \sum_{i=k+1}^{n} \sigma_{i}
    // \f]
    //
    //--------------------------------------------------------------------------
    auto sum_sigma       = RealScalar();
    const auto& sigma    = ceig.coneigenvalues();
    const auto sigma_tol = threshold * sigma(0);
    Index n1             = sigma.size();
    while (n1)
    {
        sum_sigma += sigma(n1 - 1);
        if (2 * sum_sigma > sigma_tol)
        {
            break;
        }
        --n1;
    }

    if (n1 == Index())
    {
        return ResultType();
    }

    //--------------------------------------------------------------------------
    //
    // Apply transformation matrix
    //
    //  A1 = U.adjoint() * S * U.conjugate()
    //  b1 = U.adjoint() * b
    //
    //--------------------------------------------------------------------------

    MatrixType A1(n1, n1);
    VectorType b1(n1);
    auto U = ceig.coneigenvectors().leftCols(n1);
    A1.noalias() =
        U.adjoint() * fn.exponents().matrix().asDiagonal() * U.conjugate();
    b1.noalias() = U.adjoint() * b;

    //--------------------------------------------------------------------------
    //
    // Compute eigenvalue decomposition of the (k x k) matrix, A1. Since A1
    // real/complex symmetric matrix, the eigen decomposition has the form
    //
    //   A1 = X2 * D * X2.transpose(), (X2.transpose() * X2 = I).
    //
    //--------------------------------------------------------------------------
    EigenSolverType eig(A1, Eigen::ComputeEigenvectors);

    if (IsComplex)
    {
        //
        // Enforce X2.transpose() * X2 = I
        //
        using EigenVectorsType = typename Eigen::internal::remove_all<decltype(
            eig.eigenvectors())>::type;
        auto& X2 = *const_cast<EigenVectorsType*>(&eig.eigenvectors());
        for (Index j = 0; j < X2.cols(); ++j)
        {
            auto xj          = X2.col(j);
            const auto t     = (xj.transpose() * xj).value();
            const auto scale = RealScalar(1) / std::sqrt(t);
            xj *= scale;
        }
    }

    //
    // Apply the state space transformation by X2,
    //
    // A2 = X2.transpose() * A1 * X2 = D
    // b2 = X2.transpose() * b1
    // c2 = b1 * X2 = b2.transpose()
    //
    // Finally parameters for truncated exponential sum can be obtained as
    //
    // p' = D.diagonal()
    // w' = c2 * b2 = square(b2)
    //
    auto b2      = b.head(n1);
    b2.noalias() = eig.eigenvectors().transpose() * b1;
    return ResultType(eig.eigenvalues(), b2.array().square());
}

} // namespace: mxpfit

#endif /* MXPFIT_BALANCED_TRUNCATION_HPP */


/**
 * @file MPCExample.cpp
 * @author Giulio Romualdi
 * @copyright Released under the terms of the BSD 3-Clause License
 * @date 2018
 */

// osqp-eigen
#include "OsqpEigen/OsqpEigen.h"

// eigen
#include <Eigen/Dense>

#include <iostream>
#include <fstream>

void setDynamicsMatrices(Eigen::Matrix<double, 2, 2> &a, Eigen::Matrix<double, 2, 1> &b)
{
    a << 1.,      0.020,
        0.,      0.9661;

    b << 0.,
        0.0315;
}

void setInequalityConstraints(Eigen::Matrix<double, 2, 1> &xMax, Eigen::Matrix<double, 2, 1> &xMin,
                              Eigen::Matrix<double, 1, 1> &uMax, Eigen::Matrix<double, 1, 1> &uMin)
{
    double u0 = 0.0;

    // input inequality constraints
    uMin << -6.0 - u0;

    uMax << 10.0 - u0;

    // state inequality constraints
    // TODO : change to present pos +/- ranges
    xMin << -100, -6.0;

    xMax << 100, 10.0;
}

void setWeightMatrices(Eigen::DiagonalMatrix<double, 2> &Q, Eigen::DiagonalMatrix<double, 1> &R)
{
    Q.diagonal() << 2, 0;
    R.diagonal() << 0.2;
}

void castMPCToQPHessian(const Eigen::DiagonalMatrix<double, 2> &Q, const Eigen::DiagonalMatrix<double, 1> &R, int mpcWindow,
                        Eigen::SparseMatrix<double> &hessianMatrix, int Nx, int Nu)
{

    hessianMatrix.resize(Nx*(mpcWindow+1) + Nu * mpcWindow, Nx*(mpcWindow+1) + Nu * mpcWindow);

    //populate hessian matrix
    for(int i = 0; i<Nx*(mpcWindow+1) + Nu * mpcWindow; i++){
        if(i < Nx*(mpcWindow+1)){
            int posQ=i%Nx;
            float value = Q.diagonal()[posQ];
            if(value != 0)
                hessianMatrix.insert(i,i) = value;
        }
        else{
            int posR=i%Nu;
            float value = R.diagonal()[posR];
            if(value != 0)
                hessianMatrix.insert(i,i) = value;
        }
    }
}

void castMPCToQPGradient(const Eigen::DiagonalMatrix<double, 2> &Q, const Eigen::Matrix<double, 2, 1> &xRef, int mpcWindow,
                         Eigen::VectorXd &gradient, int Nx, int Nu)
{

    Eigen::Matrix<double,2,1> Qx_ref;
    Qx_ref = Q * (-xRef);

    // populate the gradient vector
    gradient = Eigen::VectorXd::Zero(Nx*(mpcWindow+1) +  Nu*mpcWindow, 1);
    for(int i = 0; i<Nx*(mpcWindow+1); i++){
        int posQ=i%Nx;
        float value = Qx_ref(posQ,0);
        gradient(i,0) = value;
    }
}

void castMPCToQPConstraintMatrix(const Eigen::Matrix<double, 2, 2> &dynamicMatrix, const Eigen::Matrix<double, 2, 1> &controlMatrix,
                                 int mpcWindow, Eigen::SparseMatrix<double> &constraintMatrix, int Nx, int Nu)
{
    constraintMatrix.resize(Nx*(mpcWindow+1)  + Nx*(mpcWindow+1) + Nu * mpcWindow, Nx*(mpcWindow+1) + Nu * mpcWindow);

    // populate linear constraint matrix
    for(int i = 0; i<Nx*(mpcWindow+1); i++){
        constraintMatrix.insert(i,i) = -1;
    }

    for(int i = 0; i < mpcWindow; i++)
        for(int j = 0; j<Nx; j++)
            for(int k = 0; k<Nx; k++){
                float value = dynamicMatrix(j,k);
                if(value != 0){
                    constraintMatrix.insert(Nx * (i+1) + j, Nx * i + k) = value;
                }
            }

    for(int i = 0; i < mpcWindow; i++)
        for(int j = 0; j < Nx; j++)
            for(int k = 0; k < Nu; k++){
                float value = controlMatrix(j,k);
                if(value != 0){
                    constraintMatrix.insert(Nx*(i+1)+j, Nu*i+k+Nx*(mpcWindow + 1)) = value;
                }
            }

    for(int i = 0; i<Nx*(mpcWindow+1) + Nu*mpcWindow; i++){
        constraintMatrix.insert(i+(mpcWindow+1)*Nx,i) = 1;
    }
}

void castMPCToQPConstraintVectors(const Eigen::Matrix<double, 2, 1> &xMax, const Eigen::Matrix<double, 2, 1> &xMin,
                                   const Eigen::Matrix<double, 1, 1> &uMax, const Eigen::Matrix<double, 1, 1> &uMin,
                                   const Eigen::Matrix<double, 2, 1> &x0,
                                   int mpcWindow, Eigen::VectorXd &lowerBound, Eigen::VectorXd &upperBound, int Nx, int Nu)
{
    // evaluate the lower and the upper inequality vectors
    Eigen::VectorXd lowerInequality = Eigen::MatrixXd::Zero(Nx*(mpcWindow+1) +  Nu * mpcWindow, 1);
    Eigen::VectorXd upperInequality = Eigen::MatrixXd::Zero(Nx*(mpcWindow+1) +  Nu * mpcWindow, 1);
    for(int i=0; i<mpcWindow+1; i++){
        lowerInequality.block(Nx*i,0,Nx,1) = xMin;
        upperInequality.block(Nx*i,0,Nx,1) = xMax;
    }
    for(int i=0; i<mpcWindow; i++){
        lowerInequality.block(Nu * i + Nx * (mpcWindow + 1), 0, Nu, 1) = uMin;
        upperInequality.block(Nu * i + Nx * (mpcWindow + 1), 0, Nu, 1) = uMax;
    }

    // evaluate the lower and the upper equality vectors
    Eigen::VectorXd lowerEquality = Eigen::MatrixXd::Zero(Nx*(mpcWindow+1),1 );
    Eigen::VectorXd upperEquality;
    lowerEquality.block(0,0,Nx,1) = -x0;
    upperEquality = lowerEquality;
    lowerEquality = lowerEquality;

    // merge inequality and equality vectors
    lowerBound = Eigen::MatrixXd::Zero(2*Nx*(mpcWindow+1) +  Nu*mpcWindow,1 );
    lowerBound << lowerEquality,
        lowerInequality;

    upperBound = Eigen::MatrixXd::Zero(2*Nx*(mpcWindow+1) +  Nu*mpcWindow,1 );
    upperBound << upperEquality,
        upperInequality;
}

void updateConstraintVectors(const Eigen::Matrix<double, 2, 1> &x0,
                             Eigen::VectorXd &lowerBound, Eigen::VectorXd &upperBound, int Nx)
{
    lowerBound.block(0,0,Nx,1) = -x0;
    upperBound.block(0,0,Nx,1) = -x0;
}

double getErrorNorm(const Eigen::Matrix<double, 2, 1> &x,
                    const Eigen::Matrix<double, 2, 1> &xRef, int Nx)
{
    // evaluate the error
    Eigen::Matrix<double, 2, 1> error = x - xRef;

    // return the norm
    return error.norm();
}

int main()
{
    // set the preview window
    int mpcWindow = 200;
    const int Nx = 2;
    const int Nu = 1;

    // allocate the dynamics matrices
    Eigen::Matrix<double, Nx, Nx> a;
    Eigen::Matrix<double, Nx, Nu> b;

    // allocate the constraints vector
    Eigen::Matrix<double, Nx, 1> xMax;
    Eigen::Matrix<double, Nx, 1> xMin;
    Eigen::Matrix<double, Nu, 1> uMax;
    Eigen::Matrix<double, Nu, 1> uMin;

    // allocate the weight matrices
    Eigen::DiagonalMatrix<double, Nx> Q;
    Eigen::DiagonalMatrix<double, Nu> R;

    // allocate the initial and the reference state space
    Eigen::Matrix<double, Nx, 1> x0;
    Eigen::Matrix<double, Nx, 1> xRef;

    // allocate QP problem matrices and vectores
    Eigen::SparseMatrix<double> hessian;
    Eigen::VectorXd gradient;
    Eigen::SparseMatrix<double> linearMatrix;
    Eigen::VectorXd lowerBound;
    Eigen::VectorXd upperBound;

    // set the initial and the desired states
    x0 << 0, 0 ;
    xRef <<  1/0.127, 0;

    // set MPC problem quantities
    setDynamicsMatrices(a, b);
    setInequalityConstraints(xMax, xMin, uMax, uMin);
    setWeightMatrices(Q, R);

    // cast the MPC problem as QP problem
    castMPCToQPHessian(Q, R, mpcWindow, hessian, Nx, Nu);
    castMPCToQPGradient(Q, xRef, mpcWindow, gradient, Nx, Nu);
    castMPCToQPConstraintMatrix(a, b, mpcWindow, linearMatrix, Nx, Nu);
    castMPCToQPConstraintVectors(xMax, xMin, uMax, uMin, x0, mpcWindow, lowerBound, upperBound, Nx, Nu);

    // instantiate the solver
    OsqpEigen::Solver solver;

    // settings
    //solver.settings()->setVerbosity(false);
    solver.settings()->setWarmStart(true);

    // set the initial data of the QP solver
    solver.data()->setNumberOfVariables(Nx * (mpcWindow + 1) + Nu * mpcWindow);
    solver.data()->setNumberOfConstraints(2 * Nx * (mpcWindow + 1) + Nu * mpcWindow);
    if(!solver.data()->setHessianMatrix(hessian)) return 1;
    if(!solver.data()->setGradient(gradient)) return 1;
    if(!solver.data()->setLinearConstraintsMatrix(linearMatrix)) return 1;
    if(!solver.data()->setLowerBound(lowerBound)) return 1;
    if(!solver.data()->setUpperBound(upperBound)) return 1;

    // instantiate the solver
    if(!solver.initSolver()) return 1;

    // controller input and QPSolution vector
    Eigen::VectorXd ctr;
    Eigen::VectorXd QPSolution;

    // number of iteration steps
    int numberOfSteps = 200;

    std::ofstream myfile;
    myfile.open ("mpc_log.csv");
    myfile << "Log of MPC by osqp in C++.\n";
    myfile << "x1,x2,\n";

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

        // solve the QP problem
        if(solver.solveProblem() != OsqpEigen::ErrorExitFlag::NoError) return 1;

        // get the controller input
        QPSolution = solver.getSolution();
        ctr = QPSolution.block(Nx * (mpcWindow + 1), 0, Nu, 1);

        // save data into file
        auto x0Data = x0.data();

        // propagate the model
        x0 = a * x0 + b * ctr;
        myfile << x0[0] << "," << x0[1] <<",\n";

        // update the constraint bound
        updateConstraintVectors(x0, lowerBound, upperBound, Nx);
        if(!solver.updateBounds(lowerBound, upperBound)) return 1;
      }
    myfile.close();
    return 0;
}


/**
 * @file bias_svd_test.cpp
 * @author Siddharth Agrawal
 * @author Wenhao Huang
 *
 * Test the BiasSVDFunction class.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/bias_svd/bias_svd.hpp>

#include <ensmallen.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::svd;

BOOST_AUTO_TEST_SUITE(BiasSVDTest);

BOOST_AUTO_TEST_CASE(BiasSVDFunctionRandomEvaluate)
{
  // Define useful constants.
  const size_t numUsers = 100;
  const size_t numItems = 100;
  const size_t numRatings = 1000;
  const size_t maxRating = 5;
  const size_t rank = 10;
  const size_t numTrials = 50;

  // Make a random rating dataset.
  arma::mat data = arma::randu(3, numRatings);
  data.row(0) = floor(data.row(0) * numUsers);
  data.row(1) = floor(data.row(1) * numItems);
  data.row(2) = floor(data.row(2) * maxRating + 0.5);

  // Manually set last row to maximum user and maximum item.
  data(0, numRatings - 1) = numUsers - 1;
  data(1, numRatings - 1) = numItems - 1;

  // Make a BiasSVDFunction with zero regularization.
  BiasSVDFunction<arma::mat> biasSVDFunc(data, rank, 0);

  for (size_t i = 0; i < numTrials; i++)
  {
    arma::mat parameters = arma::randu(rank + 1, numUsers + numItems);

    // Calculate cost by summing up cost of each example.
    double cost = 0;
    for (size_t j = 0; j < numRatings; j++)
    {
      const size_t user = data(0, j);
      const size_t item = data(1, j) + numUsers;

      const double rating = data(2, j);
      const double userBias = parameters(rank, user);
      const double itemBias = parameters(rank, item);
      const double ratingError = rating - userBias - itemBias -
          arma::dot(parameters.col(user).subvec(0, rank - 1),
                    parameters.col(item).subvec(0, rank - 1));
      const double ratingErrorSquared = ratingError * ratingError;

      cost += ratingErrorSquared;
    }

    // Compare calculated cost and value obtained using Evaluate().
    BOOST_REQUIRE_CLOSE(cost, biasSVDFunc.Evaluate(parameters), 1e-5);
  }
}

BOOST_AUTO_TEST_CASE(BiasSVDFunctionRegularizationEvaluate)
{
  // Define useful constants.
  const size_t numUsers = 100;
  const size_t numItems = 100;
  const size_t numRatings = 1000;
  const size_t maxRating = 5;
  const size_t rank = 10;
  const size_t numTrials = 50;

  // Make a random rating dataset.
  arma::mat data = arma::randu(3, numRatings);
  data.row(0) = floor(data.row(0) * numUsers);
  data.row(1) = floor(data.row(1) * numItems);
  data.row(2) = floor(data.row(2) * maxRating + 0.5);

  // Manually set last row to maximum user and maximum item.
  data(0, numRatings - 1) = numUsers - 1;
  data(1, numRatings - 1) = numItems - 1;

  // Make three BiasSVDFunction objects with different amounts of
  // regularization.
  BiasSVDFunction<arma::mat> biasSVDFuncNoReg(data, rank, 0);
  BiasSVDFunction<arma::mat> biasSVDFuncSmallReg(data, rank, 0.5);
  BiasSVDFunction<arma::mat> biasSVDFuncBigReg(data, rank, 20);

  for (size_t i = 0; i < numTrials; i++)
  {
    arma::mat parameters = arma::randu(rank + 1, numUsers + numItems);

    // Calculate the regularization contributions of parameters corresponding to
    // each rating and sum them up.
    double smallRegTerm = 0;
    double bigRegTerm = 0;
    for (size_t j = 0; j < numRatings; j++)
    {
      const size_t user = data(0, j);
      const size_t item = data(1, j) + numUsers;

      const double userVecNorm = arma::norm(parameters.col(user), 2);
      const double itemVecNorm = arma::norm(parameters.col(item), 2);

      smallRegTerm += 0.5 * (userVecNorm * userVecNorm +
                             itemVecNorm * itemVecNorm);
      bigRegTerm += 20 * (userVecNorm * userVecNorm +
                          itemVecNorm * itemVecNorm);
    }

    // Cost with regularization should be close to the sum of cost without
    // regularization and the regularization terms.
    BOOST_REQUIRE_CLOSE(biasSVDFuncNoReg.Evaluate(parameters) + smallRegTerm,
        biasSVDFuncSmallReg.Evaluate(parameters), 1e-5);
    BOOST_REQUIRE_CLOSE(biasSVDFuncNoReg.Evaluate(parameters) + bigRegTerm,
        biasSVDFuncBigReg.Evaluate(parameters), 1e-5);
  }
}

BOOST_AUTO_TEST_CASE(BiasSVDFunctionGradient)
{
  // Define useful constants.
  const size_t numUsers = 50;
  const size_t numItems = 50;
  const size_t numRatings = 100;
  const size_t maxRating = 5;
  const size_t rank = 10;

  // Make a random rating dataset.
  arma::mat data = arma::randu(3, numRatings);
  data.row(0) = floor(data.row(0) * numUsers);
  data.row(1) = floor(data.row(1) * numItems);
  data.row(2) = floor(data.row(2) * maxRating + 0.5);

  // Manually set last row to maximum user and maximum item.
  data(0, numRatings - 1) = numUsers - 1;
  data(1, numRatings - 1) = numItems - 1;

  arma::mat parameters = arma::randu(rank + 1, numUsers + numItems);

  // Make two BiasSVDFunction objects, one with regularization and one
  // without.
  BiasSVDFunction<arma::mat> biasSVDFunc1(data, rank, 0);
  BiasSVDFunction<arma::mat> biasSVDFunc2(data, rank, 0.5);

  // Calculate gradients for both the objects.
  arma::mat gradient1, gradient2;
  biasSVDFunc1.Gradient(parameters, gradient1);
  biasSVDFunc2.Gradient(parameters, gradient2);

  // Perturbation constant.
  const double epsilon = 0.0001;
  double costPlus1, costMinus1, numGradient1;
  double costPlus2, costMinus2, numGradient2;

  for (size_t i = 0; i < rank; i++)
  {
    for (size_t j = 0; j < numUsers + numItems; j++)
    {
      // Perturb parameter with a positive constant and get costs.
      parameters(i, j) += epsilon;
      costPlus1 = biasSVDFunc1.Evaluate(parameters);
      costPlus2 = biasSVDFunc2.Evaluate(parameters);

      // Perturb parameter with a negative constant and get costs.
      parameters(i, j) -= 2 * epsilon;
      costMinus1 = biasSVDFunc1.Evaluate(parameters);
      costMinus2 = biasSVDFunc2.Evaluate(parameters);

      // Compute numerical gradients using the costs calculated above.
      numGradient1 = (costPlus1 - costMinus1) / (2 * epsilon);
      numGradient2 = (costPlus2 - costMinus2) / (2 * epsilon);

      // Restore the parameter value.
      parameters(i, j) += epsilon;

      // Compare numerical and backpropagation gradient values.
      if (std::abs(gradient1(i, j)) <= 1e-6)
        BOOST_REQUIRE_SMALL(numGradient1, 1e-5);
      else
        BOOST_REQUIRE_CLOSE(numGradient1, gradient1(i, j), 0.02);

      if (std::abs(gradient2(i, j)) <= 1e-6)
        BOOST_REQUIRE_SMALL(numGradient2, 1e-5);
      else
        BOOST_REQUIRE_CLOSE(numGradient2, gradient2(i, j), 0.02);
    }
  }
}

BOOST_AUTO_TEST_CASE(BiasSVDOutputSizeTest)
{
  // Define useful constants.
  const size_t numUsers = 100;
  const size_t numItems = 50;
  const size_t numRatings = 500;
  const size_t maxRating = 5;
  const size_t rank = 5;
  const size_t iterations = 10;

  // Make a random rating dataset.
  arma::mat data = arma::randu(3, numRatings);
  data.row(0) = floor(data.row(0) * numUsers);
  data.row(1) = floor(data.row(1) * numItems);
  data.row(2) = floor(data.row(2) * maxRating + 0.5);

  // Manually set last row to maximum user and maximum item.
  data(0, numRatings - 1) = numUsers - 1;
  data(1, numRatings - 1) = numItems - 1;

  // Resulting user/item matrices/bias.
  arma::mat userLatent, itemLatent;
  arma::vec userBias, itemBias;

  // Apply Bias SVD.
  BiasSVD<> biasSVD(iterations);
  biasSVD.Apply(data, rank, itemLatent, userLatent, itemBias, userBias);

  // Check the size of outputs.
  BOOST_REQUIRE_EQUAL(itemLatent.n_rows, numItems);
  BOOST_REQUIRE_EQUAL(itemLatent.n_cols, rank);
  BOOST_REQUIRE_EQUAL(userLatent.n_rows, rank);
  BOOST_REQUIRE_EQUAL(userLatent.n_cols, numUsers);
  BOOST_REQUIRE_EQUAL(itemBias.n_elem, numItems);
  BOOST_REQUIRE_EQUAL(userBias.n_elem, numUsers);
}

BOOST_AUTO_TEST_CASE(BiasSVDFunctionOptimize)
{
  // Define useful constants.
  const size_t numUsers = 50;
  const size_t numItems = 50;
  const size_t numRatings = 100;
  const size_t iterations = 30;
  const size_t rank = 10;
  const double alpha = 0.01;
  const double lambda = 0.01;

  // Initiate random parameters.
  arma::mat parameters = arma::randu(rank + 1, numUsers + numItems);

  // Make a random rating dataset.
  arma::mat data = arma::randu(3, numRatings);
  data.row(0) = floor(data.row(0) * numUsers);
  data.row(1) = floor(data.row(1) * numItems);

  // Manually set last row to maximum user and maximum item.
  data(0, numRatings - 1) = numUsers - 1;
  data(1, numRatings - 1) = numItems - 1;

  // Make rating entries based on the parameters.
  for (size_t i = 0; i < numRatings; i++)
  {
    const size_t user = data(0, i);
    const size_t item = data(1, i) + numUsers;
    const double userBias = parameters(rank, user);
    const double itemBias = parameters(rank, item);
    data(2, i) = userBias + itemBias +
        arma::dot(parameters.col(user).subvec(0, rank - 1),
                  parameters.col(item).subvec(0, rank - 1));
  }

  // Make the Bias SVD function and the optimizer.
  BiasSVDFunction<arma::mat> biasSVDFunc(data, rank, lambda);
  ens::StandardSGD optimizer(alpha, iterations * numRatings);

  // Obtain optimized parameters after training.
  arma::mat optParameters = arma::randu(rank + 1, numUsers + numItems);
  optimizer.Optimize(biasSVDFunc, optParameters);

  // Get predicted ratings from optimized parameters.
  arma::mat predictedData(1, numRatings);
  for (size_t i = 0; i < numRatings; i++)
  {
    const size_t user = data(0, i);
    const size_t item = data(1, i) + numUsers;
    const double userBias = optParameters(rank, user);
    const double itemBias = optParameters(rank, item);
    predictedData(0, i) = userBias + itemBias +
        arma::dot(optParameters.col(user).subvec(0, rank - 1),
                  optParameters.col(item).subvec(0, rank - 1));
  }

  // Calculate relative error.
  const double relativeError = arma::norm(data.row(2) - predictedData, "frob") /
                               arma::norm(data, "frob");

  // Relative error should be small.
  BOOST_REQUIRE_SMALL(relativeError, 1e-2);
}

// The test is only compiled if the user has specified OpenMP to be
// used.
#ifdef HAS_OPENMP

// Test Bias SVD with parallel SGD.
BOOST_AUTO_TEST_CASE(BiasSVDFunctionParallelOptimize)
{
  // Define useful constants.
  const size_t numUsers = 50;
  const size_t numItems = 50;
  const size_t numRatings = 100;
  const size_t rank = 10;
  const double alpha = 0.01;
  const double lambda = 0.01;

  // Initiate random parameters.
  arma::mat parameters = arma::randu(rank + 1, numUsers + numItems);

  // Make a random rating dataset.
  arma::mat data = arma::randu(3, numRatings);
  data.row(0) = floor(data.row(0) * numUsers);
  data.row(1) = floor(data.row(1) * numItems);

  // Manually set last row to maximum user and maximum item.
  data(0, numRatings - 1) = numUsers - 1;
  data(1, numRatings - 1) = numItems - 1;

  // Make rating entries based on the parameters.
  for (size_t i = 0; i < numRatings; i++)
  {
    const size_t user = data(0, i);
    const size_t item = data(1, i) + numUsers;
    const double userBias = parameters(rank, user);
    const double itemBias = parameters(rank, item);
    data(2, i) = userBias + itemBias +
        arma::dot(parameters.col(user).subvec(0, rank - 1),
                  parameters.col(item).subvec(0, rank - 1));
  }

  // Make the Bias SVD function and the optimizer.
  BiasSVDFunction<arma::mat> biasSVDFunc(data, rank, lambda);

  ens::ConstantStep decayPolicy(alpha);

  // Iterate till convergence.
  // The threadShareSize is chosen such that each function gets optimized.
  ens::ParallelSGD<ens::ConstantStep> optimizer(0,
      std::ceil((float) biasSVDFunc.NumFunctions() / omp_get_max_threads()), 1e-5,
      true, decayPolicy);

  // Obtain optimized parameters after training.
  arma::mat optParameters = arma::randu(rank + 1, numUsers + numItems);
  optimizer.Optimize(biasSVDFunc, optParameters);

  // Get predicted ratings from optimized parameters.
  arma::mat predictedData(1, numRatings);
  for (size_t i = 0; i < numRatings; i++)
  {
    const size_t user = data(0, i);
    const size_t item = data(1, i) + numUsers;
    const double userBias = optParameters(rank, user);
    const double itemBias = optParameters(rank, item);
    predictedData(0, i) = userBias + itemBias +
        arma::dot(optParameters.col(user).subvec(0, rank - 1),
                  optParameters.col(item).subvec(0, rank - 1));
  }

  // Calculate relative error.
  const double relativeError = arma::norm(data.row(2) - predictedData, "frob") /
                               arma::norm(data, "frob");

  // Relative error should be small.
  BOOST_REQUIRE_SMALL(relativeError, 1e-2);
}

#endif

BOOST_AUTO_TEST_SUITE_END();


#pragma once
#include <Eigen/Core>
#include "datatypes.hpp"
#include <Eigen/Eigen>
#include "profile.hpp"
#include "integration.hpp"
#include "transformation.hpp"

namespace gd {

using namespace std;
USING_PART_OF_NAMESPACE_EIGEN

class Grid1d {
public:
	virtual int findindex(double) { return 0; }
	virtual double indexto_x(int) { return 0; }
	//virtual void indexto_x(int) { return 0; }
	virtual bool inrange(double) { return false; }
	virtual int length() {return 0;}
};

template<class Base=Grid1d>
class Grid1dRegular : public Base {
public:
	Grid1dRegular(double x1, double x2, int gridpoints) : x1(x1), x2(x2), _gridpoints(gridpoints) {}
	int findindex(double x) {
		return x < x1 ? 0 :
			(x >= x2 ? _gridpoints-1 : (int)((x-x1)/(x2-x1)*(_gridpoints-1)));
	}
	double indexto_x(int i) {
		return x1 + (x2-x1)/(length()-1)*i;
	}
	bool inrange(double r) {
		return (r >= x1) && (r < x2);
	}
	int length() { return _gridpoints; }
	double x1, x2;
	int _gridpoints;
};

/*class Grid2d {
public:
	virtual int findindex(double, double) { return 0; }
	virtual bool inrange(double, double) { return false; }
	virtual int length() {return 0;}
};*/

template<class GridX=Grid1d, class GridY=Grid1d>
class Grid2d {
public:
	typedef GridX GridXType;
	typedef GridY GridYType;
	Grid2d(GridX* gridx, GridY* gridy) : gridx(gridx), gridy(gridy) {}
	void findindex2d(double x, double y, int& xi, int &yi) {
		//double r = sqrt(x*x+y*y);
		//double phi = fmod(atan2(y,x)+2*M_PI, 2*M_PI);
		xi = gridx->findindex(x);
		yi = gridy->findindex(y);
	}
	int findindex(double x, double y) {
		//double r = sqrt(x*x+y*y);
		//double phi = fmod(atan2(y,x)+2*M_PI, 2*M_PI);
		return gridx->findindex(x) * gridy->length() + gridy->findindex(y);;
	}
	void indexto_xy(int i, int j, double &x, double &y) {
		x = gridx->indexto_x(i);
		y = gridy->indexto_x(j);
	}
	bool inrange(double x, double y) {
		//double r = sqrt(x*x+y*y);
		//double phi = fmod(atan2(y,x)+2*M_PI, 2*M_PI);
		return gridx->inrange(x) && gridy->inrange(y);
	}
	int length() {
		return gridx->length() * gridy->length();
	}
	GridX* gridx;
	GridY* gridy;
};

template<class GridX=Grid1d, class GridY=Grid1d, class GridZ=Grid1d>
class Grid3d {
public:
	typedef GridX GridXType;
	typedef GridY GridYType;
	typedef GridZ GridZType;
	Grid3d(GridX* gridx, GridY* gridy, GridZ* gridz) : gridx(gridx), gridy(gridy), gridz(gridz) {}
	void findindex3d(double x, double y, double z, int& xi, int &yi, int &zi) {
		//double r = sqrt(x*x+y*y);
		//double phi = fmod(atan2(y,x)+2*M_PI, 2*M_PI);
		xi = gridx->findindex(x);
		yi = gridy->findindex(y);
		zi = gridz->findindex(z);
	}
	int findindex(double x, double y, double z) {
		//double r = sqrt(x*x+y*y);
		//double phi = fmod(atan2(y,x)+2*M_PI, 2*M_PI);
		return (gridx->findindex(x) * gridy->length() + gridy->findindex(y)) * gridz->length() + gridz->findindex(z);
	}
	void indexto_xyz(int i, int j, int k, double &x, double &y, double &z) {
		x = gridx->indexto_x(i);
		y = gridy->indexto_x(j);
		z = gridz->indexto_x(k);
	}
	bool inrange(double x, double y, double z) {
		//double r = sqrt(x*x+y*y);
		//double phi = fmod(atan2(y,x)+2*M_PI, 2*M_PI);
		return gridx->inrange(x) && gridy->inrange(y) && gridz->inrange(z);
	}
	int length() {
		return gridx->length() * gridy->length() * gridz->length();
	}
	GridX* gridx;
	GridY* gridy;
	GridZ* gridz;
};

/*
		basis_evaluator<2, Basis::degree, Basis> basis_evaluator;
		if(mesh.inrange(x))
			value = basis_evaluator.eval(solution, this, xi, u, yi, v);
*/

template<int DIM, class B, int I=B::degree, class BI=B, class T=double>
struct basis_evaluator;

template<int DIM, class B, int I, class BI, class T>
struct basis_evaluator
{
	typedef basis_evaluator<DIM, B, I-1, typename BI::next_type, T> next_typeI;
	typedef basis_evaluator<DIM-1, B, B::degree, B, T> next_typeDIM;
	next_typeI nextI;
	next_typeDIM nextDIM;

	template<class... Ts>
	T eval(int i, double u, Ts... ts) {
		if((i % (B::degree+1)) == I) { // first search for right i
			BI basis;
			return basis(u) * nextDIM.eval(i/(B::degree+1), ts...);
		} else {
			return nextI.eval(i, u, ts...);
		}
	}
};

template<int DIM, class B,  class BI, class T>
struct basis_evaluator<DIM, B, -1, BI, T>
{
	template<class... Ts>
	T eval(int, double, Ts...) {
		return 1;
	}
};

template<class B, int I, class BI, class T>
struct basis_evaluator<0, B, I, BI, T>
{
	T eval(int) {
		return 1;
	}
};

template<int DIM, int I, class B, class BI, class T>
struct basis_function_integrator;

//template<int DIM, int I, class B, class BI, class T>
//struct basis_function_integrator<2,I,B,BI,T>
template<int DIM, int I, class B, class BI, class T>
struct basis_function_integrator
{
	typedef basis_function_integrator<DIM,   I-1, B, typename BI::next_type, T> next_typeI;
	typedef basis_function_integrator<DIM-1, B::degree, B, B, T> next_typeDIM;
	T x1, x2;
	next_typeI nextI;
	next_typeDIM nextDIM;

	template<class... Ts>
	basis_function_integrator(T x1, T x2, Ts... ts) : x1(x1), x2(x2), nextI(x1, x2, ts...), nextDIM(ts...) {
		//cout << "DIM " << DIM << " " << x1 << " " << x2 << endl;
	}

	template<class F>
	T integrate_function2d(F f, int i) {
		if((i % (B::degree+1)) == I) { // first search for right i
			BI basis;
			double dx = (x2-x1);
			auto fi = [&](double x) -> double { return basis((x-this->x1)/dx) / dx  * this->nextDIM.integrate_function2d_y(f, i / (B::degree+1), x); };
			IntegratorGSL<> integratorGSL(fi);
			//cout << "integrate: " << x1 << " to " << x2 << endl;
			return integratorGSL.integrate(x1, x2);
		} else {
			return nextI.integrate_function2d(f, i);
		}
	}
	template<class F>
	T integrate_function2d_y(F f, int i, double x) {
		if((i % (B::degree+1)) == I) { // first search for right i
			BI basis;
			double dx = (x2-x1);
			auto fi = [&](double y) -> double { return basis((y-this->x1)/dx)/dx * f(x, y); };
			IntegratorGSL<> integratorGSL(fi);
			//cout << "integrate: " << x1 << " to " << x2 << endl;
			return integratorGSL.integrate(x1, x2);
		} else {
			return nextI.integrate_function2d_y(f, i, x);
		}
	}
};

template<int DIM, class B, class BI, class T>
struct basis_function_integrator<DIM, -1, B, BI, T>
{
	template<class... Ts>
	basis_function_integrator(T, T, Ts...) {}
	template<class F>
	T integrate_function2d(F, int) { return 1; }
	template<class F>
	T integrate_function2d_y(F, int, double) { return 1; }
};

template<int I, class B, class BI, class T>
struct basis_function_integrator<0, I, B, BI, T>
{
	template<class F>
	T integrate_function(F, int) { return 1; }
};

template<int DIM, int I, int J, class B, class BI, class BJ, class T=double>
struct basis_integrator;

template<int DIM, int I, int J, class B, class BI, class BJ, class T>
struct basis_integrator
{
	typedef basis_integrator<DIM, I-1, J, B, typename BI::next_type, BJ, T> next_typeI;
	typedef basis_integrator<DIM, I, J-1, B, BI, typename BJ::next_type, T> next_typeJ;
	typedef basis_integrator<DIM-1, B::degree, B::degree, B, B, B, T> next_typeDIM;
	next_typeI nextI;
	next_typeJ nextJ;
	next_typeDIM nextDIM;

	//template<class... Ts>
	T integrate(int i, int j) {
		if((i % (B::degree+1)) == I) { // first search for right i
			if((j  % (B::degree+1)) == J) { // then right j
				BI basis1;
				BJ basis2;
				auto f = [&](double x) ->double { return basis1(x) * basis2(x) * this->nextDIM.integrate(i / (B::degree+1), j / (B::degree+1)); };
				IntegratorGSL<> integratorGSL(f);
				return integratorGSL.integrate(0, 1);
			}  else {
				return nextJ.integrate(i, j);
			}
		} else {
			return nextI.integrate(i, j);
		}
	}
};

// sentinels
template<int I, int J, class B, class BI, class BJ, class T>
struct basis_integrator<0, I, J, B, BI, BJ, T>
{
	template<class... Ts>
	T integrate(int, int) {
		return 1;
	}
};

template<int DIM, int J, class B, class BI, class BJ, class T>
struct basis_integrator<DIM, -1, J, B, BI, BJ, T>
{
	template<class... Ts>
	T integrate(int, int) {
		return 1;
	}
};

template<int DIM, int I, class B, class BI, class BJ, class T>
struct basis_integrator<DIM, I, -1, B, BI, BJ, T>
{
	template<class... Ts>
	T integrate(int, int) {
		return 1;
	}
};

template<int DIM, class Basis, class T=double>
struct MeshRegularNodalHelper;

template<class Basis, class T>
struct MeshRegularNodalHelper<0, Basis, T> {
	typedef Basis basis_type;
	enum { dof_per_cell = 1 };
};

template<int DIM, class Basis, class T>
struct MeshRegularNodalHelper {
	typedef Basis basis_type;
	typedef MeshRegularNodalHelper<DIM, Basis, T> type;
	typedef MeshRegularNodalHelper<DIM-1, Basis, T> sub_type;
	enum { dof_per_cell = (Basis::degree+1) * sub_type::dof_per_cell };

	/*int get_dof() { return dof;}
	int get_n_cells() { return n_cells;}
	int dof_index(int cell_index, int local_index) {
		return  cell_index*(dof_per_cell-1)+local_index;
	}*/
};

template<int DIM, class Basis, class T=double>
class MeshRegularNodal;

template<class Basis, class T>
class MeshRegularNodal<2, Basis, T> {
public:
	typedef Basis basis_type;
	T x1, x2;
	int n_cells_x;
	int n_cells_y;
	Grid1dRegular<> xgrid;
	Grid1dRegular<> ygrid;
	Grid2d<Grid1dRegular<>, Grid1dRegular<>> grid;
	MatrixXd M;
	//Transformation1d_in_3d* transformation;
	int dof, dofx, dofy;
	int dof1d;
	enum { dof_per_cell = MeshRegularNodalHelper<2, Basis, T>::dof_per_cell };
	enum { dof_per_cell1d = MeshRegularNodalHelper<1, Basis, T>::dof_per_cell };

	MeshRegularNodal(T x1, T y1, T x2, T y2, int n_cells_x, int n_cells_y) : n_cells_x(n_cells_x), n_cells_y(n_cells_y), xgrid(x1, x2, n_cells_x+1), ygrid(y1, y2, n_cells_y+1), grid(&xgrid, &ygrid), M(1, 1) {

		if(Basis::degree == 0) {
			dofx = n_cells_x;
			dofy = n_cells_y;
		} else {
			dofx = (1 + n_cells_x) + (dof_per_cell1d-2)*n_cells_x; // 1 dof per border + dofs inside the cel
			dofy = (1 + n_cells_y) + (dof_per_cell1d-2)*n_cells_y; // 1 dof per border + dofs inside the cel
		}
		dof = dofx * dofy;
		//cout << "n_cells_x = " << n_cells_x << " dofx = " << dofx << endl;
		//cout << "n_cells_y = " << n_cells_y << " dofy = " << dofy << endl;
		//cout << "dof = " << dof << " dof_per_cell = " << dof_per_cell << endl;
		//MatrixXd m = MatrixXd::Zero(dof, dof);
		M.resize(dof, dof);
		M =  MatrixXd::Zero(dof, dof);
		//T scale = 1; //TODO: (x2-x1)/n_cells;

		//basis_integrator<1, Basis::degree, Basis::degree, Basis, Basis, Basis, T> bi;
		//cout  << "test 00 " << bi.integrate(0, 0) << endl;
		//cout  << "test 01 " << bi.integrate(0, 1) << endl;
		//cout  << "test 11 " << bi.integrate(1, 1) << endl;

		T integrals[dof_per_cell][dof_per_cell];
		for(int j = 0; j < dof_per_cell; j++) {
			for(int k = 0; k < (j+1); k++) {
				basis_integrator<2, Basis::degree, Basis::degree, Basis, Basis, Basis, T> bi;
				T integral = bi.integrate(j, k);
				//typedef selfintegrator<Basis::degree, Basis::degree> selfintegrator_type;
				//selfintegrator_type si;
				//double integral = si.integrate(j,k);
				//cout << j << " " << k << " " << integral << endl;
				integrals[j][k] = integral;
				integrals[k][j] = integral;
			}
		}
		for(int xi = 0; xi < n_cells_x; xi++) {
			for(int yi = 0; yi < n_cells_y; yi++) {
				for(int j = 0; j < dof_per_cell; j++) {
				for(int k = 0; k < dof_per_cell; k++) {
					int i1 = this->dof_index(xi, yi, j);
					int i2 = this->dof_index(xi, yi, k);
					//cout << "xi = " << xi << " yi = " << yi << " j = " << j << " k = " << k;
					//cout << "            i1 = " << i1 << " i2 = " << i2 << endl;
					M(i1, i2) += integrals[j][k];
				}}
			}
		}
		/*for(int i = 0; i < n_cells; i++) {
			for(int j = 0; j < dof_per_cell; j++) {
				for(int k = 0; k < dof_per_cell; k++) {
					int i1 = i*(dof_per_cell-1)+j;
					int i2 = i*(dof_per_cell-1)+k;
					m(i1, i2) = m(i1, i2) + integrals[j][k];
				}
			}
		}*/
		//cout << M << endl;
		//m = m * scale;
		//ctrans = m.inverse();
	}

	double eval_(double_vector solution_, double x, double y) {
		VectorXd solution = VectorXd::Map(solution_.data().begin(), solution_.size());
		return eval(solution, x, y);
	}

	double eval(VectorXd& solution, double x, double y) {
		/*int cell_index = mesh.findindex(x);
		T xleft = mesh.indexto_x(cell_index);
		T xright = mesh.indexto_x(cell_index+1);
		*/
		int xi = xgrid.findindex(x);
		int yi = ygrid.findindex(y);
		T x1 = xgrid.indexto_x(xi);
		T x2 = xgrid.indexto_x(xi+1);
		T y1 = ygrid.indexto_x(yi);
		T y2 = ygrid.indexto_x(yi+1);
		double u = (x-x1)/(x2-x1);
		double v = (y-y1)/(y2-y1);
		//grid.index
		//T dx = xright-xleft;
		double value = 0;
		//cout << "xi = " << xi;
		//cout << " yi = " << yi;
		if(grid.inrange(x, y)) {
			basis_evaluator<2, Basis> basis_evaluator;
			for(int i = 0; i < dof_per_cell; i++) {
				//cout << " i = " << i;
				//cout << " index = " << dof_index(xi, yi, i) << endl;
				double a = solution(dof_index(xi, yi, i));
				value += a * basis_evaluator.eval(i, u, v);
			}
		} else {
			cout << "x and y not in range: (" << x << "," << y << ")" << endl;
		}
		return value;
	}

	double basis_uv(int i, double u, double v) {
		basis_evaluator<2, Basis> basis_evaluator;
		return basis_evaluator.eval(i, u, v);
	}

	void solve_coordinates(double_vector inner_products, double_vector coordinates) {
		assert((int)inner_products.size() == dof);
		assert((int)coordinates.size() == dof);
		VectorXd x = VectorXd::Map(inner_products.data().begin(), inner_products.size());
		VectorXd a(dof);
		M.llt().solve(x, &a);
		VectorXd::Map(coordinates.data().begin(), coordinates.size()) = a;
	}

	void test(double_vector result_, double scale1, double scale2) {
		auto f = [&](double x, double y) -> double { return cos(x * scale1 + y * scale2); };
		VectorXd x = VectorXd::Zero(dof);
		assert((int)result_.size() == dof);
		for(int yi = 0; yi < n_cells_y; yi++) {
			for(int xi = 0; xi < n_cells_x; xi++) {
				for(int i = 0; i < dof_per_cell; i++) {
					double x1, x2;
					double y1, y2;
					grid.indexto_xy(xi, yi, x1, y1);
					grid.indexto_xy(xi+1, yi+1, x2, y2);
					//cout << "integrate x[" << x1 << " " << x2 << "] y[" << y1 << " " << y2 << "]";
					basis_function_integrator<2, Basis::degree, Basis, Basis, T> bfi(x1, x2, y1, y2);
					double a = bfi.integrate_function2d(f, i);
					x(dof_index(xi, yi, i)) += a;
					//cout << " " << a;
 					//cout << endl;
				}
			}
		}
		VectorXd a(dof);
		M.llt().solve(x, &a);
		//VectorXd result = VectorXd::Map(result_.data().begin(), result_.size());
		VectorXd::Map(result_.data().begin(), result_.size()) = a;
		//result = a;
	}

	//int get_dof() { return dof; }
	int dof_index(int x_index, int y_index, int basis_index) {
		if(Basis::degree == 0) {
			return x_index + y_index * dofx;
		}
		int xi = x_index*(dof_per_cell1d-1) + basis_index % dof_per_cell1d;
		int yi = y_index*(dof_per_cell1d-1) + basis_index / dof_per_cell1d;
		return xi + yi * dofx;
	}
	int get_dof() { return dof; }
	int get_dof_per_cell() { return dof_per_cell; }
	/*int get_n_cells() { return grid->;}
	int dof_index(int cell_index, int local_index) {
		return  cell_index*(dof_per_cell-1)+local_index;
	}*/

}; // class MeshRegularNodal<2...>

template<class Basis, class T>
class MeshRegularNodal<3, Basis, T> {
	public:
		typedef Basis basis_type;
		T x1, x2;
		int n_cells_x;
		int n_cells_y;
		int n_cells_z;
		Grid1dRegular<> xgrid;
		Grid1dRegular<> ygrid;
		Grid1dRegular<> zgrid;
		Grid3d<Grid1dRegular<>, Grid1dRegular<>, Grid1dRegular<>> grid;
		MatrixXd M;
	//Transformation1d_in_3d* transformation;
		int dof, dofx, dofy, dofz;
		int dof1d;
		enum { dof_per_cell = MeshRegularNodalHelper<3, Basis, T>::dof_per_cell };
		enum { dof_per_cell1d = MeshRegularNodalHelper<1, Basis, T>::dof_per_cell };

	MeshRegularNodal(T x1, T y1, T z1, T x2, T y2, T z2, int n_cells_x, int n_cells_y, int n_cells_z) : n_cells_x(n_cells_x), n_cells_y(n_cells_y), n_cells_z(n_cells_z), xgrid(x1, x2, n_cells_x+1), ygrid(y1, y2, n_cells_y+1), zgrid(z1, z2, n_cells_z+1), grid(&xgrid, &ygrid, &zgrid), M(1, 1) {

		if(Basis::degree == 0) {
			dofx = n_cells_x;
			dofy = n_cells_y;
			dofz = n_cells_z;
		} else {
			dofx = (1 + n_cells_x) + (dof_per_cell1d-2)*n_cells_x; // 1 dof per border + dofs inside the cel
			dofy = (1 + n_cells_y) + (dof_per_cell1d-2)*n_cells_y; // 1 dof per border + dofs inside the cel
			dofz = (1 + n_cells_z) + (dof_per_cell1d-2)*n_cells_z; // 1 dof per border + dofs inside the cel
		}
		dof = dofx * dofy * dofz;
		cout << "n_cells_x = " << n_cells_x << " dofx = " << dofx << endl;
		cout << "n_cells_y = " << n_cells_y << " dofy = " << dofy << endl;
		cout << "n_cells_z = " << n_cells_z << " dofz = " << dofz << endl;
		cout << "dof = " << dof << " dof_per_cell = " << dof_per_cell << endl;
		//MatrixXd m = MatrixXd::Zero(dof, dof);
		M.resize(dof, dof);
		M =  MatrixXd::Zero(dof, dof);
		//T scale = 1; //TODO: (x2-x1)/n_cells;

		//basis_integrator<1, Basis::degree, Basis::degree, Basis, Basis, Basis, T> bi;
		//cout  << "test 00 " << bi.integrate(0, 0) << endl;
		//cout  << "test 01 " << bi.integrate(0, 1) << endl;
		//cout  << "test 11 " << bi.integrate(1, 1) << endl;

		T integrals[dof_per_cell][dof_per_cell];
		for(int j = 0; j < dof_per_cell; j++) {
			for(int k = 0; k < (j+1); k++) {
				basis_integrator<2, Basis::degree, Basis::degree, Basis, Basis, Basis, T> bi;
				T integral = bi.integrate(j, k);
				//typedef selfintegrator<Basis::degree, Basis::degree> selfintegrator_type;
				//selfintegrator_type si;
				//double integral = si.integrate(j,k);
				//cout << j << " " << k << " " << integral << endl;
				integrals[j][k] = integral;
				integrals[k][j] = integral;
			}
		}
		for(int xi = 0; xi < n_cells_x; xi++) {
			for(int yi = 0; yi < n_cells_y; yi++) {
				for(int zi = 0; zi < n_cells_z; zi++) {
					for(int j = 0; j < dof_per_cell; j++) {
						for(int k = 0; k < dof_per_cell; k++) {
							int i1 = this->dof_index(xi, yi, zi, j);
							int i2 = this->dof_index(xi, yi, zi, k);
							//cout << "xi = " << xi << " yi = " << yi << " zi = " << zi << " j = " << j << " k = " << k;
							//cout << "            i1 = " << i1 << " i2 = " << i2 << endl;
							M(i1, i2) += integrals[j][k];
					}}
				}
			}
		}
		/*for(int i = 0; i < n_cells; i++) {
			for(int j = 0; j < dof_per_cell; j++) {
				for(int k = 0; k < dof_per_cell; k++) {
					int i1 = i*(dof_per_cell-1)+j;
					int i2 = i*(dof_per_cell-1)+k;
					m(i1, i2) = m(i1, i2) + integrals[j][k];
				}
			}
		}*/
		//cout << M << endl;
		//m = m * scale;
		//ctrans = m.inverse();
	}

		double eval_(double_vector solution_, double x, double y, double z) {
			VectorXd solution = VectorXd::Map(solution_.data().begin(), solution_.size());
			return eval(solution, x, y, z);
		}

		double eval(VectorXd& solution, double x, double y, double z) {
		/*int cell_index = mesh.findindex(x);
		T xleft = mesh.indexto_x(cell_index);
		T xright = mesh.indexto_x(cell_index+1);
		*/
			int xi = xgrid.findindex(x);
			int yi = ygrid.findindex(y);
			int zi = zgrid.findindex(z);
			T x1 = xgrid.indexto_x(xi);
			T x2 = xgrid.indexto_x(xi+1);
			T y1 = ygrid.indexto_x(yi);
			T y2 = ygrid.indexto_x(yi+1);
			T z1 = zgrid.indexto_x(zi);
			T z2 = zgrid.indexto_x(zi+1);
			double u = (x-x1)/(x2-x1);
			double v = (y-y1)/(y2-y1);
			double w = (z-z1)/(z2-z1);
		//grid.index
		//T dx = xright-xleft;
			double value = 0;
		//cout << "xi = " << xi;
		//cout << " yi = " << yi;
			if(grid.inrange(x, y, z)) {
				basis_evaluator<3, Basis> basis_evaluator;
				for(int i = 0; i < dof_per_cell; i++) {
				//cout << " i = " << i;
				//cout << " index = " << dof_index(xi, yi, i) << endl;
					double a = solution(dof_index(xi, yi, zi, i));
					value += a * basis_evaluator.eval(i, u, v, w);
				}
			} else {
				cout << "x, y and z not in range: (" << x << "," << y << "," << z << ")" << endl;
			}
			return value;
		}

		double basis_uv(int i, double u, double v, double w) {
			basis_evaluator<3, Basis> basis_evaluator;
			return basis_evaluator.eval(i, u, v, w);
		}

		void solve_coordinates(double_vector inner_products, double_vector coordinates) {
			assert((int)inner_products.size() == dof);
			assert((int)coordinates.size() == dof);
			VectorXd x = VectorXd::Map(inner_products.data().begin(), inner_products.size());
			VectorXd a(dof);
			M.llt().solve(x, &a);
			VectorXd::Map(coordinates.data().begin(), coordinates.size()) = a;
		}

		/*void test(double_vector result_, double scale1, double scale2) {
			auto f = [&](double x, double y) -> double { return cos(x * scale1 + y * scale2); };
			VectorXd x = VectorXd::Zero(dof);
			assert(result_.size() == dof);
			for(int yi = 0; yi < n_cells_y; yi++) {
				for(int xi = 0; xi < n_cells_x; xi++) {
					for(int i = 0; i < dof_per_cell; i++) {
						double x1, x2;
						double y1, y2;
						grid.indexto_xy(xi, yi, x1, y1);
						grid.indexto_xy(xi+1, yi+1, x2, y2);
					//cout << "integrate x[" << x1 << " " << x2 << "] y[" << y1 << " " << y2 << "]";
						basis_function_integrator<2, Basis::degree, Basis, Basis, T> bfi(x1, x2, y1, y2);
						double a = bfi.integrate_function2d(f, i);
						x(dof_index(xi, yi, i)) += a;
					//cout << " " << a;
 					//cout << endl;
					}
				}
			}
			VectorXd a(dof);
			M.llt().solve(x, &a);
		//VectorXd result = VectorXd::Map(result_.data().begin(), result_.size());
			VectorXd::Map(result_.data().begin(), result_.size()) = a;
		//result = a;
		}*/

	//int get_dof() { return dof; }
		int dof_index(int x_index, int y_index, int z_index, int basis_index) {
			if(Basis::degree == 0) {
				return x_index + y_index * dofx + z_index * dofx * dofy;
			}
			int xi = x_index*(dof_per_cell1d-1) + basis_index % dof_per_cell1d;
			int yi = y_index*(dof_per_cell1d-1) + (basis_index / dof_per_cell1d)  % dof_per_cell1d ;
			int zi = z_index*(dof_per_cell1d-1) + basis_index / (dof_per_cell1d * dof_per_cell1d);
			return xi + yi * dofx + zi * dofx * dofy;
		}
		int get_dof() { return dof; }
		int get_dof_per_cell() { return dof_per_cell; }
	/*int get_n_cells() { return grid->;}
	int dof_index(int cell_index, int local_index) {
		return  cell_index*(dof_per_cell-1)+local_index;
	}*/

	};

template<class Basis, class T>
class MeshRegularNodal<1, Basis, T> {
public:
	typedef Basis basis_type;
	T x1, x2;
	int n_cells_x;
	Grid1dRegular<> xgrid;
	Grid1dRegular<>& grid;
	MatrixXd M;
//Transformation1d_in_3d* transformation;
	int dof, dofx;
	int dof1d;
	enum { dof_per_cell = MeshRegularNodalHelper<1, Basis, T>::dof_per_cell };
	enum { dof_per_cell1d = MeshRegularNodalHelper<1, Basis, T>::dof_per_cell };

	MeshRegularNodal(T x1, T x2, int n_cells_x) : n_cells_x(n_cells_x), xgrid(x1, x2, n_cells_x+1), grid(xgrid), M(1, 1) {

	if(Basis::degree == 0) {
		dofx = n_cells_x;
	} else {
		dofx = (1 + n_cells_x) + (dof_per_cell1d-2)*n_cells_x; // 1 dof per border + dofs inside the cel
	}
	dof = dofx;
	cout << "n_cells_x = " << n_cells_x << " dofx = " << dofx << endl;
	cout << "dof = " << dof << " dof_per_cell = " << dof_per_cell << endl;
	//MatrixXd m = MatrixXd::Zero(dof, dof);
	M.resize(dof, dof);
	M =  MatrixXd::Zero(dof, dof);
	//T scale = 1; //TODO: (x2-x1)/n_cells;

	//basis_integrator<1, Basis::degree, Basis::degree, Basis, Basis, Basis, T> bi;
	//cout  << "test 00 " << bi.integrate(0, 0) << endl;
	//cout  << "test 01 " << bi.integrate(0, 1) << endl;
	//cout  << "test 11 " << bi.integrate(1, 1) << endl;

	T integrals[dof_per_cell][dof_per_cell];
	for(int j = 0; j < dof_per_cell; j++) {
		for(int k = 0; k < (j+1); k++) {
			basis_integrator<2, Basis::degree, Basis::degree, Basis, Basis, Basis, T> bi;
			T integral = bi.integrate(j, k);
			//typedef selfintegrator<Basis::degree, Basis::degree> selfintegrator_type;
			//selfintegrator_type si;
			//double integral = si.integrate(j,k);
			//cout << j << " " << k << " " << integral << endl;
			integrals[j][k] = integral;
			integrals[k][j] = integral;
		}
	}
	for(int xi = 0; xi < n_cells_x; xi++) {
		for(int j = 0; j < dof_per_cell; j++) {
			for(int k = 0; k < dof_per_cell; k++) {
				int i1 = this->dof_index(xi, j);
				int i2 = this->dof_index(xi, k);
				//cout << "xi = " << xi << " yi = " << yi << " zi = " << zi << " j = " << j << " k = " << k;
				//cout << "            i1 = " << i1 << " i2 = " << i2 << endl;
				M(i1, i2) += integrals[j][k];
			}}
	}
	/*for(int i = 0; i < n_cells; i++) {
		for(int j = 0; j < dof_per_cell; j++) {
			for(int k = 0; k < dof_per_cell; k++) {
				int i1 = i*(dof_per_cell-1)+j;
				int i2 = i*(dof_per_cell-1)+k;
				m(i1, i2) = m(i1, i2) + integrals[j][k];
			}
		}
	}*/
	//cout << M << endl;
	//m = m * scale;
	//ctrans = m.inverse();
}

	double eval_(double_vector solution_, double x) {
		VectorXd solution = VectorXd::Map(solution_.data().begin(), solution_.size());
		return eval(solution, x);
	}

	template<class Array>
	double eval(Array& solution, double x) {
	/*int cell_index = mesh.findindex(x);
	T xleft = mesh.indexto_x(cell_index);
	T xright = mesh.indexto_x(cell_index+1);
	*/
		int xi = xgrid.findindex(x);
		T x1 = xgrid.indexto_x(xi);
		T x2 = xgrid.indexto_x(xi+1);
		double u = (x-x1)/(x2-x1);
	//grid.index
	//T dx = xright-xleft;
		double value = 0;
	//cout << "xi = " << xi;
	//cout << " yi = " << yi;
		if(grid.inrange(x)) {
			basis_evaluator<1, Basis> basis_evaluator;
			for(int i = 0; i < dof_per_cell; i++) {
			//cout << " i = " << i;
			//cout << " index = " << dof_index(xi, yi, i) << endl;
				double a = solution[dof_index(xi, i)];
				value += a * basis_evaluator.eval(i, u);
			}
		} else {
		cout << "x not in range: (" << x << endl;
		}
		return value;
	}

	double eval(VectorXd& solution, double x) {
	/*int cell_index = mesh.findindex(x);
	T xleft = mesh.indexto_x(cell_index);
	T xright = mesh.indexto_x(cell_index+1);
	*/
		int xi = xgrid.findindex(x);
		T x1 = xgrid.indexto_x(xi);
		T x2 = xgrid.indexto_x(xi+1);
		double u = (x-x1)/(x2-x1);
	//grid.index
	//T dx = xright-xleft;
		double value = 0;
	//cout << "xi = " << xi;
	//cout << " yi = " << yi;
		if(grid.inrange(x)) {
			basis_evaluator<1, Basis> basis_evaluator;
			for(int i = 0; i < dof_per_cell; i++) {
			//cout << " i = " << i;
			//cout << " index = " << dof_index(xi, yi, i) << endl;
				double a = solution(dof_index(xi, i));
				value += a * basis_evaluator.eval(i, u);
			}
		} else {
			cout << "x not in range: (" << x << endl;
		}
		return value;
	}

	double basis_uv(int i, double u) {
		basis_evaluator<1, Basis> basis_evaluator;
		return basis_evaluator.eval(i, u);
	}

	void solve_coordinates(double_vector inner_products, double_vector coordinates) {
		assert((int)inner_products.size() == dof);
		assert((int)coordinates.size() == dof);
		VectorXd x = VectorXd::Map(inner_products.data().begin(), inner_products.size());
		VectorXd a(dof);
		M.llt().solve(x, &a);
		VectorXd::Map(coordinates.data().begin(), coordinates.size()) = a;
	}

	/*void test(double_vector result_, double scale1, double scale2) {
		auto f = [&](double x, double y) -> double { return cos(x * scale1 + y * scale2); };
		VectorXd x = VectorXd::Zero(dof);
		assert(result_.size() == dof);
		for(int yi = 0; yi < n_cells_y; yi++) {
			for(int xi = 0; xi < n_cells_x; xi++) {
				for(int i = 0; i < dof_per_cell; i++) {
					double x1, x2;
					double y1, y2;
					grid.indexto_xy(xi, yi, x1, y1);
					grid.indexto_xy(xi+1, yi+1, x2, y2);
				//cout << "integrate x[" << x1 << " " << x2 << "] y[" << y1 << " " << y2 << "]";
					basis_function_integrator<2, Basis::degree, Basis, Basis, T> bfi(x1, x2, y1, y2);
					double a = bfi.integrate_function2d(f, i);
					x(dof_index(xi, yi, i)) += a;
				//cout << " " << a;
				//cout << endl;
				}
			}
		}
		VectorXd a(dof);
		M.llt().solve(x, &a);
	//VectorXd result = VectorXd::Map(result_.data().begin(), result_.size());
		VectorXd::Map(result_.data().begin(), result_.size()) = a;
	//result = a;
	}*/

//int get_dof() { return dof; }
	int dof_index(int x_index, int basis_index) {
		if(Basis::degree == 0) {
			return x_index;
		}
		int xi = x_index*(dof_per_cell1d-1) + basis_index % dof_per_cell1d;
		return xi;
	}
	int get_dof() { return dof; }
	int get_dof_per_cell() { return dof_per_cell; }
/*int get_n_cells() { return grid->;}
int dof_index(int cell_index, int local_index) {
	return  cell_index*(dof_per_cell-1)+local_index;
}*/

};

}


#include "ExponentialPlusPiecewisePolynomial.h"
#include <Eigen/Core>
#include <random>
#include <iostream>
#include <cmath>
#include "testUtil.h"

using namespace std;
using namespace Eigen;

default_random_engine generator;

template <typename CoefficientType>
void testSimpleCase() {
  typedef ExponentialPlusPiecewisePolynomial<CoefficientType> ExponentialPlusPiecewisePolynomialType;
  typedef typename ExponentialPlusPiecewisePolynomialType::MatrixX MatrixX;
  typedef typename ExponentialPlusPiecewisePolynomialType::VectorX VectorX;
  int num_coefficients = 5;
  int num_segments = 1;

  MatrixX K = MatrixX::Random(1, 1);
  MatrixX A = MatrixX::Random(1, 1);
  MatrixX alpha = MatrixX::Random(1, 1);

  auto segment_times = PiecewiseFunction::randomSegmentTimes(num_segments, generator);
  auto polynomial_part = PiecewisePolynomial<CoefficientType>::random(1, 1, num_coefficients, segment_times);

  ExponentialPlusPiecewisePolynomial<CoefficientType> expPlusPp(K, A, alpha, polynomial_part);
  ExponentialPlusPiecewisePolynomial<CoefficientType> derivative = expPlusPp.derivative();

  uniform_real_distribution<CoefficientType> uniform(expPlusPp.getStartTime(), expPlusPp.getEndTime());
  double t = uniform(generator);
  auto check = K(0) * std::exp(A(0) * (t - expPlusPp.getStartTime())) * alpha(0) + polynomial_part.scalarValue(t);
  auto derivative_check = K(0) * A(0) * std::exp(A(0) * (t - expPlusPp.getStartTime())) * alpha(0) + polynomial_part.derivative().scalarValue(t);

  valuecheck(check, expPlusPp.value(t)(0), 1e-8);
  valuecheck(derivative_check, derivative.value(t)(0), 1e-8);
}

int main(int argc, char **argv) {
  testSimpleCase<double>();
  std::cout << "test passed";
  return 0;
}