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

/*
 Copyright (C) 2000, 2001, 2002, 2003 RiskMap srl
 Copyright (C) 2003, 2004, 2005, 2006, 2007, 2008 StatPro Italia srl
 Copyright (C) 2007, 2008, 2009, 2015 Ferdinando Ametrano
 Copyright (C) 2007, 2009 Roland Lichters
 Copyright (C) 2015 Maddalena Zanzi
 Copyright (C) 2015 Paolo Mazzocchi
 Copyright (C) 2018 Matthias Lungwitz

 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 ratehelpers.hpp
    \brief deposit, FRA, futures, and various swap rate helpers
*/

#ifndef quantlib_ratehelpers_hpp
#define quantlib_ratehelpers_hpp

#include <ql/termstructures/bootstraphelper.hpp>
#include <ql/instruments/vanillaswap.hpp>
#include <ql/instruments/bmaswap.hpp>
#include <ql/instruments/futures.hpp>
#include <ql/time/calendar.hpp>
#include <ql/time/daycounter.hpp>
#include <ql/time/calendars/unitedstates.hpp>

#include <boost/optional.hpp>

namespace QuantLib {

    class SwapIndex;
    class Quote;

    typedef BootstrapHelper<YieldTermStructure> RateHelper;
    typedef RelativeDateBootstrapHelper<YieldTermStructure>
                                                        RelativeDateRateHelper;

    //! Rate helper for bootstrapping over IborIndex futures prices
    class FuturesRateHelper : public RateHelper {
      public:
        FuturesRateHelper(const Handle<Quote>& price,
                          const Date& iborStartDate,
                          Natural lengthInMonths,
                          const Calendar& calendar,
                          BusinessDayConvention convention,
                          bool endOfMonth,
                          const DayCounter& dayCounter,
                          Handle<Quote> convexityAdjustment = Handle<Quote>(),
                          Futures::Type type = Futures::IMM);
        FuturesRateHelper(Real price,
                          const Date& iborStartDate,
                          Natural lengthInMonths,
                          const Calendar& calendar,
                          BusinessDayConvention convention,
                          bool endOfMonth,
                          const DayCounter& dayCounter,
                          Rate convexityAdjustment = 0.0,
                          Futures::Type type = Futures::IMM);
        FuturesRateHelper(const Handle<Quote>& price,
                          const Date& iborStartDate,
                          const Date& iborEndDate,
                          const DayCounter& dayCounter,
                          Handle<Quote> convexityAdjustment = Handle<Quote>(),
                          Futures::Type type = Futures::IMM);
        FuturesRateHelper(Real price,
                          const Date& iborStartDate,
                          const Date& endDate,
                          const DayCounter& dayCounter,
                          Rate convexityAdjustment = 0.0,
                          Futures::Type type = Futures::IMM);
        FuturesRateHelper(const Handle<Quote>& price,
                          const Date& iborStartDate,
                          const ext::shared_ptr<IborIndex>& iborIndex,
                          const Handle<Quote>& convexityAdjustment = Handle<Quote>(),
                          Futures::Type type = Futures::IMM);
        FuturesRateHelper(Real price,
                          const Date& iborStartDate,
                          const ext::shared_ptr<IborIndex>& iborIndex,
                          Rate convexityAdjustment = 0.0,
                          Futures::Type type = Futures::IMM);
        //! \name RateHelper interface
        //@{
        Real impliedQuote() const override;
        //@}
        //! \name FuturesRateHelper inspectors
        //@{
        Real convexityAdjustment() const;
        //@}
		//********************************************************************
		//Deriscope: Added inspectors
		Time const yearFraction() const { return yearFraction_; }
		//********************************************************************
        //! \name Visitability
        //@{
        void accept(AcyclicVisitor&) override;
        //@}
      private:
        Time yearFraction_;
        Handle<Quote> convAdj_;
    };

    //! Rate helper for bootstrapping over deposit rates
    class DepositRateHelper : public RelativeDateRateHelper {
      public:
        DepositRateHelper(const Handle<Quote>& rate,
                          const Period& tenor,
                          Natural fixingDays,
                          const Calendar& calendar,
                          BusinessDayConvention convention,
                          bool endOfMonth,
                          const DayCounter& dayCounter);
        DepositRateHelper(Rate rate,
                          const Period& tenor,
                          Natural fixingDays,
                          const Calendar& calendar,
                          BusinessDayConvention convention,
                          bool endOfMonth,
                          const DayCounter& dayCounter);
        DepositRateHelper(const Handle<Quote>& rate,
                          const ext::shared_ptr<IborIndex>& iborIndex);
        DepositRateHelper(Rate rate,
                          const ext::shared_ptr<IborIndex>& iborIndex);
        //! \name RateHelper interface
        //@{
        Real impliedQuote() const override;
        void setTermStructure(YieldTermStructure*) override;
		//********************************************************************
		//Deriscope: Added inspectors
		Date const & fixingDate() const { return fixingDate_; }
		ext::shared_ptr<IborIndex> const & iborIndex() const { return iborIndex_; }
		//********************************************************************
        //@}
        //! \name Visitability
        //@{
        void accept(AcyclicVisitor&) override;
        //@}
      private:
        void initializeDates() override;
        Date fixingDate_;
        ext::shared_ptr<IborIndex> iborIndex_;
        RelinkableHandle<YieldTermStructure> termStructureHandle_;
    };

    //! Rate helper for bootstrapping over %FRA rates
    class FraRateHelper : public RelativeDateRateHelper {
      public:
        FraRateHelper(const Handle<Quote>& rate,
                      Natural monthsToStart,
                      Natural monthsToEnd,
                      Natural fixingDays,
                      const Calendar& calendar,
                      BusinessDayConvention convention,
                      bool endOfMonth,
                      const DayCounter& dayCounter,
                      Pillar::Choice pillar = Pillar::LastRelevantDate,
                      Date customPillarDate = Date(),
                      bool useIndexedCoupon = true);
        FraRateHelper(Rate rate,
                      Natural monthsToStart,
                      Natural monthsToEnd,
                      Natural fixingDays,
                      const Calendar& calendar,
                      BusinessDayConvention convention,
                      bool endOfMonth,
                      const DayCounter& dayCounter,
                      Pillar::Choice pillar = Pillar::LastRelevantDate,
                      Date customPillarDate = Date(),
                      bool useIndexedCoupon = true);
        FraRateHelper(const Handle<Quote>& rate,
                      Natural monthsToStart,
                      const ext::shared_ptr<IborIndex>& iborIndex,
                      Pillar::Choice pillar = Pillar::LastRelevantDate,
                      Date customPillarDate = Date(),
                      bool useIndexedCoupon = true);
        FraRateHelper(Rate rate,
                      Natural monthsToStart,
                      const ext::shared_ptr<IborIndex>& iborIndex,
                      Pillar::Choice pillar = Pillar::LastRelevantDate,
                      Date customPillarDate = Date(),
                      bool useIndexedCoupon = true);
        FraRateHelper(const Handle<Quote>& rate,
                      Period periodToStart,
                      Natural lengthInMonths,
                      Natural fixingDays,
                      const Calendar& calendar,
                      BusinessDayConvention convention,
                      bool endOfMonth,
                      const DayCounter& dayCounter,
                      Pillar::Choice pillar = Pillar::LastRelevantDate,
                      Date customPillarDate = Date(),
                      bool useIndexedCoupon = true);
        FraRateHelper(Rate rate,
                      Period periodToStart,
                      Natural lengthInMonths,
                      Natural fixingDays,
                      const Calendar& calendar,
                      BusinessDayConvention convention,
                      bool endOfMonth,
                      const DayCounter& dayCounter,
                      Pillar::Choice pillar = Pillar::LastRelevantDate,
                      Date customPillarDate = Date(),
                      bool useIndexedCoupon = true);
        FraRateHelper(const Handle<Quote>& rate,
                      Period periodToStart,
                      const ext::shared_ptr<IborIndex>& iborIndex,
                      Pillar::Choice pillar = Pillar::LastRelevantDate,
                      Date customPillarDate = Date(),
                      bool useIndexedCoupon = true);
        FraRateHelper(Rate rate,
                      Period periodToStart,
                      const ext::shared_ptr<IborIndex>& iborIndex,
                      Pillar::Choice pillar = Pillar::LastRelevantDate,
                      Date customPillarDate = Date(),
                      bool useIndexedCoupon = true);
        FraRateHelper(const Handle<Quote>& rate,
                      Natural immOffsetStart,
                      Natural immOffsetEnd,
                      const ext::shared_ptr<IborIndex>& iborIndex,
                      Pillar::Choice pillar = Pillar::LastRelevantDate,
                      Date customPillarDate = Date(),
                      bool useIndexedCoupon = true);
        FraRateHelper(Rate rate,
                      Natural immOffsetStart,
                      Natural immOffsetEnd,
                      const ext::shared_ptr<IborIndex>& iborIndex,
                      Pillar::Choice pillar = Pillar::LastRelevantDate,
                      Date customPillarDate = Date(),
                      bool useIndexedCoupon = true);
        //! \name RateHelper interface
        //@{
        Real impliedQuote() const override;
        void setTermStructure(YieldTermStructure*) override;
        //@}
		//********************************************************************
		//Deriscope: Added inspectors
		Date const & fixingDate() const { return fixingDate_; }
		boost::optional<Period> const & periodToStart() const { return periodToStart_; }
		ext::shared_ptr<IborIndex> const & iborIndex() const { return iborIndex_; }
		boost::optional<Natural> const immOffsetStart() const { return immOffsetStart_; }
		boost::optional<Natural> const immOffsetEnd() const { return immOffsetEnd_; }
		Pillar::Choice const pillarChoice() const { return pillarChoice_; }
		bool const useIndexedCoupon() const { return useIndexedCoupon_; }
		Real const spanningTime() const { return spanningTime_; }
		//********************************************************************
        //! \name Visitability
        //@{
        void accept(AcyclicVisitor&) override;
        //@}
      private:
        void initializeDates() override;
        Date fixingDate_;
        boost::optional<Period> periodToStart_;
        boost::optional<Natural> immOffsetStart_, immOffsetEnd_;
        Pillar::Choice pillarChoice_;
        ext::shared_ptr<IborIndex> iborIndex_;
        RelinkableHandle<YieldTermStructure> termStructureHandle_;
        bool useIndexedCoupon_;
        Real spanningTime_;
    };

    //! Rate helper for bootstrapping over swap rates
    /*! \todo use input SwapIndex to create the swap */
    class SwapRateHelper : public RelativeDateRateHelper {
      public:
        SwapRateHelper(const Handle<Quote>& rate,
                       const ext::shared_ptr<SwapIndex>& swapIndex,
                       Handle<Quote> spread = Handle<Quote>(),
                       const Period& fwdStart = 0 * Days,
                       // exogenous discounting curve
                       Handle<YieldTermStructure> discountingCurve = Handle<YieldTermStructure>(),
                       Pillar::Choice pillar = Pillar::LastRelevantDate,
                       Date customPillarDate = Date(),
                       bool endOfMonth = false);
        SwapRateHelper(const Handle<Quote>& rate,
                       const Period& tenor,
                       Calendar calendar,
                       // fixed leg
                       Frequency fixedFrequency,
                       BusinessDayConvention fixedConvention,
                       DayCounter fixedDayCount,
                       // floating leg
                       const ext::shared_ptr<IborIndex>& iborIndex,
                       Handle<Quote> spread = Handle<Quote>(),
                       const Period& fwdStart = 0 * Days,
                       // exogenous discounting curve
                       Handle<YieldTermStructure> discountingCurve = Handle<YieldTermStructure>(),
                       Natural settlementDays = Null<Natural>(),
                       Pillar::Choice pillar = Pillar::LastRelevantDate,
                       Date customPillarDate = Date(),
                       bool endOfMonth = false);
        SwapRateHelper(Rate rate,
                       const ext::shared_ptr<SwapIndex>& swapIndex,
                       Handle<Quote> spread = Handle<Quote>(),
                       const Period& fwdStart = 0 * Days,
                       // exogenous discounting curve
                       Handle<YieldTermStructure> discountingCurve = Handle<YieldTermStructure>(),
                       Pillar::Choice pillar = Pillar::LastRelevantDate,
                       Date customPillarDate = Date(),
                       bool endOfMonth = false);
        SwapRateHelper(Rate rate,
                       const Period& tenor,
                       Calendar calendar,
                       // fixed leg
                       Frequency fixedFrequency,
                       BusinessDayConvention fixedConvention,
                       DayCounter fixedDayCount,
                       // floating leg
                       const ext::shared_ptr<IborIndex>& iborIndex,
                       Handle<Quote> spread = Handle<Quote>(),
                       const Period& fwdStart = 0 * Days,
                       // exogenous discounting curve
                       Handle<YieldTermStructure> discountingCurve = Handle<YieldTermStructure>(),
                       Natural settlementDays = Null<Natural>(),
                       Pillar::Choice pillar = Pillar::LastRelevantDate,
                       Date customPillarDate = Date(),
                       bool endOfMonth = false);
        //! \name RateHelper interface
        //@{
        Real impliedQuote() const override;
        void setTermStructure(YieldTermStructure*) override;
        //@}
        //! \name SwapRateHelper inspectors
        //@{
        Spread spread() const;
        ext::shared_ptr<VanillaSwap> swap() const;
        const Period& forwardStart() const;
        //@}
        //! \name Visitability
        //@{
        void accept(AcyclicVisitor&) override;
        //@}
      protected:
        void initializeDates() override;
        Natural settlementDays_;
        Period tenor_;
        Pillar::Choice pillarChoice_;
        Calendar calendar_;
        BusinessDayConvention fixedConvention_;
        Frequency fixedFrequency_;
        DayCounter fixedDayCount_;
        ext::shared_ptr<IborIndex> iborIndex_;
        ext::shared_ptr<VanillaSwap> swap_;
        RelinkableHandle<YieldTermStructure> termStructureHandle_;
        Handle<Quote> spread_;
        bool endOfMonth_;
        Period fwdStart_;
        Handle<YieldTermStructure> discountHandle_;
        RelinkableHandle<YieldTermStructure> discountRelinkableHandle_;
    };

    //! Rate helper for bootstrapping over BMA swap rates
    class BMASwapRateHelper : public RelativeDateRateHelper {
      public:
        BMASwapRateHelper(const Handle<Quote>& liborFraction,
                          const Period& tenor, // swap maturity
                          Natural settlementDays,
                          Calendar calendar,
                          // bma leg
                          const Period& bmaPeriod,
                          BusinessDayConvention bmaConvention,
                          DayCounter bmaDayCount,
                          ext::shared_ptr<BMAIndex> bmaIndex,
                          // ibor leg
                          ext::shared_ptr<IborIndex> index);
        //! \name RateHelper interface
        //@{
        Real impliedQuote() const override;
        void setTermStructure(YieldTermStructure*) override;
		//********************************************************************
		//Deriscope: Added inspectors
		Period const & tenor() const { return tenor_; }
		Natural const settlementDays() const { return settlementDays_; }
		Calendar const & calendar() const { return calendar_; }
		Period const & bmaPeriod() const { return bmaPeriod_; }
		BusinessDayConvention const bmaConvention() const { return bmaConvention_; }
		DayCounter const & bmaDayCount() const { return bmaDayCount_; }
		ext::shared_ptr<BMAIndex> const & bmaIndex() const { return bmaIndex_; }
		ext::shared_ptr<IborIndex> const & iborIndex() const { return iborIndex_; }
		ext::shared_ptr<BMASwap> const & swap() const { return swap_; }
		//********************************************************************
        //@}
        //! \name Visitability
        //@{
        void accept(AcyclicVisitor&) override;
        //@}
    protected:
      void initializeDates() override;
      Period tenor_;
      Natural settlementDays_;
      Calendar calendar_;
      Period bmaPeriod_;
      BusinessDayConvention bmaConvention_;
      DayCounter bmaDayCount_;
      ext::shared_ptr<BMAIndex> bmaIndex_;
      ext::shared_ptr<IborIndex> iborIndex_;

      ext::shared_ptr<BMASwap> swap_;
      RelinkableHandle<YieldTermStructure> termStructureHandle_;
    };

    //! Rate helper for bootstrapping over Fx Swap rates
    /*! The forward is given by `fwdFx = spotFx + fwdPoint`.

        `isFxBaseCurrencyCollateralCurrency` indicates if the base
        currency of the FX currency pair is the one used as collateral.

        `calendar` is usually the joint calendar of the two currencies
        in the pair.

        `tradingCalendar` can be used when the cross pairs don't
        include the currency of the business center (usually USD; the
        corresponding calendar is `UnitedStates`).  If given, it will
        be used for adjusting the earliest settlement date and for
        setting the latest date. Due to FX spot market conventions, it
        is not sufficient to pass a JointCalendar with UnitedStates
        included as `calendar`; with regard the earliest date, this
        calendar is only used in case the spot date of the two
        currencies is not a US business day.

        \warning The ON fx swaps can be achieved by setting
                 `fixingDays` to 0 and using a tenor of '1d'. The same
                 tenor should be used for TN swaps, with `fixingDays`
                 set to 1.  However, handling ON and TN swaps for
                 cross rates without USD is not trivial and should be
                 treated with caution. If today is a US holiday, ON
                 trade is not possible. If tomorrow is a US Holiday,
                 the ON trade will be at least two business days long
                 in the other countries and the TN trade will not
                 exist. In such cases, if this helper is used for
                 curve construction, probably it is safer not to pass
                 a trading calendar to the ON and TN helpers and
                 provide fwdPoints that will yield proper level of
                 discount factors.
    */
    class FxSwapRateHelper : public RelativeDateRateHelper {
      public:
        FxSwapRateHelper(const Handle<Quote>& fwdPoint,
                         Handle<Quote> spotFx,
                         const Period& tenor,
                         Natural fixingDays,
                         Calendar calendar,
                         BusinessDayConvention convention,
                         bool endOfMonth,
                         bool isFxBaseCurrencyCollateralCurrency,
                         Handle<YieldTermStructure> collateralCurve,
                         Calendar tradingCalendar = Calendar());
        //! \name RateHelper interface
        //@{
        Real impliedQuote() const override;
        void setTermStructure(YieldTermStructure*) override;
        //@}
        //! \name FxSwapRateHelper inspectors
        //@{
        Real spot() const { return spot_->value(); }
        Period tenor() const { return tenor_; }
        Natural fixingDays() const { return fixingDays_; }
        Calendar calendar() const { return cal_; }
        BusinessDayConvention businessDayConvention() const { return conv_; }
        bool endOfMonth() const { return eom_; }
        bool isFxBaseCurrencyCollateralCurrency() const {
                                return isFxBaseCurrencyCollateralCurrency_; }
        Calendar tradingCalendar() const { return tradingCalendar_; }
        Calendar adjustmentCalendar() const { return jointCalendar_; }
        //@}
        //! \name Visitability
        //@{
        void accept(AcyclicVisitor&) override;
        //@}
    private:
      void initializeDates() override;
      Handle<Quote> spot_;
      Period tenor_;
      Natural fixingDays_;
      Calendar cal_;
      BusinessDayConvention conv_;
      bool eom_;
      bool isFxBaseCurrencyCollateralCurrency_;

      RelinkableHandle<YieldTermStructure> termStructureHandle_;

      Handle<YieldTermStructure> collHandle_;
      RelinkableHandle<YieldTermStructure> collRelinkableHandle_;

      Calendar tradingCalendar_;
      Calendar jointCalendar_;
    };

    // inline

    inline Spread SwapRateHelper::spread() const {
        return spread_.empty() ? 0.0 : spread_->value();
    }

    inline ext::shared_ptr<VanillaSwap> SwapRateHelper::swap() const {
        return swap_;
    }

    inline const Period& SwapRateHelper::forwardStart() const {
        return fwdStart_;
    }

}

#endif


/*
 system.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 <iomanip>
#include <fstream>
#include "system.h"
#include "constants.h"
#include "mathfunctions.h"
#include "timer.h"
#include "memory.h"
#include "error.h"
#include "constraint.h"
#include "fcs.h"
#include "symmetry.h"
#include "fitting.h"
#include "xml_parser.h"
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/foreach.hpp>
#include <boost/optional.hpp>
#include <boost/lexical_cast.hpp>

using namespace ALM_NS;

System::System(ALM *alm): Pointers(alm) {}

System::~System()
{
    memory->deallocate(x_cartesian);
    memory->deallocate(atomlist_class);
    memory->deallocate(magmom);
}

void System::init()
{
    using namespace std;

    int i, j;

    cout << " SYSTEM" << endl;
    cout << " ======" << endl << endl;

    recips(lavec, rlavec);

    cout.setf(ios::scientific);

    cout << "  Lattice Vector" << endl;
    cout << setw(16) << lavec[0][0] << setw(15) << lavec[1][0] << setw(15) << lavec[2][0] << " : a1" << endl;
    cout << setw(16) << lavec[0][1] << setw(15) << lavec[1][1] << setw(15) << lavec[2][1] << " : a2" << endl;
    cout << setw(16) << lavec[0][2] << setw(15) << lavec[1][2] << setw(15) << lavec[2][2] << " : a3" << endl;
    cout << endl;

    double vec_tmp[3][3];
    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            vec_tmp[i][j] = lavec[j][i];
        }
    }

    cell_volume = volume(vec_tmp[0], vec_tmp[1], vec_tmp[2]);
    cout << "  Cell volume = " << cell_volume << " (a.u)^3" << endl << endl;

    cout << "  Reciprocal Lattice Vector" << std::endl;
    cout << setw(16) << rlavec[0][0] << setw(15) << rlavec[0][1] << setw(15) << rlavec[0][2] << " : b1" << endl;
    cout << setw(16) << rlavec[1][0] << setw(15) << rlavec[1][1] << setw(15) << rlavec[1][2] << " : b2" << endl;
    cout << setw(16) << rlavec[2][0] << setw(15) << rlavec[2][1] << setw(15) << rlavec[2][2] << " : b3" << endl;
    cout << endl;

    cout << "  Atomic species:" << endl;
    for (i = 0; i < nkd; ++i) {
        cout << setw(6) << i + 1 << setw(5) << kdname[i] << endl;
    }
    cout << endl;

    cout << "  Atomic positions in fractional basis and atomic species" << endl;
    for (i = 0; i < nat; ++i) {
        cout << setw(6) << i + 1;
        cout << setw(15) << xcoord[i][0];
        cout << setw(15) << xcoord[i][1];
        cout << setw(15) << xcoord[i][2];
        cout << setw(5) << kd[i] << endl;
    }
    cout << endl << endl;
    cout.unsetf(ios::scientific);

    // Generate Cartesian coordinate

    memory->allocate(x_cartesian, nat, 3);

    for (i = 0; i < nat; ++i) {
        for (j = 0; j < 3; ++j) {
            x_cartesian[i][j] = xcoord[i][j];
        }
    }
    frac2cart(x_cartesian);
    setup_atomic_class(kd);

    if (lspin) {
        cout << "  MAGMOM is given. The magnetic moments of each atom are as follows:" << endl;
        for (i = 0; i < nat; ++i) {
            cout << setw(6) << i + 1;
            cout << setw(5) << magmom[i][0];
            cout << setw(5) << magmom[i][1];
            cout << setw(5) << magmom[i][2];
            cout << endl;
        }
        cout << endl;
        if (noncollinear == 0) {
            cout << "  NONCOLLINEAR = 0: magnetic moments are considered as scalar variables." << endl;
        } else if (noncollinear == 1) {
            cout << "  NONCOLLINEAR = 1: magnetic moments are considered as vector variables." << endl;
            if (symmetry->trev_sym_mag) {
                cout << "  TREVSYM = 1: Time-reversal symmetry will be considered for generating magnetic space group" << endl;
            } else {
                cout << "  TREVSYM = 0: Time-reversal symmetry will NOT be considered for generating magnetic space group" << endl;
            }
        }
        cout << endl << endl;
    }

    timer->print_elapsed();
    cout << " --------------------------------------------------------------" << endl;
    cout << endl;
}

void System::recips(double aa[3][3], double bb[3][3])
{
    /*
    Calculate Reciprocal Lattice Vectors

    Here, BB is just the inverse matrix of AA (multiplied by factor 2 Pi)

    BB = 2 Pi AA^{-1},
    = t(b1, b2, b3)

    (b11 b12 b13)
    = (b21 b22 b23)
    (b31 b32 b33),

    b1 = t(b11, b12, b13) etc.
    */

    double det;
    det = aa[0][0] * aa[1][1] * aa[2][2]
    + aa[1][0] * aa[2][1] * aa[0][2]
    + aa[2][0] * aa[0][1] * aa[1][2]
    - aa[0][0] * aa[2][1] * aa[1][2]
    - aa[2][0] * aa[1][1] * aa[0][2]
    - aa[1][0] * aa[0][1] * aa[2][2];

    if (std::abs(det) < eps12) {
        error->exit("recips", "Lattice Vector is singular");
    }

    double factor = 2.0 * pi / det;

    bb[0][0] = (aa[1][1] * aa[2][2] - aa[1][2] * aa[2][1]) * factor;
    bb[0][1] = (aa[0][2] * aa[2][1] - aa[0][1] * aa[2][2]) * factor;
    bb[0][2] = (aa[0][1] * aa[1][2] - aa[0][2] * aa[1][1]) * factor;

    bb[1][0] = (aa[1][2] * aa[2][0] - aa[1][0] * aa[2][2]) * factor;
    bb[1][1] = (aa[0][0] * aa[2][2] - aa[0][2] * aa[2][0]) * factor;
    bb[1][2] = (aa[0][2] * aa[1][0] - aa[0][0] * aa[1][2]) * factor;

    bb[2][0] = (aa[1][0] * aa[2][1] - aa[1][1] * aa[2][0]) * factor;
    bb[2][1] = (aa[0][1] * aa[2][0] - aa[0][0] * aa[2][1]) * factor;
    bb[2][2] = (aa[0][0] * aa[1][1] - aa[0][1] * aa[1][0]) * factor;
}

void System::frac2cart(double **xf)
{
    // x_cartesian = A x_fractional

    int i, j;

    double *x_tmp;
    memory->allocate(x_tmp, 3);

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

        rotvec(x_tmp, xf[i], lavec);

        for (j = 0; j < 3; ++j) {
            xf[i][j] = x_tmp[j];
        }
    }
    memory->deallocate(x_tmp);
}

void System::load_reference_system_xml(std::string file_reference_fcs, const int order_fcs, double *const_out)
{
    using namespace boost::property_tree;
    ptree pt;

    int nat_ref, natmin_ref, ntran_ref;
    int **intpair_ref;
    std::string str_error;
    double *fcs_ref;
    int nfcs_ref;

    try {
        read_xml(file_reference_fcs, pt);
    }
    catch (std::exception &e) {
        if (order_fcs == 0) {
            str_error = "Cannot open file FC2XML ( " + file_reference_fcs + " )";
        } else if (order_fcs == 1) {
            str_error = "Cannot open file FC3XML ( " + file_reference_fcs + " )";
        }
        error->exit("load_reference_system_xml", str_error.c_str());
    }

    nat_ref = boost::lexical_cast<int>(get_value_from_xml(pt, "Data.Structure.NumberOfAtoms"));
    ntran_ref = boost::lexical_cast<int>(get_value_from_xml(pt, "Data.Symmetry.NumberOfTranslations"));
    natmin_ref = nat_ref / ntran_ref;

    if (natmin_ref != symmetry->natmin) {
        error->exit("load_reference_system_xml", "The number of atoms in the primitive cell is not consistent.");
    }

    if (order_fcs == 0) {
        nfcs_ref = boost::lexical_cast<int>(get_value_from_xml(pt, "Data.ForceConstants.HarmonicUnique.NFC2"));

        if (nfcs_ref != fcs->ndup[0].size()) {
            error->exit("load_reference_system_xml", "The number of harmonic force constants is not the same.");
        }

    } else if (order_fcs == 1) {
        nfcs_ref = boost::lexical_cast<int>(get_value_from_xml(pt, "Data.ForceConstants.CubicUnique.NFC3"));

        if (nfcs_ref != fcs->ndup[1].size()) {
            error->exit("load_reference_system_xml", "The number of cubic force constants is not the same.");
        }
    }
    memory->allocate(fcs_ref, nfcs_ref);
    memory->allocate(intpair_ref, nfcs_ref, 3);

    int counter = 0;

    if (order_fcs == 0) {
        BOOST_FOREACH (const ptree::value_type& child_, pt.get_child("Data.ForceConstants.HarmonicUnique")) {
            if (child_.first == "FC2") {
                const ptree& child = child_.second;
                const std::string str_intpair = child.get<std::string>("<xmlattr>.pairs");
                const std::string str_multiplicity = child.get<std::string>("<xmlattr>.multiplicity");

                std::istringstream is(str_intpair);
                is >> intpair_ref[counter][0] >> intpair_ref[counter][1];
                fcs_ref[counter] = boost::lexical_cast<double>(child.data());
                ++counter;
            }
        }
    } else if (order_fcs == 1) {
        BOOST_FOREACH (const ptree::value_type& child_, pt.get_child("Data.ForceConstants.CubicUnique")) {
            if (child_.first == "FC3") {
                const ptree& child = child_.second;
                const std::string str_intpair = child.get<std::string>("<xmlattr>.pairs");
                const std::string str_multiplicity = child.get<std::string>("<xmlattr>.multiplicity");

                std::istringstream is(str_intpair);
                is >> intpair_ref[counter][0] >> intpair_ref[counter][1] >> intpair_ref[counter][2];
                fcs_ref[counter] = boost::lexical_cast<double>(child.data());
                ++counter;
            }
        }
    }

    int i;
    std::set<FcProperty> list_found;
    std::set<FcProperty>::iterator iter_found;
    int *ind;
    int nterms = order_fcs + 2;
    memory->allocate(ind, nterms);

    list_found.clear();

    for (std::vector<FcProperty>::iterator p = fcs->fc_set[order_fcs].begin();
        p != fcs->fc_set[order_fcs].end(); ++p) {
            FcProperty list_tmp = *p; // Using copy constructor
            for (i = 0; i < nterms; ++i) {
                ind[i] = list_tmp.elems[i];
            }
            list_found.insert(FcProperty(nterms, list_tmp.coef, ind, list_tmp.mother));
    }
    //
    //     for (i = 0; i < nfcs_ref; ++i) {
    //         constraint->const_mat[i][i] = 1.0;
    //     }

    for (i = 0; i < nfcs_ref; ++i) {
        iter_found = list_found.find(FcProperty(nterms, 1.0, intpair_ref[i], 1));
        if (iter_found == list_found.end()) {
            error->exit("load_reference_system", "Cannot find equivalent force constant, number: ", i + 1);
        }
        FcProperty arrtmp = *iter_found;
        const_out[arrtmp.mother] = fcs_ref[i];
    }

    memory->deallocate(intpair_ref);
    memory->deallocate(fcs_ref);
    memory->deallocate(ind);
    list_found.clear();
}

void System::load_reference_system()
{
    int i;
    int iat, jat;
    int icrd;

    unsigned int nat_s, nkd_s;
    unsigned int natmin_ref, ntran_ref;
    double lavec_s[3][3];
    int *kd_s;
    double **xcoord_s;
    int *map_ref;
    int **map_p2s_s;
    Symmetry::Maps *map_s2p_s;
    std::ifstream ifs_fc2;

    ifs_fc2.open(constraint->fc2_file.c_str(), std::ios::in);
    if(!ifs_fc2) error->exit("calc_constraint_matrix", "cannot open file fc2_file");

    bool is_found_system = false;

    int nparam_harmonic_ref;
    int nparam_harmonic = fcs->ndup[0].size();

    std::string str_tmp;

    while(!ifs_fc2.eof() && !is_found_system)
    {
        std::getline(ifs_fc2, str_tmp);
        if (str_tmp == "##SYSTEM INFO") {

            is_found_system = true;

            std::getline(ifs_fc2, str_tmp);
            for (i = 0; i < 3; ++i) {
                ifs_fc2 >> lavec_s[0][i] >> lavec_s[1][i] >> lavec_s[2][i];
            }
            ifs_fc2.ignore();
            std::getline(ifs_fc2, str_tmp);
            ifs_fc2 >> nkd_s;
            ifs_fc2.ignore();
            std::getline(ifs_fc2, str_tmp);
            std::getline(ifs_fc2, str_tmp);

            ifs_fc2 >> nat_s >> natmin_ref >> ntran_ref;

            if (natmin_ref != symmetry->natmin) {
                error->exit("load_reference_system", "The number of atoms in the primitive cell is not consistent");
            }

            if (nat_s != nat) {
                std::cout << "The number of atoms in the reference system differs from input." << std::endl;
                std::cout << "Trying to map the related force constants (^o^)" << std::endl << std::endl;
            }

            memory->allocate(xcoord_s, nat_s, 3);
            memory->allocate(kd_s, nat_s);
            memory->allocate(map_p2s_s, natmin_ref, ntran_ref);
            memory->allocate(map_s2p_s, nat_s);

            unsigned int ikd, itran, icell;
            std::getline(ifs_fc2, str_tmp);
            std::getline(ifs_fc2, str_tmp);
            for (i = 0; i < nat_s; ++i) {
                ifs_fc2 >> str_tmp >> ikd >> xcoord_s[i][0] >> xcoord_s[i][1] >> xcoord_s[i][2] >> itran >> icell;
                kd_s[i] = ikd;
                map_p2s_s[icell - 1][itran - 1] = i;
                map_s2p_s[i].atom_num = icell - 1;
                map_s2p_s[i].tran_num = itran - 1;
            }
        }
    }
    if (!is_found_system) error->exit("load_reference_system", "SYSTEM INFO flag not found in the fc2_file");

    //
    // Generate Mapping Information (big supercell -> small supercell)
    //

    double *xtmp;
    double *xdiff;
    int **intpair_tmp;

    memory->allocate(xtmp, 3);
    memory->allocate(xdiff, 3);
    memory->allocate(map_ref, nat_s);

    bool map_found;
    double dist;

    for (iat = 0; iat < nat_s; ++iat) {
        map_found = false;

        rotvec(xtmp, xcoord_s[iat], lavec_s);
        rotvec(xtmp, xtmp, rlavec);

        for (icrd = 0; icrd < 3; ++icrd) xtmp[icrd] /= 2.0 * pi;

        for (jat = 0; jat < nat; ++jat) {
            for (icrd = 0; icrd < 3; ++icrd) {
                xdiff[icrd] = xtmp[icrd] - xcoord[jat][icrd];
                xdiff[icrd] = std::fmod(xdiff[icrd], 1.0);
            }
            dist = xdiff[0] * xdiff[0] + xdiff[1] * xdiff[1] + xdiff[2] * xdiff[2];

            if (dist < eps12 && kd_s[iat] == kd[jat]) {
                map_ref[iat] = jat;
                map_found = true;
                break;
            }
        }
        if (!map_found) error->exit("load_reference_system", "Could not find an equivalent atom for atom ", iat + 1);
    }

    memory->deallocate(xtmp);
    memory->deallocate(xdiff);
    memory->deallocate(xcoord_s);
    memory->deallocate(kd_s);

    ifs_fc2.clear();
    ifs_fc2.seekg(0, std::ios_base::beg);

    double *fc2_ref;

    bool is_found_fc2 = false;

    while(!ifs_fc2.eof() && !is_found_fc2)
    {
        std::getline(ifs_fc2, str_tmp);
        if (str_tmp == "##HARMONIC FORCE CONSTANTS")
        {
            ifs_fc2 >> nparam_harmonic_ref;
            if (nparam_harmonic_ref < nparam_harmonic) {
                error->exit("load_reference_system", "Reference file doesn't contain necessary fc2. (too few)");
            } else if (nparam_harmonic_ref > nparam_harmonic){
                error->exit("load_reference_system","Reference file contains extra force constants." );
            }

            is_found_fc2 = true;

            memory->allocate(fc2_ref, nparam_harmonic);
            memory->allocate(intpair_tmp, nparam_harmonic, 2);

            for (i = 0; i < nparam_harmonic; ++i) {
                ifs_fc2 >> fc2_ref[i] >> intpair_tmp[i][0] >> intpair_tmp[i][1];
            }

            std::set<FcProperty> list_found;
            std::set<FcProperty>::iterator iter_found;
            int *ind;
            memory->allocate(ind, 2);

            list_found.clear();
            for (std::vector<FcProperty>::iterator p = fcs->fc_set[0].begin(); p != fcs->fc_set[0].end(); ++p) {
                FcProperty list_tmp = *p; // Using copy constructor
                for (i = 0; i < 2; ++i){
                    ind[i] = list_tmp.elems[i];
                }
                list_found.insert(FcProperty(2, list_tmp.coef, ind, list_tmp.mother));
            }

            for (i = 0; i < nparam_harmonic; ++i) {
                constraint->const_mat[i][i] = 1.0;
            }

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

                iter_found = list_found.find(FcProperty(2, 1.0, intpair_tmp[i], 1));
                if (iter_found == list_found.end()) {
                    error->exit("load_reference_system", "Cannot find equivalent force constant, number: ", i + 1);
                }
                FcProperty arrtmp = *iter_found;
                constraint->const_rhs[arrtmp.mother] = fc2_ref[i];
            }

            memory->deallocate(intpair_tmp);
            memory->deallocate(ind);
            memory->deallocate(fc2_ref);
            list_found.clear();
        }
    }

    if(!is_found_fc2) error->exit("load_reference_system", "HARMONIC FORCE CONSTANTS flag not found in the fc2_file");
    ifs_fc2.close();
}

double System::volume(double vec1[3], double vec2[3], double vec3[3])
{
    double vol;

    vol = std::abs(vec1[0]*(vec2[1]*vec3[2] - vec2[2]*vec3[1])
        + vec1[1]*(vec2[2]*vec3[0] - vec2[0]*vec3[2])
        + vec1[2]*(vec2[0]*vec3[1] - vec2[1]*vec3[0]));

    return vol;
}

void System::setup_atomic_class(int *kd)
{
    // In the case of collinear calculation, spin moments are considered as scalar
    // variables. Therefore, the same elements with different magnetic moments are
    // considered as different types. In noncollinear calculations,
    // magnetic moments are not considered in this stage. They will be treated
    // separately in symmetry.cpp where spin moments will be rotated and flipped
    // using time-reversal symmetry.

    unsigned int i;
    AtomType type_tmp;
    std::set<AtomType> set_type;
    set_type.clear();

    for (i = 0; i < nat; ++i) {
        type_tmp.element = kd[i];

        if (noncollinear == 0) {
            type_tmp.magmom = magmom[i][2];
        } else {
            type_tmp.magmom = 0.0;
        }
        set_type.insert(type_tmp);
    }

    nclassatom = set_type.size();

    memory->allocate(atomlist_class, nclassatom);

    for (i = 0; i < nat; ++i) {
        int count = 0;
        for (std::set<AtomType>::iterator it = set_type.begin(); it != set_type.end(); ++it) {
            if (noncollinear) {
                if (kd[i] == (*it).element) {
                    atomlist_class[count].push_back(i);
                }
            } else {
                if (kd[i] == (*it).element && std::abs(magmom[i][2] - (*it).magmom) < eps6) {
                    atomlist_class[count].push_back(i);
                }
            }
            ++count;
        }
    }
    set_type.clear();
}


// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ASTRONOMY_COORDINATE_SKY_POINT_HPP
#define BOOST_ASTRONOMY_COORDINATE_SKY_POINT_HPP

#include <string>
#include <type_traits>
#include <cmath>

#include <boost/units/io.hpp>
#include <boost/static_assert.hpp>

#include <boost/astronomy/coordinate/frame.hpp>
#include <boost/astronomy/detail/is_base_template_of.hpp>

namespace boost { namespace astronomy { namespace coordinate {

namespace bu = boost::units;
namespace bg = boost::geometry;

/*!sky_point is used to represent a point(coordinate) in the sky*/
template <typename CoordinateSystem>
struct sky_point
{
    ///@cond INTERNAL
    BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_frame_of
            <boost::astronomy::coordinate::base_frame, CoordinateSystem>::value),
            "Template argument is expected to be a frame class");
    ///@endcond

protected:
    CoordinateSystem point;

public:
    typedef CoordinateSystem system;

    //constructors

    //!default constructor
    sky_point() {}

    //!create point with the given coordinates
    sky_point(CoordinateSystem const& object) : point(object) {}

    //!create point with providing representation and differential class object
    template <typename Representation, typename Differential>
    sky_point
    (
        Representation const& representation_data,
        Differential const& differential_data
    )
    {
        BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
            <boost::astronomy::coordinate::base_representation, Representation>::value),
            "argument type is expected to be a representation class");

        BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
            <boost::astronomy::coordinate::base_differential, Differential>::value),
            "argument type is expected to be a differential class");

        point = CoordinateSystem(representation_data, differential_data);
    }

    //!create point with direct values of representation and differential
    sky_point
    (
        typename CoordinateSystem::representation::quantity1 const& lat,
        typename CoordinateSystem::representation::quantity2 const& lon,
        typename CoordinateSystem::representation::quantity3 const& distance,
        typename CoordinateSystem::differential::quantity1 const& pm_lat,
        typename CoordinateSystem::differential::quantity2 const& pm_lon_coslat,
        typename CoordinateSystem::differential::quantity3 const& radian_velocity
    )
    {
        point = CoordinateSystem(lat, lon, distance, pm_lat, pm_lon_coslat,
                    radian_velocity);
    }

    template<class OtherCoordinateSystem>
    sky_point(sky_point<OtherCoordinateSystem> const& object);

    //!constructing from direct value of representation
    sky_point
    (
        typename CoordinateSystem::representation::quantity1 const& lat,
        typename CoordinateSystem::representation::quantity2 const& lon,
        typename CoordinateSystem::representation::quantity3 const& distance
    )
    {
        point = CoordinateSystem(lat, lon, distance);
    }

    //constructing from name of object if available in the calatoge
    sky_point(std::string const& name);

    std::string get_constillation();

    sky_point<CoordinateSystem> from_name(std::string const& name);

    //!angular separation between two coordinates in radians
    bu::quantity<bu::si::plane_angle> separation(sky_point<CoordinateSystem> const&
        object) const
    {
        return this->point.get_angular_separation(object.get_point());
    }

    //!returns positional angle in the radian
    bu::quantity<bu::si::plane_angle> positional_angle(sky_point<CoordinateSystem>
        const& object) const
    {
        auto p1 = make_spherical_representation(this->point.get_data());
        auto p2 = make_spherical_representation(object.get_point().get_data());

        auto diff = p2.get_lon() - p1.get_lon();

        double temp_p1 = static_cast<bu::quantity<bu::si::plane_angle,
            typename CoordinateSystem::representation::type>>(p1.get_lat()).value();
        double temp_p2 = static_cast<bu::quantity<bu::si::plane_angle,
            typename CoordinateSystem::representation::type>>(p2.get_lat()).value();
        double temp_diff = static_cast<bu::quantity<bu::si::plane_angle,
            typename CoordinateSystem::representation::type>>(diff).value();

        double coslat = std::cos(temp_p2);

        double x = std::sin(temp_p2) * std::cos(temp_p1) -
            coslat * std::sin(temp_p1) * std::cos(temp_diff);
        double y = std::sin(temp_diff) * coslat;

        return bu::quantity<bu::si::plane_angle>::from_value(std::atan2(x, y));
    }

    //!returns true if both coordinate systems are same else returns false
    template<class OtherCoordinateSystem>
    bool is_equivalent_system(sky_point<OtherCoordinateSystem> const& object)
    {
        return std::is_same<CoordinateSystem, OtherCoordinateSystem>::value;
    }

    template<class OtherCoordinateSystem>
    sky_point<OtherCoordinateSystem> transform_to();

    //!returns the point
    CoordinateSystem get_point() const
    {
        return this->point;
    }

    //!sets the point with given object
    void set_point(CoordinateSystem const& otherPoint)
    {
        this->point = otherPoint;
    }

}; //sky_point

}}} //namespace boost::astronomy::coordinate

#endif // !BOOST_ASTRONOMY_COORDINATE_SKY_POINT_HPP


#ifndef MATHEVAL_IMPLEMENTATION
#error "Do not include parser_def.hpp directly!"
#endif

#pragma once

#include "ast.hpp"
#include "ast_adapted.hpp"
#include "math.hpp"
#include "parser.hpp"

#include <boost/spirit/include/phoenix.hpp>
#define BOOST_SPIRIT_NO_PREDEFINED_TERMINALS
#include <boost/spirit/include/qi.hpp>

#include <cmath>
#include <iostream>
#include <limits>
#include <sstream>
#include <string>

namespace matheval {

namespace qi = boost::spirit::qi;

namespace parser {

template <typename Iterator>
grammar<Iterator>::grammar() : grammar::base_type(expression) {
    qi::_2_type _2;
    qi::_3_type _3;
    qi::_4_type _4;

    qi::alnum_type alnum;
    qi::alpha_type alpha;
    qi::double_type double_;
    qi::lexeme_type lexeme;
    qi::raw_type raw;

    // clang-format off

    constant.add
        ("e"      , boost::math::constants::e<double>())
        ("epsilon", std::numeric_limits<double>::epsilon())
        ("phi"    , boost::math::constants::phi<double>())
        ("pi"     , boost::math::constants::pi<double>())
        ;

    ufunc.add
        ("abs"  , static_cast<double (*)(double)>(&std::abs))
        ("acos" , static_cast<double (*)(double)>(&std::acos))
        ("asin" , static_cast<double (*)(double)>(&std::asin))
        ("atan" , static_cast<double (*)(double)>(&std::atan))
        ("ceil" , static_cast<double (*)(double)>(&std::ceil))
        ("cos"  , static_cast<double (*)(double)>(&std::cos))
        ("cosh" , static_cast<double (*)(double)>(&std::cosh))
        ("deg"  , static_cast<double (*)(double)>(&math::deg))
        ("exp"  , static_cast<double (*)(double)>(&std::exp))
        ("floor", static_cast<double (*)(double)>(&std::floor))
        ("isinf", static_cast<double (*)(double)>(&math::isinf))
        ("isnan", static_cast<double (*)(double)>(&math::isnan))
        ("log"  , static_cast<double (*)(double)>(&std::log))
        ("log10", static_cast<double (*)(double)>(&std::log10))
        ("rad"  , static_cast<double (*)(double)>(&math::rad))
        ("sgn"  , static_cast<double (*)(double)>(&math::sgn))
        ("sin"  , static_cast<double (*)(double)>(&std::sin))
        ("sinh" , static_cast<double (*)(double)>(&std::sinh))
        ("sqrt" , static_cast<double (*)(double)>(&std::sqrt))
        ("tan"  , static_cast<double (*)(double)>(&std::tan))
        ("tanh" , static_cast<double (*)(double)>(&std::tanh))
        ;

    bfunc.add
        ("atan2", static_cast<double (*)(double, double)>(&std::atan2))
        ("pow"  , static_cast<double (*)(double, double)>(&std::pow))
        ;

    unary_op.add
        ("+", static_cast<double (*)(double)>(&math::plus))
        ("-", static_cast<double (*)(double)>(&math::minus))
        ("!", static_cast<double (*)(double)>(&math::unary_not))
        ;

    additive_op.add
        ("+", static_cast<double (*)(double, double)>(&math::plus))
        ("-", static_cast<double (*)(double, double)>(&math::minus))
        ;

    multiplicative_op.add
        ("*", static_cast<double (*)(double, double)>(&math::multiplies))
        ("/", static_cast<double (*)(double, double)>(&math::divides))
        ("%", static_cast<double (*)(double, double)>(&std::fmod))
        ;

    logical_op.add
        ("&&", static_cast<double (*)(double, double)>(&math::logical_and))
        ("||", static_cast<double (*)(double, double)>(&math::logical_or))
        ;

    relational_op.add
        ("<" , static_cast<double (*)(double, double)>(&math::less))
        ("<=", static_cast<double (*)(double, double)>(&math::less_equals))
        (">" , static_cast<double (*)(double, double)>(&math::greater))
        (">=", static_cast<double (*)(double, double)>(&math::greater_equals))
        ;

    equality_op.add
        ("==", static_cast<double (*)(double, double)>(&math::equals))
        ("!=", static_cast<double (*)(double, double)>(&math::not_equals))
        ;

    power.add
        ("**", static_cast<double (*)(double, double)>(&std::pow))
        ;

    expression =
        logical.alias()
        ;

    logical =
        equality >> *(logical_op > equality)
        ;

    equality =
        relational >> *(equality_op > relational)
        ;

    relational =
        additive >> *(relational_op > additive)
        ;

    additive =
        multiplicative >> *(additive_op > multiplicative)
        ;

    multiplicative =
        factor >> *(multiplicative_op > factor)
        ;

    factor =
        primary >> *( power > factor )
        ;

    unary =
        ufunc > '(' > expression > ')'
        ;

    binary =
        bfunc > '(' > expression > ',' > expression > ')'
        ;

    variable =
        raw[lexeme[alpha >> *(alnum | '_')]]
        ;

    primary =
          double_
        | ('(' > expression > ')')
        | (unary_op > primary)
        | binary
        | unary
        | constant
        | variable
        ;

    // clang-format on

    expression.name("expression");
    logical.name("logical");
    equality.name("equality");
    relational.name("relational");
    additive.name("additive");
    multiplicative.name("multiplicative");
    factor.name("factor");
    variable.name("variable");
    primary.name("primary");
    unary.name("unary");
    binary.name("binary");

    // typedef boost::phoenix::function<error_handler<Iterator> >
    // error_handler_function; qi::on_error<qi::fail>(expression,
    //        error_handler_function(error_handler<Iterator>())(
    //            "Error! Expecting ", qi::_4, qi::_3));
    qi::on_error<qi::fail>(
        expression,
        boost::phoenix::bind(boost::phoenix::ref(err_handler), _3, _2, _4));
}

} // namespace parser

} // namespace matheval


/**
 All of this code is written by Aman Agrawal
 (Indian Institute of Technology, Delhi)
*/

#include <bits/stdc++.h>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <Eigen/QR>
#include "time.h"
#include <thread>
#include <chrono>

#include "genotype.h"
#include "mailman.h"
#include "arguments.h"
#include "helper.h"
#include "storage.h"

#if SSE_SUPPORT==1
	#define fastmultiply fastmultiply_sse
	#define fastmultiply_pre fastmultiply_pre_sse
#else
	#define fastmultiply fastmultiply_normal
	#define fastmultiply_pre fastmultiply_pre_normal
#endif

using namespace Eigen;
using namespace std;

// Storing in RowMajor Form
typedef Matrix<double, Dynamic, Dynamic, RowMajor> MatrixXdr;

//Intermediate Variables
//
// How to batch columns:
int blocksize;
double **partialsums;
double *sum_op;

// Intermediate computations in E-step.
// Size = 3^(log_3(n)) * k
double **yint_e;
//  n X k
double ***y_e;

// Intermediate computations in M-step.
// Size = nthreads X 3^(log_3(n)) * k
double **yint_m;
//  nthreads X log_3(n) X k
double ***y_m;

struct timespec t0;

clock_t total_begin = clock();

genotype g;
MatrixXdr geno_matrix; //(p,n)

int MAX_ITER;
int k,p,n;
int k_orig;

MatrixXdr c; //(p,k)
MatrixXdr x; //(k,n)
MatrixXdr v; //(p,k)
MatrixXdr means; //(p,1)
MatrixXdr stds; //(p,1)

options command_line_opts;

bool debug = false;
bool check_accuracy = false;
bool var_normalize=false;
int accelerated_em=0;
double convergence_limit;
bool memory_efficient = false;
bool missing=false;
bool fast_mode = true;
bool text_version = false;
int nthreads = 1;

void multiply_y_pre_fast_thread (int begin, int end, MatrixXdr &op, int Ncol_op, double *yint_m, double **y_m, double *partialsums, MatrixXdr &res){
	for(int seg_iter = begin; seg_iter < end; seg_iter++){
		mailman::fastmultiply ( g.segment_size_hori, g.Nindv, Ncol_op, g.p[seg_iter], op, yint_m, partialsums, y_m);
		int p_base = seg_iter * g.segment_size_hori;
		for(int p_iter=p_base; (p_iter < p_base + g.segment_size_hori) && (p_iter < g.Nsnp) ; p_iter++ ){
			for(int k_iter = 0; k_iter < Ncol_op; k_iter++)
				res(p_iter, k_iter) = y_m [p_iter - p_base][k_iter];
		}
	}
}

void multiply_y_post_fast_thread (int begin, int end, MatrixXdr &op, int Ncol_op, double *yint_e, double **y_e, double *partialsums){
	for (int i = 0 ; i < g.Nindv ; i++) {
		memset (y_e[i], 0, blocksize * sizeof(double));
	}

	for(int seg_iter = begin; seg_iter < end; seg_iter++){
		mailman::fastmultiply_pre (g.segment_size_hori, g.Nindv, Ncol_op, seg_iter * g.segment_size_hori, g.p[seg_iter], op, yint_e, partialsums, y_e);
	}
}

/*
 * M-step: Compute C = Y E
 * Y : p X n genotype matrix
 * E : n K k matrix: X^{T} (XX^{T})^{-1}
 * C = p X k matrix
 *
 * op : E
 * Ncol_op : k
 * res : C
 * subtract_means :
 */
void multiply_y_pre_fast(MatrixXdr &op, int Ncol_op ,MatrixXdr &res,bool subtract_means){

	for(int k_iter = 0; k_iter < Ncol_op; k_iter++){
		sum_op[k_iter] = op.col(k_iter).sum();
	}

	#if DEBUG==1
		if(debug){
			print_time ();
			cout <<"Starting mailman on premultiply"<<endl;
			cout << "Nops = " << Ncol_op << "\t" <<g.Nsegments_hori << endl;
			cout << "Segment size = " << g.segment_size_hori << endl;
			cout << "Matrix size = " <<g.segment_size_hori<<"\t" <<g.Nindv << endl;
			cout << "op = " <<  op.rows () << "\t" << op.cols () << endl;
		}
	#endif

	//TODO: Memory Effecient SSE FastMultipy

	nthreads = (nthreads > g.Nsegments_hori) ? g.Nsegments_hori: nthreads;

	std::thread th [nthreads];
	int perthread = g.Nsegments_hori/nthreads;
//	cout << g.Nsegments_hori << "\t" << nthreads << "\t" << perthread << endl;
	int t = 0;
	for (; t < nthreads - 1; t++) {
//		cout << "Launching thread " << t << endl;
		th[t] = std::thread (multiply_y_pre_fast_thread, t * perthread , (t+1)*perthread, std::ref (op), Ncol_op, yint_m[t], y_m[t], partialsums[t], std::ref(res));
	}

	th[t] = std::thread (multiply_y_pre_fast_thread, t * perthread , g.Nsegments_hori  - 1, std::ref (op), Ncol_op, yint_m[t], y_m[t], partialsums[t], std::ref(res));

	for (int t = 0 ; t < nthreads; t++) {
		th[t].join ();
	}

/*
	for(int seg_iter = 0; seg_iter < g.Nsegments_hori - 1; seg_iter++){
		mailman::fastmultiply ( g.segment_size_hori, g.Nindv, Ncol_op, g.p[seg_iter], op, yint_m, partialsums, y_m);
		int p_base = seg_iter * g.segment_size_hori;
		for(int p_iter=p_base; (p_iter < p_base + g.segment_size_hori) && (p_iter < g.Nsnp) ; p_iter++ ){
			for(int k_iter = 0; k_iter < Ncol_op; k_iter++)
				res(p_iter, k_iter) = y_m [p_iter - p_base][k_iter];
		}
	}
*/

	int last_seg_size = (g.Nsnp % g.segment_size_hori !=0 ) ? g.Nsnp % g.segment_size_hori : g.segment_size_hori;
	mailman::fastmultiply(last_seg_size, g.Nindv, Ncol_op, g.p[g.Nsegments_hori-1], op, yint_m[0], partialsums[0], y_m[0]);
	int p_base = (g.Nsegments_hori - 1) * g.segment_size_hori;
	for(int p_iter = p_base; (p_iter < p_base + g.segment_size_hori) && (p_iter < g.Nsnp) ; p_iter++){
		for(int k_iter = 0; k_iter < Ncol_op; k_iter++)
			res(p_iter, k_iter) = y_m [0][p_iter - p_base][k_iter];
	}

	#if DEBUG==1
		if(debug){
			print_time ();
			cout <<"Ending mailman on premultiply"<<endl;
		}
	#endif

	if(!subtract_means)
		return;

	for(int p_iter = 0; p_iter < p; p_iter++){
 		for(int k_iter = 0 ; k_iter < Ncol_op; k_iter++){
			res(p_iter, k_iter) = res(p_iter, k_iter) - (g.get_col_mean(p_iter)*sum_op[k_iter]);
			if(var_normalize)
				res(p_iter, k_iter) = res(p_iter, k_iter)/(g.get_col_std(p_iter));
 		}
 	}

}

/*
 * E-step: Compute X = D Y
 * Y : p X n genotype matrix
 * D : k X p matrix: (C^T C)^{-1} C^{T}
 * X : k X n matrix
 *
 * op_orig : D
 * Nrows_op : k
 * res : X
 * subtract_means :
 */
void multiply_y_post_fast(MatrixXdr &op_orig, int Nrows_op, MatrixXdr &res,bool subtract_means){

	MatrixXdr op;
	op = op_orig.transpose();

	if(var_normalize && subtract_means){
		for(int p_iter = 0; p_iter < p ; p_iter++){
			for(int k_iter = 0; k_iter < Nrows_op; k_iter++)
				op(p_iter,k_iter) = op(p_iter,k_iter) / (g.get_col_std(p_iter));
		}
	}

	#if DEBUG==1
		if(debug){
			print_time ();
			cout <<"Starting mailman on postmultiply"<<endl;
		}
	#endif

	int Ncol_op = Nrows_op;

	nthreads = (nthreads > g.Nsegments_hori) ? g.Nsegments_hori: nthreads;

	std::thread th [nthreads];
	int perthread = g.Nsegments_hori/nthreads;
//	cout << "post: " << g.segment_size_hori << "\t" << g.Nsegments_hori << "\t" << nthreads << "\t" << perthread << endl;
	int t = 0;
	for (; t < nthreads - 1; t++) {
//		cout << "Launching " << t << endl;
		th[t] = std::thread ( multiply_y_post_fast_thread,t * perthread, (t+1)*perthread, std::ref(op), Ncol_op, yint_e[t], y_e[t], partialsums[t]);

	}
//	cout << "Launching " << t << endl;
	th[t] = std::thread ( multiply_y_post_fast_thread, t * perthread, g.Nsegments_hori - 1, std::ref(op), Ncol_op, yint_e[t], y_e[t], partialsums[t]);
	for (int t = 0 ; t < nthreads; t++) {
		th[t].join ();
	}
//	cout << "Joined "<< endl;

/*
	int seg_iter;
	for(seg_iter = 0; seg_iter < g.Nsegments_hori-1; seg_iter++){
		mailman::fastmultiply_pre (g.segment_size_hori, g.Nindv, Ncol_op, seg_iter * g.segment_size_hori, g.p[seg_iter], op, yint_e, partialsums[0], y_e);
	}
*/

	for (int t = 1 ; t < nthreads; t++){
		for(int n_iter = 0; n_iter < n; n_iter++)
			for(int k_iter = 0; k_iter < Ncol_op; k_iter++)
				y_e [0][n_iter][k_iter] += y_e[t][n_iter][k_iter];
	}

	int last_seg_size = (g.Nsnp % g.segment_size_hori !=0 ) ? g.Nsnp % g.segment_size_hori : g.segment_size_hori;
	mailman::fastmultiply_pre(last_seg_size, g.Nindv, Ncol_op, (g.Nsegments_hori-1) * g.segment_size_hori, g.p[g.Nsegments_hori-1], op, yint_e[0], partialsums[0], y_e[0]);

	for(int n_iter = 0; n_iter < n; n_iter++)  {
		for(int k_iter = 0; k_iter < Ncol_op; k_iter++) {
			res(k_iter,n_iter) = y_e[0][n_iter][k_iter];
			y_e[0][n_iter][k_iter] = 0;
		}
	}

	#if DEBUG==1
		if(debug){
			print_time ();
			cout <<"Ending mailman on postmultiply"<<endl;
		}
	#endif

	if(!subtract_means)
		return;

	double *sums_elements = new double[Ncol_op];
 	memset (sums_elements, 0, Nrows_op * sizeof(int));

 	for(int k_iter=0;k_iter<Ncol_op;k_iter++){
 		double sum_to_calc=0.0;
 		for(int p_iter=0;p_iter<p;p_iter++)
 			sum_to_calc += g.get_col_mean(p_iter)*op(p_iter,k_iter);
 		sums_elements[k_iter] = sum_to_calc;
 	}
 	for(int k_iter=0;k_iter<Ncol_op;k_iter++){
 		for(int n_iter=0;n_iter<n;n_iter++)
 			res(k_iter,n_iter) = res(k_iter,n_iter) - sums_elements[k_iter];
 	}

}

void multiply_y_pre_naive_mem(MatrixXdr &op, int Ncol_op ,MatrixXdr &res){
	for(int p_iter=0;p_iter<p;p_iter++){
		for(int k_iter=0;k_iter<Ncol_op;k_iter++){
			double temp=0;
			for(int n_iter=0;n_iter<n;n_iter++)
				temp+= g.get_geno(p_iter,n_iter,var_normalize)*op(n_iter,k_iter);
			res(p_iter,k_iter)=temp;
		}
	}
}

void multiply_y_post_naive_mem(MatrixXdr &op, int Nrows_op ,MatrixXdr &res){
	for(int n_iter=0;n_iter<n;n_iter++){
		for(int k_iter=0;k_iter<Nrows_op;k_iter++){
			double temp=0;
			for(int p_iter=0;p_iter<p;p_iter++)
				temp+= op(k_iter,p_iter)*(g.get_geno(p_iter,n_iter,var_normalize));
			res(k_iter,n_iter)=temp;
		}
	}
}

void multiply_y_pre_naive(MatrixXdr &op, int Ncol_op ,MatrixXdr &res){
	res = geno_matrix * op;
}

void multiply_y_post_naive(MatrixXdr &op, int Nrows_op ,MatrixXdr &res){
	res = op * geno_matrix;
}

void multiply_y_post(MatrixXdr &op, int Nrows_op ,MatrixXdr &res,bool subtract_means){
    if(fast_mode)
        multiply_y_post_fast(op,Nrows_op,res,subtract_means);
    else{
		if(memory_efficient)
			multiply_y_post_naive_mem(op,Nrows_op,res);
		else
			multiply_y_post_naive(op,Nrows_op,res);
	}
}

void multiply_y_pre(MatrixXdr &op, int Ncol_op ,MatrixXdr &res,bool subtract_means){
    if(fast_mode)
        multiply_y_pre_fast(op,Ncol_op,res,subtract_means);
    else{
		if(memory_efficient)
			multiply_y_pre_naive_mem(op,Ncol_op,res);
		else
			multiply_y_pre_naive(op,Ncol_op,res);
	}
}

pair<double,double> get_error_norm(MatrixXdr &c){
	HouseholderQR<MatrixXdr> qr(c);
	MatrixXdr Q;
	Q = qr.householderQ() * MatrixXdr::Identity(p,k);
	MatrixXdr q_t(k,p);
	q_t = Q.transpose();
	MatrixXdr b(k,n);
	// Need this for subtracting the correct mean in case of missing data
	if(missing){
		multiply_y_post(q_t,k,b,false);
		// Just calculating b from seen data
		MatrixXdr M_temp(k,1);
		M_temp = q_t * means;
		for(int j=0;j<n;j++){
			MatrixXdr M_to_remove(k,1);
			M_to_remove = MatrixXdr::Zero(k,1);
			for(int i=0;i<g.not_O_j[j].size();i++){
				int idx = g.not_O_j[j][i];
				M_to_remove = M_to_remove + (Q.row(idx).transpose()*g.get_col_mean(idx));
			}
			b.col(j) -= (M_temp - M_to_remove);
		}
	}
	else{
		multiply_y_post(q_t,k,b,true);
	}

	JacobiSVD<MatrixXdr> b_svd(b, ComputeThinU | ComputeThinV);
	MatrixXdr u_l,d_l,v_l;
	if(fast_mode)
        u_l = b_svd.matrixU();
    else
        u_l = Q * b_svd.matrixU();
	v_l = b_svd.matrixV();
	d_l = MatrixXdr::Zero(k,k);
	for(int kk=0;kk<k; kk++)
		d_l(kk,kk) = (b_svd.singularValues())(kk);

	MatrixXdr u_k,v_k,d_k;
	u_k = u_l.leftCols(k_orig);
	v_k = v_l.leftCols(k_orig);
	d_k = MatrixXdr::Zero(k_orig,k_orig);
	for(int kk =0 ; kk < k_orig ; kk++)
		d_k(kk,kk)  =(b_svd.singularValues())(kk);

	MatrixXdr b_l,b_k;
    b_l = u_l * d_l * (v_l.transpose());
    b_k = u_k * d_k * (v_k.transpose());

    if(fast_mode){
        double temp_k = b_k.cwiseProduct(b).sum();
        double temp_l = b_l.cwiseProduct(b).sum();
        double b_knorm = b_k.norm();
        double b_lnorm = b_l.norm();
        double norm_k = (b_knorm*b_knorm) - (2*temp_k);
        double norm_l = (b_lnorm*b_lnorm) - (2*temp_l);
        return make_pair(norm_k,norm_l);
    }
    else{
        MatrixXdr e_l(p,n);
        MatrixXdr e_k(p,n);
        for(int p_iter=0;p_iter<p;p_iter++){
            for(int n_iter=0;n_iter<n;n_iter++){
                e_l(p_iter,n_iter) = g.get_geno(p_iter,n_iter,var_normalize) - b_l(p_iter,n_iter);
                e_k(p_iter,n_iter) = g.get_geno(p_iter,n_iter,var_normalize) - b_k(p_iter,n_iter);
            }
        }

        double ek_norm = e_k.norm();
        double el_norm = e_l.norm();
        return make_pair(ek_norm,el_norm);
    }
}

/* Run one iteration of EM when genotypes are not missing
 * c_orig : p X k matrix
 * Output: c_new : p X k matrix
 */
MatrixXdr run_EM_not_missing(MatrixXdr &c_orig){

	#if DEBUG==1
		if(debug){
			print_time ();
			cout << "Enter: run_EM_not_missing" << endl;
		}
	#endif

 	// c_temp : k X p matrix: (C^T C)^{-1} C^{T}
	MatrixXdr c_temp(k,p);
	MatrixXdr c_new(p,k);
	c_temp = ( (c_orig.transpose()*c_orig).inverse() ) * (c_orig.transpose());

	#if DEBUG==1
		if(debug){
			print_timenl ();
		}
	#endif

 	/* E-step: Compute X = D Y
 	* Y : p X n genotype matrix
 	* D : k X p matrix: (C^T C)^{-1} C^{T}
 	* X : k X n matrix
 	*  x_fn: X
 	*  c_temp: D
 	*/
	MatrixXdr x_fn(k,n);
	multiply_y_post(c_temp,k,x_fn,true);

	#if DEBUG==1
		if(debug){
			print_timenl ();
		}
	#endif

	// x_temp : n X k matrix X^{T} (XX^{T})^{-1}
	MatrixXdr x_temp(n,k);
	x_temp = (x_fn.transpose()) * ((x_fn*(x_fn.transpose())).inverse());

	/* M-step: C = Y E
 	* Y : p X n genotype matrix
 	* E : n K k matrix: X^{T} (XX^{T})^{-1}
 	* C = p X k matrix
 	* c_new : C
 	* x_temp : E
 	*/
	multiply_y_pre(x_temp,k,c_new,true);

	#if DEBUG==1
		if(debug){
			print_time ();
			cout << "Exiting: run_EM_not_missing" << endl;
		}
	#endif

	return c_new;
}

MatrixXdr run_EM_missing(MatrixXdr &c_orig){

	MatrixXdr c_new(p,k);

	MatrixXdr mu(k,n);

	// E step
	MatrixXdr c_temp(k,k);
	c_temp = c_orig.transpose() * c_orig;

	MatrixXdr T(k,n);
	MatrixXdr c_fn;
	c_fn = c_orig.transpose();
	multiply_y_post(c_fn,k,T,false);

	MatrixXdr M_temp(k,1);
	M_temp = c_orig.transpose() *  means;

	for(int j=0;j<n;j++){
		MatrixXdr D(k,k);
		MatrixXdr M_to_remove(k,1);
		D = MatrixXdr::Zero(k,k);
		M_to_remove = MatrixXdr::Zero(k,1);
		for(int i=0;i<g.not_O_j[j].size();i++){
			int idx = g.not_O_j[j][i];
			D = D + (c_orig.row(idx).transpose() * c_orig.row(idx));
			M_to_remove = M_to_remove + (c_orig.row(idx).transpose()*g.get_col_mean(idx));
		}
		mu.col(j) = (c_temp-D).inverse() * ( T.col(j) - M_temp + M_to_remove);
	}

	#if DEBUG==1
		if(debug){
			ofstream x_file;
			x_file.open((string(command_line_opts.OUTPUT_PATH)+string("x_in_fn_vals.txt")).c_str());
			x_file<<std::setprecision(15)<<mu<<endl;
			x_file.close();
		}
	#endif

	// M step

	MatrixXdr mu_temp(k,k);
	mu_temp = mu * mu.transpose();
	MatrixXdr T1(p,k);
	MatrixXdr mu_fn;
	mu_fn = mu.transpose();
	multiply_y_pre(mu_fn,k,T1,false);
	MatrixXdr mu_sum(k,1);
	mu_sum = MatrixXdr::Zero(k,1);
	mu_sum = mu.rowwise().sum();

	for(int i=0;i<p;i++){
		MatrixXdr D(k,k);
		MatrixXdr mu_to_remove(k,1);
		D = MatrixXdr::Zero(k,k);
		mu_to_remove = MatrixXdr::Zero(k,1);
		for(int j=0;j<g.not_O_i[i].size();j++){
			int idx = g.not_O_i[i][j];
			D = D + (mu.col(idx) * mu.col(idx).transpose());
			mu_to_remove = mu_to_remove + (mu.col(idx));
		}
		c_new.row(i) = (((mu_temp-D).inverse()) * (T1.row(i).transpose() -  ( g.get_col_mean(i) * (mu_sum-mu_to_remove)))).transpose();
		double mean;
		mean = g.get_col_sum(i);
		mean = mean -  (c_orig.row(i)*(mu_sum-mu_to_remove))(0,0);
		mean = mean * 1.0 / (n-g.not_O_i[i].size());
		g.update_col_mean(i,mean);
	}

	// IMPORTANT: Update the value of means variable present locally, so that for next iteration, updated value of means is used.
	for(int i=0;i<p;i++){
		means(i,0) = g.get_col_mean(i);
		// Also updating std, just for consistency, though, it is not used presently.
		stds(i,0) = g.get_col_std(i);
	}

	return c_new;
}

MatrixXdr run_EM(MatrixXdr &c_orig){

	if(missing)
		return run_EM_missing(c_orig);
	else
		return run_EM_not_missing(c_orig);
}

void print_vals(){

	HouseholderQR<MatrixXdr> qr(c);
	MatrixXdr Q;
	Q = qr.householderQ() * MatrixXdr::Identity(p,k);
	MatrixXdr q_t(k,p);
	q_t = Q.transpose();
	MatrixXdr b(k,n);

	// Need this for subtracting the correct mean in case of missing data
	if(missing){
		multiply_y_post(q_t,k,b,false);
		// Just calculating b from seen data
		MatrixXdr M_temp(k,1);
		M_temp = q_t * means;
		for(int j=0;j<n;j++){
			MatrixXdr M_to_remove(k,1);
			M_to_remove = MatrixXdr::Zero(k,1);
			for(int i=0;i<g.not_O_j[j].size();i++){
				int idx = g.not_O_j[j][i];
				M_to_remove = M_to_remove + (Q.row(idx).transpose()*g.get_col_mean(idx));
			}
			b.col(j) -= (M_temp - M_to_remove);
		}
	}
	else{
		multiply_y_post(q_t,k,b,true);
	}

	JacobiSVD<MatrixXdr> b_svd(b, ComputeThinU | ComputeThinV);
	MatrixXdr u_l;
	u_l = b_svd.matrixU();
	MatrixXdr v_l;
	v_l = b_svd.matrixV();
	MatrixXdr u_k;
	MatrixXdr v_k,d_k;
	u_k = u_l.leftCols(k_orig);
	v_k = v_l.leftCols(k_orig);

	ofstream evec_file;
	evec_file.open((string(command_line_opts.OUTPUT_PATH)+string("evecs.txt")).c_str());
	evec_file<< std::setprecision(15) << Q*u_k << endl;
	evec_file.close();
	ofstream eval_file;
	eval_file.open((string(command_line_opts.OUTPUT_PATH)+string("evals.txt")).c_str());
	for(int kk =0 ; kk < k_orig ; kk++)
		eval_file << std::setprecision(15)<< (b_svd.singularValues())(kk)*(b_svd.singularValues())(kk)/g.Nsnp<<endl;
	eval_file.close();

	ofstream proj_file;
	proj_file.open((string(command_line_opts.OUTPUT_PATH) + string("projections.txt")).c_str());
	proj_file << std::setprecision(15)<< v_k<<endl;
	proj_file.close();
	if(debug){
		ofstream c_file;
		c_file.open((string(command_line_opts.OUTPUT_PATH)+string("cvals.txt")).c_str());
		c_file<<std::setprecision(15)<<c<<endl;
		c_file.close();

		ofstream means_file;
		means_file.open((string(command_line_opts.OUTPUT_PATH)+string("means.txt")).c_str());
		means_file<<std::setprecision(15)<<means<<endl;
		means_file.close();

		d_k = MatrixXdr::Zero(k_orig,k_orig);
		for(int kk =0 ; kk < k_orig ; kk++)
			d_k(kk,kk)  =(b_svd.singularValues())(kk);
		MatrixXdr x_k;
		x_k = d_k * (v_k.transpose());
		ofstream x_file;
		x_file.open((string(command_line_opts.OUTPUT_PATH) + string("xvals.txt")).c_str());
		x_file<<std::setprecision(15)<<x_k.transpose()<<endl;
		x_file.close();
	}
}

int main(int argc, char const *argv[]){

	auto start = std::chrono::system_clock::now();

	clock_t io_begin = clock();
    clock_gettime (CLOCK_REALTIME, &t0);

	pair<double,double> prev_error = make_pair(0.0,0.0);
	double prevnll=0.0;

	parse_args(argc,argv);

	//TODO: Memory Effecient Version of Mailman

	memory_efficient = command_line_opts.memory_efficient;
	text_version = command_line_opts.text_version;
    fast_mode = command_line_opts.fast_mode;
	missing = command_line_opts.missing;
    MAX_ITER =  command_line_opts.max_iterations ;
	k_orig = command_line_opts.num_of_evec ;
	debug = command_line_opts.debugmode ;
	check_accuracy = command_line_opts.getaccuracy;
	var_normalize = command_line_opts.var_normalize;
	accelerated_em = command_line_opts.accelerated_em;
	nthreads = command_line_opts.nthreads;

	if(text_version){
		if(fast_mode)
			g.read_txt_mailman(command_line_opts.GENOTYPE_FILE_PATH,missing);
		else
			g.read_txt_naive(command_line_opts.GENOTYPE_FILE_PATH,missing);
	}
	else{
		g.read_plink(command_line_opts.GENOTYPE_FILE_PATH,missing,fast_mode);
	}

	//TODO: Implement these codes.
	if(missing && !fast_mode){
		cout<<"Missing version works only with mailman i.e. fast mode\n EXITING..."<<endl;
		exit(-1);
	}
	if(fast_mode && memory_efficient){
		cout<<"Memory effecient version for mailman EM not yet implemented"<<endl;
		cout<<"Ignoring Memory effecient Flag"<<endl;
	}
	if(missing && var_normalize){
		cout<<"Missing version works only without variance normalization\n EXITING..."<<endl;
		exit(-1);
	}

	k = k_orig + command_line_opts.l;
	k = (int)ceil(k/10.0)*10;
	command_line_opts.l = k - k_orig;
	p = g.Nsnp;
	n = g.Nindv;
	convergence_limit = command_line_opts.convergence_limit;
	bool toStop=false;
	if(convergence_limit!=-1)
		toStop=true;

	if (command_line_opts.given_seed)
		srand(command_line_opts.seed);
	else
		srand((unsigned int) time(0));

	c.resize(p,k);
	x.resize(k,n);
	v.resize(p,k);
	means.resize(p,1);
	stds.resize(p,1);

	if(!fast_mode && !memory_efficient){
		geno_matrix.resize(p,n);
		g.generate_eigen_geno(geno_matrix,var_normalize);
	}

	clock_t io_end = clock();

	//TODO: Initialization of c with gaussian distribution
	c = MatrixXdr::Random(p,k);

	// Initial intermediate data structures
	// Operate in blocks to improve caching
	//
	blocksize = k;
	int hsegsize = g.segment_size_hori; 	// = log_3(n)
	int hsize = pow(3,hsegsize);
	int vsegsize = g.segment_size_ver; 		// = log_3(p)
	int vsize = pow(3,vsegsize);

	partialsums = new double* [nthreads];
	yint_m = new double* [nthreads];
	for (int t = 0 ; t < nthreads ; t++) {
		partialsums[t] = new double [blocksize];
		yint_m[t] = new double [hsize*blocksize];
		memset (yint_m[t], 0, hsize*blocksize * sizeof(double));
	}

	sum_op = new double[blocksize];

	yint_e = new double* [nthreads];
	for (int t = 0 ; t < nthreads ; t++) {
		yint_e[t] = new double [hsize*blocksize];
		memset (yint_e[t], 0, hsize*blocksize * sizeof(double));
	}

	y_e  = new double**[nthreads];
	for (int t = 0 ; t < nthreads ; t++) {
		y_e[t]  = new double*[g.Nindv];
		for (int i = 0 ; i < g.Nindv ; i++) {
			y_e[t][i] = new double[blocksize];
			memset (y_e[t][i], 0, blocksize * sizeof(double));
		}
	}

	y_m = new double**[nthreads];
	for (int t = 0 ; t < nthreads; t++){
		y_m[t] = new double*[hsegsize];
		for (int i = 0 ; i < hsegsize ; i++)
			y_m[t][i] = new double[blocksize];

	}

	for(int i=0;i<p;i++){
		means(i,0) = g.get_col_mean(i);
		stds(i,0) = g.get_col_std(i);
	}

	ofstream c_file;
	if(debug){
		c_file.open((string(command_line_opts.OUTPUT_PATH)+string("cvals_orig.txt")).c_str());
		c_file<<std::setprecision(15)<<c<<endl;
		c_file.close();
		printf("Read Matrix\n");
	}

	cout<<"Running on Dataset of "<<g.Nsnp<<" SNPs and "<<g.Nindv<<" Individuals"<<endl;

	#if SSE_SUPPORT==1
		if(fast_mode)
			cout<<"Using Optimized SSE FastMultiply"<<endl;
	#endif

	clock_t it_begin = clock();
	for(int i=0;i<MAX_ITER;i++){

		MatrixXdr c1,c2,cint,r,v;
		double a,nll;
		if(debug){
			print_time ();
			cout << "*********** Begin epoch " << i << "***********" << endl;
		}
		if(accelerated_em!=0){
			#if DEBUG==1
				if(debug){
					print_time();
					cout << "Before EM" << endl;
				}
			#endif
			c1 = run_EM(c);
			c2 = run_EM(c1);
			#if DEBUG==1
				if(debug){
					print_time();
					cout << "After EM but before acceleration" << endl;
				}
			#endif
			r = c1-c;
			v = (c2-c1) - r;
			a = -1.0 * r.norm() / (v.norm()) ;
			if(accelerated_em==1){
				if(a>-1){
					a=-1;
					cint=c2;
				}
				else {
					cint = c - 2*a*r + a*a*v;
					nll = get_error_norm(cint).second;
					if(i>0){
						while(nll>prevnll && a<-1){
							a = 0.5 * (a-1);
							cint = c - 2*a*r +(a*a*v);
							nll = get_error_norm(cint).second;
						}
					}
				}
				c = cint;
			}
			else if(accelerated_em==2){
				cint = c - 2*a*r + a*a*v;
				c = cint;
				// c = run_EM(cint);
			}
		}
		else{
			c = run_EM(c);
		}

		if ( accelerated_em == 1 || check_accuracy || toStop) {
			pair<double,double> e = get_error_norm(c);
			prevnll = e.second;
			if(check_accuracy)
				cout<<"Iteration "<<i+1<<"  "<<std::setprecision(15)<<e.first<<"  "<<e.second<<endl;
			if(abs(e.first-prev_error.first)<=convergence_limit){
				cout<<"Breaking after "<<i+1<<" iterations"<<endl;
				break;
			}
			prev_error = e;
		}
		if(debug){
			print_time ();
			cout << "*********** End epoch " << i << "***********" << endl;
		}

	}
	clock_t it_end = clock();

    print_vals();

	clock_t total_end = clock();
	double io_time = double(io_end - io_begin) / CLOCKS_PER_SEC;
	double avg_it_time = double(it_end - it_begin) / (MAX_ITER * 1.0 * CLOCKS_PER_SEC);
	double total_time = double(total_end - total_begin) / CLOCKS_PER_SEC;
	cout<<"IO Time:  "<< io_time << "\nAVG Iteration Time:  "<<avg_it_time<<"\nTotal runtime:   "<<total_time<<endl;

	std::chrono::duration<double> wctduration = std::chrono::system_clock::now() - start;
	cout <<"Wall clock time = " <<  wctduration.count() << endl;

	delete[] sum_op;
	for (int t = 0 ; t < nthreads ; t++){
		delete[] yint_e[t];
	}
	delete[] yint_e;

	for (int t = 0 ; t < nthreads ; t++){
		delete[] yint_m[t];
		delete[] partialsums[t];
	}
	delete[] yint_m;
	delete[] partialsums;

	for (int t = 0 ; t < nthreads ; t++){
		for (int i  = 0 ; i < hsegsize; i++)
			delete[] y_m [t][i];
		delete[] y_m[t];
	}
	delete[] y_m;

	for (int t = 0 ; t < nthreads ; t++){
		for (int i  = 0 ; i < g.Nindv; i++)
			delete[] y_e[t][i];
		delete[] y_e[t];
	}
	delete[] y_e;

	return 0;
}


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver 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
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#ifndef INTEGRATORHELPERFUNCTIONS_HPP
#define INTEGRATORHELPERFUNCTIONS_HPP

#include <cmath>
#include <vector>

#include "Config.hpp"

#include <Eigen/Core>

#include <boost/mpl/at.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/map.hpp>

#include "cavity/Element.hpp"
#include "utils/QuadratureRules.hpp"

namespace integrator {
/*! \typedef Diagonal
 *  \brief functor handle to the calculation of the diagonal elements
 */
typedef pcm::function<double(const Element &)> Diagonal;

/*! \typedef KernelS
 *  \brief functor handle to the kernelS method in IGreensFunction
 */
typedef pcm::function<double(const Eigen::Vector3d &, const Eigen::Vector3d &)> KernelS;

/*! \typedef KernelD
 *  \brief functor handle to the kernelD method in IGreensFunction
 */
typedef pcm::function<double(const Eigen::Vector3d &, const Eigen::Vector3d &, const Eigen::Vector3d &)> KernelD;

/*! Approximate collocation diagonal for S
 *  \param[in] factor scaling factor for diagonal elements
 *  \param[in] eps    permittivity
 *  \param[in] el     finite element
 */
inline double SI(double factor, double eps, const Element & el)
{
    return (factor * std::sqrt(4 * M_PI / el.area()) * 1.0 / eps);
}

/*! Approximate collocation diagonal for D
 *  \param[in] factor scaling factor for diagonal elements
 *  \param[in] el     finite element
 */
inline double DI(double factor, const Element & el)
{
    return (-factor * std::sqrt(M_PI / el.area()) * 1.0 / el.sphere().radius);
}

/*! Returns matrix representation of the single layer operator by collocation
 *  \param[in] elements list of finite elements
 *  \param[in] diagS    functor for the evaluation of the diagonal of S
 *  \param[in] kernS    function for the evaluation of the off-diagonal of S
 */
inline Eigen::MatrixXd singleLayer(const std::vector<Element> & elements,
                                   const Diagonal & diagS, const KernelS & kernS)
{
    PCMSolverIndex mat_size = elements.size();
    Eigen::MatrixXd S = Eigen::MatrixXd::Zero(mat_size, mat_size);
    for (PCMSolverIndex i = 0; i < mat_size; ++i) {
        // Fill diagonal
        S(i, i) = diagS(elements[i]);
        Eigen::Vector3d source = elements[i].center();
        for (PCMSolverIndex j = 0; j < mat_size; ++j) {
            // Fill off-diagonal
            Eigen::Vector3d probe = elements[j].center();
            if (i != j) S(i, j) = kernS(source, probe);
        }
    }
    return S;
}

/*! Returns matrix representation of the double layer operator by collocation
 *  \param[in] elements list of finite elements
 *  \param[in] diagD    functor for the evaluation of the diagonal of D
 *  \param[in] kernD    function for the evaluation of the off-diagonal of D
 */
inline Eigen::MatrixXd doubleLayer(const std::vector<Element> & elements,
                                   const Diagonal & diagD, const KernelD & kernD)
{
    PCMSolverIndex mat_size = elements.size();
    Eigen::MatrixXd D = Eigen::MatrixXd::Zero(mat_size, mat_size);
    for (PCMSolverIndex i = 0; i < mat_size; ++i) {
        // Fill diagonal
        D(i, i) = diagD(elements[i]);
        Eigen::Vector3d source = elements[i].center();
        for (PCMSolverIndex j = 0; j < mat_size; ++j) {
            // Fill off-diagonal
            Eigen::Vector3d probe = elements[j].center();
            Eigen::Vector3d probeNormal = elements[j].normal();
            probeNormal.normalize();
            if (i != j) D(i, j) = kernD(probeNormal, source, probe);
        }
    }
    return D;
}

/*! \brief Integrates a single layer type operator on a single spherical polygon
 *  \date 2014
 *  \tparam PhiPoints Gaussian rule to be used in the angular phi integration
 *  \tparam ThetaPoints Gaussian rule to be used in the angular theta integration
 *
 *  This is needed for the numerical evaluation of the diagonal elements when using
 *  centroid collocation.
 */
template <int PhiPoints, int ThetaPoints>
double integrateS(const KernelS & F, const Element & e)
{
    double result = 0.0;

    // Get the quadrature rules for azimuthal and polar integrations
    namespace mpl = boost::mpl;
    typedef typename mpl::at<rules_map, mpl::int_<PhiPoints> >::type PhiPolicy;
    typedef typename mpl::at<rules_map, mpl::int_<ThetaPoints> >::type ThetaPolicy;
    QuadratureRule<PhiPolicy> phiRule;
    QuadratureRule<ThetaPolicy> thetaRule;
    int upper_phi = PhiPoints / 2; // Upper limit for loop on phi points
    int upper_theta = ThetaPoints / 2; // Upper limit for loop on theta points

    // Extract relevant data from Element
    int nVertices = e.nVertices();
    Eigen::Vector3d normal = e.normal();
    Sphere sph = e.sphere();
    Eigen::Matrix3Xd vertices = e.vertices();
    Eigen::Matrix3Xd arcs = e.arcs();

    // Calculation of the tangent and the bitangent (binormal) vectors
    // Tangent, Bitangent and Normal form a local reference frame:
    // T <-> x; B <-> y; N <-> z
    Eigen::Vector3d tangent, bitangent;
    tangent_and_bitangent(normal, tangent, bitangent);

    std::vector<double> theta(nVertices), phi(nVertices), phinumb(nVertices+1);
    std::vector<int> numb(nVertices+1);
    // Clean-up heap crap
    std::fill_n(theta.begin(),   nVertices,   0.0);
    std::fill_n(phi.begin(),     nVertices,   0.0);
    std::fill_n(numb.begin(),    nVertices+1, 0);
    std::fill_n(phinumb.begin(), nVertices+1, 0.0);
    // Populate arrays and redefine tangent and bitangent
    e.spherical_polygon(tangent, bitangent, theta, phi, phinumb, numb);

    // Actual integration occurs here
    for (int i = 0; i < nVertices; ++i) { // Loop on edges
        double phiLower = phinumb[i];
        double phiUpper = phinumb[i+1];
        double phiA= (phiUpper - phiLower) / 2.0;
        double phiB = (phiUpper + phiLower) / 2.0;
        double thetaLower = theta[numb[i]];
        double thetaUpper = theta[numb[i+1]];
        double thetaMax = 0.0;
        Eigen::Vector3d oc = (arcs.col(i) - sph.center) / sph.radius;
        double oc_norm = oc.norm();
        double oc_norm2 = std::pow(oc_norm, 2);
        for (int j = 0; j < upper_phi; ++j) { // Loop on Gaussian points: phi integration
            for (int k = 0; k <= 1; ++k) {
                double ph = (2*k - 1) * phiA * phiRule.abscissa(j) + phiB;
                double cos_ph = std::cos(ph);
                double sin_ph = std::sin(ph);
                // We need to calculate the upper bound for the integration on theta, which depends on phi
                if (oc_norm2 < 1.0e-07) { // This means that the edge is centered on the same sphere the tessera belongs to
                    double cotg_thmax = (std::sin(ph-phiLower) / std::tan(thetaUpper) + std::sin(phiUpper-ph) / std::tan(
                                thetaLower)) / std::sin(phiUpper - phiLower);
                    thetaMax = std::atan(1.0 / cotg_thmax);
                } else {
                    Eigen::Vector3d scratch;
                    scratch << tangent.dot(oc), bitangent.dot(oc), normal.dot(oc);
                    double aa = std::pow(tangent.dot(oc)*cos_ph + bitangent.dot(oc)*sin_ph,
                            2) + std::pow(normal.dot(oc), 2);
                    double bb = -normal.dot(oc) * oc_norm2;
                    double cc = std::pow(oc_norm2,
                            2) - std::pow(tangent.dot(oc)*cos_ph + bitangent.dot(oc)*sin_ph, 2);
                    double ds = std::pow(bb, 2) - aa*cc;
                    if (ds < 0.0) ds = 0.0;
                    double cs = (-bb + std::sqrt(ds)) / aa;
                    if (cs > 1.0) cs = 1.0;
                    if (cs < -1.0) cs = 1.0;
                    thetaMax = std::acos(cs);
                }
                double scratch = 0.0;
                if (!(thetaMax < 1.0e-08)) {
                    double thetaA = thetaMax / 2.0;
                    for (int l = 0; l < upper_theta; ++l) { // Loop on Gaussian points: theta integration
                        for (int m = 0; m <= 1; ++m) {
                            double th = (2*m - 1) * thetaA * thetaRule.abscissa(l) + thetaA;
                            double cos_th = std::cos(th);
                            double sin_th = std::sin(th);
                            Eigen::Vector3d point;
                            point(0) = tangent(0) * sin_th * cos_ph
                                + bitangent(0) * sin_th * sin_ph
                                + normal(0) * (cos_th - 1.0);
                            point(1) = tangent(1) * sin_th * cos_ph
                                + bitangent(1) * sin_th * sin_ph
                                + normal(1) * (cos_th - 1.0);
                            point(2) = tangent(2) * sin_th * cos_ph
                                + bitangent(2) * sin_th * sin_ph
                                + normal(2) * (cos_th - 1.0);
                            double value = F(point,
                                    Eigen::Vector3d::Zero()); // Evaluate integrand at Gaussian point
                            scratch += std::pow(sph.radius, 2) * value * sin_th * thetaA * thetaRule.weight(l);
                        }
                    }
                    result += scratch * phiA * phiRule.weight(j);
                }
            }
        }
    }
    return result;
}

/*! \brief Integrates a double layer type operator on a single spherical polygon
 *  \date 2014
 *  \tparam PhiPoints Gaussian rule to be used in the angular phi integration
 *  \tparam ThetaPoints Gaussian rule to be used in the angular theta integration
 *
 *  This is needed for the numerical evaluation of the diagonal elements when using
 *  centroid collocation.
 */
template <int PhiPoints, int ThetaPoints>
double integrateD(const KernelD & F, const Element & e)
{
    double result = 0.0;

    // Get the quadrature rules for azimuthal and polar integrations
    namespace mpl = boost::mpl;
    typedef typename mpl::at<rules_map, mpl::int_<PhiPoints> >::type PhiPolicy;
    typedef typename mpl::at<rules_map, mpl::int_<ThetaPoints> >::type ThetaPolicy;
    QuadratureRule<PhiPolicy> phiRule;
    QuadratureRule<ThetaPolicy> thetaRule;
    int upper_phi = PhiPoints / 2; // Upper limit for loop on phi points
    int upper_theta = ThetaPoints / 2; // Upper limit for loop on theta points

    // Extract relevant data from Element
    int nVertices = e.nVertices();
    Eigen::Vector3d normal = e.normal();
    Sphere sph = e.sphere();
    Eigen::Matrix3Xd vertices = e.vertices();
    Eigen::Matrix3Xd arcs = e.arcs();

    // Calculation of the tangent and the bitangent (binormal) vectors
    // Tangent, Bitangent and Normal form a local reference frame:
    // T <-> x; B <-> y; N <-> z
    Eigen::Vector3d tangent, bitangent;
    tangent_and_bitangent(normal, tangent, bitangent);

    std::vector<double> theta(nVertices), phi(nVertices), phinumb(nVertices+1);
    std::vector<int> numb(nVertices+1);
    // Clean-up heap crap
    std::fill_n(theta.begin(),   nVertices,   0.0);
    std::fill_n(phi.begin(),     nVertices,   0.0);
    std::fill_n(numb.begin(),    nVertices+1, 0);
    std::fill_n(phinumb.begin(), nVertices+1, 0.0);
    // Populate arrays and redefine tangent and bitangent
    e.spherical_polygon(tangent, bitangent, theta, phi, phinumb, numb);

    // Actual integration occurs here
    for (int i = 0; i < nVertices; ++i) { // Loop on edges
        double phiLower = phinumb[i]; // Lower vertex of edge
        double phiUpper = phinumb[i+1]; // Upper vertex of edge
        double phiA = (phiUpper - phiLower) / 2.0;
        double phiB = (phiUpper + phiLower) / 2.0;
        double thetaLower = theta[numb[i]];
        double thetaUpper = theta[numb[i+1]];
        double thetaMax = 0.0;
        Eigen::Vector3d oc = (arcs.col(i) - sph.center) / sph.radius;
        double oc_norm = oc.norm();
        double oc_norm2 = std::pow(oc_norm, 2);
        for (int j = 0; j < upper_phi; ++j) { // Loop on Gaussian points: phi integration
            for (int k = 0; k <= 1; ++k) {
                double ph = (2*k - 1) * phiA * phiRule.abscissa(j) + phiB;
                double cos_phi = std::cos(ph);
                double sin_phi = std::sin(ph);
                if (oc_norm2 < 1.0e-07) { // This should check if oc_norm2 is zero
                    double cotg_thmax = (std::sin(ph-phiLower) / std::tan(thetaUpper) + std::sin(phiUpper-ph) / std::tan(
                                thetaLower)) / std::sin(phiUpper - phiLower);
                    thetaMax = std::atan(1.0 / cotg_thmax);
                } else {
                    Eigen::Vector3d scratch;
                    scratch << tangent.dot(oc), bitangent.dot(oc), normal.dot(oc);
                    double aa = std::pow(tangent.dot(oc)*cos_phi + bitangent.dot(oc)*sin_phi,
                            2) + std::pow(normal.dot(oc), 2);
                    double bb = -normal.dot(oc) * oc_norm2;
                    double cc = std::pow(oc_norm2,
                            2) - std::pow(tangent.dot(oc)*cos_phi + bitangent.dot(oc)*sin_phi, 2);
                    double ds = std::pow(bb, 2) - aa*cc;
                    if (ds < 0.0) ds = 0.0;
                    double cs = (-bb + std::sqrt(ds)) / aa;
                    if (cs > 1.0) cs = 1.0;
                    if (cs < -1.0) cs = 1.0;
                    thetaMax = std::acos(cs);
                }
                double thetaA = thetaMax / 2.0;
                double scratch = 0.0;
                if (!(thetaMax < 1.0e-08)) {
                    for (int l = 0; l < upper_theta; ++l) { // Loop on Gaussian points: theta integration
                        for (int m = 0; m <= 1; ++m) {
                            double th = (2*m - 1) * thetaA  * thetaRule.abscissa(l) + thetaA;
                            double cos_theta = std::cos(th);
                            double sin_theta = std::sin(th);
                            Eigen::Vector3d point;
                            point(0) = tangent(0) * sin_theta * cos_phi
                                + bitangent(0) * sin_theta * sin_phi
                                + normal(0) * (cos_theta - 1.0);
                            point(1) = tangent(1) * sin_theta * cos_phi
                                + bitangent(1) * sin_theta * sin_phi
                                + normal(1) * (cos_theta - 1.0);
                            point(2) = tangent(2) * sin_theta * cos_phi
                                + bitangent(2) * sin_theta * sin_phi
                                + normal(2) * (cos_theta - 1.0);
                            double value = F(e.normal(),
                                    Eigen::Vector3d::Zero(),
                                    point); // Evaluate integrand at Gaussian point
                            scratch += std::pow(sph.radius, 2) * value * sin_theta * thetaA * thetaRule.weight(l);
                        }
                    }
                    result += scratch * phiA * phiRule.weight(j);
                }
            }
        }
    }
    return result;
}
} // namespace integrator

#endif // INTEGRATORHELPERFUNCTIONS_HPP


#include <cmath>
#include <iostream>
#include <vector>

#include <boost/align/aligned_allocator.hpp>
#include <boost/align/aligned_delete.hpp>

#include <boost/simd/function/cos.hpp>
#include <boost/simd/function/load.hpp>
#include <boost/simd/function/sin.hpp>
#include <boost/simd/function/sincos.hpp>
#include <boost/simd/function/store.hpp>
#include <boost/simd/pack.hpp>

int main(int argc, char** argv)
{
  namespace bs = boost::simd;
  namespace ba = boost::alignment;

  using pack_t = bs::pack<float>;

  std::size_t num_elements = 1024;
  std::size_t alignment    = pack_t::alignment;
  //! [transcendental-declare]
  std::vector<float, ba::aligned_allocator<float, pack_t::alignment>> X(num_elements);
  std::vector<float, ba::aligned_allocator<float, pack_t::alignment>> sinX(num_elements);
  std::vector<float, ba::aligned_allocator<float, pack_t::alignment>> cosX(num_elements);
  //! [transcendental-declare]

  //! [transcendental-scalar]
  for (int i = 0; i < num_elements; ++i) {
    sinX[i] = std::sin(X[i]);
    cosX[i] = std::cos(X[i]);
  }
  //! [transcendental-scalar]

  //! [transcendental-calc-individ]
  for (int i = 0; i < num_elements; i += pack_t::static_size) {
    pack_t v0 = bs::load<pack_t>(&X[i]);
    bs::store(bs::sin(v0), &sinX[i]);
    bs::store(bs::cos(v0), &sinX[i]);
  }
  //! [transcendental-calc-individ]

  //! [transcendental-calc-combine]
  for (int i = 0; i < num_elements; i += pack_t::static_size) {
    pack_t v0 = bs::load<pack_t>(&X[i]);
    auto res  = bs::sincos(v0);
    bs::store(res.first, &sinX[i]);
    bs::store(res.second, &cosX[i]);
  }
  //! [transcendental-calc-combine]

  //! [transcendental-calc-small]
  for (int i = 0; i < num_elements; i += pack_t::static_size) {
    pack_t v0 = bs::load<pack_t>(&X[i]);
    bs::store(bs::sin(v0, bs::tag::small_), &sinX[i]);
  }
  //! [transcendental-calc-small]
}