#ifndef OSRM_OPENING_HOURS_HPP
#define OSRM_OPENING_HOURS_HPP

#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/spirit/include/phoenix.hpp>
#include <boost/spirit/include/qi.hpp>

#include <boost/io/ios_state.hpp>

#include <algorithm>
#include <cctype>
#include <iomanip>
#include <iterator>
#include <limits>
#include <string>

namespace osrm
{
namespace util
{

// Helper classes for "opening hours" format http://wiki.openstreetmap.org/wiki/Key:opening_hours
// Supported simplified features in CheckOpeningHours:
// - Year/Month/Day ranges
// - Weekday ranges
// - Time ranges
// Not supported:
// - Week numbers
// - Holidays, events, variables dates
// - Day offsets and periodic ranges
struct OpeningHours
{
    enum Modifier
    {
        unknown,
        open,
        closed,
        off,
        is24_7
    };

    struct Time
    {
        enum Event : unsigned char
        {
            invalid,
            none,
            dawn,
            sunrise,
            sunset,
            dusk
        };

        Event event;
        std::int32_t minutes;

        Time() : event(invalid), minutes(0) {}
        Time(Event event) : event(event), minutes(0) {}
        Time(char hour, char min) : event(none), minutes(hour * 60 + min) {}
        Time(Event event, bool positive, const Time &offset)
            : event(event), minutes(positive ? offset.minutes : -offset.minutes)
        {
        }
    };

    struct TimeSpan
    {
        Time from, to;
        TimeSpan() = default;
        TimeSpan(const Time &from_, const Time &to_) : from(from_), to(to_)
        {
            if (to.minutes < from.minutes)
                to.minutes += 24 * 60;
        }

        bool IsInRange(const struct tm &time, bool &use_curr_day, bool &use_next_day) const
        {
            // TODO: events are not handled
            if (from.event != OpeningHours::Time::none || to.event != OpeningHours::Time::none)
                return false;

            const auto minutes = time.tm_hour * 60 + time.tm_min;
            if (to.minutes > 24 * 60)
            {
                use_curr_day = (from.minutes <= minutes); // in range [from, 24:00) current day
                use_next_day = (minutes < to.minutes - 24 * 60); // in range [00:00, to) next day
            }
            else
            {
                use_curr_day =
                    (from.minutes <= minutes && minutes < to.minutes); // in range [from, to)
                use_next_day = false;                                  // do not use the next day
            }

            return use_curr_day || use_next_day;
        }
    };

    struct WeekdayRange
    {
        int weekdays, overnight_weekdays;
        WeekdayRange() = default;
        WeekdayRange(unsigned char from, unsigned char to)
        {
            // weekdays mask for [from, to], e.g [2, 5] -> 0111100, [5, 2] -> 1100111,
            //  [3, 3] -> 0001000, [0,6] -> 1111111, [6,0] -> 1000001, [4, 3] -> 1111111
            weekdays = (from <= to) ? ((1 << (to - from + 1)) - 1) << from
                                    : ~(((1 << (from - to - 1)) - 1) << (to + 1));
            weekdays &= 0x7f;
            overnight_weekdays = (weekdays << 1) | (weekdays & 0x40 ? 1 : 0);
        }

        bool IsInRange(const struct tm &time, bool use_curr_day, bool use_next_day) const
        {
            return (use_curr_day && weekdays & (1 << time.tm_wday)) ||
                   (use_next_day && overnight_weekdays & (1 << time.tm_wday));
        }
    };

    struct Monthday
    {
        int year;
        char month;
        char day;
        Monthday() = default;
        Monthday(int year) : year(year), month(0), day(0) {}
        Monthday(int year, char month, char day) : year(year), month(month), day(day) {}

        bool IsValid() const { return year > 0 || month != 0 || day != 0; }
        bool operator==(const Monthday &rhs) const
        {
            return std::tie(year, month, day) == std::tie(rhs.year, rhs.month, rhs.day);
        }
    };

    struct MonthdayRange
    {
        Monthday from, to;
        MonthdayRange() : from(0, 0, 0), to(0, 0, 0) {}
        MonthdayRange(const Monthday &from, const Monthday &to) : from(from), to(to) {}

        bool IsInRange(const struct tm &time, bool use_curr_day, bool use_next_day) const
        {
            using boost::gregorian::date;
            using boost::gregorian::date_duration;

            const auto year = time.tm_year + 1900;
            const auto month = time.tm_mon + 1;

            date date_current(year, month, time.tm_mday);
            date date_from(boost::gregorian::min_date_time);
            date date_to(boost::gregorian::max_date_time);

            if (from.IsValid())
            {
                date_from = (from.day == 0) ? date(from.year == 0 ? year : from.year,
                                                   from.month == 0 ? month : from.month,
                                                   1)
                                            : date(from.year == 0 ? year : from.year,
                                                   from.month == 0 ? month : from.month,
                                                   from.day);
            }
            if (to.IsValid())
            {
                date_to = date(to.year == 0 ? (from.year == 0 ? year : from.year) : to.year,
                               to.month == 0 ? (from.month == 0 ? month : from.month) : to.month,
                               1);
                date_to = (to.day == 0) ? date_to.end_of_month()
                                        : date(date_to.year(), date_to.month(), to.day);
            }
            else if (to == Monthday())
            {
                date_to = date_from;
            }

            if (!use_curr_day)
                date_from += date_duration(1);
            if (use_next_day && date_to != date(boost::gregorian::max_date_time))
                date_to += date_duration(1);

            return date_from <= date_current && date_current <= date_to;
        }
    };

    OpeningHours() : modifier(open) {}

    bool IsInRange(const struct tm &time) const
    {
        bool use_curr_day = true;  // the first matching time uses the current day
        bool use_next_day = false; // the first matching time uses the next day
        return
            // the value is in range if time is not specified or is in any time range
            // (also modifies use_curr_day and use_next_day flags to handle overnight day ranges,
            // e.g. for 22:00-03:00 and 2am -> use_curr_day = false and use_next_day = true)
            (times.empty() || std::any_of(times.begin(),
                                          times.end(),
                                          [&time, &use_curr_day, &use_next_day](const auto &x) {
                                              return x.IsInRange(time, use_curr_day, use_next_day);
                                          }))
            // .. and if weekdays are not specified or matches weekdays range
            && (weekdays.empty() ||
                std::any_of(weekdays.begin(),
                            weekdays.end(),
                            [&time, use_curr_day, use_next_day](const auto &x) {
                                return x.IsInRange(time, use_curr_day, use_next_day);
                            }))
            // .. and if month-day ranges are not specified or is in any month-day range
            && (monthdays.empty() ||
                std::any_of(monthdays.begin(),
                            monthdays.end(),
                            [&time, use_curr_day, use_next_day](const auto &x) {
                                return x.IsInRange(time, use_curr_day, use_next_day);
                            }));
    }

    std::vector<TimeSpan> times;
    std::vector<WeekdayRange> weekdays;
    std::vector<MonthdayRange> monthdays;
    Modifier modifier;
};

#ifndef NDEBUG
// Debug output stream operators for use with BOOST_SPIRIT_DEBUG
inline std::ostream &operator<<(std::ostream &stream, const OpeningHours::Modifier value)
{
    switch (value)
    {
    case OpeningHours::unknown:
        return stream << "unknown";
    case OpeningHours::open:
        return stream << "open";
    case OpeningHours::closed:
        return stream << "closed";
    case OpeningHours::off:
        return stream << "off";
    case OpeningHours::is24_7:
        return stream << "24/7";
    }
    return stream;
}

inline std::ostream &operator<<(std::ostream &stream, const OpeningHours::Time::Event value)
{
    switch (value)
    {
    case OpeningHours::Time::dawn:
        return stream << "dawn";
    case OpeningHours::Time::sunrise:
        return stream << "sunrise";
    case OpeningHours::Time::sunset:
        return stream << "sunset";
    case OpeningHours::Time::dusk:
        return stream << "dusk";
    default:
        break;
    }
    return stream;
}

inline std::ostream &operator<<(std::ostream &stream, const OpeningHours::Time &value)
{
    boost::io::ios_flags_saver ifs(stream);
    if (value.event == OpeningHours::Time::invalid)
        return stream << "???";
    if (value.event == OpeningHours::Time::none)
        return stream << std::setfill('0') << std::setw(2) << value.minutes / 60 << ":"
                      << std::setfill('0') << std::setw(2) << value.minutes % 60;
    stream << value.event;
    if (value.minutes != 0)
        stream << value.minutes;
    return stream;
}

inline std::ostream &operator<<(std::ostream &stream, const OpeningHours::TimeSpan &value)
{
    return stream << value.from << "-" << value.to;
}

inline std::ostream &operator<<(std::ostream &stream, const OpeningHours::Monthday &value)
{
    bool empty = true;
    if (value.year != 0)
    {
        stream << (int)value.year;
        empty = false;
    };
    if (value.month != 0)
    {
        stream << (empty ? "" : "/") << (int)value.month;
        empty = false;
    };
    if (value.day != 0)
    {
        stream << (empty ? "" : "/") << (int)value.day;
    };
    return stream;
}

inline std::ostream &operator<<(std::ostream &stream, const OpeningHours::WeekdayRange &value)
{
    boost::io::ios_flags_saver ifs(stream);
    return stream << std::hex << std::setfill('0') << std::setw(2) << value.weekdays;
}

inline std::ostream &operator<<(std::ostream &stream, const OpeningHours::MonthdayRange &value)
{
    return stream << value.from << "-" << value.to;
}

inline std::ostream &operator<<(std::ostream &stream, const OpeningHours &value)
{
    if (value.modifier == OpeningHours::is24_7)
        return stream << OpeningHours::is24_7;

    for (auto x : value.monthdays)
        stream << x << ", ";
    for (auto x : value.weekdays)
        stream << x << ", ";
    for (auto x : value.times)
        stream << x << ", ";
    return stream << " |" << value.modifier << "|";
}
#endif

namespace detail
{

namespace
{
namespace ph = boost::phoenix;
namespace qi = boost::spirit::qi;
}

template <typename Iterator, typename Skipper = qi::blank_type>
struct opening_hours_grammar : qi::grammar<Iterator, Skipper, std::vector<OpeningHours>()>
{
    // http://wiki.openstreetmap.org/wiki/Key:opening_hours/specification
    opening_hours_grammar() : opening_hours_grammar::base_type(time_domain)
    {
        using qi::_1;
        using qi::_a;
        using qi::_b;
        using qi::_c;
        using qi::_r1;
        using qi::_pass;
        using qi::_val;
        using qi::eoi;
        using qi::lit;
        using qi::char_;
        using qi::uint_;
        using oh = osrm::util::OpeningHours;

        // clang-format off

        // General syntax
        time_domain = rule_sequence[ph::push_back(_val, _1)] % any_rule_separator;

        rule_sequence
            = lit("24/7")[ph::bind(&oh::modifier, _val) = oh::is24_7]
            | (selector_sequence[_val = _1] >> -rule_modifier[ph::bind(&oh::modifier, _val) = _1] >> -comment)
            | comment
            ;

        any_rule_separator = char_(';') | lit("||") | additional_rule_separator;

        additional_rule_separator = char_(',');

        // Rule modifiers
        rule_modifier.add
            ("unknown", oh::unknown)
            ("open", oh::open)
            ("closed", oh::closed)
            ("off", oh::off)
            ;

        // Selectors
        selector_sequence = (wide_range_selectors(_a) >> small_range_selectors(_a))[_val = _a];

        wide_range_selectors
            = (-monthday_selector(_r1)
               >> -year_selector(_r1)
               >> -week_selector(_r1) // TODO week_selector
              ) >> -lit(':')
            ;

        small_range_selectors = -(weekday_selector(_r1) >> (&~lit(',') | eoi)) >> -time_selector(_r1);

        // Time selector
        time_selector = (timespan % ',')[ph::bind(&OpeningHours::times, _r1) = _1];

        timespan
            = (time[_a = _1]
               >> -(lit('+')[_b = ph::construct<OpeningHours::Time>(24, 0)]
                    | ('-' >> extended_time[_b = _1]
                       >> -('+' | '/' >> (minute | hour_minutes))))
               )[_val = ph::construct<OpeningHours::TimeSpan>(_a, _b)]
            ;

        time = hour_minutes | variable_time;

        extended_time = extended_hour_minutes | variable_time;

        variable_time
            = event[_val = ph::construct<OpeningHours::Time>(_1)]
            | ('(' >> event[_a = _1] >> plus_or_minus[_b = _1] >> hour_minutes[_c = _1] >> ')')
            [_val = ph::construct<OpeningHours::Time>(_a, _b, _c)]
            ;

        event.add
            ("dawn", OpeningHours::Time::dawn)
            ("sunrise", OpeningHours::Time::sunrise)
            ("sunset", OpeningHours::Time::sunset)
            ("dusk", OpeningHours::Time::dusk)
            ;

        // Weekday selector
        weekday_selector
            = (holiday_sequence(_r1) >> -(char_(", ") >> weekday_sequence(_r1)))
            | (weekday_sequence(_r1) >> -(char_(", ") >> holiday_sequence(_r1)))
            ;

        weekday_sequence = (weekday_range % ',')[ph::bind(&OpeningHours::weekdays, _r1) = _1];

        weekday_range
            = wday[_a = _1, _b = _1]
            >> -(('-' >> wday[_b = _1])
                 | ('[' >> (nth_entry % ',') >> ']' >> -day_offset))
            [_val = ph::construct<OpeningHours::WeekdayRange>(_a, _b)]
            ;

        holiday_sequence = (lit("SH") >> -day_offset) | lit("PH");

        nth_entry = nth | nth >> '-' >> nth | '-' >> nth;

        nth = char_("12345");

        day_offset = plus_or_minus >> uint_ >> lit("days");

        // Week selector
        week_selector = (lit("week ") >> week) % ',';

        week = weeknum >> -('-' >> weeknum >> -('/' >> uint_));

        // Month selector
        monthday_selector = (monthday_range % ',')[ph::bind(&OpeningHours::monthdays, _r1) = _1];

        monthday_range
            = (date_from[ph::bind(&OpeningHours::MonthdayRange::from, _val) = _1]
               >> -date_offset
               >> '-'
               >> date_to[ph::bind(&OpeningHours::MonthdayRange::to, _val) = _1]
               >> -date_offset)
            | (date_from[ph::bind(&OpeningHours::MonthdayRange::from, _val) = _1]
               >> -(date_offset
                    >> -lit('+')[ph::bind(&OpeningHours::MonthdayRange::from, _val) = ph::construct<OpeningHours::Monthday>(-1)]
                    ))
            ;

        date_offset = (plus_or_minus >> wday) | day_offset;

        date_from
            = ((-year[_a = _1] >> ((month[_b = _1] >> -daynum[_c = _1]) | daynum[_c = _1]))
               | variable_date)
            [_val = ph::construct<OpeningHours::Monthday>(_a, _b, _c)]
            ;

        date_to
            = date_from[_val = _1]
            | daynum[_val = ph::construct<OpeningHours::Monthday>(0, 0, _1)]
            ;

        variable_date = lit("easter");

        // Year selector
        year_selector = (year_range % ',')[ph::bind(&OpeningHours::monthdays, _r1) = _1];

        year_range
            = year[ph::bind(&OpeningHours::MonthdayRange::from, _val) = ph::construct<OpeningHours::Monthday>(_1)]
            >> -(('-' >> year[ph::bind(&OpeningHours::MonthdayRange::to, _val) = ph::construct<OpeningHours::Monthday>(_1)]
                  >> -('/' >> uint_))
                 | lit('+')[ph::bind(&OpeningHours::MonthdayRange::to, _val) = ph::construct<OpeningHours::Monthday>(-1)]);

        // Basic elements
        plus_or_minus = lit('+')[_val = true] | lit('-')[_val = false];

        hour = uint2_p[_pass = bind([](unsigned x) { return x <= 24; }, _1), _val = _1];

        extended_hour = uint2_p[_pass = bind([](unsigned x) { return x <= 48; }, _1), _val = _1];

        minute = uint2_p[_pass = bind([](unsigned x) { return x < 60; }, _1), _val = _1];

        hour_minutes =
            hour[_a = _1] >> ':' >> minute[_val = ph::construct<OpeningHours::Time>(_a, _1)];

        extended_hour_minutes = extended_hour[_a = _1] >> ':' >>
                                minute[_val = ph::construct<OpeningHours::Time>(_a, _1)];

        wday.add
            ("Su", 0)
            ("Mo", 1)
            ("Tu", 2)
            ("We", 3)
            ("Th", 4)
            ("Fr", 5)
            ("Sa", 6)
            ;

        daynum
            = uint2_p[_pass = bind([](unsigned x) { return 01 <= x && x <= 31; }, _1), _val = _1]
            >> (&~lit(':') | eoi)
            ;

        weeknum = uint2_p[_pass = bind([](unsigned x) { return 01 <= x && x <= 53; }, _1), _val = _1];

        month.add
            ("Jan", 1)
            ("Feb", 2)
            ("Mar", 3)
            ("Apr", 4)
            ("May", 5)
            ("Jun", 6)
            ("Jul", 7)
            ("Aug", 8)
            ("Sep", 9)
            ("Oct", 10)
            ("Nov", 11)
            ("Dec", 12)
            ;

        year = uint4_p[_pass = bind([](unsigned x) { return x > 1900; }, _1), _val = _1];

        comment = lit('"') >> *(~qi::char_('"')) >> lit('"');

        // clang-format on

        BOOST_SPIRIT_DEBUG_NODES((time_domain)(rule_sequence)(any_rule_separator)(
            selector_sequence)(wide_range_selectors)(small_range_selectors)(time_selector)(
            timespan)(time)(extended_time)(variable_time)(weekday_selector)(weekday_sequence)(
            weekday_range)(holiday_sequence)(nth_entry)(nth)(day_offset)(week_selector)(week)(
            monthday_selector)(monthday_range)(date_offset)(date_from)(date_to)(variable_date)(
            year_selector)(year_range)(plus_or_minus)(hour_minutes)(extended_hour_minutes)(comment)(
            hour)(extended_hour)(minute)(daynum)(weeknum)(year));
    }

    qi::rule<Iterator, Skipper, std::vector<OpeningHours>()> time_domain;
    qi::rule<Iterator, Skipper, OpeningHours()> rule_sequence;
    qi::rule<Iterator, Skipper, void()> any_rule_separator, additional_rule_separator;
    qi::rule<Iterator, Skipper, OpeningHours(), qi::locals<OpeningHours>> selector_sequence;
    qi::symbols<char const, OpeningHours::Modifier> rule_modifier;
    qi::rule<Iterator, Skipper, void(OpeningHours &)> wide_range_selectors, small_range_selectors,
        time_selector, weekday_selector, year_selector, monthday_selector, week_selector;

    // Time rules
    qi::rule<Iterator,
             Skipper,
             OpeningHours::TimeSpan(),
             qi::locals<OpeningHours::Time, OpeningHours::Time>>
        timespan;

    qi::rule<Iterator, Skipper, OpeningHours::Time()> time, extended_time;

    qi::rule<Iterator,
             Skipper,
             OpeningHours::Time(),
             qi::locals<OpeningHours::Time::Event, bool, OpeningHours::Time>>
        variable_time;

    qi::rule<Iterator, Skipper, OpeningHours::Time(), qi::locals<unsigned>> hour_minutes,
        extended_hour_minutes;

    qi::symbols<char const, OpeningHours::Time::Event> event;

    qi::rule<Iterator, Skipper, bool()> plus_or_minus;

    // Weekday rules
    qi::rule<Iterator, Skipper, void(OpeningHours &)> weekday_sequence, holiday_sequence;

    qi::rule<Iterator,
             Skipper,
             OpeningHours::WeekdayRange(),
             qi::locals<unsigned char, unsigned char>>
        weekday_range;

    // Monthday rules
    qi::rule<Iterator, Skipper, OpeningHours::MonthdayRange()> monthday_range;

    qi::rule<Iterator, Skipper, OpeningHours::Monthday(), qi::locals<unsigned, unsigned, unsigned>>
        date_from;

    qi::rule<Iterator, Skipper, OpeningHours::Monthday()> date_to;

    // Year rules
    qi::rule<Iterator, Skipper, OpeningHours::MonthdayRange()> year_range;

    // Unused rules
    qi::rule<Iterator, Skipper, void()> nth_entry, nth, day_offset, week, date_offset,
        variable_date, comment;

    // Basic rules and parsers
    qi::rule<Iterator, Skipper, unsigned()> hour, extended_hour, minute, daynum, weeknum, year;
    qi::symbols<char const, unsigned char> wday, month;
    qi::uint_parser<unsigned, 10, 2, 2> uint2_p;
    qi::uint_parser<unsigned, 10, 4, 4> uint4_p;
};
}

inline std::vector<OpeningHours> ParseOpeningHours(const std::string &str)
{
    auto it(str.begin()), end(str.end());
    const detail::opening_hours_grammar<decltype(it)> static grammar;

    std::vector<OpeningHours> result;
    bool ok = boost::spirit::qi::phrase_parse(it, end, grammar, boost::spirit::qi::blank, result);

    if (!ok || it != end)
        return std::vector<OpeningHours>();

    return result;
}

inline bool CheckOpeningHours(const std::vector<OpeningHours> &input, const struct tm &time)
{
    bool is_open = false;
    for (auto &opening_hours : input)
    {
        if (opening_hours.modifier == OpeningHours::is24_7)
            return true;

        if (opening_hours.IsInRange(time))
        {
            is_open = opening_hours.modifier == OpeningHours::open;
        }
    }

    return is_open;
}

} // util
} // osrm

#endif // OSRM_OPENING_HOURS_HPP


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

#include "clotho/data_spaces/population_space/population_space_row.hpp"
#include <boost/random/bernoulli_distribution.hpp>
#include "clotho/data_spaces/crossover/position_classifier.hpp"
#include "clotho/data_spaces/generators/position_distribution_helper.hpp"
#include "clotho/data_spaces/generators/crossover_event_distribution_helper.hpp"

#include "clotho/data_spaces/crossover/block_crossover_method.hpp"

namespace clotho {
namespace genetics {

template < class RNG, class PopulationType, class AlleleType >
class common_crossover;

template < class RNG, class BlockType, class WeightType, class AlleleType >
class common_crossover< RNG, population_space_row< BlockType, WeightType >, AlleleType >  {
public:
    typedef RNG random_engine_type;

    typedef population_space_row< BlockType, WeightType >       space_type;
    typedef typename space_type::genome_pointer                 genome_pointer;
    typedef AlleleType allele_type;

    typedef PositionClassifier< typename allele_type::position_vector > classifier_type;
    typedef typename classifier_type::event_type                        event_type;

    typedef block_crossover_method< classifier_type, BlockType >     method_type;

    typedef typename position_distribution_helper< typename allele_type::position_type >::type  position_distribution_type;
    typedef typename crossover_event_distribution_helper< double >::type                    event_distribution_type;

    common_crossover( random_engine_type * rng, allele_type * alleles, double recomb_rate, double bias_rate ) :
        m_rng( rng )
        , m_alleles( alleles )
        , m_event_dist(recomb_rate)
        , m_bias_dist( bias_rate )
    { }

    void operator()( genome_pointer p0_start, genome_pointer p0_end, genome_pointer p1_start, genome_pointer p1_end, genome_pointer offspring, genome_pointer offspring_end ) {
        event_type evts;
        fill_events( evts, m_event_dist(*m_rng));
        classifier_type cfier0( &m_alleles->getPositions(), evts );

        run_crossover_task( cfier0, p0_start, p0_end, p1_start, p1_end, offspring, offspring_end, m_bias_dist(*m_rng));
    }

    virtual ~common_crossover() {}

protected:

    void fill_events( event_type & evt, unsigned int N ) {
        while( N-- ) {
            evt.push_back( m_pos_dist( *m_rng ) );
        }
    }

    void run_crossover_task( const classifier_type & cls, genome_pointer p0_start, genome_pointer p0_end, genome_pointer p1_start, genome_pointer p1_end, genome_pointer offspring, genome_pointer offspring_end, bool should_swap_strands ) {
        if( cls.event_count() == 0 ) {
            genome_pointer first = p0_start, last = p0_end;
            if( should_swap_strands ) {
                first = p1_start;
                last = p1_end;
            }

            while( first != last ) {
                *offspring++ = *first++;
            }

            while( offspring != offspring_end ) {
                *offspring++ = method_type::bit_helper_type::ALL_UNSET;
            }
        } else if( should_swap_strands ) {
            method_type met(cls);
            met( p1_start, p1_end, p0_start, p0_end, offspring, offspring_end);
        } else {
            method_type met(cls);
            met( p0_start, p0_end, p1_start, p1_end, offspring, offspring_end);
        }
    }

    random_engine_type          * m_rng;
    allele_type                 * m_alleles;
    event_distribution_type     m_event_dist;
    boost::random::bernoulli_distribution< double > m_bias_dist;
    position_distribution_type m_pos_dist;
};

}   // namespace genetics
}   // namespace clotho

#endif  // CLOTHO_COMMON_CROSSOVER_HPP_


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2014 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2014 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2014 Mateusz Loskot, London, UK.
// Copyright (c) 2013-2014 Adam Wulkiewicz, Lodz, Poland.

// This file was modified by Oracle on 2013-2020.
// Modifications copyright (c) 2013-2020, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISJOINT_AREAL_AREAL_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISJOINT_AREAL_AREAL_HPP

#include <boost/geometry/core/point_type.hpp>

#include <boost/geometry/algorithms/covered_by.hpp>
#include <boost/geometry/algorithms/detail/for_each_range.hpp>
#include <boost/geometry/algorithms/detail/point_on_border.hpp>

#include <boost/geometry/algorithms/detail/disjoint/linear_linear.hpp>
#include <boost/geometry/algorithms/detail/disjoint/segment_box.hpp>

#include <boost/geometry/algorithms/for_each.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace disjoint
{

template <typename Geometry1, typename Geometry2, typename Strategy>
inline bool point_on_border_covered_by(Geometry1 const& geometry1,
                                       Geometry2 const& geometry2,
                                       Strategy const& strategy)
{
    typename geometry::point_type<Geometry1>::type pt;
    return geometry::point_on_border(pt, geometry1)
        && geometry::covered_by(pt, geometry2, strategy);
}

/*!
\tparam Strategy point_in_geometry strategy
*/
template <typename Geometry1, typename Geometry2, typename Strategy>
inline bool rings_containing(Geometry1 const& geometry1,
                             Geometry2 const& geometry2,
                             Strategy const& strategy)
{
    return geometry::detail::any_range_of(geometry2, [&](auto const& range)
    {
        return point_on_border_covered_by(range, geometry1, strategy);
    });
}

template <typename Geometry1, typename Geometry2>
struct areal_areal
{
    /*!
    \tparam Strategy relate (segments intersection) strategy
    */
    template <typename Strategy>
    static inline bool apply(Geometry1 const& geometry1,
                             Geometry2 const& geometry2,
                             Strategy const& strategy)
    {
        if ( ! disjoint_linear<Geometry1, Geometry2>::apply(geometry1, geometry2, strategy) )
        {
            return false;
        }

        // If there is no intersection of segments, they might located
        // inside each other

        // We check that using a point on the border (external boundary),
        // and see if that is contained in the other geometry. And vice versa.

        if ( rings_containing(geometry1, geometry2, strategy)
          || rings_containing(geometry2, geometry1, strategy) )
        {
            return false;
        }

        return true;
    }
};

template <typename Areal, typename Box>
struct areal_box
{
    /*!
    \tparam Strategy relate (segments intersection) strategy
    */
    template <typename Strategy>
    static inline bool apply(Areal const& areal,
                             Box const& box,
                             Strategy const& strategy)
    {
        if (! geometry::all_segments_of(areal, [&](auto const& s)
              {
                  return disjoint_segment_box::apply(s, box, strategy);
              }) )
        {
            return false;
        }

        // If there is no intersection of any segment and box,
        // the box might be located inside areal geometry

        if ( point_on_border_covered_by(box, areal, strategy) )
        {
            return false;
        }

        return true;
    }
};

}} // namespace detail::disjoint
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Areal1, typename Areal2>
struct disjoint<Areal1, Areal2, 2, areal_tag, areal_tag, false>
    : detail::disjoint::areal_areal<Areal1, Areal2>
{};

template <typename Areal, typename Box>
struct disjoint<Areal, Box, 2, areal_tag, box_tag, false>
    : detail::disjoint::areal_box<Areal, Box>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISJOINT_AREAL_AREAL_HPP


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file     gateway_utils.cpp
 * \author   Collin Johnson
 *
 * Definition of utility functions for constructing gateways:
 *
 *   - are_gateways_intersecting
 *   - are_gatway_angle_close
 *   - is_gateway_untraversable
 *   - select_straightest_gateway_boundary
 */

#include "hssh/local_topological/area_detection/gateways/gateway_utils.h"
#include "hssh/local_topological/area_detection/gateways/endpoint_validator.h"
#include "hssh/local_topological/area_detection/local_topo_isovist_field.h"
#include "hssh/local_topological/area_detection/voronoi/voronoi_utils.h"
#include "hssh/local_topological/gateway.h"
#include "hssh/local_topological/voronoi_skeleton_grid.h"
#include "utils/algorithm_ext.h"
#include "utils/ray_tracing.h"
#include <array>
#include <boost/range/iterator_range.hpp>

namespace vulcan
{
namespace hssh
{

const uint8_t kValidEndpointMask = SKELETON_CELL_OCCUPIED | SKELETON_CELL_FRONTIER;
const uint8_t kValidCenterMask = SKELETON_CELL_REDUCED_SKELETON;

cell_t skeleton_cell_between_endpoints(const Line<int>& cellBoundary, const VoronoiSkeletonGrid& grid);
cell_t nearest_valid_location(cell_t start, int searchRadius, uint8_t validMask, const VoronoiSkeletonGrid& grid);

bool are_gateways_intersecting(const Gateway& lhs, const Gateway& rhs)
{
    Point<int> intersection;

    return lhs.intersectsWithCellBoundary(rhs.cellBoundary(), intersection);
}

bool are_gateway_angles_close(const Gateway& lhs,
                              const Gateway& rhs,
                              double closeAngleThreshold,
                              double endpointDistanceThreshold)
{
    // Arrange the gateways so their closest endpoints are in the 0 index of an array, which allows the calculated angle
    // for each to show if they are really pointing the same direction or not.
    auto lhsBoundary = lhs.boundary();
    auto rhsBoundary = rhs.boundary();

    std::array<Point<double>, 2> lhsEnds = {{lhsBoundary.a, lhsBoundary.b}};
    std::array<Point<double>, 2> rhsEnds = {{rhsBoundary.a, rhsBoundary.b}};

    int closestLhsIndex = 0;
    int closestRhsIndex = 0;
    float minDistance = 10000000000.0f;

    for (int n = 0; n < 2; ++n) {
        for (int i = 0; i < 2; ++i) {
            float endpointDistance = distance_between_points(lhsEnds[n], rhsEnds[i]);
            if (endpointDistance < minDistance) {
                minDistance = endpointDistance;
                closestLhsIndex = n;
                closestRhsIndex = i;
            }
        }
    }

    if (closestLhsIndex != 0) {
        std::swap(lhsEnds[0], lhsEnds[1]);
    }

    if (closestRhsIndex != 0) {
        std::swap(rhsEnds[0], rhsEnds[1]);
    }

    bool areAnglesClose =
      std::abs(angle_diff(angle_to_point(lhsEnds[0], lhsEnds[1]), angle_to_point(rhsEnds[0], rhsEnds[1])))
      < closeAngleThreshold;
    bool areEndpointsClose = minDistance < endpointDistanceThreshold;

    return areAnglesClose && areEndpointsClose;
}

bool is_gateway_traversable(const Gateway& gateway, double robotLength, const VoronoiSkeletonGrid& skeleton)
{
    // Trace through the skeleton from the gateway cell. Go up to lengthInCells, as anything over that length isn't
    // needed. Go through as many branches as needed -- can't go through more than lengthInCells, so that's sufficient
    // Need to follow the full skeleton because a dead end might not cram in the back of a partially seen office
    std::size_t lengthInCells = robotLength * skeleton.cellsPerMeter();
    auto traces = trace_voronoi_graph(gateway.skeletonCell(), skeleton, lengthInCells, lengthInCells, false);

    // Find the longest trace for each side of the gateway
    CellToTypeMap<std::size_t> cellToMaxLength;
    for (auto& t : traces.traces) {
        if (t.points.size() > 1) {
            if (cellToMaxLength.find(t.points[1]) != cellToMaxLength.end()) {
                cellToMaxLength[t.points[1]] = std::max(cellToMaxLength[t.points[1]], t.points.size());
            } else {
                cellToMaxLength[t.points[1]] = t.points.size();
            }
        }
    }

    // See how many lengths along the reduced skeleton are long enough. Gateways can't exist at reduced junctions, but
    // can be at full skeleton junctions, so only check the reduced skeleton branches to see if they are long enough.
    int numLongEnough = 0;
    for (auto lengths : cellToMaxLength) {
        if ((skeleton.getClassification(lengths.first.x, lengths.first.y) & SKELETON_CELL_REDUCED_SKELETON)
            && (lengths.second == lengthInCells)) {
            ++numLongEnough;
        }
    }

    // Both branches of the reduced skeleton need to be long enough
    return numLongEnough == 2;
}

void gateway_boundary_cells(const Gateway& gateway, const VoronoiSkeletonGrid& grid, CellVector& boundaryCells)
{
    auto center = gateway.skeletonCell();
    auto boundary = gateway.cellBoundary();
    auto deltaA = boundary.a - center;
    auto deltaB = boundary.b - center;

    // Start all endpoints slightly inside the associated cell, which helps keep truncation
    // errors in the ray trace from causing the gateway to fall in the wrong cell
    Point<double> adjustedCenter(center.x + 0.01, center.y + 0.01);

    auto boundaryA =
      Line<double>(adjustedCenter, Point<double>(adjustedCenter.x + deltaA.x, adjustedCenter.y + deltaA.y));
    auto boundaryB =
      Line<double>(adjustedCenter, Point<double>(adjustedCenter.x + deltaB.x, adjustedCenter.y + deltaB.y));

    // Trace along the gateway boundary to get all cells it passes through
    utils::find_cells_along_line(boundaryA, grid, std::back_inserter(boundaryCells));
    utils::find_cells_along_line(boundaryB, grid, std::back_inserter(boundaryCells));

    utils::erase_unique(boundaryCells);
}

float gateway_cell_perimeter(const Gateway& gateway, float metersPerCell)
{
    // Iterate along the boundary. If a cell is four-way connected, then it adds 1 to perim. If 8-way connected,
    // it adds 2

    if (gateway.sizeCells() == 1) {
        return metersPerCell;
    }

    return cell_vector_perimeter(gateway.beginCells(), gateway.endCells(), metersPerCell);
}

double gateway_normal_from_source_cells(cell_t cell, const VoronoiSkeletonGrid& skeleton)
{
    double maxAngle = 0.0;
    double maxNormal = 0.0;

    auto sources = skeleton.getSourceCells(cell.x, cell.y);

    for (std::size_t n = 0; n < sources.size(); ++n) {
        for (std::size_t m = n + 1; m < sources.size(); ++m) {
            // Angular separation is relative to the skeleton cell
            double angleBetweenSources = std::abs(angle_between_points(sources[n], sources[m], cell));
            if (angleBetweenSources > maxAngle) {
                maxAngle = angleBetweenSources;
                maxNormal = angle_sum(angle_to_point(sources[n], sources[m]), M_PI_2);
            }
        }
    }

    return maxNormal;
}

boost::optional<Gateway> create_gateway_at_cell(cell_t cell,
                                                double normal,
                                                int32_t id,
                                                const VoronoiSkeletonGrid& skeleton,
                                                double maxExtraLength)
{
    auto cellBoundary = gateway_boundary_line_at_cell(cell, normal, skeleton, maxExtraLength);

    if (!cellBoundary) {
        return boost::none;
    }

    // If the cell isn't on the reduced skeleton, then find it
    if (~skeleton.getClassification(cell.x, cell.y) & SKELETON_CELL_REDUCED_SKELETON) {
        cell = skeleton_cell_between_endpoints(*cellBoundary, skeleton);

        if (!skeleton.isCellInGrid(cell)) {
            return boost::none;
        }
    }

    // Return a valid gateway that satisfies the post-condition
    Gateway gateway(skeleton.getTimestamp(), id, *cellBoundary, cell, skeleton);

    //     bool foundSkeleton = false;
    //     for(auto& cell : boost::make_iterator_range(gateway.beginCells(), gateway.endCells()))
    //     {
    //         if(skeleton.getClassification(cell.x, cell.y) & SKELETON_CELL_REDUCED_SKELETON)
    //         {
    //             if(foundSkeleton)
    //             {
    //                 std::cout << "Filtering gateway at " << gateway.skeletonCell() << " Double-crosser!";
    //                 return boost::none;
    //             }
    //
    //             foundSkeleton = true;
    //         }
    //     }

    return gateway;
}

boost::optional<Line<int>>
  gateway_boundary_line_at_cell(cell_t cell, double normal, const VoronoiSkeletonGrid& skeleton, double maxExtraLength)
{
    // The endpoints of the gateway are in the +/- pi/2 directions. Trace to the nearest obstacle.
    const double kGatewayLengthAddition = maxExtraLength;
    Line<int> cellBoundary;

    cellBoundary.a =
      utils::trace_ray_until_condition(cell,
                                       normal + M_PI_2,
                                       skeleton.getMetricDistance(cell.x, cell.y) + kGatewayLengthAddition,
                                       skeleton,
                                       VoronoiSkeletonTerminationFunc(kValidEndpointMask));
    cellBoundary.b =
      utils::trace_ray_until_condition(cell,
                                       normal - M_PI_2,
                                       skeleton.getMetricDistance(cell.x, cell.y) + kGatewayLengthAddition,
                                       skeleton,
                                       VoronoiSkeletonTerminationFunc(kValidEndpointMask));

    // If either cell boundary isn't on an unknown or occupied cell, then the ray trace failed to satisfy the condition
    // so no valid gateway exists.
    // If both ends are the same, it also can't be a valid gateway.
    if (!(skeleton.getClassification(cellBoundary.a.x, cellBoundary.a.y) & kValidEndpointMask)
        || !(skeleton.getClassification(cellBoundary.b.x, cellBoundary.b.y) & kValidEndpointMask)
        || (cellBoundary.a == cellBoundary.b)) {
        return boost::none;
    }

    return cellBoundary;
}

boost::optional<Gateway> create_gateway_between_sources(const CellVector& sources,
                                                        cell_t skeletonCell,
                                                        int32_t id,
                                                        const VoronoiSkeletonGrid& skeleton,
                                                        const VoronoiIsovistField& isovists)
{
    // Weird situation can result in a single source cell, for which there is obviously no
    if (sources.size() < 2) {
        return boost::none;
    }

    // Find the boundary amongst the gateways that is closest to the expected length, given the location of the
    // skeleton cell
    double optimalLength = 2.0 * skeleton.getMetricDistance(skeletonCell.x, skeletonCell.y) * skeleton.cellsPerMeter();
    Line<int> boundary(sources[0], sources[1]);
    for (std::size_t n = 0; n < sources.size(); ++n) {
        for (std::size_t m = n + 1; m < sources.size(); ++m) {
            double len = distance_between_points(sources[n], sources[m]);
            if (std::abs(len - optimalLength) < std::abs(length(boundary) - optimalLength)) {
                boundary.a = sources[n];
                boundary.b = sources[m];
            }
        }
    }

    // Construct a gateway from this boundary
    auto gatewaySkeleton = skeleton_cell_between_endpoints(boundary, skeleton);
    // If a valid cell was found, then create the gateway
    if ((gatewaySkeleton.x >= 0) && (gatewaySkeleton.y >= 0)) {
        assert(skeleton.getClassification(gatewaySkeleton.x, gatewaySkeleton.y) & kValidCenterMask);
        return Gateway(skeleton.getTimestamp(), id, boundary, gatewaySkeleton, isovists, skeleton);
    }
    // Otherwise no gateway exists
    else {
        return boost::none;
    }
}

boost::optional<Gateway>
  adjust_gateway_for_new_skeleton(const Gateway& gateway, const VoronoiSkeletonGrid& skeleton, int maxSearchRadius)
{
    Line<int> newBoundary;
    newBoundary.a = nearest_valid_location(utils::global_point_to_grid_cell_round(gateway.boundary().a, skeleton),
                                           maxSearchRadius,
                                           kValidEndpointMask,
                                           skeleton);
    newBoundary.b = nearest_valid_location(utils::global_point_to_grid_cell_round(gateway.boundary().b, skeleton),
                                           maxSearchRadius,
                                           kValidEndpointMask,
                                           skeleton);

    if (!skeleton.isCellInGrid(newBoundary.a) || !skeleton.isCellInGrid(newBoundary.b)) {
        std::cout << "FAILED TO ADJUST BOUNDARY: Old:" << gateway.cellBoundary() << " New:" << newBoundary << '\n';
        return boost::none;
    }

    cell_t skeletonCell = skeleton_cell_between_endpoints(newBoundary, skeleton);

    if (!skeleton.isCellInGrid(skeletonCell)) {
        std::cout << "FAILED TO ADJUST SKELETON: Old:" << gateway.skeletonCell() << " New boundary:" << newBoundary
                  << " New skeleton: " << skeletonCell << '\n';
        return boost::none;
    }

    return Gateway(skeleton.getTimestamp(), gateway.id(), newBoundary, skeletonCell, skeleton);
}

// Return a cell along the boundary that is reduced skeleton, or a cell not in the map
cell_t skeleton_cell_between_endpoints(const Line<int>& cellBoundary, const VoronoiSkeletonGrid& grid)
{
    // Find all cells along the boundary
    CellVector boundaryCells;
    utils::find_cells_along_line(cellBoundary, grid, std::back_inserter(boundaryCells));

    // See if any cells are skeleton cells on the boundary
    auto skeletonIt = std::find_if(boundaryCells.begin(), boundaryCells.end(), [&grid](cell_t c) {
        return grid.getClassification(c.x, c.y) & SKELETON_CELL_REDUCED_SKELETON;
    });

    // If didn't find a skeleton cell, then check for a four-way connected cell adjacent to the boundary. Due to
    // discretization, it is possible to step by the desired skeleton cell
    if (skeletonIt == boundaryCells.end()) {
        // If one of the skeleton cells has a neighbor cell with the desired label, then use it.
        NeighborArray neighbors;
        for (auto cell : boundaryCells) {
            int num = neighbor_cells_with_classification(cell, kValidCenterMask, grid, EIGHT_WAY, neighbors);

            if (num > 0) {
                return neighbors[0];
            }
        }

        std::cout << "Failed to find a reduced skeleton cell along cells: ";
        std::copy(boundaryCells.begin(), boundaryCells.end(), std::ostream_iterator<cell_t>(std::cout, " "));
        std::cout << '\n';
        return cell_t(-1, -1);
    }
    // Otherwise, assign the cell to be the skeleton cell that was found
    else {
        return *skeletonIt;
    }
}

// Return a cell not in the grid if no valid location is within the search radius
cell_t nearest_valid_location(cell_t start, int searchRadius, uint8_t validMask, const VoronoiSkeletonGrid& grid)
{
    // If the start cell is valid, then we're done!
    if (grid.getClassification(start) & kValidEndpointMask) {
        return start;
    }

    // Otherwise run a search through all the cells within the search radius. Check the cells in order of increasing
    // radius away from the starting cell (approximately) by searching rectangles of cells with increasing perimeter
    // centered at the start cell
    std::vector<int> indices;
    indices.push_back(0);
    for (int n = 1; n <= searchRadius; ++n) {
        indices.push_back(n);
        indices.push_back(-n);
    }

    for (int y : indices) {
        for (int x = -std::abs(y), xEnd = std::abs(y); x <= xEnd; ++x) {
            cell_t newCell(start.x + x, start.y + y);
            if (grid.getClassification(newCell.x, newCell.y) & kValidEndpointMask) {
                return newCell;
            }
        }

        // The above loop won't check cells along the line y = 0. Do that check separately right here.
        cell_t newCell(start.x + y, start.y);
        if (grid.getClassification(newCell.x, newCell.y) & kValidEndpointMask) {
            return newCell;
        }
    }

    return cell_t(-1, -1);
}

}   // namespace hssh
}   // namespace vulcan


//==================================================================
// Author       : Pointner Sebastian
// Company      : Johannes Kepler University
// Name         : SMT Macro Placer
// Workfile     : macrocircuit.hpp
//
// Date         : 22. December 2019
// Compiler     : gcc version 9.2.0 (GCC)
// Copyright    : Johannes Kepler University
// Description  : SoC Macro Circuit
//==================================================================
#ifndef MACROCIRCUIT_HPP
#define MACROCIRCUIT_HPP

#include <vector>
#include <string>
#include <fstream>
#include <cstdio>
#include <map>
#include <unordered_map>
#include <thread>
#include <chrono>
#include <mutex>
#include <cmath>

#include <z3++.h>

#include <boost/filesystem.hpp>

#include <object.hpp>
#include <components.hpp>
#include <lefdefIO.h>
#include <tree.hpp>
#include <evaluate.hpp>
#include <logger.hpp>
#include <timer.hpp>
#include <supplement.hpp>
#include <bookshelf.hpp>
#include <layout.hpp>
#include <partitioning.hpp>
#include <parquet.hpp>
#include <encoding_utils.hpp>
#include <utils.hpp>
#include <def_utils.hpp>
#include <database.hpp>
#include <plotter.hpp>
#include <hlclient.hpp>

namespace Placer {

class Evaluate;
class Partitioning;
class ParquetFrontend;

/**
 * @class MacroCircuit
 * @brief Top Class for MacroCircuit Placement
 */
class MacroCircuit: public virtual Object {
public:

    MacroCircuit();

    virtual ~MacroCircuit();

    void build_circuit();
    void partitioning();
    void encode();
    void place();

    void dump_all();
    void dump_best();

    void save_all();
    void save_best();

    void results_to_db();

    void create_statistics();

    void dump(std::ostream & stream = std::cout);

private:
    friend class Evaluate;

    // Microsoft Z3
    z3::optimize* m_z3_opt;

    Circuit::Circuit* m_circuit;
    std::vector<Macro*> m_macros;
    std::vector<MacroDefinition> m_macro_definitions;

    std::vector<Cell*> m_cells;

    std::vector<Terminal*> m_terminals;

    std::vector<Partition*> m_partitons;

    std::vector<Component*> m_components;
    Tree* m_tree;
    Evaluate* m_eval;
    Utils::Logger* m_logger;
    Utils::Timer* m_timer;
    Utils::Database* m_db;
    Utils::DefUtils* m_def_utils;
    Supplement* m_supplement;
    Bookshelf* m_bookshelf;
    Partitioning* m_partitioning;
    ParquetFrontend* m_parquet;
    EncodingUtils* m_encode;
    Plotter* m_plotter;
    HLClient* m_hl_client;

    std::map<std::string, Macro*> m_id2macro;
    std::map<std::string, Terminal*> m_id2terminal;
    std::map<std::string, Cell*> m_id2cell;
    size_t m_solutions;
    size_t m_estimated_area;

    Layout* m_layout;

    double m_standard_cell_height;

    void build_circuit_lefdef();
    void build_circuit_bookshelf();

    bool is_macro(LefDefParser::defiComponent const & macro);
    bool is_standard_cell(LefDefParser::defiComponent const & cell);

    /**
     * Thread Functions
     */
    void add_terminals();
    void add_macros();
    void add_cells();
    void area_estimator();

    void add_cell(LefDefParser::defiComponent const & cmp);

    void build_tree_from_lefdef();
    void init_tree(eInputFormat const type);

    void create_macro_definitions();

    void write_def(std::string const & name, size_t const solution);
    void write_lef(std::string const & name);

    Tree* get_tree();
    Layout* get_layout();
    size_t get_solutions();

    /**
     * SMT Encoding
     */
    void encode_smt();
    void encode_parquet();

    void config_z3();
    void run_encoding();

    void encode_components_inside_die(eRotation const type);
    void encode_components_non_overlapping(eRotation const type);
    void encode_terminals_on_frontier();
    void encode_terminals_non_overlapping();
    void encode_terminals_center_edge();
    void encode_hpwl_length();
    void encode_layout_on_grid();
    void encode_components_on_grid();
    void encode_terminals_on_grid();

    z3::expr m_components_non_overlapping;
    z3::expr m_components_inside_die;
    z3::expr m_terminals_on_frontier;
    z3::expr m_terminals_non_overlapping;
    z3::expr m_terminals_center_edge;
    z3::expr m_hpwl_cost_function;
    z3::expr m_layout_on_grid;
    z3::expr m_terminals_on_grid;
    z3::expr m_components_on_grid;
    z3::expr_vector m_hpwl_edges;

    z3::expr manhattan_distance(z3::expr const & from_x,
                                z3::expr const & from_y,
                                z3::expr const & to_x,
                                z3::expr const & to_y);

    /**
     * SMT Solving
     */
    void solve_z3_api();
    void solve_z3_no_api();
    void solve_optimathsat_no_api();
    void process_results(z3::model const & m);
    void dump_smt_instance();
    void process_key_value_results(std::map<std::string, std::vector<size_t>> & solution, size_t const id);

};

} /* namespace Placer */

#endif /* MACROCIRCUIT_HPP */


// ======================================================================
// \title  FixedAxisSe3AdapterImpl.cpp
// \author gene
// \brief  cpp file for FixedAxisSe3Adapter component implementation class
//
// \copyright
// Copyright 2009-2015, by the California Institute of Technology.
// ALL RIGHTS RESERVED.  United States Government Sponsorship
// acknowledged. Any commercial use must be negotiated with the Office
// of Technology Transfer at the California Institute of Technology.
//
// This software may be subject to U.S. export control laws and
// regulations.  By accepting this document, the user agrees to comply
// with all U.S. export laws and regulations.  User has the
// responsibility to obtain export licenses, or other export authority
// as may be required before exporting such information to foreign
// countries or providing access to foreign persons.
// ======================================================================

#include <Gnc/Utils/FixedAxisSe3Adapter/FixedAxisSe3AdapterComponentImpl.hpp>
#include "Fw/Types/BasicTypes.hpp"

#include <Eigen/Geometry>

namespace Gnc {

  // ----------------------------------------------------------------------
  // Construction, initialization, and destruction
  // ----------------------------------------------------------------------

  FixedAxisSe3AdapterComponentImpl ::
#if FW_OBJECT_NAMES == 1
    FixedAxisSe3AdapterComponentImpl(
        const char *const compName
    ) :
      FixedAxisSe3AdapterComponentBase(compName)
#else
    FixedAxisSe3AdapterImpl(void)
#endif
  {

  }

  void FixedAxisSe3AdapterComponentImpl ::
    init(
        const NATIVE_INT_TYPE instance
    )
  {
    FixedAxisSe3AdapterComponentBase::init(instance);
  }

  FixedAxisSe3AdapterComponentImpl ::
    ~FixedAxisSe3AdapterComponentImpl(void)
  {

  }

  // ----------------------------------------------------------------------
  // Handler implementations for user-defined typed input ports
  // ----------------------------------------------------------------------

  void FixedAxisSe3AdapterComponentImpl ::
    flatOutput_handler(
        const NATIVE_INT_TYPE portNum,
        ROS::mav_msgs::FlatOutput &FlatOutput
    )
  {
      using namespace ROS::geometry_msgs;
      using namespace ROS::mav_msgs;

      Eigen::Quaterniond yawQ;
      yawQ = Eigen::AngleAxisd(FlatOutput.getyaw(),
            Eigen::Vector3d::UnitZ());
      Se3FeedForward se3ff(FlatOutput.getheader(),
                           FlatOutput.getposition(),
                           FlatOutput.getvelocity(),
                           FlatOutput.getacceleration(),
                           Quaternion(yawQ.x(),
                                      yawQ.y(),
                                      yawQ.z(),
                                      yawQ.w()),
                           Vector3(0.0, 0.0, 0.0),
                           Vector3(0.0, 0.0, 0.0));
      this->se3Cmd_out(0, se3ff);
  }

  // ----------------------------------------------------------------------
  // Command handler implementations
  // ----------------------------------------------------------------------

  void FixedAxisSe3AdapterComponentImpl ::
    AXSE3ADAP_InitParams_cmdHandler(
        const FwOpcodeType opCode,
        const U32 cmdSeq
    )
  {
    // TODO
    this->cmdResponse_out(opCode, cmdSeq, Fw::COMMAND_OK);
  }

} // end namespace Gnc


#ifndef STAN_MATH_PRIM_MAT_PROB_MULTINOMIAL_LOG_HPP
#define STAN_MATH_PRIM_MAT_PROB_MULTINOMIAL_LOG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/prob/multinomial_lpmf.hpp>
#include <boost/math/tools/promotion.hpp>
#include <vector>

namespace stan {
namespace math {

/** \ingroup multivar_dists
 * @deprecated use multinomial_lpmf
 */
template <bool propto, typename T_prob>
return_type_t<T_prob> multinomial_log(
    const std::vector<int>& ns,
    const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>& theta) {
  return multinomial_lpmf<propto, T_prob>(ns, theta);
}

/** \ingroup multivar_dists
 * @deprecated use multinomial_lpmf
 */
template <typename T_prob>
return_type_t<T_prob> multinomial_log(
    const std::vector<int>& ns,
    const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>& theta) {
  return multinomial_lpmf<false>(ns, theta);
}

}  // namespace math
}  // namespace stan
#endif


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <array>
#include <boost/functional/hash.hpp>
#include <cstddef>
#include <memory>
#include <utility>

#include "DataStructures/DataBox/Prefixes.hpp"
#include "DataStructures/FixedHashMap.hpp"
#include "DataStructures/Tensor/TypeAliases.hpp"
#include "DataStructures/VariablesTag.hpp"
#include "Domain/Structure/Element.hpp"
#include "Domain/Structure/MaxNumberOfNeighbors.hpp"
#include "Domain/Tags.hpp"
#include "Evolution/DgSubcell/Tags/Mesh.hpp"
#include "Evolution/DgSubcell/Tags/NeighborData.hpp"
#include "Evolution/Systems/ScalarAdvection/FiniteDifference/Reconstructor.hpp"
#include "Evolution/Systems/ScalarAdvection/Tags.hpp"
#include "Options/Options.hpp"
#include "Utilities/TMPL.hpp"

/// \cond
template <size_t Dim>
class Direction;
template <size_t Dim>
class Element;
template <size_t Dim>
class ElementId;
template <size_t Dim>
class Mesh;
template <typename TagsList>
class Variables;
namespace evolution::dg::subcell {
class NeighborData;
}  // namespace evolution::dg::subcell
namespace gsl {
template <typename>
class not_null;
}  // namespace gsl
namespace PUP {
class er;
}  // namespace PUP
/// \endcond

namespace ScalarAdvection::fd {
/*!
 * \brief Monotised central reconstruction. See
 * ::fd::reconstruction::monotised_central() for details.
 */
template <size_t Dim>
class MonotisedCentral : public Reconstructor<Dim> {
 private:
  using face_vars_tags =
      tmpl::list<Tags::U,
                 ::Tags::Flux<Tags::U, tmpl::size_t<Dim>, Frame::Inertial>>;
  using volume_vars_tags = tmpl::list<Tags::U>;

 public:
  using options = tmpl::list<>;
  static constexpr Options::String help{
      "Monotised central reconstruction scheme."};

  MonotisedCentral() = default;
  MonotisedCentral(MonotisedCentral&&) = default;
  MonotisedCentral& operator=(MonotisedCentral&&) = default;
  MonotisedCentral(const MonotisedCentral&) = default;
  MonotisedCentral& operator=(const MonotisedCentral&) = default;
  ~MonotisedCentral() override = default;

  void pup(PUP::er& p) override;

  /// \cond
  explicit MonotisedCentral(CkMigrateMessage* msg);
  WRAPPED_PUPable_decl_base_template(Reconstructor<Dim>, MonotisedCentral);
  /// \endcond

  auto get_clone() const -> std::unique_ptr<Reconstructor<Dim>> override;

  size_t ghost_zone_size() const override { return 2; }

  using reconstruction_argument_tags = tmpl::list<
      ::Tags::Variables<volume_vars_tags>, domain::Tags::Element<Dim>,
      evolution::dg::subcell::Tags::NeighborDataForReconstructionAndRdmpTci<
          Dim>,
      evolution::dg::subcell::Tags::Mesh<Dim>>;

  template <typename TagsList>
  void reconstruct(
      gsl::not_null<std::array<Variables<TagsList>, Dim>*> vars_on_lower_face,
      gsl::not_null<std::array<Variables<TagsList>, Dim>*> vars_on_upper_face,
      const Variables<tmpl::list<Tags::U>>& volume_vars,
      const Element<Dim>& element,
      const FixedHashMap<
          maximum_number_of_neighbors(Dim) + 1,
          std::pair<Direction<Dim>, ElementId<Dim>>,
          evolution::dg::subcell::NeighborData,
          boost::hash<std::pair<Direction<Dim>, ElementId<Dim>>>>&
          neighbor_data,
      const Mesh<Dim>& subcell_mesh) const;

  template <typename TagsList>
  void reconstruct_fd_neighbor(
      gsl::not_null<Variables<TagsList>*> vars_on_face,
      const Variables<tmpl::list<Tags::U>>& volume_vars,
      const Element<Dim>& element,
      const FixedHashMap<
          maximum_number_of_neighbors(Dim) + 1,
          std::pair<Direction<Dim>, ElementId<Dim>>,
          evolution::dg::subcell::NeighborData,
          boost::hash<std::pair<Direction<Dim>, ElementId<Dim>>>>&
          neighbor_data,
      const Mesh<Dim>& subcell_mesh,
      const Direction<Dim> direction_to_reconstruct) const;
};

template <size_t Dim>
bool operator==(const MonotisedCentral<Dim>& /*lhs*/,
                const MonotisedCentral<Dim>& /*rhs*/);

template <size_t Dim>
bool operator!=(const MonotisedCentral<Dim>& lhs,
                const MonotisedCentral<Dim>& rhs);
}  // namespace ScalarAdvection::fd


/*
 * Copyright (c) 2019, 2020, 2021 SiKol Ltd.
 *
 * Boost Software License - Version 1.0 - August 17th, 2003
 *
 * Permission is hereby granted, free of charge, to any person or organization
 * obtaining a copy of the software and accompanying documentation covered by
 * this license (the "Software") to use, reproduce, display, distribute,
 * execute, and transmit the Software, and to prepare derivative works of the
 * Software, and to permit third-parties to whom the Software is furnished to
 * do so, all subject to the following:
 *
 * The copyright notices in the Software and this entire statement, including
 * the above license grant, this restriction and the following disclaimer,
 * must be included in all copies of the Software, in whole or in part, and
 * all derivative works of the Software, unless such copies or derivative
 * works are solely in the form of machine-executable object code generated by
 * a source language processor.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
 * SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
 * FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.
 */

#ifndef SK_CONFIG_PARSER_NUMERIC_HXX_INCLUDED
#define SK_CONFIG_PARSER_NUMERIC_HXX_INCLUDED

#include <concepts>

#include <boost/spirit/home/x3/numeric/int.hpp>
#include <boost/spirit/home/x3/numeric/real.hpp>
#include <boost/spirit/home/x3/numeric/uint.hpp>

#include <sk/config/detail/parser/bool.hxx>
#include <sk/config/parser_for.hxx>

namespace sk::config {

    /*
     * bool is here because it's an std::unsigned_integral, so if we
     * put it in its own header, anyone including numeric.hxx but not
     * the bool header would incorrectly parse bool instead of creating
     * a compile-time error.
     */
    template <> struct parser_for<bool> {
        using parser_type = detail::parser::bool_parser;
        using rule_type = bool;
        static constexpr char const name[] = "a boolean";
    };

    void propagate_value(auto & /*ctx*/, bool &to, bool from) {
        to = from;
    }

    // signed_integral
    template <std::signed_integral T> struct parser_for<T> {
        using parser_type = boost::spirit::x3::int_parser<T>;
        using rule_type = T;
        static constexpr char const name[] = "an integer";
    };

    template <std::signed_integral T>
    void propagate_value(auto & /*ctx*/, T &to, T from) {
        to = from;
    }

    // unsigned_integral
    template <std::unsigned_integral T> struct parser_for<T> {
        using parser_type = boost::spirit::x3::uint_parser<T>;
        using rule_type = T;
        static constexpr char const name[] = "a positive integer";
    };

    template <std::unsigned_integral T>
    void propagate_value(auto & /*ctx*/, T &to, T from) {
        to = from;
    }

    // floating_point
    template <typename T>
    struct config_real_policies : boost::spirit::x3::real_policies<T> {};

    template <std::floating_point T> struct parser_for<T> {
        using parser_type =
            boost::spirit::x3::real_parser<float, config_real_policies<float>>;
        using rule_type = T;
        static constexpr char const name[] = "a decimal number";
    };

    template <std::floating_point T>
    void propagate_value(auto & /*ctx*/, T &to, T from) {
        to = from;
    }

}; // namespace sk::config

#endif // SK_CONFIG_PARSER_NUMERIC_HXX_INCLUDED


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_INTERLEAVE_EVEN_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_INTERLEAVE_EVEN_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <boost/simd/meta/is_bitwise_logical.hpp>
#include <boost/simd/function/simd/bitwise_cast.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(interleave_even_
                          , (typename A0, typename A1, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::unspecified_<A0>, X>
                          , bs::pack_<bd::unspecified_<A1>, X>
                          )
   {
     A0 operator()(A0 const& a0, A1 const& a1) const
      {
        A0 that;
        const std::size_t n = bs::cardinal_of<A0>::value;
        for(std::size_t i=0;i<n;i+= 2)
        {
          that[i]   = a0[i];
          that[i+1] = a1[i];
        }
        return that;
      }
   };

   BOOST_DISPATCH_OVERLOAD(interleave_even_
                             , (typename A0, typename X)
                             , bd::cpu_
                             , bs::pack_<bs::logical_<A0>, X>
                             , bs::pack_<bs::logical_<A0>, X>
                             )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        using type = A0; //meta::as_arithmetic<A0>;
        return bitwise_cast<A0>(
          interleave_even( bitwise_cast<type>(a0), bitwise_cast<type>(a1) )
        );
      }
   };

//   #define M_IEVEN(z,n,t) (n%2 ? (t+n-1) : n)
//   BOOST_SIMD_DEFINE_SHUFFLE2(  interleave_even_, M_IEVEN, type32_ )
//   BOOST_SIMD_DEFINE_SHUFFLE2(  interleave_even_, M_IEVEN, type16_ )
//   BOOST_SIMD_DEFINE_SHUFFLE2(  interleave_even_, M_IEVEN, type8_  )
//   #undef M_IEVEN

} } }

#endif


#include "formats/cube.hpp"
#include "formats/gnuplot.hpp"
#include "formats/stm.hpp"
#include "atomistic/fundamental.hpp"
#include "types.hpp"
#include "wrappers/lapack.hpp"
#include "io.hpp"
#include <cmath>

#include <ctime>

#include <boost/format.hpp>
#include <boost/spirit/include/qi_core.hpp>
#include <boost/spirit/include/qi_eol.hpp>
#include <boost/lexical_cast.hpp>

#include <blitz/array.h>

#include <fftw3.h>

namespace formats {
namespace stm {

using namespace types;
using namespace wrappers::lapack;

bool StsCube::initialize(){
    if(levels.size() == 0){
        std::cout
            << "No states for given energy window. STS will not be performed.";
        return false;
    }

    // Steal from first cube file
    std::list< formats::WfnCube >::const_iterator
        cubeIt = levels.begin();

    // Get atoms and proper grid
    this->fileName = cubeIt->fileName;
    this->readCubeFile();

    this->title  = "STS data (z = energy)";
    this->description  = str(boost::format(
                "Range [%1.3d V,%1.3d V], delta %1.4d V, sigma %1.4d V")
            % eMin % eMax % deltaE % sigma);

    if(modeFlag == CONSTANT_Z){
        Real height = boost::lexical_cast<Real>(modeParameter);

        // Get z index of STS plane
        std::vector<types::Real> tempvector;
        tempvector.push_back(0);
        tempvector.push_back(0);
        tempvector.push_back(this->topZCoordinate() + height);
        std::vector<types::Uint> indices;
        this->grid.getNearestIndices(tempvector, indices);
        this-> zIndex = indices[2];
        std::cout << "STS will be performed at z-index "
            << zIndex << "\n";
        this->title += str(boost::format(" at z-plane %3d") % zIndex);
    }
    else if (modeFlag == PROFILE){
        stm = stm::StmCube();
        std::cout << "Reading z profile from "
            << modeParameter << "\n";
        stm.readIgorFile(modeParameter);
        this->title += " on z profile read from ";
        this->title += modeParameter;
    }

    // Adjust z dimension for energy
    Uint newIncrementCount =(unsigned int) ((eMax - eMin)/deltaE) + 1;

    if(this->cubeZ == 0){
        // Per default we simply keep the old z extent
        this->grid.directions[2].scaleVector(
                Real(grid.directions[2].getNElements())/
                Real(newIncrementCount));
        this->grid.directions[2] =
            la::Direction(grid.directions[2].getIncrementVector(),
                    newIncrementCount);
    }
    else{
        // Else the new extent is cubeZ atomic units
        Real newDZ = this->cubeZ / newIncrementCount;
        std::vector<Real> v; v.push_back(0); v.push_back(0); v.push_back(newDZ);
        this->grid.directions[2] = la::Direction(v, newIncrementCount);
        this->grid.originVector[2] =
            eMin / (eMax - eMin) * cubeZ;
    }
    grid.data = std::vector<Real>(grid.countPoints(), 0.0);

    return true;
}

bool StsCube::calculate(){

    if( ! this->initialize() ) return false;

    std::list< formats::WfnCube >::const_iterator
        cubeIt = levels.begin(),
    cubeEnd = levels.end();

    // Get all necessary planes
    formats::WfnCube tempCube;
    while(cubeIt != cubeEnd){
        std::cout<< "Processing " << cubeIt->fileName << "\n";
        tempCube = *cubeIt;
        tempCube.readCubeFile();
        if(!this->psiSquared) tempCube.squareValues();
        std::vector<Real> tempPlane;

        if(modeFlag == CONSTANT_Z){
           tempCube.getZPlane(zIndex, tempPlane);
        }
        else if(modeFlag == PROFILE){
            this->interpolateOnZProfile(tempCube.grid, tempPlane);
        }

        addLevel(tempPlane, cubeIt->energy);
        ++cubeIt;
    }

    std::cout<< "Done processing cube files...\n\n";

    // Normalize sum, s.th. values are, on average, 1
    //grid *= Real(grid.countPoints()) / grid.sum();

    return true;
}

void StsCube::interpolateOnZProfile(
        const formats::CubeGrid &grid,
        std::vector<Real> &result) const {
    Uint nX = grid.directions[0].getNElements();
    Uint nY = grid.directions[1].getNElements();
    result.reserve(nX * nY);

    Real dZ = grid.directions[2].getIncrementVector()[2];
    std::vector<Real>::const_iterator zIt = this->stm.stm.begin();
    Real valLow, valHigh, deltaLow;
    Uint zLow;
    for(Uint x = 0; x < nX; ++x){
        for(Uint y = 0; y < nY; ++y){
            zLow = Uint(*zIt / dZ);
            deltaLow = (*zIt - zLow * dZ) / dZ; // Between 0 and 1
            valLow = grid.getDataPoint(x, y, zLow);
            valHigh = grid.getDataPoint(x, y, zLow + 1);

            result.push_back(valLow * (1.0 - deltaLow) + valHigh * deltaLow);

            ++zIt;
        }
    }
}

void StsCube::addLevel(const std::vector<types::Real> &plane,
               types::Real energy){

    Uint nEnergies = grid.directions[2].getNElements();

    // Gaussian stuff
    // \$ \frac{1}{\sigma \sqrt{2\pi} e^{-\frac{(x-\mu)^2}{2\sigma^2}} \$
    // = a e^{c(x-b)^2}
    types::Real a = 1.0/(sigma * std::sqrt(2 * M_PI));
    types::Real c = -1.0/(2 * sigma * sigma);

     std::vector<Real>::const_iterator
         planeIt = plane.begin(),
         planeEnd = plane.end();
     std::vector<Real>::iterator
         dataIt = grid.data.begin();
     while(planeIt != planeEnd){
         for(Uint z = 0; z < nEnergies; ++z){
             Real e = eMin + deltaE * z;
             *dataIt += *planeIt * a * std::exp(c * (energy - e)*(energy - e));
             ++dataIt;
         }
         ++planeIt;
     }

}

void StmCube::setIsoValue(types::Real isoValue){
    this->setValue = isoValue;
    this->mode = CONSTANT_CURRENT;
    this->stm.clear();
    getZIsoSurface(isoValue, this->stm);
}

void StmCube::setZValue(types::Real zValue){
    this->setValue = zValue;
    this->mode = CONSTANT_HEIGHT;
    this->stm.clear();

    // Find largest z coordinate of atoms
    std::vector<types::Real> tempvector;
    tempvector.push_back(0);
    tempvector.push_back(0);
    tempvector.push_back(this->topZCoordinate() + zValue);
    std::vector<types::Uint> indices;
    this->grid.getNearestIndices(tempvector, indices);
    Uint zIndex = indices[2];
    std::cout << "STM will be performed at z-index "
        << zIndex << "\n";
    this->getZPlane(zIndex, this->stm);
    std::cout << "STM will be performed at z-index "
        << zIndex << "\n";
}

bool StmCube::writeIgorFile(String fileName) const {
    Uint nX = grid.directions[0].getNElements();
    Uint nY = grid.directions[1].getNElements();
    std::vector<Real>::const_iterator stmIt = stm.begin();

    Stream result = "";
    for(Uint x = 0; x<nX; ++x){
        for(Uint y=0; y<nY; ++y){
            result += str(boost::format("%12.6e") % *stmIt);
            result += " ";
            ++stmIt;
        }
        result += "\n";
    }

    return io::writeStream(fileName, result);
}

bool StmCube::readIgorFile(String fileName) {
    String content;
    io::readFile(fileName, content);

    std::string::const_iterator it = content.begin(),
        end = content.end();

    using boost::spirit::double_;
    using boost::spirit::qi::eol;
    using boost::spirit::qi::parse;
    using boost::spirit::ascii::space;
    if (! parse(
        it,
        end,
        double_ % space,
        stm
        )) throw types::parseError() << types::errinfo_parse("title or description");

   return true;
}

WfnExtrapolation::WfnExtrapolation(
        std::vector<String> fileNames,
        const cp2k::Spectrum& spectrum,
        Cube&   hartree ,
        Mode          mode    ,
        types::Real   var1    ,
        types::Real   zWidth  ,
        types::Real   approachFrom,
        types::Real   kPlaneMax,
        types::Uint   nLayers
        ){

    for(std::vector<String>::const_iterator it = fileNames.begin();
                        it != fileNames.end(); ++it){
        WfnCube tmp;
        tmp.setFileName(*it);
        wfns.push_back(tmp);
    }

    this->spectrum = spectrum;
    this->hartree = hartree;
    this->mode    = mode;
    if      ( mode == plane )       this->zStart     = var1;
    else if ( mode == rollingBall ) this->ballRadius = var1;
    else if ( mode == isoSurface )  this->isoValue   = var1;
    this->zWidth  = zWidth;
    this->approachFrom = approachFrom;
    this->kPlaneMax = kPlaneMax;
    this->nLayers     = nLayers;

}

void WfnExtrapolation::execute(){
    using namespace blitz;
    time_t t;

    this->determineRange();

    // Need some info on the grid
    // SI units energy: 2 m E/hbar^2 a0 = 2 E/Ha
    Uint nX = hartree.nX(), nY = hartree.nY(), nZ = zEndIndex + 1;
    Real dX = hartree.dX(), dY = hartree.dY(), dZ = hartree.dZ();

    std::cout << "Original wave vector grid: kx,ky = "
        << nX << "," << nY << "\n";
    int nKX = nX, nKY = nY, nK = nKX * nKY;
    if (mode == isoSurface || mode == rollingBall){
        // May choose to reduce Fourier components
        // limiting oscillation frequency/a.u. to kMax
        Real dKX = 2 * M_PI / (dX * Real(nX));
        Real dKY = 2 * M_PI / (dY * Real(nY));

        Real kXMax = nKX/2.0 * dKX;
        Real kYMax = nKY/2.0 * dKY;
        Real scale = this->kPlaneMax / sqrt(kXMax * kXMax + kYMax * kYMax);
        if( scale < 1.0 ){
            nKX = int( scale * nX);
            nKY = int( scale * nY);
            std::cout << "Reduced wave vector grid: kx,ky = " << nKX << "," << nKY << "\n";
        }

    }

    for(std::vector<WfnCube>::iterator wfn = wfns.begin();
            wfn != wfns.end(); ++wfn){

        t = clock();
        std::cout << "--------\n Processing  " << wfn->getFileName() << "\n";
        wfn->readCubeFile();
        std::cout << "Time to read cube : " << (clock() -t)/1000.0 << " ms\n";

        wfn->grid.resize(nX, nY, nZ);
        Array<types::Real,3> dataArray(
                &(wfn->grid.data[0]),
                shape(nX, nY, nZ),
                neverDeleteData);
        Real E =  spectrum.getLevel(wfn->getSpin(), wfn->getLevel());
        E -= this->surfacePotential;
        wfn->setEnergy(E);
        std::cout << "Energy level " << wfn->getEnergy() << " Ha\n";

        // Produce z profile
        std::string zProfile = io::getFileName(wfn->getFileName());
        WfnCube tmp = WfnCube(*wfn);
        tmp.squareValues();
        zProfile += ".zprofile";
        std::cout << "Writing Z profile to " << zProfile << std::endl;
        tmp.writeZProfile(zProfile, "Z profile of sqared wave function\n");

        // Get values on extrapolation surface
        wfn->grid.zSurface(this->zIndices, this->surface);
        types::String s = formats::gnuplot::writeMatrix<Real>(this->surface, nX, nY);
        String sFile = io::getFileName(wfn->getFileName()) + ".exsurf";
        io::writeStream(sFile, s);

        std::cout << "Starting extrapolation ... " << std::endl;
        if ( mode == plane )
            this->onPlane(*wfn);

        else if ( mode == isoSurface || mode == rollingBall )
            this->onSurface(*wfn, nKX, nKY);

        std::cout << "Time to extrapolate : " << (clock() -t)/1000.0 << " ms\n";

        types::String outCubeFile = "extrapolated.";
        outCubeFile += io::getFileName(wfn->getFileName());
        t = clock();
        std::cout << "Writing extrapolated cube file to " << outCubeFile << std::endl;
        wfn->writeCubeFile(outCubeFile);
        std::cout << "Time to write cube : " << (clock() -t)/1000.0 << " ms\n";

        // Produce z profile
        tmp = *wfn;
        tmp.grid.squareValues();
        std::string outZProfile = outCubeFile;
        outZProfile += ".zprofile";
        std::cout << "Writing Z profile of extrapolated cube file to " << outZProfile << std::endl;
        tmp.writeZProfile(outZProfile, "Z profile of squared extrapolated wave function\n");

        wfn->grid.data.clear();
    }
}

void WfnExtrapolation::determineRange(){
    using namespace blitz;

    Cube tmp(hartree);

    // Some preparations...
    if( mode == plane || mode == rollingBall) {
        // Find highest z-coordinate
        // Note: The slowest index in cube file format is x, so in terms of storage
        //       modifying the number of x-coordinates would be the easiest.
        std::vector< atomistic::Atom >::const_iterator it = hartree.atoms.begin(),
            end = hartree.atoms.end();
        types::Real zTop = it->coordinates[2];
        ++it;
        while(it != end) {
            if( it->coordinates[2] > zTop ) {
                zTop = it->coordinates[2];
            }
            ++it;
        }

        // Define region of interpolation
        std::vector<types::Real> tempvector;
        tempvector.push_back(0);
        tempvector.push_back(0);
        tempvector.push_back(zTop + zStart);
        std::vector<types::Uint> indices;
        hartree.grid.getNearestIndices(tempvector, indices);
        this->zStartIndex = indices[2];
        this->zSurfEndIndex = indices[2];

        tempvector[2] += zWidth;
        hartree.grid.getNearestIndices(tempvector, indices);
        this->zEndIndex = indices[2];
    }

    // Define the z indices
    if (mode == plane){
        std::cout << "Fourier transform will be performed at z-index "
            << zStartIndex << "\n";

        this->zIndices = std::vector<Uint> (hartree.nX() * hartree.nY(), zStartIndex);

    }

    else if ( mode == isoSurface ) {
        if (approachFrom < 0) hartree.grid.zIsoSurfaceOnGrid(isoValue, this->zIndices);
        else                  hartree.grid.zIsoSurfaceOnGrid(isoValue, this->zIndices, approachFrom);

        Array<types::Uint,2> zIndicesBlitz(
                &zIndices[0],
                shape(hartree.nX(), hartree.nY()),
                neverDeleteData);

        // Write some info about plane
        zStartIndex = min(zIndicesBlitz);
        Real dZ = hartree.grid.directions[2].getIncrementVector()[2];
        zSurfEndIndex = max(zIndicesBlitz);
        zEndIndex = zSurfEndIndex + Uint(zWidth / dZ);

        std::cout << "Isosurface ranges over " << (zSurfEndIndex-zStartIndex)*dZ
                  << " from z = " << zStartIndex * dZ
                  << " to " << max(zIndicesBlitz) * dZ << " [a.u.].\n";
        std::cout << "Extrapolation will be extended to "
                  << zEndIndex * dZ << " [a.u.]\n";
    }

    else if (mode == rollingBall){
        Uint nX = hartree.nX(), nY = hartree.nY(), nZ = zEndIndex + 1;
        Real dX = hartree.dX(), dY = hartree.dY(), dZ = hartree.dZ();

        // TODO if (approachFrom > 0) zMax  = approachFrom;

        std::vector<bool> boolGrid(nX*nY*nZ, false);
        std::vector<atomistic::Atom>::const_iterator
            it  = hartree.atoms.begin(),
            end = hartree.atoms.end();
        int rI = int(ballRadius / dX) + 1;
        int rJ = int(ballRadius / dY) + 1;

        Real x,y,z;
        int iX, iY, iZ;
        Real r2 = ballRadius * ballRadius;

        // Build the boolean Grid
        std::vector<types::Uint> indices;
        std::vector<Real> coords;
        while(it != end){
            indices.clear();
            coords = it->getCoordinates();
            hartree.grid.getNearestIndices(coords, indices);

            for(int i = -rI; i <= rI; ++i)
                for(int j = -rJ; j <= rJ; ++j){
                    x = i * dX;
                    y = j * dY;
                    z = r2 - x*x - y*y;

                    if (z > 0){
                        iX = (indices[0] + i) % nX;
                        iY = (indices[1] + j) % nY;
                        iZ = int(indices[2]) + int(sqrt(z) / dZ);
                        if (iZ > zEndIndex) iZ = zEndIndex;

                        boolGrid[iX*nY*nZ + iY*nZ + iZ] = true;
                    }
                }

            ++it;
        }

        // Find the topmost 'true' surface
        zIndices = std::vector<Uint>(nX*nY);
        for(int i = 0; i < nX; ++i)
            for(int j = 0; j < nY; ++j){
                int k = nZ;
                while(k > 0){
                    --k;
                    if (boolGrid[i*nY*nZ + j*nZ + k] == true){
                        zIndices[i*nY + j] = k;
                        break;
                    }
                }
                if (k == 0){
                    std::cout << "Warning: Ball rolled to bottom of cube file without finding an atom.\n";
                    zIndices[i*nY + j] = 0;
                }
            }

//        Old, much too slow way of doing things
//        std::vector<Real> tmp;
//        for(Uint x = 0; x < nX; ++x){
//            std::cout << "Here\n";
//            for(Uint y = 0; y < nY; ++y)
//                for(Uint z = zMax; z > 0; --z){
//                    tmp.push_back(x*hartree.dX());
//                    tmp.push_back(y*hartree.dY());
//                    tmp.push_back(z*hartree.dZ());
//                    if(hartree.distance(tmp) < ballRadius){
//                        zIndices[y*nX+x] = z-1;
//                        break;
//                    }
//                }
//        }

        Array<types::Uint,2> zIndicesBlitz(
                &zIndices[0],
                shape(hartree.nX(), hartree.nY()),
                neverDeleteData);

        // Write some info about plane
        zStartIndex = min(zIndicesBlitz);
        zSurfEndIndex = max(zIndicesBlitz);
        //Real dZ = hartree.dZ();
        zEndIndex = zSurfEndIndex + Uint(zWidth / dZ);

        std::cout << "Rolling-ball surface ranges over " << (zSurfEndIndex-zStartIndex)*dZ
                  << " from z = " << zStartIndex * dZ
                  << " to " << zSurfEndIndex * dZ << " [a.u.].\n";
        std::cout << "Extrapolation will be extended to "
                  << zEndIndex * dZ << " [a.u.]\n";

    }

    // Write extrapolation z indices
    String s = formats::gnuplot::writeMatrix<Uint>(this->zIndices, hartree.nX(), hartree.nY());
    String sFile = io::getFileName(hartree.getFileName()) + ".zindices";
    io::writeStream(sFile, s);

    this->setSurfacePotential();

}

void WfnExtrapolation::setSurfacePotential(){
    using namespace blitz;

    std::vector<Real> hartreeVector;
    hartree.grid.zSurface(zIndices, hartreeVector);
    Array<Real, 2> hartreeSurface (
            &hartreeVector[0],
            shape(hartree.nX(), hartree.nY()),
            neverDeleteData);
    this->surfacePotential = mean(hartreeSurface);
    Array<Real, 3> hartreeGrid (
            &(hartree.grid.data[0]),
            shape(hartree.nX(), hartree.nY(), hartree.nZ()),
            neverDeleteData);
    Real vacuumPotential = max(hartreeGrid);

    std::cout << "Average Hartree potential on extrapolation surface is "
              << surfacePotential << " Ha\n";
    std::cout << "This is " << vacuumPotential - surfacePotential
              << " Ha below the maximum Hartree potential.\n";

    // Print hartree potential for gnuplot
    types::String s = formats::gnuplot::writeMatrix(hartreeVector, hartree.nX(), hartree.nY());
    String surfaceFile = io::getFileName(hartree.getFileName()) + ".exsurf";
    io::writeStream(surfaceFile, s);

}

void WfnExtrapolation::onPlane(WfnCube& wfn){
    using namespace blitz;
    time_t t;

    // Need some info on the grid
    // SI units energy: 2 m E/hbar^2 a0 = 2 E/Ha
    Uint nX = wfn.nX(), nY = wfn.nY(), nZ = wfn.nZ();
    Real dX = hartree.dX(), dY = hartree.dY(), dZ = hartree.dZ();
    Real dKX = 2 * M_PI * dZ / (dX * Real(nX));
    Real dKY = 2 * M_PI * dZ / (dY * Real(nY));

    Real E = wfn.getEnergy() * dZ * dZ;
    Array<types::Real,3> dataArray(
            &(wfn.grid.data[0]),
            shape(nX, nY, nZ),
            neverDeleteData);

    // Do a real 2 complex fft
    // Since a(-k)=a(k)^* (or in terms of indices: a[n-k]=a[k]^*)
    // only a[k], k=0...n/2+1 (division rounded down) are retained
    types::Uint nXF = nX, nYF = nY/2 + 1;
    fftw_complex *out = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * nXF * nYF);

    Array<types::Real,2> planeDirect(
            &surface[0],
            shape(nX, nY),
            neverDeleteData);

    fftw_plan plan_forward = fftw_plan_dft_r2c_2d(nX, nY, planeDirect.data(), out, FFTW_ESTIMATE);
    fftw_execute(plan_forward);

    Array<types::Complex,2> planeFourier(
            reinterpret_cast<types::Complex *>(out),
            shape(nXF, nYF),
            neverDeleteData);

    std::vector<Real> fourierReal(nXF*nYF);
    for(int i =0;i<nXF;++i)
        for(int j =0;j<nYF;++j)
            fourierReal[i*nYF+j] = abs(planeFourier(i,j));

    // Plotting the coefficients of the Fourier
    String s = formats::gnuplot::writeMatrix<Real>(fourierReal, nXF, nYF);
    String hartleyFile = io::getFileName(wfn.getFileName()) + ".fourier";
    io::writeStream(hartleyFile, s);

    // Calculate the exponential prefactors.
    // Multiplication of Fourier coefficients with these prefactors
    // propagates them to next z plane.
    // The prefectors would only need dimensions (nX/2 +1, nY/2 +1)
    // since they are the same for G and -G. However for the sake of
    // simplicity of calculation, we prepare them here for (nX, nY/2+1) = (nXF,nYF).
    t = clock();
    Array<types::Real,2> prefactors(nXF, nYF);

    // Notice that the order of storage is 0...G -G...-1 for uneven nX
    // and 0...G-1 G -(G-1)...-1 for even NX
    prefactors( Range(0, nXF/2), Range::all())= exp(- sqrt(tensor::i * dKX * tensor::i * dKX + tensor::j *dKY * tensor::j * dKY - 2 * E) );
    // tensor::i always starts from 0, i.e. it ranges from 0...nXF/2-1 (nX even) or 0...nXF/2 (nX uneven)
    if(nX % 2 == 1)
        prefactors( Range(nXF/2 + 1, nXF - 1), Range::all())= exp(- sqrt( (nXF/2 - tensor::i) * dKX * (nXF/2 - tensor::i) * dKX + tensor::j *dKY * tensor::j * dKY - 2 * E) );
    else
        prefactors( Range(nXF/2 + 1, nXF - 1), Range::all())= exp(- sqrt( (nXF/2 -1 - tensor::i) * dKX * (nXF/2 - 1 - tensor::i) * dKX + tensor::j *dKY * tensor::j * dKY - 2 * E) );

    // Sequentially update the cube file
    Array<types::Real,2> tempDirect(nX, nY);
    Array<types::Complex,2> tempFourier(nXF, nYF);
    fftw_plan plan_backward;
    for(int zIndex = zStartIndex + 1; zIndex < nZ; ++zIndex) {
        // Propagate fourier coefficients
        planeFourier = prefactors(tensor::i, tensor::j) * planeFourier(tensor::i, tensor::j);

        // The c2r transform destroys its input
        tempFourier = planeFourier;
        plan_backward = fftw_plan_dft_c2r_2d(nX, nY, (fftw_complex*) tempFourier.data(), tempDirect.data(), FFTW_ESTIMATE);

        // Do Fourier-back transform
        fftw_execute(plan_backward); /* repeat as needed */
        tempDirect /= nX * nY;

        // Copy data
        dataArray(Range::all(), Range::all(), zIndex) = tempDirect(Range::all(), Range::all());
    }

    fftw_destroy_plan(plan_forward);
    fftw_destroy_plan(plan_backward);
    fftw_free(out);
}

void WfnExtrapolation::onSurface(WfnCube& wfn, Uint nKX, Uint nKY){
    using namespace blitz;
    time_t t;

    // Need some info on the grid
    // SI units energy: 2 m E/hbar^2 a0 = 2 E/Ha
    Uint nX = hartree.nX(), nY = hartree.nY(), nZ = zEndIndex + 1;
    Real dX = hartree.dX(), dY = hartree.dY(), dZ = hartree.dZ();
    Real dKX = 2 * M_PI * dZ / (dX * Real(nX));
    Real dKY = 2 * M_PI * dZ / (dY * Real(nY));

    Array<types::Real,3> dataArray(
            &(wfn.grid.data[0]),
            shape(nX, nY, nZ),
            neverDeleteData);
    Real E = wfn.getEnergy() * dZ * dZ;

    int nK = nKX * nKY;

    // Instead of a wave-number based criterion that is the
    // same for all wave functions, we may also choose some adaptive
    // criterion such as the one below (upper limit for z-decay of
    // involved basis functions.
//    // May choose to reduce k-grid
//    Real kXMax = nKX/2.0 * dKX;
//    Real kYMax = nKY/2.0 * dKY;
//    // Take only low-freq. Fourier components such that no basis function
//    // decays faster than f(z) \propto 10^{-minExpGoal * z}
//    Uint zSpan = zSurfEndIndex - zStartIndex;
//
//    // Translate decayCutoff to minimum exponent per z-index
//    Real minExpGoal = this->decayCutoff * dZ * log(10.0);
//    Real scaling = sqrt( (minExpGoal * minExpGoal + 2*E)
//            / (kXMax * kXMax + kYMax * kYMax) );
//    if(scaling < 1.0){
//        nKX = int( scaling * nX); kXMax = nKX/2.0 * dKX;
//        nKY = int( scaling * nY); kYMax = nKY/2.0 * dKY;
//        std::cout << "Reduced wave vector grid: kx,ky = " << nKX << "," << nKY << "\n";
//    }
//    Real minExp = - sqrt( -2.0 * E + kXMax * kXMax + kYMax * kYMax);
//    std::cout << "Strongest considered decay: x " << exp(minExp/dZ) << " per a.u.\n";
//    minExp *= Real(zSurfEndIndex) - Real(zStartIndex);
//    std::cout << "Smallest exponential term in matrix: " << exp(minExp) << "\n";

    // Build matrix for modified discrete Hartley transform
    Uint n = nX * nY;
    std::cout << "Matrix dimension is " << n << "x" << nK
        << ", i.e. " << Real(nLayers*n*nK*8)  / (1024.0 * 1024.0) << " MBytes per matrix\n";
    //exit(0);
    //        // Test: Extrapolation on plane
    //        for(int i = 0; i < zIndices.size();++i){
    //            zIndices[i] = 128;
    //        }
    //        this->zSurfEndIndex = 128;
    //        this->zStartIndex = 128;
    //        E = -0.160023 * dZ * dZ;

    std::vector<Real> A(n*nK*nLayers);
    int iX, iY, jX, jY;
    Real kX, kY, kZ;
    Real arg;
    t = clock();

    for(Uint j = 0; j < nK; ++j){
        jX = j / nKY; jX = (2* jX < nKX) ? jX : jX - nKX;
        jY = j % nKY; jY = (2* jY < nKY) ? jY : jY - nKY;

        kX = jX * dKX;
        kY = jY * dKY;
        kZ = sqrt( -2.0 * E + kX * kX+ kY * kY);

        for(Uint i = 0; i < n; ++i){
            iX = i / nY; iY = i % nY;
            arg = 2.0 * M_PI * ( Real(iX * jX) / Real(nX)  + Real(iY * jY) / Real(nY) );

            for(Uint l = 0; l < nLayers; ++l){
                // Need transposed matrix for Fortran
                A[l*n+i + j*n*nLayers] = (sin(arg) + cos(arg)) *
                    exp(- kZ * (int(zIndices[i]) - int(l) - int(zStartIndex) ) );
            }
        }
    }
    std::cout << "Matrix created in : " << (clock() -t)/1000.0 << " ms\n";

    // Prepare wave function values
    int iZ;
    std::vector<Real> hartley(n*nLayers);
    for(Uint l = 0; l < nLayers; ++l){
        for(Uint i = 0; i < n; ++i){
            iX = i/nY;
            iY = i%nY;
            iZ = int(zIndices[i]) - int(l);
            hartley[l*n + i] = wfn.grid.data[iX*nY*nZ+ iY*nZ + iZ];
        }
    }

    // Solve the linear system
    char TRANS_ = 'N';
    int  M_     = n*nLayers;
    int  N_     = nK;
    int  NRHS_  = 1;
    std::vector<Real> S(N_);
    Real RCOND_ = 1e-5;
    int  RANK_;
    std::vector<Real> WORK(1);
    int  LWORK_ = -1;
    std::vector<int> IWORK(1);
    int  ILWORK_ = -1;
    int  INFO_  = 0;
    // Writes correct WORK dimensions to WORK[0], IWORK[0]
    // SUBROUTINE DGELSD( M, N, NRHS, A, LDA, B, LDB, S, RCOND, RANK,
    //                   WORK, LWORK, IWORK, INFO )
    dgelsd_(&M_, &N_, &NRHS_,
            &*A.begin(), &M_,
            &*hartley.begin(), &M_,
            &*S.begin(), &RCOND_, &RANK_,
            &*WORK.begin(), &LWORK_, &*IWORK.begin(),
            &INFO_);
    LWORK_ = int(abs(WORK[0]));
    WORK =  std::vector<Real>(LWORK_);
    ILWORK_ = int(abs(IWORK[0]));
    IWORK =  std::vector<int>(ILWORK_);

    // Now solve this little puzzle
    std::cout << "Starting optimization of overdetermined system\n";
    t = clock();

    dgelsd_(&M_, &N_, &NRHS_,
            &*A.begin(), &M_,
            &*hartley.begin(), &M_,
            &*S.begin(), &RCOND_, &RANK_,
            &*WORK.begin(), &LWORK_, &*IWORK.begin(),
            &INFO_);
    //        dgels_(&TRANS_, &M_, &N_, &NRHS_,
    //                &*A.begin(), &M_,
    //                &*hartley.begin(), &M_,
    //                &*WORK.begin(), &LWORK_,
    //                &INFO_);
    if( INFO_ != 0 ){
        throw types::runtimeError()
            << types::errinfo_runtime("LAPACK Error: Problem with solution of least-squares problem.");
    }
    std::cout << "Found solution in : " << (clock() -t)/1000.0 << " ms\n";

    // Print condition number of problem
    std::cout << "Condition number: " << S[0] / S[nK -1] << "\n";

    //        // Plotting the coefficients of the hartley transform
    //        types::String s = formats::gnuplot::writeMatrix<Real>(hartley, nKX, nKY);
    //        String hartleyFile = io::getFileName(wfn->getFileName()) + ".hartley";
    //        io::writeStream(hartleyFile, s);

    // Re-layout hartley components to nX-nY
    std::vector<Real> tmp = hartley;
    hartley = std::vector<Real>(nX*nY, 0.0);
    Real i2, j2;
    for(int i = 0; i < nKX; ++i){
        i2 = (2 * i < nKX) ? i :  int(nX) + i - nKX ;
        for(int j = 0; j < nKY; ++j){
            j2 = (2 * j < nKY) ? j : int(nY) + j - nKY;
            hartley[i2 * nY + j2] = tmp[i* nKY + j];
        }
    }

    //        // Plotting the coefficients of the reformed hartley transform
    //        s = formats::gnuplot::writeMatrix<Real>(hartley, nX, nY);
    //        hartleyFile = io::getFileName(wfn->getFileName()) + ".hartley2";
    //        io::writeStream(hartleyFile, s);

    // Producing Fourier transform
    int nXF = nX; int nYF = nY/2 +1;
    std::vector<Complex> fourier(nXF*nYF);
    int iT,jT;
    for(int i = 0; i < nXF; ++i){
        for(int j = 0; j < nYF; ++j){
            iT = (int(nX) - i) % nX;
            jT = (int(nY) - j) % nY;

            fourier[i*nYF+j] =
                1/2.0 * (               hartley[i*nY+j] + hartley[iT*nY+jT]
                        - Complex(0,1.0)*( hartley[i*nY+j] - hartley[iT*nY+jT]) );
        }
    }

    // Plotting the coefficients of the Fourier transform
    std::vector<Real> fourierReal(fourier.size());
    for(int i =0;i<fourierReal.size();++i)
        fourierReal[i] = abs(fourier[i]);
    String s = formats::gnuplot::writeMatrix<Real>(fourierReal, nXF, nYF);
    String fileName = io::getFileName(wfn.getFileName()) + ".fourier";
    io::writeStream(fileName, s);

    // Calculating the exponential prefactors to propagate coefficients to
    // next z plane
    t = clock();
    std::vector<Real> pref(nXF*nYF);
    for(int i = 0; i < nXF; ++i){
        kX = (2*i < nX) ? i : i - int(nX) ;
        kX *= dKX;
        for(int j = 0; j < nYF; ++j){
            kY = (2*j < nY) ? j : j - int(nY);
            kY *= dKY;
            kZ = sqrt( -2.0 * E + kX * kX+ kY * kY);
            pref[i*nYF + j] = exp(- kZ );
        }
    }

    Array<types::Real,2> prefactors(
            &pref[0],
            shape(nXF, nYF),
            neverDeleteData);

    Array<types::Complex,2> planeFourier(
            &fourier[0],
            shape(nXF, nYF),
            neverDeleteData);

    Array<types::Complex,2> tempFourier(nXF, nYF);
    Array<types::Real,2> tempDirect(nX, nY);
    fftw_plan plan_backward;

    for(int zIndex = zStartIndex+1; zIndex < nZ; ++zIndex) {
        // Propagate hartley coefficients (could use matmul here)
        planeFourier = prefactors(tensor::i, tensor::j) * planeFourier(tensor::i, tensor::j);

        // The c2r transform destroys its input
        tempFourier = planeFourier;
        plan_backward = fftw_plan_dft_c2r_2d(
                nX, nY,
                (fftw_complex*) tempFourier.data(),
                tempDirect.data(),
                FFTW_ESTIMATE);

        // Perform Fourier transform backwards
        fftw_execute(plan_backward);

        // Copy data
        if (zIndex > zSurfEndIndex)
            dataArray(Range::all(), Range::all(), zIndex) = tempDirect(Range::all(), Range::all());
        else {
            for(int x = 0; x < nX; ++x){
                for(int y = 0; y < nY; ++y){
                    if (zIndex > zIndices[x*nY + y]) {
                        dataArray(x, y, zIndex) = tempDirect(x, y);
                    }
                }
            }
        }

    }
    fftw_destroy_plan(plan_backward);

}

}
}