#ifndef PERIOR_TREE_BOUNDARY_CONDITIONS
#define PERIOR_TREE_BOUNDARY_CONDITIONS
#include <periortree/point_traits.hpp>
#include <periortree/point_ops.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/static_assert.hpp>
#include <boost/config.hpp>

namespace perior
{

template<typename pointT>
struct unlimited_boundary
{
    typedef pointT point_type;
    BOOST_STATIC_ASSERT(traits::is_point<point_type>::value);
};

template<typename pointT>
struct cubic_periodic_boundary
{
    typedef pointT point_type;
    BOOST_STATIC_ASSERT(traits::is_point<point_type>::value);

//     using ::perior::ops::operator-;
//     using ::perior::ops::operator/;

    cubic_periodic_boundary(const point_type& l, const point_type& u)
        : lower_(l), upper_(u), width_(u - l), half_width_((u - l) / 2)
    {}

    explicit cubic_periodic_boundary(const point_type& u)
        : lower_(traits::zero_vector<point_type>()), upper_(u),
          width_(u), half_width_(u / 2)
    {}
    ~cubic_periodic_boundary(){}

    cubic_periodic_boundary(const cubic_periodic_boundary& rhs)
        : lower_(rhs.lower_), upper_(rhs.upper_),
          width_(rhs.width_), half_width_(rhs.half_width_)
    {}
    cubic_periodic_boundary& operator=(const cubic_periodic_boundary& rhs)
    {
        lower_ = rhs.lower_;
        upper_ = rhs.upper_;
        width_ = rhs.width_;
        half_width_ = rhs.half_width_;
        return *this;
    }

    BOOST_FORCEINLINE
    point_type const& upper() const BOOST_NOEXCEPT_OR_NOTHROW {return upper_;}
    BOOST_FORCEINLINE
    point_type const& lower() const BOOST_NOEXCEPT_OR_NOTHROW {return lower_;}
    BOOST_FORCEINLINE
    point_type const& width() const BOOST_NOEXCEPT_OR_NOTHROW {return width_;}
    BOOST_FORCEINLINE
    point_type const& half_width() const BOOST_NOEXCEPT_OR_NOTHROW {return half_width_;}

  private:

    point_type lower_, upper_;
    point_type width_, half_width_;
};

template<typename pointT>
BOOST_FORCEINLINE
typename boost::enable_if<traits::is_point<pointT>, pointT>::type
restrict_position(const pointT& p, const unlimited_boundary<pointT>& u)
    BOOST_NOEXCEPT_OR_NOTHROW
{
    return p;
}

template<typename pointT>
BOOST_FORCEINLINE
typename boost::enable_if<traits::is_point<pointT>, pointT>::type
restrict_direction(const pointT& d, const unlimited_boundary<pointT>& u)
    BOOST_NOEXCEPT_OR_NOTHROW
{
    return d;
}

template<typename pointT>
BOOST_FORCEINLINE
typename boost::enable_if<traits::is_point<pointT>, pointT>::type
restrict_position(pointT p, const cubic_periodic_boundary<pointT>& u)
    BOOST_NOEXCEPT_OR_NOTHROW
{
    for(std::size_t i=0; i<traits::dimension<pointT>::value; ++i)
    {
             if(p[i] <  u.lower()[i]){p[i] += u.width()[i];}
        else if(p[i] >= u.upper()[i]){p[i] -= u.width()[i];}
    }
    return p;
}

template<typename pointT>
BOOST_FORCEINLINE
typename boost::enable_if<traits::is_point<pointT>, pointT>::type
restrict_direction(pointT d, const cubic_periodic_boundary<pointT>& u)
    BOOST_NOEXCEPT_OR_NOTHROW
{
    for(std::size_t i=0; i<traits::dimension<pointT>::value; ++i)
    {
             if(d[i] <  -(u.half_width()[i])){d[i] += u.width()[i];}
        else if(d[i] >=   u.half_width()[i]) {d[i] -= u.width()[i];}
    }
    return d;
}

} // perior
#endif//PERIOR_TREE_BOUNDARY_CONDITIONS


/** program_L_proposition.hpp ---
*
*
* Authors: Misgana <.com> and Elias Tsegaw<tsegawelias@gmail.com>
* Copyright (c) 2020 misgana96
* This program is free software; you can redistribute it and/or modify
* it under the terms of the  MIT License as
* published by the Free Software Foundation
*
* 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
* MIT License for more details.
*
* You should have received a copy of the MIT License
* along with this program; if not, write to:
* Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
#ifndef L_PROPOSITION_HPP_INCLUDED
#define L_PROPOSITION_HPP_INCLUDED
#include "statment.hpp"
#include <iostream>
#include <vector>
#include <string>
#include <boost/variant.hpp>

namespace Wumpus
{
namespace util
{

/*
*class L_proposition -- Defines logical statment;
*Given a string separated by a space, returns a vector of strings which form the logical proposition.
*proposition(W11 or B22 and B33) will return vector with elements W11, or, B22, and, B33 sequentialy.
*/
typedef boost::variant<statment,string> cell;
static cell Land = "and";
static cell Lor ="or";
static cell Lnot="not";
static cell Limp="imp";
static cell Lbimp="bimp";

class L_proposition
{

public :

    /*
    *func tokenize -- Tokenize A string to a vectror of substrings
    */

    vector<cell> proposition(string propos);
    vector<cell> propositiont(string propos,string tvalues);

    };

}
}
#endif // L_PROPOSITION_HPP_INCLUDED


#ifndef BOOST_ALL_DYN_LINK
#define BOOST_ALL_DYN_LINK
#endif

#include <stdexcept>
#include <cmath>
#include <cstdlib>
#include <ctime>

#include <boost/filesystem.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/algorithm/string.hpp>

#include "opencv2/core/utility.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"

#include "gk_operators.h"

using namespace std;
using namespace cv;

namespace gekon {

    ker_num_t random(ker_num_t min, ker_num_t max) {
        auto rand_num = rand();
        auto ret = ker_num_t(float(min) + float(rand_num) / float(RAND_MAX / float(max - min)));
        return ret;
    }

    std::vector <candidate_t> c_blx_a(candidate_t X, candidate_t Y) {
        int rows = X.rows;
        int cols = X.cols;
        candidate_t kidX = Mat::zeros(rows, cols, KERNEL_TYPE);
        candidate_t kidY = Mat::zeros(rows, cols, KERNEL_TYPE);

        for (int r = 0; r < rows; ++r) {
            for (int c = 0; c < cols; ++c) {
                float xt = X.at<ker_num_t>(r, c);
                float yt = Y.at<ker_num_t>(r, c);
                float diff = abs(xt - yt);
                float min, max;
                if (xt > yt) {
                    min = yt - diff * c_blx_a_alpha;
                    max = xt + diff * c_blx_a_alpha;
                } else {
                    min = xt - diff * c_blx_a_alpha;
                    max = yt + diff * c_blx_a_alpha;
                }
                float rX = random(min, max);
                float rY = random(min, max);
                kidX.at<ker_num_t>(r, c) = rX;
                kidY.at<ker_num_t>(r, c) = rY;
            }
        }

        std::vector <candidate_t> kids;
        kids.push_back(kidX);
        kids.push_back(kidY);

        return kids;

    }

    std::vector <candidate_t> c_convex(candidate_t X, candidate_t Y) {
        // random vectors
        vector <ker_num_t> rand_vec;
        // new candidates
        vector <candidate_t> kids;
        int rows = X.rows, cols = X.cols;
        assert(X.rows == Y.rows && X.cols == Y.cols);
        // initialization of container types
        rand_vec.resize((unsigned int) rows * cols, 0);
        kids.resize(2, Mat::zeros(rows, cols, KERNEL_TYPE));

        for (int j = 0; j < 2; ++j) {
            for_each(rand_vec.begin(), rand_vec.end(), [](auto &iter) {
                return random(0, 1);
            });
            for (int k = 0; k < rows * cols; ++k) {
                int r = k / cols;
                int c = k % cols;
                kids[j].at<ker_num_t>(r, c) =
                        rand_vec[k] * X.at<ker_num_t>(r, c) + (1 - rand_vec[k]) * Y.at<ker_num_t>(r, c);
            }
        }

        return kids;
    }

    std::vector <candidate_t> c_simle(candidate_t X, candidate_t Y) {
        // random vectors
        vector <ker_num_t> rand_vec;
        vector<bool> decision_vec;
        // new candidates
        vector <candidate_t> kids;
        int rows = X.rows, cols = X.cols;
        assert(X.rows == Y.rows && X.cols == Y.cols);
        // initialization of container types
        rand_vec.resize((unsigned int) rows * cols, 0);
        decision_vec.resize((unsigned int) rows * cols, false);
        kids.resize(2, Mat::zeros(rows, cols, KERNEL_TYPE));

        for (int j = 0; j < 2; ++j) {
            for_each(rand_vec.begin(), rand_vec.end(), [](auto &iter) {
                return random(0, 1);
            });
            transform(rand_vec.begin(), rand_vec.end(), decision_vec.begin(), [](auto iter) {
                return iter > 0.5;
            });
            for (int k = 0; k < rows * cols; ++k) {
                int r = k / cols;
                int c = k % cols;
                kids[j].at<ker_num_t>(r, c) = (decision_vec[k]) ? X.at<ker_num_t>(r, c) : Y.at<ker_num_t>(r, c);
            }
        }
        return kids;
    }

    void m_swap(candidate_t &X, unsigned int t, unsigned int T) {
        if (random(0, 1) < m_swap_prob) {
            // Mutate!
            int rows = X.rows;
            int cols = X.cols;
            int r1 = random(-0.4, rows - 1 + 0.4); // that +-0.4 is to ensure whole interval
            int c1 = random(-0.4, cols - 1 + 0.4);
            int r2 = random(-0.4, rows - 1 + 0.4);
            int c2 = random(-0.4, cols - 1 + 0.4);
            float g = X.at<float>(r1, c1);
            X.at<float>(r1, c1) = X.at<float>(r2, c2);
            X.at<float>(r2, c2) = g;
        }

    }

    void m_dynamic(candidate_t &X, unsigned int t, unsigned int T) {
        /*
         * Dynamic mutation (Michalewicz):
         * Defined as $\Delta(t,y) = y \left( 1 - r^{ {1-\frac{t}{T} }^B }  \right)$
         *     where B = 5
         */
        int rows = X.rows;
        int cols = X.cols;
        for (int r = 0; r < rows; ++r) {
            for (int c = 0; c < cols; ++c) {
                if (random(0, 1) < m_swap_prob) {
                    X.at<ker_num_t>(r, c) =
                            X.at<ker_num_t>(r, c) * (1 - pow(random(0, 1), pow(float(1) - float(t) / T, m_dynamic_B)));
                }
            }
        }
    }
}


/*
 Copyright (c) 2010, The Barbarian Group
 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.
*/

#include "cinder/ip/EdgeDetect.h"
#include "cinder/Surface.h"
#include "cinder/CinderMath.h"

#include <boost/preprocessor/seq.hpp>

namespace cinder { namespace ip {

//     X           Y
// -1  0  1     1  2  1
// -2  0  2     0  0  0
// -1  0  1    -1 -2 -1
// NOTE: this leaves garbage in the top and bottom rows, as well as the left and right columns

template<typename T>
void edgeDetectSobel( const ChannelT<T> &srcChannel, const Area &srcArea, const ivec2 &dstLT, ChannelT<T> *dstChannel )
{
	std::pair<Area,ivec2> srcDst = clippedSrcDst( srcChannel.getBounds(), srcArea, dstChannel->getBounds(), dstLT );
	const Area &area( srcDst.first );
	const ivec2 &dstOffset( srcDst.second );
	typename CHANTRAIT<T>::SignedSum sumX, sumY;

	ptrdiff_t srcRowInc = srcChannel.getRowBytes() / sizeof(T);
	uint8_t srcPixelInc = srcChannel.getIncrement();
	uint8_t dstPixelInc = dstChannel->getIncrement();
	const T maxValue = CHANTRAIT<T>::max();
	for( int32_t y = 1; y < area.getHeight() - 1; ++y ) {
		const T *srcLine = srcChannel.getData( area.getX1() + 1, area.getY1() + y );
		T *dstLine = dstChannel->getData( dstOffset.x + area.getX1() + 1, dstOffset.y + y );
		for( int32_t x = area.getX1() + 1; x < area.getX2() - 1; ++x ) {
			sumX = -*(srcLine-srcRowInc-srcPixelInc) + *(srcLine-srcRowInc+srcPixelInc) - 2 * *(srcLine-srcPixelInc)
							+ 2 * *(srcLine+srcPixelInc) - *(srcLine+srcRowInc-srcPixelInc) + *(srcLine+srcRowInc+srcPixelInc);
			sumY = *(srcLine-srcRowInc-srcPixelInc) + 2 * *(srcLine-srcRowInc) + *(srcLine-srcRowInc+srcPixelInc)
							- *(srcLine+srcRowInc-srcPixelInc) - 2 * *(srcLine+srcPixelInc) - *(srcLine+srcRowInc+srcPixelInc);
			sumX = (typename CHANTRAIT<T>::SignedSum)math<float>::sqrt( (float)sumX * sumX + (float)sumY * sumY );
			if( sumX > maxValue )
				sumX = maxValue;
			*dstLine = static_cast<T>( sumX );
			dstLine += dstPixelInc;
			srcLine += srcPixelInc;
		}
	}
}

template<typename T>
void edgeDetectSobel( const SurfaceT<T> &srcSurface, const Area &srcArea, const ivec2 &dstLT, SurfaceT<T> *dstSurface )
{
	edgeDetectSobel( srcSurface.getChannelRed(), srcArea, dstLT, &dstSurface->getChannelRed() );
	edgeDetectSobel( srcSurface.getChannelGreen(), srcArea, dstLT, &dstSurface->getChannelGreen() );
	edgeDetectSobel( srcSurface.getChannelBlue(), srcArea, dstLT, &dstSurface->getChannelBlue() );
	if( srcSurface.hasAlpha() && dstSurface->hasAlpha() )
		edgeDetectSobel( srcSurface.getChannelAlpha(), srcArea, dstLT, &dstSurface->getChannelAlpha() );
}

template<typename T>
void edgeDetectSobel( const ChannelT<T> &srcChannel, ChannelT<T> *dstChannel )
{
	edgeDetectSobel( srcChannel, srcChannel.getBounds(), ivec2(), dstChannel );
}

template<typename T>
void edgeDetectSobel( const SurfaceT<T> &srcSurface, SurfaceT<T> *dstSuface )
{
	edgeDetectSobel( srcSurface, srcSurface.getBounds(), ivec2(), dstSuface );
}

#define edgeDetect_PROTOTYPES(r,data,T)\
	template void edgeDetectSobel( const ChannelT<T> &srcChannel, const Area &srcArea, const ivec2 &dstLT, ChannelT<T> *dstChannel ); \
	template void edgeDetectSobel( const SurfaceT<T> &srcSurface, const Area &srcArea, const ivec2 &dstLT, SurfaceT<T> *dstSurface ); \
	template void edgeDetectSobel( const ChannelT<T> &srcChannel, ChannelT<T> *dstChannel );	\
	template void edgeDetectSobel( const SurfaceT<T> &srcSurface, SurfaceT<T> *dstSurface );

BOOST_PP_SEQ_FOR_EACH( edgeDetect_PROTOTYPES, ~, (uint8_t)(uint16_t)(float) )

} } // namespace cinder::ip


// Copyright 2015. 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 OWNER 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.
//
// Author: ryan.latture@gmail.com (Ryan Latture)

#include <boost/format.hpp>
#include <chrono>
#include <cmath>
#include <exception>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <limits>

#include "threed_beam_fea.h"

namespace fea {

    namespace {
        void writeStringToTxt(std::string filename, std::string data) {
            std::ofstream output_file;
            output_file.open(filename);

            if (!output_file.is_open()) {
                throw std::runtime_error(
                        (boost::format("Error opening file %s.") % filename).str()
                );
            }
            output_file << data;
            output_file.close();
        }
    }

    inline double norm(const Node &n1, const Node &n2) {
        const Node dn = n2 - n1;
        return dn.norm();
    }

    void GlobalStiffAssembler::calcKelem(unsigned int i, const Job &job) {
        // extract element properties
        const double EA = job.props[i].EA;
        const double EIz = job.props[i].EIz;
        const double EIy = job.props[i].EIy;
        const double GJ = job.props[i].GJ;

        // store node indices of current element
        const int nn1 = job.elems[i][0];
        const int nn2 = job.elems[i][1];

        // calculate the length of the element
        const double length = norm(job.nodes[nn1], job.nodes[nn2]);

        // store the entries in the (local) elemental stiffness matrix as temporary values to avoid recalculation
        const double tmpEA = EA / length;
        const double tmpGJ = GJ / length;

        const double tmp12z = 12.0 * EIz / (length * length * length);
        const double tmp6z = 6.0 * EIz / (length * length);
        const double tmp1z = EIz / length;

        const double tmp12y = 12.0 * EIy / (length * length * length);
        const double tmp6y = 6.0 * EIy / (length * length);
        const double tmp1y = EIy / length;

        // update local elemental stiffness matrix
        Klocal(0, 0) = tmpEA;
        Klocal(0, 6) = -tmpEA;
        Klocal(1, 1) = tmp12z;
        Klocal(1, 5) = tmp6z;
        Klocal(1, 7) = -tmp12z;
        Klocal(1, 11) = tmp6z;
        Klocal(2, 2) = tmp12y;
        Klocal(2, 4) = -tmp6y;
        Klocal(2, 8) = -tmp12y;
        Klocal(2, 10) = -tmp6y;
        Klocal(3, 3) = tmpGJ;
        Klocal(3, 9) = -tmpGJ;
        Klocal(4, 2) = -tmp6y;
        Klocal(4, 4) = 4.0 * tmp1y;
        Klocal(4, 8) = tmp6y;
        Klocal(4, 10) = 2.0 * tmp1y;
        Klocal(5, 1) = tmp6z;
        Klocal(5, 5) = 4.0 * tmp1z;
        Klocal(5, 7) = -tmp6z;
        Klocal(5, 11) = 2.0 * tmp1z;
        Klocal(6, 0) = -tmpEA;
        Klocal(6, 6) = tmpEA;
        Klocal(7, 1) = -tmp12z;
        Klocal(7, 5) = -tmp6z;
        Klocal(7, 7) = tmp12z;
        Klocal(7, 11) = -tmp6z;
        Klocal(8, 2) = -tmp12y;
        Klocal(8, 4) = tmp6y;
        Klocal(8, 8) = tmp12y;
        Klocal(8, 10) = tmp6y;
        Klocal(9, 3) = -tmpGJ;
        Klocal(9, 9) = tmpGJ;
        Klocal(10, 2) = -tmp6y;
        Klocal(10, 4) = 2.0 * tmp1y;
        Klocal(10, 8) = tmp6y;
        Klocal(10, 10) = 4.0 * tmp1y;
        Klocal(11, 1) = tmp6z;
        Klocal(11, 5) = 2.0 * tmp1z;
        Klocal(11, 7) = -tmp6z;
        Klocal(11, 11) = 4.0 * tmp1z;

        // calculate unit normal vector along local x-direction
        Eigen::Vector3d nx = job.nodes[nn2] - job.nodes[nn1];
        nx.normalize();

        // calculate unit normal vector along y-direction
        const Eigen::Vector3d ny = job.props[i].normal_vec.normalized();

        // update rotation matrices
        calcAelem(nx, ny);

        // update Kelem
        Kelem = AelemT * Klocal * Aelem;
    };

    void GlobalStiffAssembler::calcAelem(const Eigen::Vector3d &nx, const Eigen::Vector3d &ny) {
        // calculate the unit normal vector in local z direction
        Eigen::Vector3d nz;
        nz = nx.cross(ny);
        const double dlz = nz.squaredNorm();
        nz /= dlz;

        // update rotation matrix
        Aelem(0, 0) = nx(0);
        Aelem(0, 1) = nx(1);
        Aelem(0, 2) = nx(2);
        Aelem(1, 0) = ny(0);
        Aelem(1, 1) = ny(1);
        Aelem(1, 2) = ny(2);
        Aelem(2, 0) = nz(0);
        Aelem(2, 1) = nz(1);
        Aelem(2, 2) = nz(2);

        Aelem(3, 3) = nx(0);
        Aelem(3, 4) = nx(1);
        Aelem(3, 5) = nx(2);
        Aelem(4, 3) = ny(0);
        Aelem(4, 4) = ny(1);
        Aelem(4, 5) = ny(2);
        Aelem(5, 3) = nz(0);
        Aelem(5, 4) = nz(1);
        Aelem(5, 5) = nz(2);

        Aelem(6, 6) = nx(0);
        Aelem(6, 7) = nx(1);
        Aelem(6, 8) = nx(2);
        Aelem(7, 6) = ny(0);
        Aelem(7, 7) = ny(1);
        Aelem(7, 8) = ny(2);
        Aelem(8, 6) = nz(0);
        Aelem(8, 7) = nz(1);
        Aelem(8, 8) = nz(2);

        Aelem(9, 9) = nx(0);
        Aelem(9, 10) = nx(1);
        Aelem(9, 11) = nx(2);
        Aelem(10, 9) = ny(0);
        Aelem(10, 10) = ny(1);
        Aelem(10, 11) = ny(2);
        Aelem(11, 9) = nz(0);
        Aelem(11, 10) = nz(1);
        Aelem(11, 11) = nz(2);

        // update transposed rotation matrix
        AelemT(0, 0) = nx(0);
        AelemT(0, 1) = ny(0);
        AelemT(0, 2) = nz(0);
        AelemT(1, 0) = nx(1);
        AelemT(1, 1) = ny(1);
        AelemT(1, 2) = nz(1);
        AelemT(2, 0) = nx(2);
        AelemT(2, 1) = ny(2);
        AelemT(2, 2) = nz(2);

        AelemT(3, 3) = nx(0);
        AelemT(3, 4) = ny(0);
        AelemT(3, 5) = nz(0);
        AelemT(4, 3) = nx(1);
        AelemT(4, 4) = ny(1);
        AelemT(4, 5) = nz(1);
        AelemT(5, 3) = nx(2);
        AelemT(5, 4) = ny(2);
        AelemT(5, 5) = nz(2);

        AelemT(6, 6) = nx(0);
        AelemT(6, 7) = ny(0);
        AelemT(6, 8) = nz(0);
        AelemT(7, 6) = nx(1);
        AelemT(7, 7) = ny(1);
        AelemT(7, 8) = nz(1);
        AelemT(8, 6) = nx(2);
        AelemT(8, 7) = ny(2);
        AelemT(8, 8) = nz(2);

        AelemT(9, 9) = nx(0);
        AelemT(9, 10) = ny(0);
        AelemT(9, 11) = nz(0);
        AelemT(10, 9) = nx(1);
        AelemT(10, 10) = ny(1);
        AelemT(10, 11) = nz(1);
        AelemT(11, 9) = nx(2);
        AelemT(11, 10) = ny(2);
        AelemT(11, 11) = nz(2);
    };

    void GlobalStiffAssembler::operator()(SparseMat &Kg, const Job &job, const std::vector<Tie> &ties) {
        int nn1, nn2;
        unsigned int row, col;
        const unsigned int dofs_per_elem = DOF::NUM_DOFS;

        // form vector to hold triplets that will be used to assemble global stiffness matrix
        std::vector<Eigen::Triplet<double> > triplets;
        triplets.reserve(40 * job.elems.size() + 4 * dofs_per_elem * ties.size());

        for (unsigned int i = 0; i < job.elems.size(); ++i) {
            // update Kelem with current elemental stiffness matrix
            calcKelem(i, job);

            // get sparse representation of the current elemental stiffness matrix
            SparseKelem = Kelem.sparseView();

            // pull out current node indices
            nn1 = job.elems[i][0];
            nn2 = job.elems[i][1];

            for (unsigned int j = 0; j < SparseKelem.outerSize(); ++j) {
                for (SparseMat::InnerIterator it(SparseKelem, j); it; ++it) {
                    row = it.row();
                    col = it.col();

                    // check position in local matrix and update corresponding global position
                    if (row < 6) {
                        // top left
                        if (col < 6) {
                            triplets.push_back(Eigen::Triplet<double>(dofs_per_elem * nn1 + row,
                                                                      dofs_per_elem * nn1 + col,
                                                                      it.value()));
                        }
                            // top right
                        else {
                            triplets.push_back(Eigen::Triplet<double>(dofs_per_elem * nn1 + row,
                                                                      dofs_per_elem * (nn2 - 1) + col,
                                                                      it.value()));
                        }
                    }
                    else {
                        // bottom left
                        if (col < 6) {
                            triplets.push_back(Eigen::Triplet<double>(dofs_per_elem * (nn2 - 1) + row,
                                                                      dofs_per_elem * nn1 + col,
                                                                      it.value()));
                        }
                            // bottom right
                        else {
                            triplets.push_back(Eigen::Triplet<double>(dofs_per_elem * (nn2 - 1) + row,
                                                                      dofs_per_elem * (nn2 - 1) + col,
                                                                      it.value()));
                        }
                    }
                }
            }
        }

        loadTies(triplets, ties);

        Kg.setFromTriplets(triplets.begin(), triplets.end());
    };

    void loadBCs(SparseMat &Kg, SparseMat &force_vec, const std::vector<BC> &BCs, unsigned int num_nodes) {
        unsigned int bc_idx;
        const unsigned int dofs_per_elem = DOF::NUM_DOFS;
        // calculate the index that marks beginning of Lagrange multiplier coefficients
        const unsigned int global_add_idx = dofs_per_elem * num_nodes;

        for (size_t i = 0; i < BCs.size(); ++i) {
            bc_idx = dofs_per_elem * BCs[i].node + BCs[i].dof;

            // update global stiffness matrix
            Kg.insert(bc_idx, global_add_idx + i) = 1;
            Kg.insert(global_add_idx + i, bc_idx) = 1;

            // update force vector. All values are already zero. Only update if BC if non-zero.
            if (std::abs(BCs[i].value) > std::numeric_limits<double>::epsilon()) {
                force_vec.insert(global_add_idx + i, 0) = BCs[i].value;
            }
        }
    };

    void loadEquations(SparseMat &Kg, const std::vector<Equation> &equations, unsigned int num_nodes, unsigned int num_bcs) {
        size_t row_idx, col_idx;
        const unsigned int dofs_per_elem = DOF::NUM_DOFS;
        const unsigned int global_add_idx = dofs_per_elem * num_nodes + num_bcs;

        for (size_t i = 0; i < equations.size(); ++i) {
            row_idx = global_add_idx + i;
            for (size_t j = 0; j < equations[i].terms.size(); ++j) {
                col_idx = dofs_per_elem * equations[i].terms[j].node_number + equations[i].terms[j].dof;
                Kg.insert(row_idx, col_idx) = equations[i].terms[j].coefficient;
                Kg.insert(col_idx, row_idx) = equations[i].terms[j].coefficient;
            }
        }
    };

    void loadTies(std::vector<Eigen::Triplet<double> > &triplets, const std::vector<Tie> &ties) {
        const unsigned int dofs_per_elem = DOF::NUM_DOFS;
        unsigned int nn1, nn2;
        double lmult, rmult, spring_constant;

        for (size_t i = 0; i < ties.size(); ++i) {
            nn1 = ties[i].node_number_1;
            nn2 = ties[i].node_number_2;
            lmult = ties[i].lmult;
            rmult = ties[i].rmult;

            for (unsigned int j = 0; j < dofs_per_elem; ++j) {
                // first 3 DOFs are linear DOFs, second 2 are rotational, last is torsional
                spring_constant = j < 3 ? lmult : rmult;

                triplets.push_back(Eigen::Triplet<double>(dofs_per_elem * nn1 + j,
                                                          dofs_per_elem * nn1 + j,
                                                          spring_constant));

                triplets.push_back(Eigen::Triplet<double>(dofs_per_elem * nn2 + j,
                                                          dofs_per_elem * nn2 + j,
                                                          spring_constant));

                triplets.push_back(Eigen::Triplet<double>(dofs_per_elem * nn1 + j,
                                                          dofs_per_elem * nn2 + j,
                                                          -spring_constant));

                triplets.push_back(Eigen::Triplet<double>(dofs_per_elem * nn2 + j,
                                                          dofs_per_elem * nn1 + j,
                                                          -spring_constant));
            }
        }
    };

    std::vector<std::vector<double> > computeTieForces(const std::vector<Tie> &ties,
                                                       const std::vector<std::vector<double> > &nodal_displacements) {
        const unsigned int dofs_per_elem = DOF::NUM_DOFS;
        unsigned int nn1, nn2;
        double lmult, rmult, spring_constant, delta1, delta2;

        std::vector<std::vector<double> > tie_forces(ties.size(), std::vector<double>(dofs_per_elem));

        for (size_t i = 0; i < ties.size(); ++i) {
            nn1 = ties[i].node_number_1;
            nn2 = ties[i].node_number_2;
            lmult = ties[i].lmult;
            rmult = ties[i].rmult;

            for (unsigned int j = 0; j < dofs_per_elem; ++j) {
                // first 3 DOFs are linear DOFs, second 2 are rotational, last is torsional
                spring_constant = j < 3 ? lmult : rmult;
                delta1 = nodal_displacements[nn1][j];
                delta2 = nodal_displacements[nn2][j];
                tie_forces[i][j] = spring_constant * (delta2 - delta1);
            }
        }
        return tie_forces;
    }

    void loadForces(SparseMat &force_vec, const std::vector<Force> &forces) {
        const unsigned int dofs_per_elem = DOF::NUM_DOFS;
        unsigned int idx;

        for (size_t i = 0; i < forces.size(); ++i) {
            idx = dofs_per_elem * forces[i].node + forces[i].dof;
            force_vec.insert(idx, 0) = forces[i].value;
        }
    };

    Summary solve(const Job &job,
                  const std::vector<BC> &BCs,
                  const std::vector<Force> &forces,
                  const std::vector<Tie> &ties,
                  const std::vector<Equation> &equations,
                  const Options &options) {
        auto initial_start_time = std::chrono::high_resolution_clock::now();

        Summary summary;
        summary.num_nodes = job.nodes.size();
        summary.num_elems = job.elems.size();
        summary.num_bcs = BCs.size();
        summary.num_ties = ties.size();

        const unsigned int dofs_per_elem = DOF::NUM_DOFS;

        // calculate size of global stiffness matrix and force vector
        const unsigned long size = dofs_per_elem * job.nodes.size() + BCs.size() + equations.size();

        // construct global stiffness matrix and force vector
        SparseMat Kg(size, size);
        SparseMat force_vec(size, 1);
        force_vec.reserve(forces.size() + BCs.size());

        // construct global assembler object and assemble global stiffness matrix
        auto start_time = std::chrono::high_resolution_clock::now();
        GlobalStiffAssembler assembleK3D = GlobalStiffAssembler();
        assembleK3D(Kg, job, ties);
        auto end_time = std::chrono::high_resolution_clock::now();
        auto delta_time = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time).count();
        summary.assembly_time_in_ms = delta_time;

        if (options.verbose)
            std::cout << "Global stiffness matrix assembled in "
            << delta_time
            << " ms.\nNow preprocessing factorization..." << std::endl;

        // load prescribed boundary conditions into stiffness matrix and force vector
        loadBCs(Kg, force_vec, BCs, job.nodes.size());

        if (equations.size() > 0) {
            loadEquations(Kg, equations, job.nodes.size(), BCs.size());
        }

        // load prescribed forces into force vector
        if (forces.size() > 0) {
            loadForces(force_vec, forces);
        }

        // compress global stiffness matrix since all non-zero values have been added.
        Kg.prune(1.e-14);
        Kg.makeCompressed();

        // initialize solver based on whether MKL should be used
#ifdef EIGEN_USE_MKL_ALL
        Eigen::PardisoLU<SparseMat> solver;
#else
        Eigen::SparseLU<SparseMat> solver;
#endif

        //Compute the ordering permutation vector from the structural pattern of Kg
        start_time = std::chrono::high_resolution_clock::now();
        solver.analyzePattern(Kg);
        end_time = std::chrono::high_resolution_clock::now();
        delta_time = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time).count();

        summary.preprocessing_time_in_ms = delta_time;

        if (options.verbose)
            std::cout << "Preprocessing step of factorization completed in "
            << delta_time
            << " ms.\nNow factorizing global stiffness matrix..." << std::endl;

        // Compute the numerical factorization
        start_time = std::chrono::high_resolution_clock::now();
        solver.factorize(Kg);
        end_time = std::chrono::high_resolution_clock::now();

        delta_time = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time).count();
        summary.factorization_time_in_ms = delta_time;

        if (options.verbose)
            std::cout << "Factorization completed in "
            << delta_time
            << " ms. Now solving system..." << std::endl;

        //Use the factors to solve the linear system
        start_time = std::chrono::high_resolution_clock::now();
        SparseMat dispSparse = solver.solve(force_vec);
        end_time = std::chrono::high_resolution_clock::now();
        delta_time = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time).count();

        summary.solve_time_in_ms = delta_time;

        if (options.verbose)
            std::cout << "System was solved in "
            << delta_time
            << " ms.\n" << std::endl;

        // convert to dense matrix
        Eigen::VectorXd disp(dispSparse);

        // convert from Eigen vector to std vector
        std::vector<std::vector<double> > disp_vec(job.nodes.size(), std::vector<double>(dofs_per_elem));
        for (size_t i = 0; i < disp_vec.size(); ++i) {
            for (unsigned int j = 0; j < dofs_per_elem; ++j)
                // round all values close to 0.0
                disp_vec[i][j] =
                        std::abs(disp(dofs_per_elem * i + j)) < options.epsilon ? 0.0 : disp(dofs_per_elem * i + j);
        }
        summary.nodal_displacements = disp_vec;

        // [calculate nodal forces
        start_time = std::chrono::high_resolution_clock::now();

        Kg = Kg.topLeftCorner(dofs_per_elem * job.nodes.size(), dofs_per_elem * job.nodes.size());
        dispSparse = dispSparse.topRows(dofs_per_elem * job.nodes.size());

        SparseMat nodal_forces_sparse = Kg * dispSparse;

        Eigen::VectorXd nodal_forces_dense(nodal_forces_sparse);

        std::vector<std::vector<double> > nodal_forces_vec(job.nodes.size(), std::vector<double>(dofs_per_elem));
        for (size_t i = 0; i < nodal_forces_vec.size(); ++i) {
            for (unsigned int j = 0; j < dofs_per_elem; ++j)
                // round all values close to 0.0
                nodal_forces_vec[i][j] = std::abs(nodal_forces_dense(dofs_per_elem * i + j)) < options.epsilon ? 0.0
                                                                                : nodal_forces_dense(
                                dofs_per_elem * i + j);
        }
        summary.nodal_forces = nodal_forces_vec;

        end_time = std::chrono::high_resolution_clock::now();

        summary.nodal_forces_solve_time_in_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
                end_time - start_time).count();
        //]

        // [ calculate forces associated with ties
        if (ties.size() > 0) {
            start_time = std::chrono::high_resolution_clock::now();
            summary.tie_forces = computeTieForces(ties, disp_vec);
            end_time = std::chrono::high_resolution_clock::now();
            summary.tie_forces_solve_time_in_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
                    end_time - start_time).count();
        }
        // ]

        // [save files specified in options
        CSVParser csv;
        start_time = std::chrono::high_resolution_clock::now();
        if (options.save_nodal_displacements) {
            csv.write(options.nodal_displacements_filename, disp_vec, options.csv_precision, options.csv_delimiter);
        }

        if (options.save_nodal_forces) {
            csv.write(options.nodal_forces_filename, nodal_forces_vec, options.csv_precision, options.csv_delimiter);
        }

        if (options.save_tie_forces) {
            csv.write(options.tie_forces_filename, summary.tie_forces, options.csv_precision, options.csv_delimiter);
        }

        end_time = std::chrono::high_resolution_clock::now();
        summary.file_save_time_in_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
                end_time - start_time).count();
        // ]

        auto final_end_time = std::chrono::high_resolution_clock::now();

        delta_time = std::chrono::duration_cast<std::chrono::milliseconds>(final_end_time - initial_start_time).count();
        summary.total_time_in_ms = delta_time;

        if (options.save_report) {
            writeStringToTxt(options.report_filename, summary.FullReport());
        }

        if (options.verbose)
            std::cout << summary.FullReport();

        return summary;
    };

} // namespace fea


#pragma once
#include <omp.h>
#include <string>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/IterativeLinearSolvers>
#include <unsupported/Eigen/SparseExtra>
#include <unsupported/Eigen/IterativeSolvers>

#include "json11.hpp"
#include "buffalo/misc/log.hpp"
#include "buffalo/concurrent_queue.hpp"

using namespace std;
using namespace json11;
using namespace Eigen;

typedef Matrix<float, Dynamic, Dynamic, RowMajor> MatrixType;
typedef RowVectorXf VectorType;

typedef Matrix<float, Dynamic, Dynamic, ColMajor> FactorType;
typedef Matrix<float, Dynamic, Dynamic, RowMajor> FactorTypeRowMajor;

struct job_t
{
    int size;
    double alpha;
    vector<vector<int>> samples;

    job_t() : size(0), alpha(0.0)
    {
    }

    void add(const vector<int>& sample)
    {
        samples.push_back(sample);
        size += (int)sample.size();
    }

    int total_samples()
    {
        return size;
    }
};

struct progress_t
{
    int num_sents;
    int num_processed_samples;
    int num_total_samples;
    double loss;
    progress_t(int a, int b, int c) :
        num_sents(a),
        num_processed_samples(b),
        num_total_samples(c) {
    }

    progress_t(int a, int b, int c, double l) :
        num_sents(a),
        num_processed_samples(b),
        num_total_samples(c),
        loss(l) {
    }
};

class Algorithm  // Algorithm? Logic? Learner?
{
public:
    Algorithm();

    virtual ~Algorithm() {}
    virtual bool init(string opt_path) = 0;
    virtual bool parse_option(string opt_path) = 0;

    bool parse_option(string opt_path, Json& j);
    void _leastsquare(Map<MatrixType>& X, int idx, MatrixType& A, VectorType& y);

public:
    char optimizer_code_ = 0;
    int num_cg_max_iters_ = 3;
    float cg_tolerance_ = 1e-10;
    float eps_ = 1e-10;
    std::shared_ptr<spdlog::logger> logger_;
};

class SGDAlgorithm : public Algorithm
{
public:
    SGDAlgorithm();
    ~SGDAlgorithm();

    bool init(string opt_path);

public:
    void release();
    void initialize_model(
        float* P, int32_t P_rows,
        float* Q, int32_t Q_rows,
        float* Qb,
        int64_t num_total_samples);

    void launch_workers();

    void initialize_adam_optimizer();
    void initialize_sgd_optimizer();
    void progress_manager();
    void add_jobs(
        int start_x,
        int next_x,
        int64_t* indptr,
        int32_t* positives);
    void update_parameters();
    void update_adam(
        FactorTypeRowMajor& grad,
        FactorTypeRowMajor& momentum,
        FactorTypeRowMajor& velocity, int i, double beta1, double beta2);
    void update_adagrad(FactorTypeRowMajor& grad,
        FactorTypeRowMajor& velocity, int i);
    void wait_until_done();
    double join();

public:
    virtual void worker(int worker_id) = 0;
    Json opt_;
    Map<FactorTypeRowMajor> P_, Q_, Qb_;

    int iters_;
    double lr_;
    string optimizer_;
    double total_processed_;

    vector<int> P_samples_per_coordinates_;
    vector<int> Q_samples_per_coordinates_;
    FactorTypeRowMajor gradP_, gradQ_, gradQb_;
    FactorTypeRowMajor momentumP_, momentumQ_, momentumQb_;
    FactorTypeRowMajor velocityP_, velocityQ_, velocityQb_;
    vector<thread> workers_;
    thread* progress_manager_;
    Queue<job_t> job_queue_;
    Queue<progress_t> progress_queue_;

};


#include "LIS2MDL.hpp"
#include "iostream"
#include <Eigen/Core>
// ---------------------------------------------------------------------------------------------------------------------
LIS2MDL::LIS2MDL(uint16_t i2cBus)
    : I2CDevice(i2cBus, LIS2MDL_ADDR), configured_{true}
{
    configured_ &= isConnectionValid();
    configured_ &= checkWhoAmI();

    if (configured_)
        setConfigRegA();

    // Configure the calibration matrices
    a_matrix_.resize(3,3);
    a_matrix_(0,0) = 2.17231459;
    a_matrix_(0,1) = -0.04305537;
    a_matrix_(0,2) =  -0.02645279;
    a_matrix_(1,0) = -0.04305537;
    a_matrix_(1,1) =   2.2034971;
    a_matrix_(1,2) =    0.02974335;
    a_matrix_(2,0) = -0.02645279;
    a_matrix_(2,1) =   0.02974335;
    a_matrix_(2,2) =   2.25164522;

    b_matrix_.resize(3,1);
    b_matrix_(0,0) = 0.11439748;
    b_matrix_(1,0) = 0.03041577;
    b_matrix_(2,0) = 0.03175254;
}

// ---------------------------------------------------------------------------------------------------------------------
uint8_t LIS2MDL::getWhoAmI()
{
    uint8_t val;
    readRegister((uint8_t)Lis2RegisterMap::WHO_AM_I, val);
    return val;
}

// ---------------------------------------------------------------------------------------------------------------------
void LIS2MDL::getMeasurement(Measurement& meas)
{
    uint8_t data[6];
    readRegisters((uint8_t)Lis2RegisterMap::OUTX_L_REG, &data[0], 6);

    Eigen::VectorXf rawMeas(3);

    uint16_t val = 0;
    val |= data[0];
    val |= (uint16_t)data[1] << 8;
    rawMeas(0) = (float)((int16_t)val) * LIS2MDL_SCALE_FACTOR / 1000;

    val = 0;
    val |= data[2];
    val |= (uint16_t)data[3] << 8;
    rawMeas(1) = (float)((int16_t)val) * LIS2MDL_SCALE_FACTOR / 1000;

    val = 0;
    val |= data[4];
    val |= (uint16_t)data[5] << 8;
    rawMeas(2) = (float)((int16_t)val) * LIS2MDL_SCALE_FACTOR / 1000;

    rawMeas = rawMeas - b_matrix_; // Correct for the hard iron offset

    meas.mag[0] = a_matrix_.row(0).dot(rawMeas);
    meas.mag[1] = a_matrix_.row(1).dot(rawMeas);
    meas.mag[2] = a_matrix_.row(2).dot(rawMeas);
}

// ---------------------------------------------------------------------------------------------------------------------
void LIS2MDL::setConfigRegA()
{
    uint8_t val = 0x0C;
    writeRegister((uint8_t)Lis2RegisterMap::CFG_REG_A, val);
}

// ---------------------------------------------------------------------------------------------------------------------
void LIS2MDL::calibrateSensor()
{
    std::cout << "Starting Magnetometer Calibration" << std::endl;
    Eigen::MatrixXd sampleData(2000, 3); // 2000 Samples which should equate to 20 seconds worth of data collecting

}

// ---------------------------------------------------------------------------------------------------------------------
void LIS2MDL::printCalibrationMatrices()
{
    std::cout << "A Matrix: " << std::endl;
    std::cout << a_matrix_ << std::endl << std::endl;

    std::cout << "B Matrix: " << std::endl;
    std::cout << b_matrix_ << std::endl << std::endl;
}


/** $Id: Muonitron.cxx 126481 2014-12-02 22:45:21Z david.schultz $
 * @file
 * @author Jakob van Santen <vansanten@wisc.edu>
 *
 * $Revision: 126481 $
 * $Date: 2014-12-02 15:45:21 -0700 (Tue, 02 Dec 2014) $
 */

#include <MuonGun/Muonitron.h>
#include <MuonGun/CompactTrack.h>
#include <dataclasses/physics/I3MCTree.h>
#include <dataclasses/physics/I3MCTreeUtils.h>
#include <phys-services/I3Calculator.h>
#include <simclasses/I3MMCTrack.h>

#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>

I3_MODULE(Muonitron);

Muonitron::Muonitron(const I3Context &ctx) : I3Module(ctx), iceWorld_(I3Constants::SurfaceElev - I3Constants::OriginElev, 6374134)
{
	AddParameter("Depths", "Propagate muons to these vertical depths (in meters)", depths_);
	AddParameter("Crust", "Air and firn layers surrounding the bulk medium", crust_);
	AddParameter("Propagator", "MuonPropagator instance", bulkPropagator_);

	AddOutBox("OutBox");
}

void
Muonitron::Configure()
{
	GetParameter("Depths", depths_);
	GetParameter("Crust", crust_);
	GetParameter("Propagator", bulkPropagator_);

	if (depths_.size() == 0)
		log_fatal("You must specify at least one vertical depth!");
	if (!crust_)
		log_fatal("No crust layers configured!");
	if (!bulkPropagator_)
		log_fatal("No MMC propagator configured!");
}

// Convert to a coordinate system where the zenith is given by the given direction
// rather than I3Direction(0,0,-1)
I3Direction
Muonitron::RotateToZenith(const I3Direction &direction, const I3Direction &dir)
{
	I3Direction p(dir);
	p.RotateZ(-direction.GetAzimuth());
	p.RotateY(-direction.GetZenith());
	p.RotateZ(direction.GetAzimuth()); //get x-y orientation right
	return p;
}

I3Position
Muonitron::RotateToZenith(const I3Direction &direction, const I3Position &pos)
{
	I3Position p(pos);
	p.RotateZ(-direction.GetAzimuth());
	p.RotateY(-direction.GetZenith());
	p.RotateZ(direction.GetAzimuth()); //get x-y orientation right
	return p;
}

I3Position
Muonitron::Impact(const I3Particle &p)
{
	// Vector pointing from the origin to the anchor point,
	// projected along the track
	double l = (p.GetPos().GetX()*p.GetDir().GetX()
	          + p.GetPos().GetY()*p.GetDir().GetY()
	          + p.GetPos().GetZ()*p.GetDir().GetZ());
	// Shift anchor point down the track to the closest approach
	// to the origin
	return I3Position(p.GetPos().GetX() - l*p.GetDir().GetX(),
	                  p.GetPos().GetY() - l*p.GetDir().GetY(),
	                  p.GetPos().GetZ() - l*p.GetDir().GetZ());
}

I3Particle
Muonitron::RotateToZenith(const I3Particle &reference, const I3Particle &part)
{
	I3Particle p(part);
	p.SetDir(RotateToZenith(reference.GetDir(), p.GetDir()));
	// Force reference axis to pass through the origin
	I3Position impact = Impact(reference);
	I3Position anchor = I3Position(p.GetPos().GetX()-impact.GetX(),
	                               p.GetPos().GetY()-impact.GetY(),
	                               p.GetPos().GetZ()-impact.GetZ());
	p.SetPos(RotateToZenith(reference.GetDir(), anchor));
	return p;
}

// Find the distance to the surface from a point at depth d
double
Muonitron::GetOverburden(double zenith, double d, double r)
{
	double ct = cos(zenith);
	return sqrt(2*r*d + ct*ct*(r-d)*(r-d) - d*d) - (r-d)*ct;
}

// Transform a detector-centered zenith angle to an earth-centered zenith angle
double
Muonitron::GetGeocentricZenith(double zenith, double d, double r)
{
	double p = GetOverburden(zenith, d, r);
	return atan2(p*sin(zenith), p*cos(zenith) + (r-d));
}

double
Muonitron::GetSurfaceZenith(double zenith, double d, double r)
{
	return zenith - GetGeocentricZenith(zenith, d, r);
}

bool
Muonitron::PropagateTrack(I3Particle &target, double slant_depth)
{
	double l = target.GetLength();
	target = bulkPropagator_->propagate(target, slant_depth);
	target.SetLength(l + std::min(slant_depth, target.GetLength()));
	return target.GetEnergy() > 0;
}

void
Muonitron::DAQ(I3FramePtr frame)
{
	I3MCTreeConstPtr mctree = frame->Get<I3MCTreeConstPtr>();
	if (!mctree)
		log_fatal("No MCTree!");

	I3MCTree::const_iterator it = mctree->begin();
	const I3Particle &primary = *it;

	std::list<I3Particle> tracks;
	for (it++; it != mctree->end(); it++)
		if (it->GetType() == I3Particle::MuPlus || it->GetType() == I3Particle::MuMinus) {
			// Transport from the atmosphere through the firn layer into the bulk ice
			I3Particle track = crust_->Ingest(*it);
			if (track.GetEnergy() > 0)
				tracks.push_back(track);
		}

	I3MuonGun::TrackBundlePtr bundle = boost::make_shared<I3MuonGun::TrackBundle>();
	BOOST_FOREACH(double vdepth, depths_) {

		// Find the slant depth from the surface of the glacier to
		// a point vdepth meters below grid center.
		double dx = GetOverburden(primary.GetDir().GetZenith(), vdepth, 6374134);
		std::vector<I3MuonGun::CompactTrack> deep_tracks;
		for (std::list<I3Particle>::iterator pit = tracks.begin(); pit != tracks.end(); ) {

			// Propagate the muon the remaining distance to reach the desired depth.
			if (PropagateTrack(*pit, dx-pit->GetLength())) {
				deep_tracks.push_back(I3MuonGun::CompactTrack(RotateToZenith(primary, *pit)));
				pit++;
			} else {
				pit = tracks.erase(pit);
			}
		}

		(*bundle)[vdepth].swap(deep_tracks);
	}

	frame->Put("MCPrimary", boost::make_shared<I3Particle>(primary));
	frame->Put("Tracks", bundle);
	PushFrame(frame);
}


#define BOOST_TEST_MODULE "test_position_restraint_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/forcefield/external/PositionRestraintInteraction.hpp>
#include <mjolnir/forcefield/local/HarmonicPotential.hpp>
#include <mjolnir/util/make_unique.hpp>
#include <random>

BOOST_AUTO_TEST_CASE(PositionRestraint_Harmonic)
{
    using traits_type      = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type        = traits_type::real_type;
    using coordinate_type  = traits_type::coordinate_type;
    using boundary_type    = traits_type::boundary_type;
    using system_type      = mjolnir::System<traits_type>;
    using potential_type   = mjolnir::HarmonicPotential<real_type>;
    using interaction_type = mjolnir::PositionRestraintInteraction<traits_type, potential_type>;

    constexpr real_type tol = 1e-8;

    auto normalize = [](const coordinate_type& v){return v / mjolnir::math::length(v);};

    const potential_type pot1{/*k =*/1.0, /*r0 =*/ 0.0};
    const potential_type pot2{/*k =*/1.0, /*r0 =*/10.0};

    interaction_type interaction(
        std::vector<std::tuple<std::size_t, coordinate_type, potential_type>>{
            std::make_tuple(0, coordinate_type{0.0,0.0,0.0}, pot1),
            std::make_tuple(1, coordinate_type{0.0,0.0,0.0}, pot2)
        });

    system_type sys(2, boundary_type{});

    sys.at(0).mass  = 1.0;
    sys.at(1).mass  = 1.0;
    sys.at(0).rmass = 1.0;
    sys.at(1).rmass = 1.0;

    sys.at(0).position = coordinate_type( 0.0,  0.0,  0.0);
    sys.at(1).position = coordinate_type(10.0,  0.0,  0.0);
    sys.at(0).velocity = coordinate_type( 0.0,  0.0,  0.0);
    sys.at(1).velocity = coordinate_type( 0.0,  0.0,  0.0);
    sys.at(0).force    = coordinate_type( 0.0,  0.0,  0.0);
    sys.at(1).force    = coordinate_type( 0.0,  0.0,  0.0);

    sys.at(0).name  = "X";
    sys.at(1).name  = "X";
    sys.at(0).group = "NONE";
    sys.at(1).group = "NONE";

    std::mt19937 rng(123456789);
    std::uniform_real_distribution<real_type> uni(-1.0, 1.0);
    std::normal_distribution<real_type> gauss(0.0, 1.0);

    for(int i = 0; i < 10000; ++i)
    {
        sys.position(0) = coordinate_type(uni(rng), uni(rng), uni(rng));
        sys.force(0)    = coordinate_type(0, 0, 0);

        const auto dist = mjolnir::math::length(sys.position(0));
        const auto deriv = pot1.derivative(dist);
        const auto coef  = std::abs(deriv);

        interaction.calc_force(sys);

        const auto force_strength = mjolnir::math::length(sys.force(0));

        BOOST_TEST(coef == force_strength, boost::test_tools::tolerance(tol));

        // forces always attract the particle to the origin.
        const auto direction = mjolnir::math::dot_product(
            normalize(sys.force(0)), normalize(-sys.position(0)));

        BOOST_TEST(direction == 1e0, boost::test_tools::tolerance(tol));
    }

    for(int i = 0; i < 10000; ++i)
    {
        sys.position(1) = coordinate_type(uni(rng), uni(rng), uni(rng));
        sys.force(1)    = coordinate_type(0, 0, 0);

        const auto offset = 10.0 * normalize(
                coordinate_type(gauss(rng), gauss(rng), gauss(rng)));
        sys.position(1) += offset; // dist ~ 10.0 +- random vector

        const auto dist  = mjolnir::math::length(sys.position(1));
        const auto deriv = pot2.derivative(dist);
        const auto coef  = std::abs(deriv);

        interaction.calc_force(sys);
        const auto force_strength = mjolnir::math::length(sys.force(1));

        BOOST_TEST(coef == force_strength, boost::test_tools::tolerance(tol));

        if(dist < 10.0) // repulsive
        {
            const auto direction = mjolnir::math::dot_product(
                normalize(sys.force(1)), normalize(sys.position(1)));
            BOOST_TEST(direction == 1e0, boost::test_tools::tolerance(tol));
        }
        else // attractive
        {
            const auto direction = mjolnir::math::dot_product(
                normalize(sys.force(1)), normalize(-sys.position(1)));
            BOOST_TEST(direction == 1e0, boost::test_tools::tolerance(tol));
        }
    }
}

BOOST_AUTO_TEST_CASE(PositionRestraint_numerical_differentiation)
{
    using traits_type      = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type        = traits_type::real_type;
    using coordinate_type  = traits_type::coordinate_type;
    using boundary_type    = traits_type::boundary_type;
    using system_type      = mjolnir::System<traits_type>;
    using potential_type   = mjolnir::HarmonicPotential<real_type>;
    using interaction_type = mjolnir::PositionRestraintInteraction<traits_type, potential_type>;

    const potential_type pot1{/*k =*/1.0, /*r0 =*/ 0.0};
    const potential_type pot2{/*k =*/1.0, /*r0 =*/10.0};

    interaction_type interaction(
        std::vector<std::tuple<std::size_t, coordinate_type, potential_type>>{
            std::make_tuple(0, coordinate_type{0.0,0.0,0.0}, pot1),
            std::make_tuple(1, coordinate_type{0.0,0.0,0.0}, pot2)
        });

    std::mt19937 rng(123456789);
    std::uniform_real_distribution<real_type> uni(-1.0, 1.0);

    for(std::size_t trial = 0; trial < 1000; ++trial)
    {
        system_type sys(2, boundary_type{});

        sys.at(0).mass  = 1.0;
        sys.at(1).mass  = 1.0;
        sys.at(0).rmass = 1.0;
        sys.at(1).rmass = 1.0;

        sys.at(0).position = coordinate_type( 0.0,  0.0,  0.0);
        sys.at(1).position = coordinate_type(10.0,  0.0,  0.0);
        sys.at(0).velocity = coordinate_type( 0.0,  0.0,  0.0);
        sys.at(1).velocity = coordinate_type( 0.0,  0.0,  0.0);
        sys.at(0).force    = coordinate_type( 0.0,  0.0,  0.0);
        sys.at(1).force    = coordinate_type( 0.0,  0.0,  0.0);

        sys.at(0).name  = "X";
        sys.at(1).name  = "X";
        sys.at(0).group = "NONE";
        sys.at(1).group = "NONE";

        sys.position(0) += coordinate_type(uni(rng), uni(rng), uni(rng));
        sys.position(1) += coordinate_type(uni(rng), uni(rng), uni(rng));

        const auto init = sys;
        interaction.initialize(init);

        constexpr real_type tol = 1e-3;
        constexpr real_type dr  = 1e-4;
        for(std::size_t idx=0; idx<sys.size(); ++idx)
        {
            {
                // ----------------------------------------------------------------
                // reset positions
                sys = init;

                // calc U(x-dx)
                const auto E0 = interaction.calc_energy(sys);

                mjolnir::math::X(sys.position(idx)) += dr;

                // calc F(x)
                interaction.calc_force(sys);

                mjolnir::math::X(sys.position(idx)) += dr;

                // calc U(x+dx)
                const auto E1 = interaction.calc_energy(sys);

                // central difference
                const auto dE = (E1 - E0) * 0.5;

                BOOST_TEST(-dE == dr * mjolnir::math::X(sys.force(idx)),
                           boost::test_tools::tolerance(tol));
            }
            {
                // ----------------------------------------------------------------
                // reset positions
                sys = init;

                // calc U(x-dx)
                const auto E0 = interaction.calc_energy(sys);

                mjolnir::math::Y(sys.position(idx)) += dr;

                // calc F(x)
                interaction.calc_force(sys);

                mjolnir::math::Y(sys.position(idx)) += dr;

                // calc U(x+dx)
                const auto E1 = interaction.calc_energy(sys);

                // central difference
                const auto dE = (E1 - E0) * 0.5;

                BOOST_TEST(-dE == dr * mjolnir::math::Y(sys.force(idx)),
                           boost::test_tools::tolerance(tol));
            }
            {
                // ----------------------------------------------------------------
                // reset positions
                sys = init;

                // calc U(x-dx)
                const auto E0 = interaction.calc_energy(sys);

                mjolnir::math::Z(sys.position(idx)) += dr;

                // calc F(x)
                interaction.calc_force(sys);

                mjolnir::math::Z(sys.position(idx)) += dr;

                // calc U(x+dx)
                const auto E1 = interaction.calc_energy(sys);

                // central difference
                const auto dE = (E1 - E0) * 0.5;

                BOOST_TEST(-dE == dr * mjolnir::math::Z(sys.force(idx)),
                           boost::test_tools::tolerance(tol));
            }
        }
    }
}

BOOST_AUTO_TEST_CASE(PositionRestraint_force_and_energy)
{
    using traits_type      = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type        = traits_type::real_type;
    using coordinate_type  = traits_type::coordinate_type;
    using boundary_type    = traits_type::boundary_type;
    using system_type      = mjolnir::System<traits_type>;
    using potential_type   = mjolnir::HarmonicPotential<real_type>;
    using interaction_type = mjolnir::PositionRestraintInteraction<traits_type, potential_type>;

    const potential_type pot1{/*k =*/1.0, /*r0 =*/ 0.0};
    const potential_type pot2{/*k =*/1.0, /*r0 =*/10.0};

    interaction_type interaction(
        std::vector<std::tuple<std::size_t, coordinate_type, potential_type>>{
            std::make_tuple(0, coordinate_type{0.0,0.0,0.0}, pot1),
            std::make_tuple(1, coordinate_type{0.0,0.0,0.0}, pot2)
        });

    std::mt19937 rng(123456789);
    std::uniform_real_distribution<real_type> uni(-1.0, 1.0);

    for(std::size_t trial = 0; trial < 1000; ++trial)
    {
        system_type sys(2, boundary_type{});

        sys.at(0).mass  = 1.0;
        sys.at(1).mass  = 1.0;
        sys.at(0).rmass = 1.0;
        sys.at(1).rmass = 1.0;

        sys.at(0).position = coordinate_type( 0.0,  0.0,  0.0);
        sys.at(1).position = coordinate_type(10.0,  0.0,  0.0);
        sys.at(0).velocity = coordinate_type( 0.0,  0.0,  0.0);
        sys.at(1).velocity = coordinate_type( 0.0,  0.0,  0.0);
        sys.at(0).force    = coordinate_type( 0.0,  0.0,  0.0);
        sys.at(1).force    = coordinate_type( 0.0,  0.0,  0.0);

        sys.at(0).name  = "X";
        sys.at(1).name  = "X";
        sys.at(0).group = "NONE";
        sys.at(1).group = "NONE";

        sys.position(0) += coordinate_type(uni(rng), uni(rng), uni(rng));
        sys.position(1) += coordinate_type(uni(rng), uni(rng), uni(rng));

        interaction.initialize(sys);
        system_type ref_sys = sys;

        constexpr real_type tol = 1e-3;

        const auto energy = interaction.calc_force_and_energy(sys);
        const auto ref_energy = interaction.calc_energy(ref_sys);
        interaction.calc_force(ref_sys);
        BOOST_TEST(ref_energy == energy, boost::test_tools::tolerance(tol));

        for(std::size_t idx=0; idx<sys.size(); ++idx)
        {
            BOOST_TEST(mjolnir::math::X(sys.force(idx)) == mjolnir::math::X(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
            BOOST_TEST(mjolnir::math::Y(sys.force(idx)) == mjolnir::math::Y(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
            BOOST_TEST(mjolnir::math::Z(sys.force(idx)) == mjolnir::math::Z(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
        }
    }
}