/*
 * serial.hpp
 *
 *  Created on: 10 Jul 2018
 *      Author: scsjd
 */

#ifndef _SERIAL_HPP_
#define _SERIAL_HPP_

#include <sstream>
#include <string>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/string.hpp>
#include <NTL/ZZ.h>
#include <NTL/ZZ_p.h>
#include <NTL/ZZX.h>
#include <NTL/vec_ZZ.h>

namespace boost {
	namespace serialization {
		template<class Archive>
		void save(Archive & ar, const NTL::ZZ& p, const unsigned int version) {
			std::ostringstream oss;
			oss << p;
			ar << oss.str();
		}

		template<class Archive>
		void load(Archive & ar, NTL::ZZ& p, const unsigned int version) {
			std::string s;
			ar >> s;
			std::istringstream iss(s);
			iss >> p;
		}

		template<class Archive>
		void serialize(Archive & ar, NTL::ZZ& p, const unsigned int version) {
			boost::serialization::split_free(ar, p, version);
		}

		template<class Archive>
		void save(Archive & ar, const NTL::ZZ_p& p, const unsigned int version) {
			std::ostringstream oss;
			oss << p;
			ar << oss.str();
		}

		template<class Archive>
		void load(Archive & ar, NTL::ZZ_p& p, const unsigned int version) {
			std::string s;
			ar >> s;
			std::istringstream iss(s);
			iss >> p;
		}

		template<class Archive>
		void serialize(Archive & ar, NTL::ZZ_p& p, const unsigned int version) {
			boost::serialization::split_free(ar, p, version);
		}

		template<class Archive>
		void save(Archive & ar, const NTL::ZZX& p, const unsigned int version) {
			std::ostringstream oss;
			oss << p;
			ar << oss.str();
		}

		template<class Archive>
		void load(Archive & ar, NTL::ZZX& p, const unsigned int version) {
			std::string s;
			ar >> s;
			std::istringstream iss(s);
			iss >> p;
		}

		template<class Archive>
		void serialize(Archive & ar, NTL::ZZX& p, const unsigned int version) {
			boost::serialization::split_free(ar, p, version);
		}

		template<class Archive>
		void save(Archive & ar, const NTL::vec_ZZ& p, const unsigned int version) {
			std::ostringstream oss;
			oss << p;
			ar << oss.str();
		}

		template<class Archive>
		void load(Archive & ar, NTL::vec_ZZ& p, const unsigned int version) {
			std::string s;
			ar >> s;
			std::istringstream iss(s);
			iss >> p;
		}

		template<class Archive>
		void serialize(Archive & ar, NTL::vec_ZZ& p, const unsigned int version) {
			boost::serialization::split_free(ar, p, version);
		}

		template<class Archive>
		void save(Archive & ar, const NTL::vec_ZZ_p& p, const unsigned int version) {
			std::ostringstream oss;
			oss << p;
			ar << oss.str();
		}

		template<class Archive>
		void load(Archive & ar, NTL::vec_ZZ_p& p, const unsigned int version) {
			std::string s;
			ar >> s;
			std::istringstream iss(s);
			iss >> p;
		}

		template<class Archive>
		void serialize(Archive & ar, NTL::vec_ZZ_p& p, const unsigned int version) {
			boost::serialization::split_free(ar, p, version);
		}
	}
}

#endif /* _SERIAL_HPP_ */


//
// Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH.
// Copyright (c) 2015-2017, University of Bremen
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice, this
//   list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
#ifndef __MAPS_BRESENHAM_LINE__
#define __MAPS_BRESENHAM_LINE__

#include <Eigen/Core>

namespace maps { namespace tools
{

/**
 * This class use useful for calculation
 * straight lines in grid. The Bresenham Algorithm
 * is a fast but inaccurate way to do this.
 * Inaccurate means, that the result will have
 * aliasing artifacts.
 *
 * The implementation of the core algorithm was taken from an wikipedia article.
 * */
class Bresenham {
public:
    typedef Eigen::Vector2i Point;
    Bresenham(const Point& start, const Point& end);

    /**
     * Inits the algorithm.
     * The method may be used to 'reinit' the class
     * after a line was already interpolated.
     * */
    void init(const Point& start, const Point& end);
    void init(int startX, int startY, int endX, int endY);

    /**
     * Calculates the next point in the line
     * and returns it over the given parameters.
     *
     * returns false if the end point is reached an
     *              no further point can be calculated.
     *
     * */
    bool getNextPoint(Point& next);

private:
    bool hasP;
    int x0,y0, x1, y1, dx, sx, dy, sy, err;
};

}}  // namespace maps

#endif


/*
 fcs.cpp

 Copyright (c) 2014, 2015, 2016 Terumasa Tadano

 This file is distributed under the terms of the MIT license.
 Please see the file 'LICENCE.txt' in the root directory
 or http://opensource.org/licenses/mit-license.php for information.
*/

#include <iostream>
#include <string>
#include <cmath>
#include "../external/combination.hpp"
#include <boost/lexical_cast.hpp>
#include "files.h"
#include "interaction.h"
#include "error.h"
#include "memory.h"
#include "fcs.h"
#include "symmetry.h"
#include "system.h"
#include "timer.h"
#include "constants.h"

using namespace ALM_NS;

Fcs::Fcs(ALM *alm) : Pointers(alm){};
Fcs::~Fcs() {};

void Fcs::init(){

    int i;
    int maxorder = interaction->maxorder;

    std::cout << " FORCE CONSTANT" << std::endl;
    std::cout << " ==============" << std::endl << std::endl;

    memory->allocate(nints, maxorder);
    memory->allocate(nzero, maxorder);
    memory->allocate(fc_set, maxorder);
    memory->allocate(ndup, maxorder);

    for (i = 0; i < maxorder; ++i) nzero[i] = 0;
    generate_fclists(maxorder);

    std::cout << std::endl;
    for (i = 0; i < maxorder; ++i) {
        std::cout << "  Number of " << std::setw(9) << interaction->str_order[i] << " FCs : " << ndup[i].size();
	std::cout << std::endl;
    }
    std::cout << std::endl;

    // sort fc_set

    for (int order = 0; order < maxorder; ++order) {
        if (ndup[order].size() > 0) {
            std::sort(fc_set[order].begin(), fc_set[order].begin() + ndup[order][0]);
            int nbegin = ndup[order][0];
            int nend;
            for (unsigned int mm = 1; mm < ndup[order].size(); ++mm) {
                nend  = nbegin + ndup[order][mm];
                std::sort(fc_set[order].begin() + nbegin, fc_set[order].begin() + nend);
                nbegin += ndup[order][mm];
            }
        }
    }

    memory->deallocate(nints);
    memory->deallocate(nzero);
    timer->print_elapsed();
    std::cout << " --------------------------------------------------------------" << std::endl;
    std::cout << std::endl;
}

void Fcs::generate_fclists(int maxorder)
{
    int i, j;
    int i1, i2;
    int order;
    int i_prim;
    int *atmn, *atmn_mapped;
    int *ind, *ind_mapped;
    int *ind_tmp, *ind_mapped_tmp;
    int nxyz;
    unsigned int isym;

    double c_tmp;

    int **xyzcomponent;

    int nmother;
    int nat = system->nat;

    bool is_zero;
    bool *is_searched;

    std::cout << "  Finding symmetrically-independent force constants ..." << std::endl;

    memory->allocate(atmn, maxorder + 1);
    memory->allocate(atmn_mapped, maxorder + 1);
    memory->allocate(ind, maxorder + 1);
    memory->allocate(ind_mapped, maxorder + 1);
    memory->allocate(ind_tmp, maxorder - 1);
    memory->allocate(ind_mapped_tmp, maxorder + 1);
    memory->allocate(is_searched, 3 * nat);

    for (order = 0; order < maxorder; ++order) {

        std::cout << "   " << std::setw(8) << interaction->str_order[order] << " ...";

        fc_set[order].clear();
        ndup[order].clear();
        nmother = 0;

        nxyz = static_cast<int>(std::pow(3.0, order + 2));

        memory->allocate(xyzcomponent, nxyz, order + 2);
        get_xyzcomponent(order + 2, xyzcomponent);

        std::set<IntList> list_found;

        for (std::set<IntList>::iterator iter = interaction->pairs[order].begin(); iter != interaction->pairs[order].end(); ++iter) {

            for (i = 0; i < order + 2; ++i) atmn[i] = (*iter).iarray[i];

            for (i1 = 0; i1 < nxyz; ++i1) {
                for (i = 0; i < order + 2; ++i) ind[i] = 3 * atmn[i] + xyzcomponent[i1][i];

                if (!is_ascending(order + 2, ind)) continue;

                i_prim = min_inprim(order + 2, ind);
                std::swap(ind[0], ind[i_prim]);
                sort_tail(order + 2, ind);

                is_zero = false;

                if (list_found.find(IntList(order + 2, ind)) != list_found.end()) continue; // Already exits!

                // Search symmetrically-dependent parameter set

                int ndeps = 0;

                for (isym = 0; isym < symmetry->nsym; ++isym) {

                    if (!symmetry->sym_available[isym]) continue;

                    for (i = 0; i < order + 2; ++i) atmn_mapped[i] = symmetry->map_sym[atmn[i]][isym];

                    if (!is_inprim(order + 2, atmn_mapped)) continue;

                    for (i2 = 0; i2 < nxyz; ++i2) {
                        c_tmp = coef_sym(order + 2, isym, xyzcomponent[i1], xyzcomponent[i2]);
                        if (std::abs(c_tmp) > eps12) {
                            for (i = 0; i < order + 2; ++i) ind_mapped[i] = 3 * atmn_mapped[i] + xyzcomponent[i2][i];

                            i_prim = min_inprim(order + 2, ind_mapped);
                            std::swap(ind_mapped[0], ind_mapped[i_prim]);
                            sort_tail(order + 2, ind_mapped);

                            if (!is_zero) {
                                bool zeroflag = true;
                                for (i = 0; i < order + 2; ++i) {
                                    zeroflag = zeroflag & (ind[i] == ind_mapped[i]);
                                }
                                zeroflag = zeroflag & (std::abs(c_tmp + 1.0) < eps8);
                                is_zero = zeroflag;
                            }

                            // Add to found list (set) and fcset (vector) if the created is new one.

                            if (list_found.find(IntList(order + 2, ind_mapped)) == list_found.end()) {
                                list_found.insert(IntList(order + 2, ind_mapped));

                                fc_set[order].push_back(FcProperty(order + 2, c_tmp, ind_mapped, nmother));
                                ++ndeps;

                                // Add equivalent interaction list (permutation) if there are two or more indices
                                // which belong to the primitive cell.
                                // This procedure is necessary for fitting.

                                for (i = 0; i < 3 * nat; ++i) is_searched[i] = false;
                                is_searched[ind_mapped[0]] = true;
                                for (i = 1; i < order + 2; ++i) {
                                    if ((!is_searched[ind_mapped[i]]) && is_inprim(ind_mapped[i])) {

                                        for (j = 0; j < order + 2; ++j) ind_mapped_tmp[j] = ind_mapped[j];
                                        std::swap(ind_mapped_tmp[0], ind_mapped_tmp[i]);
                                        sort_tail(order + 2, ind_mapped_tmp);
                                        fc_set[order].push_back(FcProperty(order + 2, c_tmp, ind_mapped_tmp, nmother));

                                        ++ndeps;

                                        is_searched[ind_mapped[i]] = true;
                                    }
                                }

                            }
                        }
                    }
                } // close symmetry loop

                if (is_zero) {
                    for (i = 0; i < ndeps; ++i) fc_set[order].pop_back();
                    ++nzero[order];
                } else {
                    ndup[order].push_back(ndeps);
                    ++nmother;
                }

            } // close xyz component loop
        } // close atom number loop (iterator)

        memory->deallocate(xyzcomponent);
        list_found.clear();
        std::cout << " done. " << std::endl;
    } //close order loop

    memory->deallocate(atmn);
    memory->deallocate(atmn_mapped);
    memory->deallocate(ind);
    memory->deallocate(ind_mapped);
    memory->deallocate(ind_tmp);
    memory->deallocate(ind_mapped_tmp);
    memory->deallocate(is_searched);

    std::cout << "  Finished!" << std::endl;
}

double Fcs::coef_sym(const int n, const int symnum, const int *arr1, const int *arr2)
{
    double tmp = 1.0;
    int i;

    for (i = 0; i < n; ++i) {
        tmp *= symmetry->symrel[symnum][arr2[i]][arr1[i]];
    }
    return tmp;
}

bool Fcs::is_ascending(const int n, const int *arr)
{
    int i;
    for (i = 0; i < n - 1; ++i) {
        if (arr[i] > arr[i+1]) return false;
    }
    return true;
}

int Fcs::min_inprim(const int n, const int *arr)
{
    int i, j, atmnum;
    int natmin = symmetry->natmin;
    int minloc;
    int *ind;

    memory->allocate(ind, n);

    for (i = 0; i < n; ++i) {

        ind[i] = 3 * system->nat;
        atmnum = arr[i] / 3;

        for (j = 0; j < natmin; ++j) {
            if (symmetry->map_p2s[j][0] == atmnum) {
                ind[i] = arr[i];
                continue;
            }
        }
    }

    int minval = ind[0];
    minloc = 0;

    for (i = 0; i < n; ++i) {
        if (ind[i] < minval) {
            minval = ind[i];
            minloc = i;
        }
    }

    memory->deallocate(ind);
    return minloc;
}

bool Fcs::is_inprim(const int n, const int *arr)
{
    int i, j;
    int natmin = symmetry->natmin;

    for (i = 0; i < n; ++i) {
        for (j = 0; j < natmin; ++j) {
            if (symmetry->map_p2s[j][0] == arr[i]) return true;
        }
    }
    return false;
}

bool Fcs::is_inprim(const int n)
{
    int i, atmn;
    int natmin = symmetry->natmin;

    atmn = n / 3;

    for (i = 0; i < natmin; ++i) {
        if (symmetry->map_p2s[i][0] == atmn) return true;
    }

    return false;
}

void Fcs::get_xyzcomponent(int n, int **xyz)
{
    // Return xyz component for the given order using boost algorithm library

    std::vector<int> v;
    int i;

    for (i = 0; i < n; ++i) {
        v.push_back(0);
        v.push_back(1);
        v.push_back(2);
    }

    std::sort(v.begin(), v.end());

    int m = 0;

    do {
        xyz[m][0] = v[0];
        for (i = 1; i < n; ++i) xyz[m][i] = v[i];
        ++m;
    } while(boost::next_partial_permutation(v.begin(), v.begin() + n, v.end()));
}

void Fcs::sort_tail(const int n, int *arr)
{
    int i, m;

    m = n - 1;
    int *ind_tmp;

    memory->allocate(ind_tmp, m);

    for (i = 0; i < m; ++i) {
        ind_tmp[i] = arr[i + 1];
    }

    interaction->insort(m, ind_tmp);

    for (i = 0; i < m; ++i) {
        arr[i + 1] = ind_tmp[i];
    }

    memory->deallocate(ind_tmp);
}

std::string Fcs::easyvizint(const int n)
{
    int atmn;
    int crdn;
    atmn = n / 3 + 1;
    crdn = n % 3;
    std::string str_crd[3] = {"x", "y", "z"};
    std::string str_tmp;

    str_tmp = boost::lexical_cast<std::string>(atmn);
    str_tmp += str_crd[crdn];

    return  str_tmp;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2007, 2008, 2009, 2010 Ferdinando Ametrano
 Copyright (C) 2007 Chiara Fornarola
 Copyright (C) 2009 StatPro Italia srl
 Copyright (C) 2009 Nathan Abbott

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file bondfunctions.hpp
    \brief bond functions
*/

#ifndef quantlib_bond_functions_hpp
#define quantlib_bond_functions_hpp

#include <ql/cashflows/duration.hpp>
#include <ql/cashflow.hpp>
#include <ql/interestrate.hpp>
#include <boost/shared_ptr.hpp>

namespace QuantLib {

    // forward declarations
    class Bond;
    class DayCounter;
    class YieldTermStructure;

    //! Bond adapters of CashFlows functions
    /*! See CashFlows for functions' documentation.

        These adapters calls into CashFlows functions passing as input the
        Bond cashflows, the dirty price (i.e. npv) calculated from clean
        price, the bond settlement date (unless another date is given), zero
        ex-dividend days, and excluding any cashflow on the settlement date.

        Prices are always clean, as per market convention.
    */
    struct BondFunctions {
        //! \name Date inspectors
        //@{
        static Date startDate(const Bond& bond);
        static Date maturityDate(const Bond& bond);
        static bool isTradable(const Bond& bond,
                               Date settlementDate = Date());
        //@}

        //! \name CashFlow inspectors
        //@{
        static Leg::const_reverse_iterator
        previousCashFlow(const Bond& bond,
                         Date refDate = Date());
        static Leg::const_iterator nextCashFlow(const Bond& bond,
                                                Date refDate = Date());
        static Date previousCashFlowDate(const Bond& bond,
                                         Date refDate = Date());
        static Date nextCashFlowDate(const Bond& bond,
                                     Date refDate = Date());
        static Real previousCashFlowAmount(const Bond& bond,
                                           Date refDate = Date());
        static Real nextCashFlowAmount(const Bond& bond,
                                       Date refDate = Date());
        //@}

        //! \name Coupon inspectors
        //@{
        static Rate previousCouponRate(const Bond& bond,
                                       Date settlementDate = Date());
        static Rate nextCouponRate(const Bond& bond,
                                   Date settlementDate = Date());
        static Date accrualStartDate(const Bond& bond,
                                     Date settlementDate = Date());
        static Date accrualEndDate(const Bond& bond,
                                   Date settlementDate = Date());
        static Date referencePeriodStart(const Bond& bond,
                                         Date settlementDate = Date());
        static Date referencePeriodEnd(const Bond& bond,
                                       Date settlementDate = Date());
        static Time accrualPeriod(const Bond& bond,
                                  Date settlementDate = Date());
        static BigInteger accrualDays(const Bond& bond,
                                      Date settlementDate = Date());
        static Time accruedPeriod(const Bond& bond,
                                  Date settlementDate = Date());
        static BigInteger accruedDays(const Bond& bond,
                                      Date settlementDate = Date());
        static Real accruedAmount(const Bond& bond,
                                  Date settlementDate = Date());
        //@}

        //! \name YieldTermStructure functions
        //@{
        static Real cleanPrice(const Bond& bond,
                               const YieldTermStructure& discountCurve,
                               Date settlementDate = Date());
        static Real bps(const Bond& bond,
                        const YieldTermStructure& discountCurve,
                        Date settlementDate = Date());
        static Rate atmRate(const Bond& bond,
                            const YieldTermStructure& discountCurve,
                            Date settlementDate = Date(),
                            Real cleanPrice = Null<Real>());
        //@}

        //! \name Yield (a.k.a. Internal Rate of Return, i.e. IRR) functions
        //@{
        static Real cleanPrice(const Bond& bond,
                               const InterestRate& yield,
                               Date settlementDate = Date());
        static Real cleanPrice(const Bond& bond,
                               Rate yield,
                               const DayCounter& dayCounter,
                               Compounding compounding,
                               Frequency frequency,
                               Date settlementDate = Date());
        static Real dirtyPrice(const Bond& bond,
                               const InterestRate& yield,
                               Date settlementDate = Date());
        static Real dirtyPrice(const Bond& bond,
                               Rate yield,
                               const DayCounter& dayCounter,
                               Compounding compounding,
                               Frequency frequency,
                               Date settlementDate = Date());
        static Real bps(const Bond& bond,
                        const InterestRate& yield,
                        Date settlementDate = Date());
        static Real bps(const Bond& bond,
                        Rate yield,
                        const DayCounter& dayCounter,
                        Compounding compounding,
                        Frequency frequency,
                        Date settlementDate = Date());
        static Rate yield(const Bond& bond,
                          Real cleanPrice,
                          const DayCounter& dayCounter,
                          Compounding compounding,
                          Frequency frequency,
                          Date settlementDate = Date(),
                          Real accuracy = 1.0e-10,
                          Size maxIterations = 100,
                          Rate guess = 0.05);
        static Time duration(const Bond& bond,
                             const InterestRate& yield,
                             Duration::Type type = Duration::Modified,
                             Date settlementDate = Date() );
        static Time duration(const Bond& bond,
                             Rate yield,
                             const DayCounter& dayCounter,
                             Compounding compounding,
                             Frequency frequency,
                             Duration::Type type = Duration::Modified,
                             Date settlementDate = Date() );
        static Real convexity(const Bond& bond,
                              const InterestRate& yield,
                              Date settlementDate = Date());
        static Real convexity(const Bond& bond,
                              Rate yield,
                              const DayCounter& dayCounter,
                              Compounding compounding,
                              Frequency frequency,
                              Date settlementDate = Date());
        static Real basisPointValue(const Bond& bond,
                                    const InterestRate& yield,
                                    Date settlementDate = Date());
        static Real basisPointValue(const Bond& bond,
                                    Rate yield,
                                    const DayCounter& dayCounter,
                                    Compounding compounding,
                                    Frequency frequency,
                                    Date settlementDate = Date());
        static Real yieldValueBasisPoint(const Bond& bond,
                                         const InterestRate& yield,
                                         Date settlementDate = Date());
        static Real yieldValueBasisPoint(const Bond& bond,
                                         Rate yield,
                                         const DayCounter& dayCounter,
                                         Compounding compounding,
                                         Frequency frequency,
                                         Date settlementDate = Date());
        //@}

        //! \name Z-spread functions
        //@{
        static Real cleanPrice(const Bond& bond,
                               const boost::shared_ptr<YieldTermStructure>& discount,
                               Spread zSpread,
                               const DayCounter& dayCounter,
                               Compounding compounding,
                               Frequency frequency,
                               Date settlementDate = Date());
        static Spread zSpread(const Bond& bond,
                              Real cleanPrice,
                              const boost::shared_ptr<YieldTermStructure>&,
                              const DayCounter& dayCounter,
                              Compounding compounding,
                              Frequency frequency,
                              Date settlementDate = Date(),
                              Real accuracy = 1.0e-10,
                              Size maxIterations = 100,
                              Rate guess = 0.0);
        //@}

    };

}

#endif


/*
* Level_8 Exercise 3:
* Source file for Visitor Class
*
* variant visitor that moves the shapes
*
* @file Visitor.cpp
* @author Chunyu Yuan
* @version 1.0 02/22/2021
*
*/

#include<iostream> // Standard Input / Output Streams Library
#include <boost/variant/static_visitor.hpp>
#include "Point.hpp" //header file for Array class
#include "Line.hpp"  // header file for Line class
#include "Circle.hpp" // header file for circle class
#include "Visitor.hpp"

using namespace Chunyu::CAD;//namespace declaration for using Chunyu::CAD

// Default constructor
Visitor::Visitor(): boost::static_visitor<void>(),m_dx(0), m_dy(0) //colon syntax
{

}

// Copy constructor
Visitor::Visitor(const Visitor& source): boost::static_visitor<void>(),m_dx(source.m_dx), m_dy(source.m_dy)
{

}

// Constructor with x- and y-coordinates argument
Visitor::Visitor(double new_x, double new_y):boost::static_visitor<void>(), m_dx(new_x), m_dy(new_y)
{

}

// Destructor
Visitor::~Visitor()
{

}

// Assignment operator
Visitor& Visitor::operator = (const Visitor& source)
{
	if (this == &source)  //check if the address is the same
		return *this;     //if same, return itself
	//copy source x coordinate and assign this visitor
	m_dx = source.m_dx;
	//copy source y coordinate and assign this visitor
	m_dy = source.m_dy;

	return *this; //return this visitor
}

// () operator for Point,visit a point
void Visitor::operator () (Point& p) const
{
	p.X(p.X() + m_dx); //move the point's x coordinate m_dx
	p.Y(p.Y() + m_dy); // move the point's y coordinate m_dy
}

// () operator for Line, visit a line
void Visitor::operator () (Line& l) const
{
	//get line's start point and assign to tempory point p1
	Point p1 = l.startP();
	//get line's end point and assign to tempory point p2
	Point p2 = l.endP();

	//move the point p1's x coordinate m_dx
	p1.X(p1.X() + m_dx);
	//move the point p1's y coordinate m_dy
	p1.Y(p1.Y() + m_dy);
	//move the point p2's x coordinate m_dx
	p2.X(p2.X() + m_dx);
	//move the point p2's y coordinate m_dy
	p2.Y(p2.Y() + m_dy);

	//set line's start point to p1
	l.startP(p1);
	//set line's end point to p2
	l.endP(p2);
}

// () operator for Circle, visit a circle
void Visitor::operator () (Circle& c) const
{
	//get circle's centre point and assign to tempory point p1
	Point p1 = c.CentrePoint();

	//move the point p1's x coordinate m_dx
	p1.X(p1.X() + m_dx);
	//move the point p1's y coordinate m_dy
	p1.Y(p1.Y() + m_dy);

	//set ircle centre point
	c.CentrePoint(p1);
}


/*
 * Copyright (c) 2011-2021, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/main/LICENSE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 *     * Redistributions of source code must retain the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer.
 *     * Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials
 *       provided with the distribution.
 *     * Neither the name of the Georgia Tech Research Corporation nor
 *       the names of its contributors may be used to endorse or
 *       promote products derived from this software without specific
 *       prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS
 * IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL GEORGIA
 * TECH RESEARCH CORPORATION BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 * OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

/**
 * @file RRT.h
 * @author Tobias Kunz, Can Erdogan
 * @date Jan 31, 2013
 * @brief The generic RRT implementation. It can be inherited for modifications
 * to collision checking, sampling and etc.
 */

#ifndef DART_PLANNING_RRT_HPP_
#define DART_PLANNING_RRT_HPP_

#include <list>
#include <vector>
#include <Eigen/Core>

#include "dart/dynamics/SmartPointer.hpp"
#include "dart/simulation/World.hpp"

namespace flann {
template <class A>
class L2;
template <class A>
class Index;
} // namespace flann

namespace dart {

namespace simulation {
class World;
}
namespace dynamics {
class Skeleton;
}

namespace planning {

/// The rapidly-expanding random tree implementation
class RRT
{
public:
  /// To get byte-aligned Eigen vectors
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /// The result of attempting to create a new node to reach a target node
  typedef enum
  {
    STEP_COLLISION, // Collided with obstacle. No node added.
    STEP_REACHED,   // The target node is closer than step size (so reached). No
                    // node added.
    STEP_PROGRESS   // One node added.
  } StepResult;

public:
  // Initialization constants and search variables

  const int
      ndim; ///< Number of dof we can manipulate (may be less than robot's)
  const double stepSize; ///< Step size at each node creation

  int activeNode; ///< Last added node or the nearest node found after a search
  std::vector<int> parentVector; ///< The ith node in configVector has parent
                                 ///< with index pV[i]

  /// All visited configs
  // NOTE We are using pointers for the VectorXd's because flann copies the
  // pointers for the data points and we give it the copies made in the heap
  std::vector<const Eigen::VectorXd*> configVector;

public:
  //// Constructor with a single root
  RRT(dart::simulation::WorldPtr world,
      dart::dynamics::SkeletonPtr robot,
      const std::vector<std::size_t>& dofs,
      const Eigen::VectorXd& root,
      double stepSize = 0.02);

  /// Constructor with multiple roots (so, multiple trees)
  RRT(simulation::WorldPtr world,
      dynamics::SkeletonPtr robot,
      const std::vector<std::size_t>& dofs,
      const std::vector<Eigen::VectorXd>& roots,
      double stepSize = 0.02);

  /// Destructor
  virtual ~RRT()
  {
  }

  /// Reach for a random node by repeatedly extending nodes from the nearest
  /// neighbor in the tree. Stop if there is a collision.
  bool connect();

  /// Reach for a target by repeatedly extending nodes from the nearest
  /// neighbor. Stop if collide.
  bool connect(const Eigen::VectorXd& target);

  /// Try a single step with the given "stepSize" to a random configuration.
  /// Fail if collide.
  StepResult tryStep();

  /// Try a single step with the given "stepSize" to the given configuration.
  /// Fail if collide.
  StepResult tryStep(const Eigen::VectorXd& qtry);

  /// Tries to extend tree towards provided sample
  virtual StepResult tryStepFromNode(const Eigen::VectorXd& qtry, int NNidx);

  /// Checks if the given new configuration is in collision with an obstacle.
  /// Moreover, it is a an opportunity for child classes to change the new
  /// configuration if there is a need. For instance, task constrained planners
  /// might want to sample around this point and replace it with a better (less
  /// erroroneous due to constraint) node.
  virtual bool newConfig(
      std::list<Eigen::VectorXd>& intermediatePoints,
      Eigen::VectorXd& qnew,
      const Eigen::VectorXd& qnear,
      const Eigen::VectorXd& qtarget);

  /// Returns the distance between the current active node and the given node.
  /// TODO This might mislead the users to thinking returning the distance
  /// between the given target and the nearest neighbor.
  double getGap(const Eigen::VectorXd& target);

  /// Traces the path from some node to the initConfig node - useful in creating
  /// the full path after the goal is reached.
  void tracePath(
      int node, std::list<Eigen::VectorXd>& path, bool reverse = false);

  /// Returns the number of nodes in the tree.
  std::size_t getSize();

  /// Implementation-specific function for checking collisions
  virtual bool checkCollisions(const Eigen::VectorXd& c);

  /// Returns a random configuration with the specified node IDs
  virtual Eigen::VectorXd getRandomConfig();

protected:
  simulation::WorldPtr world; ///< The world that the robot is in
  dynamics::SkeletonPtr
      robot; ///< The ID of the robot for which a plan is generated
  std::vector<std::size_t>
      dofs; ///< The dofs of the robot the planner can manipulate

  /// The underlying flann data structure for fast nearest neighbor searches
  flann::Index<flann::L2<double> >* index;

  /// Returns a random value between the given minimum and maximum value
  double randomInRange(double min, double max);

  /// Returns the nearest neighbor to query point
  virtual int getNearestNeighbor(const Eigen::VectorXd& qsamp);

  /// Adds a new node to the tree
  virtual int addNode(const Eigen::VectorXd& qnew, int parentId);
};

} // namespace planning
} // namespace dart

#endif // DART_PLANNING_RRT_HPP_


/////////////////////////////////////////////////////////////
//                                                         //
// Copyright (c) 2003-2017 by The University of Queensland //
// Centre for Geoscience Computing                         //
// http://earth.uq.edu.au/centre-geoscience-computing      //
//                                                         //
// Primary Business: Brisbane, Queensland, Australia       //
// Licensed under the Open Software License version 3.0    //
// http://www.apache.org/licenses/LICENSE-2.0              //
//                                                         //
/////////////////////////////////////////////////////////////

#include <boost/version.hpp>
#include <iostream>
#include <sstream>
#include "Python/esys/lsm/util/QuaternionPy.h"
#include "Python/esys/lsm/util/Vec3Py.h"
#include "Foundation/StringUtil.h"
#include "Python/BoostPythonUtil/Util.h"

using namespace boost::python;
using namespace esys::lsm;

namespace esys
{
  namespace lsm
  {
    QuaternionPy::QuaternionPy() : Quaternion()
    {
    }

    QuaternionPy::QuaternionPy(
      double q0,
      double q1,
      double q2,
      double q3
    ) : Quaternion(q0, Vec3(q1, q2, q3))
    {
    }

    QuaternionPy::QuaternionPy(const Vec3Py &v)
      : Quaternion(
          cos(v.norm()/2.0),
          v*(sin(v.norm()/2.0)/v.norm())
        )
    {
    }

    QuaternionPy::QuaternionPy(const QuaternionPy &q) : Quaternion(q)
    {
    }

    QuaternionPy::QuaternionPy(const Quaternion &q) : Quaternion(q)
    {
    }

    QuaternionPy::QuaternionPy(const boost::python::object &pyOb)
    {
      if (esys::lsm::bpu::len(pyOb) == 4)
      {
        *this =
          QuaternionPy(
            boost::python::extract<double>(pyOb[0]),
            boost::python::extract<double>(pyOb[1]),
            boost::python::extract<double>(pyOb[2]),
            boost::python::extract<double>(pyOb[3])
          );
      }
      else
      {
        std::stringstream msg;
        msg
          << "Could not extract (q0,q1,q2,q3) elements from: "
          << boost::python::extract<std::string>(boost::python::str(pyOb))();
        throw runtime_error(msg.str());
      }
    }

    int QuaternionPy::len() const
    {
      return 4;
    }

    int QuaternionPy::getIndex(int i) const
    {
      const int origI = i;
      if (i < 0)
      {
          i += len();
      }
      if ((i >= len()) || i < 0)
      {
        std::stringstream msg;
        msg << "Index " << origI << " out of range [0,4)";
        PyErr_SetString(PyExc_IndexError, msg.str().c_str());
        throw_error_already_set();
      }
      return i;
    }

    double QuaternionPy::getItem(int i) const
    {
      return ((i=getIndex(i)) == 0) ? return_sca() : return_vec()[i-1];
    }

    void QuaternionPy::setItem(int i, double val)
    {
      i = getIndex(i);
      if (i == 0)
      {
        set_scalar(val);
      }
      else
      {
        Vec3 v = return_vec();
        v[i-1] = val;
        set_vector(v);
      }
    }

    std::string QuaternionPy::toString() const
    {
      return StringUtil::toString(*this);
    }

    boost::python::list QuaternionPy::toList() const
    {
      boost::python::list l;
      l.append(getItem(0));
      l.append(getItem(1));
      l.append(getItem(2));
      l.append(getItem(3));
      return l;
    }

    boost::python::tuple QuaternionPy::toTuple() const
    {
      return boost::python::tuple(toList());
    }

    boost::python::tuple
    QuaternionPy::PickleSuite::getinitargs(QuaternionPy const& q)
    {
      return q.toTuple();
    }

    Vec3Py QuaternionPy::asAngleAxis() const
    {
      return Vec3Py(Quaternion::asAngleAxis());
    }

    boost::python::tuple QuaternionPy::asAngleAxisPair() const
    {
      Quaternion::AngleAxisPair p = Quaternion::asAngleAxisPair();
      return boost::python::make_tuple(p.first, Vec3Py(p.second));
    }

    using boost::python::arg;
    void exportQuaternion()
    {
      // Disable autogeneration of C++ signatures (Boost 1.34.0 and higher)
      // for Epydoc which stumbles over indentation in the automatically generated strings.
      boost::python::docstring_options no_autogen(true,false);

      class_<esys::lsm::QuaternionPy>(
        "Quaternion",
        "A quaternion.",
        init<>()
      )
      .def(init<const object &>())
      .def(init<const Vec3Py &>())
      .def(init<const QuaternionPy &>())
      .def(
        init<double,double,double,double>(
          (arg("q0"), arg("q1"), arg("q2"), arg("q3") ),
          "Constructs quaternion with specifed component values.\n"
          "@type q0: float\n"
          "@kwarg q0: Scalar part, index 0.\n"
          "@type q1: float\n"
          "@kwarg q1: index 1\n"
          "@type q2: float\n"
          "@kwarg q2: index 2\n"
          "@type q3: float\n"
          "@kwarg q3: index 3\n"
        )
      )
      .def(
        "normalise",
        &QuaternionPy::normalize,
        "Normalises this quaternion.\n"
      )
      .def(
        "normalize",
        &QuaternionPy::normalize,
        "Normalizes this quaternion.\n"
      )
      .def(
        "asAngleAxis",
        &QuaternionPy::asAngleAxis,
        "Returns angle and axis rotation representation, where"
        " angle is the magnitude of the returned "
        " L{Vec3<esys.lsm.util.Vec3>} object.\n"
        "@rtype: L{Vec3<esys.lsm.util.Vec3>}\n"
        "@return: angle and axis rotation representation."
      )
      .def(
        "asAngleAxisPair",
        &QuaternionPy::asAngleAxis,
        "Returns angle and axis rotation representation as a"
        " two element tuple, the first element is the"
        " angle and the second element is the"
        " L{Vec3<esys.lsm.util.Vec3>} axis.\n"
        "@rtype: (float, L{Vec3<esys.lsm.util.Vec3>})\n"
        "@return: angle and axis rotation tuple-pair."
      )
      .def(self_ns::str(self))
      .def(boost::python::self == boost::python::self)
      .def("__len__", &QuaternionPy::len)
      .def("__getitem__", &QuaternionPy::getItem)
      .def("__setitem__", &QuaternionPy::setItem)
      .def(
        "toList",
        &QuaternionPy::toList,
        "Returns a python list of 4 elements.\n"
        "@rtype: list of four floats\n"
        "@return: C{[self[0],self[1],self[2],self[3]]}"
      )
      .def(
        "toTuple",
        &QuaternionPy::toTuple,
        "Returns a python tuple of 4 elements.\n"
        "@rtype: tuple of four floats\n"
        "@return: C{(self[0],self[1],self[2],self[3])}"
      )
      .def_pickle(QuaternionPy::PickleSuite())
      ;
    }
  }
}

std::ostream &operator<<(std::ostream &oStream, const esys::lsm::QuaternionPy &quat)
{
  oStream
    << quat.return_sca() << " "
    << quat.return_vec()[0] << " "
    << quat.return_vec()[1] << " "
    << quat.return_vec()[2];
  return oStream;
}


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

#ifndef MESH_PERIODIC_HPP_
#define MESH_PERIODIC_HPP_

#include <vector>
#include <utility>
#include <algorithm>
#include <cstdlib>
#include <limits>
#include <forward_list>
#include <boost/iterator/iterator_facade.hpp>
#include "config.hpp"
#include "util.hpp"
#include "interval.hpp"
#include "numa.hpp"
#include "mesh_rect.hpp"

template <unsigned int Dim>
class FEMMeshRectP1Periodic {
public:
	constexpr static index_t noNeighbor       = INDEX_T_NO_NEIGHBOR;
	constexpr static index_t maxValidNeighbor = INDEX_T_MAX_VALID;

	constexpr static unsigned int nDimensions() {
		return Dim;
	}
	constexpr static unsigned int nDoFPerElement() {
		return twoToThePowerOfN(Dim);
	}
	constexpr static unsigned int nHypersufacesPerElement() {
		return 2*Dim;
	}
private:
	struct ElementData {
		short_index_t multiIndex[Dim];
		index_t dofIndices[nDoFPerElement()];
		index_t neighborIndices[nHypersufacesPerElement()];

		void print(index_t j) const {
			std::cout << "Element " << j << " mit Multiindex ( ";
			for(uint i=0; i<Dim; ++i) {
				std::cout << multiIndex[i] << " ";
			}
			std::cout << ") und Freiheitsgraden ";
			for(uint i=0; i<nDoFPerElement(); ++i) {
				std::cout << dofIndices[i] << " ";
			}
//			std::cout << " und Nachbarn ";
//			for(uint i=0; i<nHypersufacesPerElement(); ++i) {
//				std::cout << neighborIndices[i] << " ";
//			}
			std::cout << std::endl;
		}
	};
	struct DoFData {
		short_index_t multiIndex[Dim];

		void print(index_t j) const {
			std::cout << "DoF " << j << " mit Multiindex ( ";
			for(uint i=0; i<Dim; ++i) {
				std::cout << multiIndex[i] << " ";
			}
			std::cout << ")" << std::endl;
		}
	};

	numa_vector<ElementData> _elementData;
	std::vector<bool> _elementHasDomainBorder;
	std::vector<index_t> _elementsWithDomainBorder;
	numa_vector<DoFData> _dofData;

#ifdef SPRAT_BUILD_WITH_OPENMP
	std::vector<std::forward_list<index_t>> _elementsOfDoF;
	std::vector<numa_vector<index_t>> _independentElementDecomposition;
#endif //#ifdef SPRAT_BUILD_WITH_OPENMP

	constexpr static unsigned int twoToThePowerOfN(unsigned int n) {
		return (n>0 ? 2*twoToThePowerOfN(n-1) : 1);
	}

	index_t multiIndexToElementIndexInFullyPopulatedMesh(short_index_t multiIndex[Dim]) {
		index_t result = multiIndex[0];
		for(uint i=1; i<Dim; ++i) {
			result *= dimensions[i].nElements;
			result += multiIndex[i];
		}
		return result;
	}

	void findElementsWithDomainBorder() {
		_elementsWithDomainBorder.resize(0);
		_elementHasDomainBorder.resize(_elementData.size());
		for(index_t i=0; i<_elementData.size(); ++i) {
			_elementHasDomainBorder[i] = false;
		}
	}

	void createElementsOfDoF(index_t numDoF, std::vector<ElementData> const& elemData) {
#ifdef SPRAT_BUILD_WITH_OPENMP
		_elementsOfDoF.resize(numDoF);

		for(index_t i=0; i<numDoF; ++i) {
			_elementsOfDoF[i].clear();
		}

		for(index_t i=0; i<elemData.size(); ++i) {
			for(uint k=0; k<nDoFPerElement(); ++k) {
				_elementsOfDoF[elemData[i].dofIndices[k]].push_front(i);
			}
		}
#endif //#ifdef SPRAT_BUILD_WITH_OPENMP
	}

	void createMaximalIndependentElementSets(std::vector<ElementData> const& elemData) {
#ifdef SPRAT_BUILD_WITH_OPENMP
		static constexpr uint noColor = std::numeric_limits<uint>::max();
		//static constexpr uint noColor = UINT_MAX; // For Intel compiler bug
		std::vector<uint> elementColor(elemData.size(), noColor);
		index_t nUncolored = elemData.size();
		uint nColors = 0;

		for(uint currentColor=0; nUncolored>0; ++currentColor) {
			for(index_t i=0; i<elemData.size(); ++i) {
				if(elementColor[i] != noColor) {
					continue;
				}

				bool hasNeighbourOfCurrentColor = false;
				for(uint k=0; k<nDoFPerElement() && !hasNeighbourOfCurrentColor; ++k) {
					const index_t dof = elemData[i].dofIndices[k];
					for(auto it=_elementsOfDoF[dof].cbegin(); it!=_elementsOfDoF[dof].cend(); ++it) {
						if(elementColor[*it] == currentColor) {
							hasNeighbourOfCurrentColor = true;
							break;
						}
					}
				}
				if(!hasNeighbourOfCurrentColor) {
					elementColor[i] = currentColor;
					nUncolored -= 1;
				}
			}

			nColors += 1;
		}

		std::vector<std::vector<index_t>> independentElementDecompositionTemp(nColors);
		for(uint color=0; color<nColors; ++color) {
			independentElementDecompositionTemp[color].clear();
		}
		for(index_t i=0; i<elemData.size(); ++i) {
			independentElementDecompositionTemp[elementColor[i]].push_back(i);
		}

		_independentElementDecomposition.resize(nColors);
		for(uint color=0; color<nColors; ++color) {
			_independentElementDecomposition[color].resize(independentElementDecompositionTemp[color].size());
			foreach_omp_index(i, _independentElementDecomposition[color].size(), shared(color,independentElementDecompositionTemp), {
					_independentElementDecomposition[color][i] = independentElementDecompositionTemp[color][i];
			})
		}

		// Debug output
		//for(index_t i=0; i<nElements(); ++i) {
		//	std::cout << "Element " << i << " hat Farbe " << elementColor[i] << std::endl;
		//}
		//std::cout << std::endl;
		//for(uint color=0; color<nColors; ++color) {
		//	std::cout << "Farbe " << color << " haben die Elemente:";
		//	for(index_t i=0; i<_independentElementDecomposition[color].size(); ++i) {
		//		std::cout << " " << _independentElementDecomposition[color][i];
		//	}
		//	std::cout << std::endl;
		//}
#endif //#ifdef SPRAT_BUILD_WITH_OPENMP
	}

public:
	RectMeshDimension dimensions[Dim];

	FEMMeshRectP1Periodic() {}

	FEMMeshRectP1Periodic(std::vector<RectMeshDimension> const& dimensions_in) :
		FEMMeshRectP1Periodic(&dimensions_in[0]) {}

	FEMMeshRectP1Periodic(RectMeshDimension const * const dimensions_in) {
		index_t totalElements = 1;
		index_t totalDoF = 1;
		for(uint i=0; i<Dim; ++i) {
			dimensions[i] = dimensions_in[i];
			totalElements *= dimensions[i].nElements;
			totalDoF *= dimensions[i].nNodes-1;
		}

		std::vector<ElementData> elementDataTemp(totalElements);
		_dofData.resize(totalDoF);

		for(index_t index=0; index<totalElements; ++index) {
			for(uint k=0; k<Dim; ++k) {
				// *** multiIndex
				index_t divideAway = index;
				for(uint j=Dim-1; j>k; --j) {
					divideAway /= dimensions[j].nElements;
				}
				elementDataTemp[index].multiIndex[k] = divideAway % dimensions[k].nElements;
			}
			for(uint k=0; k<Dim; ++k) {
				// *** neighborIndices

				// find left neighbor
				short_index_t leftNeighborIndex[Dim];
				for(uint j=0; j<Dim; ++j) {
					leftNeighborIndex[j] = elementDataTemp[index].multiIndex[j];
				}
				if(leftNeighborIndex[k] == 0) {
					leftNeighborIndex[k] = dimensions[k].nElements-1;
				} else {
					leftNeighborIndex[k] -= 1;
				}
				elementDataTemp[index].neighborIndices[2*k  ] = multiIndexToElementIndexInFullyPopulatedMesh(leftNeighborIndex);

				// find right neighbor
				short_index_t rightNeighborIndex[Dim];
				for(uint j=0; j<Dim; ++j) {
					rightNeighborIndex[j] = elementDataTemp[index].multiIndex[j];
				}
				if(rightNeighborIndex[k] == dimensions[k].nElements-1) {
					rightNeighborIndex[k] = 0;
				} else {
					rightNeighborIndex[k] += 1;
				}
				elementDataTemp[index].neighborIndices[2*k+1] = multiIndexToElementIndexInFullyPopulatedMesh(rightNeighborIndex);
			}
			for(uint k=0; k<nDoFPerElement(); ++k) {
				// *** dofIndices
				uint dofPosition[Dim];
				uint divideAway = k;
				for(uint j=0; j<Dim; ++j) {
					dofPosition[Dim-1-j] = divideAway%2;
					divideAway /= 2;
				}
				index_t dofIndex = 0;
				for(uint j=0; j<Dim; ++j) {
					dofIndex *= dimensions[j].nNodes-1;
					if(!(elementDataTemp[index].multiIndex[j] == dimensions[j].nElements-1 && dofPosition[j] == 1)) {
						dofIndex += elementDataTemp[index].multiIndex[j] + dofPosition[j];
					}
				}
				elementDataTemp[index].dofIndices[k] = dofIndex;
			}
		}

		createElementsOfDoF(totalDoF, elementDataTemp);
		createMaximalIndependentElementSets(elementDataTemp);

		_elementData.resize(elementDataTemp.size());
		foreachElementIndependently(auto tau, *this, shared(elementDataTemp), {
				_elementData[tau] = elementDataTemp[tau];
		})
		elementDataTemp.clear();

		findElementsWithDomainBorder();

		//std::cout << "There are " << _elementData.size() << " elements in the master mesh." << std::endl;

		foreach_omp_index(index, totalDoF, , {
			for(uint k=0; k<Dim; ++k) {
				index_t divideAway = index;
				for(uint j=Dim-1; j>k; --j) {
					divideAway /= (dimensions[j].nNodes-1);
				}
				_dofData[index].multiIndex[k] = divideAway % (dimensions[k].nNodes-1);
			}
		})

//		for(uint i=0; i<_elementData.size(); ++i) {
//			_elementData[i].print(i);
//		}
//		for(uint i=0; i<_dofData.size(); ++i) {
//			_dofData[i].print(i);
//		}
	}

#ifdef SPRAT_BUILD_WITH_OPENMP
	std::vector<numa_vector<index_t>> const& getIndependentElementDecomposition() const {
		return _independentElementDecomposition;
	}
#endif //#ifdef SPRAT_BUILD_WITH_OPENMP

	index_t nElements() const {
		return _elementData.size();
	}
	index_t nLocalElements() const {
		return nElements();
	}
	index_t nElementsWithDomainBorder() const {
		return _elementsWithDomainBorder.size();
	}
	index_t nDoF() const {
		return _dofData.size();
	}
	index_t nSpatialDoF() const {
		return _dofData.size() / dimensions[Dim-1].nNodes;
	}
	index_t nLocalDoF() const {
		return nDoF();
	}

	class ElementT {
	public:
		constexpr static unsigned int nDoFPerElement() {
			return FEMMeshRectP1Periodic<Dim>::twoToThePowerOfN(Dim);
		}
	private:

		FEMMeshRectP1Periodic<Dim> const& _femMesh;
		const index_t _index;

		// WARNING: Initially, end must be vec.size()
		index_t binarySearchInSortedIndexVector(std::vector<index_t> const& vec, index_t begin, index_t end, index_t value) const {
			const index_t mid = (begin+end)/2;
			const index_t mid_value = vec[mid];
			if(mid_value == value) {
				return mid;
			}
			else if(value < mid_value) {
				return binarySearchInSortedIndexVector(vec, begin, mid, value);
			}
			else {
				return binarySearchInSortedIndexVector(vec, mid, end, value);
			}
		}

	public:
		ElementT(FEMMeshRectP1Periodic<Dim> const& femMesh, index_t index) : _femMesh(femMesh), _index(index) {}

		operator index_t() const {
			return _index;
		}
		index_t index() const {
			return _index;
		}

		uint indexInDimension(uint dimension) const {
			return _femMesh._elementData[_index].multiIndex[dimension];
		}

		real diamInDimension(uint dimension) const {
			return _femMesh.dimensions[dimension].elementDiameter[indexInDimension(dimension)];
		}

		index_t globalDoFIndex(uint localDoFIndex) const {
			return _femMesh._elementData[_index].dofIndices[localDoFIndex];
		}

		index_t const * globalDoFIndices() const {
			return _femMesh._elementData[_index].dofIndices;
		}

		FEMMeshRectP1Periodic<Dim> const& getMesh() const {
			return _femMesh;
		}

		/*
		 * Returns - \sum_{k=1}^d \int_\tau \frac{\partial}{\partial x_k} \prod_{l} \phi_i_l (x_l)  *  \frac{\partial}{\partial x_k} \prod_{m} \phi_i_m (x_m)  dx
		 * Assertion: i and j are *local* DoF Indices (\in {0,1,2, ..., nDofPerElement()})
		 */
		real integratePartIntLaplace(uint i, uint j) const {
			uint iMultiIndex[Dim];
			uint jMultiIndex[Dim];

			for(uint k=0; k<Dim; ++k) {
				iMultiIndex[Dim-1-k] = i%2;
				i /= 2;
				jMultiIndex[Dim-1-k] = j%2;
				j /= 2;
			}

			const real intValue[4] = {1.0/3.0, 1.0/6.0, 1.0/6.0, 1.0/3.0}; // (i_x/y, j_x/y): (0,0), (0,1), (1,0), (1,1)
			const real diffValue[4] = {1.0, -1.0, -1.0, 1.0}; // (i_x/y, j_x/y): (0,0), (0,1), (1,0), (1,1)
			real result = 0.0;
			for(uint k=0; k<Dim; ++k) {
				real dimResult = 1.0;
				for(uint l=0; l<Dim; ++l) {
					if(l != k) {
						dimResult *= diamInDimension(l) * intValue[2*iMultiIndex[l] + jMultiIndex[l]];
					}
					else {
						dimResult *= diffValue[2*iMultiIndex[l] + jMultiIndex[l]] / diamInDimension(l);
					}
				}
				result += dimResult;
			}
			return -1.0 * result;
		}

		bool hasDomainBorder() const {
			return _femMesh._elementHasDomainBorder[_index];
		}

		index_t iAmTheNThElementWithDomainBorder() const {
			//const index_t result = std::find(_femMesh._elementsWithDomainBorder.begin(), _femMesh._elementsWithDomainBorder.end(), _index) - _femMesh._elementsWithDomainBorder.begin();
			//if(result >= _femMesh._elementsWithDomainBorder.size()) {
			//	std::cout << "IAmTheNThElementWithDomainBorder: This should NOT happen!" << std::endl;
			//}

			return binarySearchInSortedIndexVector(_femMesh._elementsWithDomainBorder, 0, _femMesh._elementsWithDomainBorder.size(), _index);
		}
		uint nDomainBorderHypesurfaces() const {
			uint result = 0;
			for(uint i=0; i<nHypersufacesPerElement(); ++i) {
				if(neighborElementIndices()[i] == noNeighbor) {
					result += 1;
				}
			}
			return result;
		}
		index_t neighborElementIndex(short_index_t index) const {
			return _femMesh._elementData[_index].neighborIndices[index];
		}
		index_t const * neighborElementIndices() const {
			return _femMesh._elementData[_index].neighborIndices;
		}

		/*
		 * Terminology:
		 * (Global) DoF -- all DoF accessible by this compute node
		 * LocalDoF     -- DoF genuinely belonging to this compute node
		 * ElementDoF   -- DoF belonging to one element
		 */
		class ElementDoFT {
		private:
			uint _localIndex;
			index_t _globalIndex;

		public:
			ElementDoFT(uint localIndex, index_t globalIndex) : _localIndex(localIndex), _globalIndex(globalIndex) {
				//std::cout << "Constructing ElementDoFT with localIndex " << _localIndex << " and global index " << _globalIndex << std::endl;
			}

			operator uint() const {
				return _localIndex;
			}
			uint index() const {
				return _localIndex;
			}
			uint localIndex() const {
				return _localIndex;
			}

			index_t globalIndex() const {
				return _globalIndex;
			}

			bool operator==(ElementDoFT const& other) const {
				return _localIndex == other._localIndex;
			}
			bool operator!=(ElementDoFT const& other) const {
				return _localIndex != other._localIndex;
			}
			bool operator<(ElementDoFT const& other) const {
				return _localIndex < other._localIndex;
			}
			bool operator<=(ElementDoFT const& other) const {
				return _localIndex <= other._localIndex;
			}
			bool operator>(ElementDoFT const& other) const {
				return _localIndex > other._localIndex;
			}
			bool operator>=(ElementDoFT const& other) const {
				return _localIndex >= other._localIndex;
			}
		};

		typename FEMMeshRectP1Periodic<Dim>::ElementT::ElementDoFT elementDoF(uint localIndex) const {
			return typename FEMMeshRectP1Periodic<Dim>::ElementT::ElementDoFT(localIndex, globalDoFIndex(localIndex));
		}

		class IterateElementDoF {
		private:
			index_t const*const _globalDoF;

		public:
			IterateElementDoF(typename FEMMeshRectP1Periodic<Dim>::ElementT element) :
				_globalDoF(element.globalDoFIndices()) {}

			class const_iterator : public boost::iterator_facade<
			const_iterator
			, typename FEMMeshRectP1Periodic<Dim>::ElementT::ElementDoFT const
			, boost::random_access_traversal_tag
			, typename FEMMeshRectP1Periodic<Dim>::ElementT::ElementDoFT const
			, signed_short_index_t> {
			public:
				//const_iterator() : _element(0), _elementDoFIndex(0) {}
				explicit const_iterator(index_t const* globalDoF) : _globalDoF(globalDoF), _localIndex(0) {}
				const_iterator(index_t const* globalDoF, short_index_t localIndex) : _globalDoF(globalDoF), _localIndex(localIndex) {}

			private:
				friend class boost::iterator_core_access;

				void increment() { ++_localIndex; }
				void decrement() { --_localIndex; }
				void advance(signed_short_index_t n) { _localIndex+=n; }
				signed_short_index_t distance_to(const_iterator const& other) const { return other._localIndex - this->_localIndex; }

				bool equal(const_iterator const& other) const {	return this->_localIndex == other._localIndex; }

				typename FEMMeshRectP1Periodic<Dim>::ElementT::ElementDoFT const dereference() const {
					return typename FEMMeshRectP1Periodic<Dim>::ElementT::ElementDoFT(_localIndex, _globalDoF[_localIndex]);
				}

				index_t const* _globalDoF;
				signed_short_index_t _localIndex;
			};
			typedef const_iterator iterator;

			iterator begin() const { return iterator(_globalDoF); }
			iterator end() const { return iterator(_globalDoF, FEMMeshRectP1Periodic<Dim>::nDoFPerElement()); }
			const_iterator cbegin() const { return const_iterator(_globalDoF); }
			const_iterator cend() const { return const_iterator(_globalDoF, FEMMeshRectP1Periodic<Dim>::nDoFPerElement()); }
		};

		class HypersurfaceT {
		public:
			constexpr static unsigned int nDoFPerHypersurface() {
				return Dim;
			}
		private:
			typename FEMMeshRectP1Periodic<Dim>::ElementT const& _element;
			short_index_t _surfaceIndex;
		public:
			HypersurfaceT(typename FEMMeshRectP1Periodic<Dim>::ElementT const& element, uint localIndex) :
				_element(element),
				_surfaceIndex(localIndex)
			{}

			operator short_index_t() const {
				return _surfaceIndex;
			}
			short_index_t index() const {
				return _surfaceIndex;
			}

			uint elementDoFIndex(uint hypersurfaceDoFIndex) const {
				const uint normalDimension = _surfaceIndex/2;
				const uint normalEntry = _surfaceIndex%2;
				const uint allOnes = ~(0U);
				const uint maskLowerPart = ~(allOnes << (Dim-1-normalDimension));
				const uint maskHigherPart = allOnes << (Dim-1-normalDimension);
				const uint result = (normalEntry<<(Dim-1-normalDimension))    |
				                    (hypersurfaceDoFIndex & maskLowerPart)    |
				                    ((hypersurfaceDoFIndex & maskHigherPart)<<1);
				//std::cout << "Für hypersurfaceDoFIndex=" << hypersurfaceDoFIndex << " von Hypersurface " << _surfaceIndex <<
				//		" ist elementDoFIndex=" << result << std::endl;
				return result;
			}

			class HypersurfaceDoFT {
			private:
				uint _hypersurfaceIndex;
				uint _elementIndex;

			public:
				HypersurfaceDoFT(uint hypersurfaceIndex, index_t elementIndex) : _hypersurfaceIndex(hypersurfaceIndex), _elementIndex(elementIndex) {}

				operator uint() const {
					return _hypersurfaceIndex;
				}
				uint index() const {
					return _hypersurfaceIndex;
				}

				uint elementDoFIndex() const {
					return _elementIndex;
				}

				bool operator==(HypersurfaceDoFT const& other) const {
					return _hypersurfaceIndex == other._hypersurfaceIndex;
				}
				bool operator!=(HypersurfaceDoFT const& other) const {
					return _hypersurfaceIndex != other._hypersurfaceIndex;
				}
				bool operator<(HypersurfaceDoFT const& other) const {
					return _hypersurfaceIndex < other._hypersurfaceIndex;
				}
				bool operator<=(HypersurfaceDoFT const& other) const {
					return _hypersurfaceIndex <= other._hypersurfaceIndex;
				}
				bool operator>(HypersurfaceDoFT const& other) const {
					return _hypersurfaceIndex > other._hypersurfaceIndex;
				}
				bool operator>=(HypersurfaceDoFT const& other) const {
					return _hypersurfaceIndex >= other._hypersurfaceIndex;
				}
			};

			typename FEMMeshRectP1Periodic<Dim>::ElementT::HypersurfaceT::HypersurfaceDoFT hypersurfaceDoF(uint hypersurfaceDoFIndex) const {
				return typename FEMMeshRectP1Periodic<Dim>::ElementT::HypersurfaceT::HypersurfaceDoFT(hypersurfaceDoFIndex, elementDoFIndex(hypersurfaceDoFIndex));
			}

			real surfaceIntegral(HypersurfaceDoFT const& i, HypersurfaceDoFT const& j, uint dimension) {
				uint i_elem = i.elementDoFIndex();
				uint j_elem = j.elementDoFIndex();

				const uint normalDimension = _surfaceIndex/2;
				if(normalDimension != dimension) {
					return 0.0;
				}

				uint iMultiIndex[Dim];
				uint jMultiIndex[Dim];

				for(uint k=0; k<Dim; ++k) {
					iMultiIndex[Dim-1-k] = i_elem%2;
					i_elem /= 2;
					jMultiIndex[Dim-1-k] = j_elem%2;
					j_elem /= 2;
				}

				const real intValue[2] = {1.0/3.0, 1.0/6.0};

				const bool isLeftSurface = (_surfaceIndex%2 == 0);
				real result = (isLeftSurface ? -1.0 : 1.0);
				for(uint k=0; k<Dim; ++k) {
					if(k != normalDimension) {
						result *= intValue[(iMultiIndex[k]!=jMultiIndex[k] ? 1 : 0)] * _element.diamInDimension(k);
					}
				}
				return result;
			}

			class IterateHypersurfaceDoF {
			private:
				typename FEMMeshRectP1Periodic<Dim>::ElementT::HypersurfaceT const& _hypersurface;

			public:
				IterateHypersurfaceDoF(typename FEMMeshRectP1Periodic<Dim>::ElementT::HypersurfaceT const& hypersurface) :
					_hypersurface(hypersurface) {
					//std::cout << "IterateHypersurfaceDoF(hypersurface), hypersurface = " << &hypersurface << std::endl;
				}

				class const_iterator : public boost::iterator_facade<
				const_iterator
				, typename FEMMeshRectP1Periodic<Dim>::ElementT::HypersurfaceT::HypersurfaceDoFT const
				, boost::random_access_traversal_tag
				, typename FEMMeshRectP1Periodic<Dim>::ElementT::HypersurfaceT::HypersurfaceDoFT const
				, signed_short_index_t> {
				public:
					//const_iterator() : _element(0), _elementDoFIndex(0) {}
					explicit const_iterator(typename FEMMeshRectP1Periodic<Dim>::ElementT::HypersurfaceT const& hypersurface) : _hypersurface(hypersurface), _localIndex(0) {
						//std::cout << "IterateHypersurfaceDoF::const_iterator(hypersurface), hypersurface = " << &hypersurface << std::endl;
					}
					const_iterator(typename FEMMeshRectP1Periodic<Dim>::ElementT::HypersurfaceT const& hypersurface, short_index_t localIndex) : _hypersurface(hypersurface), _localIndex(localIndex) {
						//std::cout << "IterateHypersurfaceDoF::const_iterator(hypersurface, index), hypersurface = " << &hypersurface << std::endl;
					}

				private:
					friend class boost::iterator_core_access;

					void increment() { ++_localIndex; }
					void decrement() { --_localIndex; }
					void advance(signed_short_index_t n) { _localIndex+=n; }
					signed_short_index_t distance_to(const_iterator const& other) const { return other._localIndex - this->_localIndex; }

					bool equal(const_iterator const& other) const {	return this->_localIndex == other._localIndex; }

					typename FEMMeshRectP1Periodic<Dim>::ElementT::HypersurfaceT::HypersurfaceDoFT const dereference() const {
						return _hypersurface.hypersurfaceDoF(_localIndex);
					}

					typename FEMMeshRectP1Periodic<Dim>::ElementT::HypersurfaceT const& _hypersurface;
					signed_short_index_t _localIndex;
				};
				typedef const_iterator iterator;

				iterator begin() const { return iterator(_hypersurface); }
				iterator end() const { return iterator(_hypersurface, FEMMeshRectP1Periodic<Dim>::ElementT::HypersurfaceT::nDoFPerHypersurface()); }
				const_iterator cbegin() const { return const_iterator(_hypersurface); }
				const_iterator cend() const { return const_iterator(_hypersurface, FEMMeshRectP1Periodic<Dim>::ElementT::HypersurfaceT::nDoFPerHypersurface()); }
			};
		};

		typename FEMMeshRectP1Periodic<Dim>::ElementT::HypersurfaceT hypersurface(uint surfaceIndex) const {
			return typename FEMMeshRectP1Periodic<Dim>::ElementT::HypersurfaceT(*this, surfaceIndex);
		}

		class IterateElementDomainBoundaryHypersurfaces {
		private:
			typename FEMMeshRectP1Periodic<Dim>::ElementT const& _element;
			const uint _nDomainBoundaryHypersurfaces;
		public:
			IterateElementDomainBoundaryHypersurfaces(typename FEMMeshRectP1Periodic<Dim>::ElementT const& element) :
				_element(element),
				_nDomainBoundaryHypersurfaces(element.nDomainBorderHypesurfaces()){}

			class const_iterator : public boost::iterator_facade<
			const_iterator
			, typename FEMMeshRectP1Periodic<Dim>::ElementT::HypersurfaceT const
			, boost::random_access_traversal_tag
			, typename FEMMeshRectP1Periodic<Dim>::ElementT::HypersurfaceT const
			, int> {
			public:
				const_iterator() : _element(0), _hypersurfaceIndex(0) {}
				//explicit const_iterator(FEMMeshRectP1<Dim> const& femMesh) : _femMesh(femMesh), _elementIndex(0) {}
				explicit const_iterator(typename FEMMeshRectP1Periodic<Dim>::ElementT const& element) : _element(&element), _hypersurfaceIndex(0) {}
				const_iterator(typename FEMMeshRectP1Periodic<Dim>::ElementT const& element, int hypersurfaceIndex) : _element(&element), _hypersurfaceIndex(hypersurfaceIndex) {}

			private:
				friend class boost::iterator_core_access;

				void increment() { ++_hypersurfaceIndex; }
				void decrement() { --_hypersurfaceIndex; }
				void advance(int n) { _hypersurfaceIndex+=n; }
				int distance_to(const_iterator const& other) const { return  other._hypersurfaceIndex - this->_hypersurfaceIndex; }

				bool equal(const_iterator const& other) const {	return this->_hypersurfaceIndex == other._hypersurfaceIndex; }

				typename FEMMeshRectP1Periodic<Dim>::ElementT::HypersurfaceT const dereference() const {
					uint nBorderSurface = 0;
					for(int i=0; i<nHypersufacesPerElement(); ++i) {
						if(_element->neighborElementIndex(i) == noNeighbor) {
							if(nBorderSurface == _hypersurfaceIndex) {
								nBorderSurface = i;
								break;
							}
							nBorderSurface += 1;
						}
					}
					return _element->hypersurface(nBorderSurface);
				}

				typename FEMMeshRectP1Periodic<Dim>::ElementT const* _element;
				int _hypersurfaceIndex;
			};
			typedef const_iterator iterator;

			iterator begin() const { return iterator(_element); }
			iterator end() const { return iterator(_element, _nDomainBoundaryHypersurfaces); }
			const_iterator cbegin() const { return const_iterator(_element); }
			const_iterator cend() const { return const_iterator(_element, _nDomainBoundaryHypersurfaces); }
		};
	};

	ElementT const element(index_t index) const {
		return ElementT(*this, index);
	}

	class DoFT {
	private:
		FEMMeshRectP1Periodic<Dim> const& _femMesh;
		const index_t _index;

	public:
		DoFT(FEMMeshRectP1Periodic<Dim> const& femMesh, index_t index) : _femMesh(femMesh), _index(index) {}

		operator index_t() const {
			return _index;
		}
		index_t index() const {
			return _index;
		}

		uint indexInDimension(uint dimension) const {
			return _femMesh._dofData[_index].multiIndex[dimension];
		}

		real positionInDimension(uint dimension) const {
			return _femMesh.dimensions[dimension].nodes[indexInDimension(dimension)];
		}

		real spatialDistance(DoFT const& other) const {
			real result = 0.0;

			for(uint i=0; i<Dim-1; ++i) {
				const real otherPos = other.positionInDimension(i);
				const real thisPos = positionInDimension(i);
				const real a = _femMesh.dimensions[i].nodes.front();
				const real b = _femMesh.dimensions[i].nodes.back();

				const real diffNormal = fabs(otherPos - thisPos);
				const real diffWrappedA =
						otherPos - a +
						b - thisPos;
				const real diffWrappedB =
						thisPos - a +
						b - otherPos;

				const real diff = min(diffNormal, diffWrappedA, diffWrappedB);
				result += diff*diff;
			}

			return sqrt(result);
		}
	};

	DoFT dof(index_t index) const {
		return DoFT(*this, index);
	}

	/*
	 * Iterations
	 */

	class IterateMeshElements {
	private:
		FEMMeshRectP1Periodic<Dim> const& _femMesh;
		const index_t _beginIndex;
		const index_t _endIndex;

	public:
		IterateMeshElements(FEMMeshRectP1Periodic<Dim> const& femMesh, index_t beginIndex, index_t endIndex) :
			_femMesh(femMesh), _beginIndex(beginIndex), _endIndex(endIndex) {}

		class const_iterator : public boost::iterator_facade<
		const_iterator
		, typename FEMMeshRectP1Periodic<Dim>::ElementT const
		, boost::random_access_traversal_tag
		, typename FEMMeshRectP1Periodic<Dim>::ElementT const
		, signed_index_t> {
		public:
			const_iterator() : _femMesh(0), _elementIndex(0) {}
			//explicit const_iterator(FEMMeshRectP1<Dim> const& femMesh) : _femMesh(femMesh), _elementIndex(0) {}
			const_iterator(FEMMeshRectP1Periodic<Dim> const& femMesh, index_t elementIndex) : _femMesh(&femMesh), _elementIndex(elementIndex) {}

		private:
			friend class boost::iterator_core_access;

			void increment() { ++_elementIndex; }
			void decrement() { --_elementIndex; }
			void advance(signed_index_t n) { _elementIndex+=n; }
			signed_index_t distance_to(const_iterator const& other) const { return  other._elementIndex - this->_elementIndex; }

			bool equal(const_iterator const& other) const {	return this->_elementIndex == other._elementIndex; }

			typename FEMMeshRectP1Periodic<Dim>::ElementT const dereference() const {
				return _femMesh->element(_elementIndex);
			}

			FEMMeshRectP1Periodic<Dim> const * _femMesh;
			signed_index_t _elementIndex;
		};
		typedef const_iterator iterator;

		iterator begin() const { return iterator(_femMesh, _beginIndex); }
		iterator end() const { return iterator(_femMesh, _endIndex); }
		const_iterator cbegin() const { return const_iterator(_femMesh, _beginIndex); }
		const_iterator cend() const { return const_iterator(_femMesh, _endIndex); }
	};

	class IterateDoF {
	private:
		FEMMeshRectP1Periodic<Dim> const& _femMesh;
		const index_t _beginIndex;
		const index_t _endIndex;
	public:
		IterateDoF(FEMMeshRectP1Periodic<Dim> const& femMesh, index_t beginIndex, index_t endIndex) :
			_femMesh(femMesh), _beginIndex(beginIndex), _endIndex(endIndex) {}

		class const_iterator : public boost::iterator_facade<
		const_iterator
		, typename FEMMeshRectP1Periodic<Dim>::DoFT const
		, boost::random_access_traversal_tag
		, typename FEMMeshRectP1Periodic<Dim>::DoFT const
		, signed_index_t> {
		public:
			//const_iterator() : _element(0), _elementDoFIndex(0) {}
			//explicit const_iterator(Mesh_2DRect const& mesh) : _mesh(mesh), _dofIndex(0) {}
			const_iterator(FEMMeshRectP1Periodic<Dim> const& femMesh, index_t dofIndex) : _femMesh(&femMesh), _dofIndex(dofIndex) {}

		private:
			friend class boost::iterator_core_access;

			void increment() { ++_dofIndex; }
			void decrement() { --_dofIndex; }
			void advance(signed_index_t n) { _dofIndex+=n; }
			signed_index_t distance_to(const_iterator const& other) const { return other._dofIndex - this->_dofIndex; }

			bool equal(const_iterator const& other) const {	return this->_dofIndex == other._dofIndex; }

			typename FEMMeshRectP1Periodic<Dim>::DoFT const dereference() const {
				return _femMesh->dof(_dofIndex);
			}

			FEMMeshRectP1Periodic<Dim> const* _femMesh;
			signed_index_t _dofIndex;
		};
		typedef const_iterator iterator;

		iterator begin() const { return iterator(_femMesh, _beginIndex); }
		iterator end() const { return iterator(_femMesh, _endIndex); }
		const_iterator cbegin() const { return const_iterator(_femMesh, _beginIndex); }
		const_iterator cend() const { return const_iterator(_femMesh, _endIndex); }
	};
	class IterateDoFStrided {
	private:
		FEMMeshRectP1Periodic<Dim> const& _femMesh;
		const index_t _beginIndex;
		const index_t _endIndex;
		const index_t _stride;
	public:
		IterateDoFStrided(FEMMeshRectP1Periodic<Dim> const& femMesh, index_t beginIndex, index_t endIndex, index_t stride) :
			_femMesh(femMesh), _beginIndex(beginIndex), _endIndex(endIndex), _stride(stride) {}

		class const_iterator : public boost::iterator_facade<
		const_iterator
		, typename FEMMeshRectP1Periodic<Dim>::DoFT const
		, boost::random_access_traversal_tag
		, typename FEMMeshRectP1Periodic<Dim>::DoFT const
		, signed_index_t> {
		public:
			//const_iterator() : _element(0), _elementDoFIndex(0) {}
			//explicit const_iterator(Mesh_2DRect const& mesh) : _mesh(mesh), _dofIndex(0) {}
			const_iterator(FEMMeshRectP1Periodic<Dim> const& femMesh, index_t dofIndex, index_t stride) : _femMesh(&femMesh), _dofBaseIndex(dofIndex/stride), _stride(stride) {}

		private:
			friend class boost::iterator_core_access;

			void increment() { ++_dofBaseIndex; }
			void decrement() { --_dofBaseIndex; }
			void advance(signed_index_t n) { _dofBaseIndex+=n; }
			signed_index_t distance_to(const_iterator const& other) const { return other._dofBaseIndex - this->_dofBaseIndex; }

			bool equal(const_iterator const& other) const {	return this->_dofBaseIndex == other._dofBaseIndex; }

			typename FEMMeshRectP1Periodic<Dim>::DoFT const dereference() const {
				return _femMesh->dof(_dofBaseIndex*_stride);
			}

			FEMMeshRectP1Periodic<Dim> const* _femMesh;
			signed_index_t _dofBaseIndex;
			signed_index_t _stride;
		};
		typedef const_iterator iterator;

		iterator begin() const { return iterator(_femMesh, _beginIndex, _stride); }
		iterator end() const { return iterator(_femMesh, _endIndex, _stride); }
		const_iterator cbegin() const { return const_iterator(_femMesh, _beginIndex, _stride); }
		const_iterator cend() const { return const_iterator(_femMesh, _endIndex, _stride); }
	};

	/*
	 * Returns \prod_{k=1}^n  \int_0^1 \tilde{\phi}_i_k (x_k) * \tilde{\phi}_j_k (x_k) dx_k
	 * Assertion: i and j are *local* DoF Indices (\in {0,1,2, ..., nDofPerElement()})
	 */
	static real integrateReferenceElement(uint i, uint j) {
		uint iMultiIndex[Dim];
		uint jMultiIndex[Dim];

		for(uint k=0; k<Dim; ++k) {
			iMultiIndex[Dim-1-k] = i%2;
			i /= 2;
			jMultiIndex[Dim-1-k] = j%2;
			j /= 2;
		}

		const real intValue[4] = {1.0/3.0, 1.0/6.0, 1.0/6.0, 1.0/3.0}; // (i_x/y, j_x/y): (0,0), (0,1), (1,0), (1,1)
		real result = 1.0;
		for(uint k=0; k<Dim; ++k) {
			result *= intValue[2*iMultiIndex[k] + jMultiIndex[k]];
		}
		return result;
	}

	/*
	 * Returns \prod_{k=1,k!=l}^n  \int_0^1 \tilde{\phi}_i_k (x_k) * \tilde{\phi}_j_k (x_k) dx_k *
	 *         \int_0^1 (d/dx_l \tilde{\phi}_i_l (x_l)) * \tilde{\phi}_j_l (x_l) dx
	 * Assertion: i and j are *local* DoF Indices (\in {0,1,2, ..., nDofPerElement()})
	 */
	static real integratePartIntDerivativeReferenceElement(uint i, uint j, uint derivativeDimension) {
		uint iMultiIndex[Dim];
		uint jMultiIndex[Dim];

		for(uint k=0; k<Dim; ++k) {
			iMultiIndex[Dim-1-k] = i%2;
			i /= 2;
			jMultiIndex[Dim-1-k] = j%2;
			j /= 2;
		}

		const real intValue[4] = {1.0/3.0, 1.0/6.0, 1.0/6.0, 1.0/3.0}; // (i_x/y, j_x/y): (0,0), (0,1), (1,0), (1,1)
		const real intDerivativeXValue[4] = {-0.5, -0.5, 0.5, 0.5}; // (i_x, j_x): (0,0), (0,1), (1,0), (1,1)
		real result = 1.0;
		for(uint k=0; k<Dim; ++k) {
			if(k == derivativeDimension) {
				result *= intDerivativeXValue[2*iMultiIndex[k] + jMultiIndex[k]];
			}
			else {
				result *= intValue[2*iMultiIndex[k] + jMultiIndex[k]];
			}
		}
		return result;
	}
};

template <unsigned int Dim>
static inline typename FEMMeshRectP1Periodic<Dim>::IterateMeshElements Elements(FEMMeshRectP1Periodic<Dim> const& femMesh) {
	return typename FEMMeshRectP1Periodic<Dim>::IterateMeshElements(femMesh, 0, femMesh.nElements());
}
template <unsigned int Dim>
static inline typename FEMMeshRectP1Periodic<Dim>::IterateMeshElements LocalElements(FEMMeshRectP1Periodic<Dim> const& femMesh) {
	return typename FEMMeshRectP1Periodic<Dim>::IterateMeshElements(femMesh, 0, femMesh.nLocalElements());
}
template <unsigned int Dim>
static inline typename FEMMeshRectP1Periodic<Dim>::IterateMeshElements NeighborElements(FEMMeshRectP1Periodic<Dim> const& femMesh) {
	return typename FEMMeshRectP1Periodic<Dim>::IterateMeshElements(femMesh, femMesh.nLocalElements(), femMesh.nElements());
}

template <unsigned int Dim>
static inline UIntRange ElementDoFIndices(FEMMeshRectP1Periodic<Dim> const& femMesh) {
	return UIntRange(0, FEMMeshRectP1Periodic<Dim>::nDoFPerElement());
}
//template <typename ElemT>
//static inline typename ElemT::IterateElementDoF ElementDoF(ElemT const& element) {
//	return typename ElemT::IterateElementDoF(element);
//}
//template <typename ElemT>
//static inline typename ElemT::IterateElementDomainBoundaryHypersurfaces DomainBoundaryHypersurfaces(ElemT const& element) {
//	return typename ElemT::IterateElementDomainBoundaryHypersurfaces(element);
//}
//template <typename HSurfaceT>
//static inline typename HSurfaceT::IterateHypersurfaceDoF HypersurfaceDoF(HSurfaceT const& hypersurface) {
//	return typename HSurfaceT::IterateHypersurfaceDoF(hypersurface);
//}

template <unsigned int Dim>
static inline typename FEMMeshRectP1Periodic<Dim>::IterateDoF DoF(FEMMeshRectP1Periodic<Dim> const& femMesh) {
	return typename FEMMeshRectP1Periodic<Dim>::IterateDoF(femMesh, 0, femMesh.nDoF());
}
template <unsigned int Dim>
static inline typename FEMMeshRectP1Periodic<Dim>::IterateDoF LocalDoF(FEMMeshRectP1Periodic<Dim> const& femMesh) {
	return typename FEMMeshRectP1Periodic<Dim>::IterateDoF(femMesh, 0, femMesh.nLocalDoF());
}
template <unsigned int Dim>
static inline typename FEMMeshRectP1Periodic<Dim>::IterateDoF GhostDoF(FEMMeshRectP1Periodic<Dim> const& femMesh) {
	return typename FEMMeshRectP1Periodic<Dim>::IterateDoF(femMesh, femMesh.nLocalDoF(), femMesh.nDoF());
}

template <unsigned int Dim>
static inline typename FEMMeshRectP1Periodic<Dim>::IterateDoFStrided BaseLevelDoF(FEMMeshRectP1Periodic<Dim> const& femMesh) {
	return typename FEMMeshRectP1Periodic<Dim>::IterateDoFStrided(femMesh, 0, femMesh.nDoF(), femMesh.dimensions[Dim-1].nNodes);
}

#endif /* MESH_PERIODIC_HPP_ */


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <boost/bind.hpp>

#include "Tudat/SimulationSetup/EnvironmentSetup/createRadiationPressureInterface.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/sphericalBodyShapeModel.h"
#include "Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.h"

namespace tudat
{

namespace simulation_setup
{

//! Function to obtain (by reference) the position functions and radii of occulting bodies
void getOccultingBodiesInformation(
        const NamedBodyMap& bodyMap, const std::vector< std::string >& occultingBodies,
        std::vector< std::function< Eigen::Vector3d( ) > >& occultingBodyPositions,
        std::vector< double >& occultingBodyRadii )
{
    // Iterate over occulring bodies and retrieve radius and position function.
    for( unsigned int i = 0; i < occultingBodies.size( ); i++ )
    {
        if( bodyMap.count( occultingBodies[ i ] ) == 0 )
        {
           throw std::runtime_error( "Error, could not find body " + occultingBodies[ i ] +
                                     " in body map when making occulting body settings" );
        }
        else
        {
            std::shared_ptr< basic_astrodynamics::BodyShapeModel > shapeModel =
                    bodyMap.at( occultingBodies.at( i ) )->getShapeModel( );
            if( shapeModel == nullptr )
            {
                throw std::runtime_error( "Error, no shape model for " + occultingBodies[ i ] +
                                          " when making occulting body settings" );
            }
            occultingBodyPositions.push_back(
                        std::bind( &Body::getPosition, bodyMap.at( occultingBodies[ i ] ) ) );
            occultingBodyRadii.push_back( shapeModel->getAverageRadius( ) );
        }
    }
}

//! Function to create a radiation pressure interface.
std::shared_ptr< electro_magnetism::RadiationPressureInterface > createRadiationPressureInterface(
        const std::shared_ptr< RadiationPressureInterfaceSettings > radiationPressureInterfaceSettings,
        const std::string& bodyName, const NamedBodyMap& bodyMap )
{
    std::shared_ptr< electro_magnetism::RadiationPressureInterface > radiationPressureInterface;

    // Check type of radiation pressure interface
    switch( radiationPressureInterfaceSettings->getRadiationPressureType( ) )
    {
    case cannon_ball:
    {
        // Check type consistency.
        std::shared_ptr< CannonBallRadiationPressureInterfaceSettings > cannonBallSettings =
                std::dynamic_pointer_cast< CannonBallRadiationPressureInterfaceSettings >(
                    radiationPressureInterfaceSettings );
        if( cannonBallSettings == nullptr )
        {
            throw std::runtime_error( "Error when making cannon ball radiation interface, type does not match object" );
        }

        // Retrieve source body and check consistency.
        if( bodyMap.count( radiationPressureInterfaceSettings->getSourceBody( ) ) == 0 )
        {
            throw std::runtime_error( "Error when making cannon ball radiation interface, source not found.");
        }

        std::shared_ptr< Body > sourceBody =
                bodyMap.at( radiationPressureInterfaceSettings->getSourceBody( ) );

        // Get reqruied data for occulting bodies.
        std::vector< std::string > occultingBodies = cannonBallSettings->getOccultingBodies( );
        std::vector< std::function< Eigen::Vector3d( ) > > occultingBodyPositions;
        std::vector< double > occultingBodyRadii;
        getOccultingBodiesInformation(
                    bodyMap, occultingBodies, occultingBodyPositions, occultingBodyRadii );

        // Retrive radius of source if occultations are used.
        double sourceRadius;
        if( occultingBodyPositions.size( ) > 0 )
        {
            std::shared_ptr< basic_astrodynamics::BodyShapeModel > sourceShapeModel =
                    sourceBody->getShapeModel( );

            if( sourceShapeModel == nullptr )
            {
                throw std::runtime_error( "Error when making occulted body, source body " +
                                          radiationPressureInterfaceSettings->getSourceBody( ) +
                                          " does not have a shape" );
            }
            else
            {
                sourceRadius = sourceShapeModel->getAverageRadius( );
            }
        }
        else
        {
            sourceRadius = 0.0;
        }

        // Create function returning radiated power.
        std::function< double( ) > radiatedPowerFunction;
        if( defaultRadiatedPowerValues.count(
                    radiationPressureInterfaceSettings->getSourceBody( ) ) == 0 )
        {
            throw std::runtime_error( "Error, no radiated power found for " +
                                      radiationPressureInterfaceSettings->getSourceBody( ) );
        }
        else
        {
            radiatedPowerFunction = [ = ]( ){ return
                        defaultRadiatedPowerValues.at(
                            radiationPressureInterfaceSettings->getSourceBody( ) ); };
        }

        // Create radiation pressure interface.
        radiationPressureInterface =
                std::make_shared< electro_magnetism::RadiationPressureInterface >(
                    radiatedPowerFunction,
                    std::bind( &Body::getPosition, sourceBody ),
                    std::bind( &Body::getPosition, bodyMap.at( bodyName ) ),
                    cannonBallSettings->getRadiationPressureCoefficient( ),
                    cannonBallSettings->getArea( ), occultingBodyPositions, occultingBodyRadii,
                    sourceRadius );
        break;

    }
    default:
        throw std::runtime_error(
                    "Error, radiation pressure type" + std::to_string(
                        radiationPressureInterfaceSettings->getRadiationPressureType( ) ) +
                    "not recognized for body" + bodyName );
    }

    return radiationPressureInterface;
}

} // namespace simulation_setup

} // namespace tudat