#pragma once

#include "Material.hpp"
#include "Vec3.hpp"
#include "Ray.hpp"
#include "Color.hpp"

#include <boost\optional.hpp>

#ifdef USE_AMP
#include <amp_math.h>
#endif

namespace Smurf {
    enum ActiveMaterial { ActiveMatte, ActiveGlossy };

    struct RayHit {
        RayHit() : depth{0}, hitPoint{0.0, 0.0, 0.0}, tMin{0.0}  { }
        RayHit(double tMin) : tMin{tMin} { }
        RayHit(double tMin, Vec3<double> hitPoint, int depth) : depth{depth}, hitPoint{hitPoint}, tMin{tMin} { }

        int depth;
        Vec3<double> hitPoint;
        double tMin;
    };

    struct GeometricObject {
        GeometricObject() : color{0.7F, 0.65F, 1.0F} { }
        GeometricObject(Color color) : color{color} { }
        GeometricObject(Color color, Matte material) : color{ color }, matte{ material }, active{ ActiveMaterial::ActiveMatte } { }
        GeometricObject(Color color, Glossy material) : color{ color }, glossy{ material }, active{ ActiveMaterial::ActiveGlossy } { }
        virtual boost::optional<RayHit> onRayCast(const Ray& ray) = 0;
        virtual ~GeometricObject() { }
        // Probably replace this later
        Color color;
        Matte matte;
        Glossy glossy;
        ActiveMaterial active;
    };

    class Plane : public GeometricObject {
    public:
        Plane() : point{0.0, 0.0, 0.0}, normal{0.0, 1.0, 0.0} { }
        Plane(const Vec3<double>& point, const Vec3<double>& normal, Color color) : GeometricObject{color},
                                                                                point{point},
                                                                                normal{normal} { }
        Plane(const Vec3<double>& point, const Vec3<double>& normal, Color color, Matte matte) : GeometricObject{ color, matte },
                                                                                point{ point },
                                                                                normal{ normal } { }
        Plane(const Vec3<double>& point, const Vec3<double>& normal, Color color, Glossy glossy) : GeometricObject{ color, glossy },
                                                                                point{ point },
                                                                                normal{ normal } { }
        boost::optional<RayHit> onRayCast(const Ray& ray) override {
            auto t = (point - ray.origin) * normal / (ray.direction * normal);
            if (t > 0) {
                RayHit hit(t);
                return boost::optional<RayHit>(hit);
            }
            // Didn't hit
            return boost::optional<RayHit>();
        }
        #ifdef USE_AMP
        const Vec3<double>& getPoint() {
            return point;
        }
        const Vec3<double>& getNormal() {
            return normal;
        }
        #endif
    private:
        Vec3<double> point;
        Vec3<double> normal;
    };

    class Sphere : public GeometricObject {
    public:
        Sphere() : center{0.0, 0.0, 0.0}, radius{1.0} { }
        Sphere(const Vec3<double>& center, double radius) : center{center},
                                                            radius{radius} { }
        Sphere(const Vec3<double>& center, double radius, Matte material) : GeometricObject{ {0.0F, 0.0F, 0.0F}, material },
                                                                            center{ center },
                                                                            radius{ radius } { }
        Sphere(const Vec3<double>& center, double radius, Glossy material) : GeometricObject{ { 0.0F, 0.0F, 0.0F }, material },
                                                                            center{ center },
                                                                            radius{ radius } { }
        boost::optional<RayHit> onRayCast(const Ray& ray) override {
            auto temp = ray.origin - center;
            auto a = ray.direction * ray.direction;
            auto b = ray.direction * (2.0 * temp);
            auto c = temp * temp - radius * radius;
            auto discriminant = b * b - (4.0 * a * c);

            // Didn't hit
            if (discriminant < 0.0) return boost::optional<RayHit>();

            auto e = sqrt(discriminant);
            auto quadraticDenominator = 2.0 * a;

            auto finalize = [](double tMin) -> boost::optional<RayHit> {
                RayHit hit(tMin);
                return boost::optional<RayHit>(hit);
            };

            auto t = (-b - e) / quadraticDenominator;
            if (t > 0) return finalize(t);

            t = (-b + e) / quadraticDenominator;
            if (t > 0) return finalize(t);

            // Didn't hit
            return boost::optional<RayHit>();
        }
        #ifdef USE_AMP
        const Vec3<double>& getCenter() {
            return center;
        }
        double getRadius() {
            return radius;
        }
        #endif
    private:
        Vec3<double> center;
        double radius;
    };

    class Rectangle : public GeometricObject {
    public:
        Rectangle() : point{0.0, 0.0, 0.0}, a{5.0, 0.0, 0.0}, b{0.0, -5.0, 0.0}, normal{0.0, 0.0, 1.0} { }
        Rectangle(Vec3<double> point, Vec3<double> a, Vec3<double> b, Vec3<double> normal) : point{point}, a{a}, b{b}, normal{normal} { }
        Rectangle(Vec3<double> point, Vec3<double> a, Vec3<double> b, Vec3<double> normal, Matte material) : GeometricObject{ { 0.0F, 0.0F, 0.0F }, material }, point{ point }, a{ a }, b{ b }, normal{ normal } { }
        Rectangle(Vec3<double> point, Vec3<double> a, Vec3<double> b, Vec3<double> normal, Glossy material) : GeometricObject{ { 0.0F, 0.0F, 0.0F }, material }, point{ point }, a{ a }, b{ b }, normal{ normal } { }

        boost::optional<RayHit> onRayCast(const Ray& ray) override {
            double t = (point - ray.origin) * normal / (ray.direction * normal);
            if (t <= 0) return {};

            auto temp = ray.origin + t * ray.direction;
            auto tempDir = temp - point;

            auto tempDirDotSide = tempDir * a;
            if (tempDirDotSide > a.lengthSquared() || tempDirDotSide < 0.0) {
                return {};
            }

            tempDirDotSide = tempDir * b;
            if (tempDirDotSide > b.lengthSquared() || tempDirDotSide < 0.0) {
                return {};
            }

            return {{t}};
        }
        #ifdef USE_AMP
        const Vec3<double>& getPoint() {
            return point;
        }
        const Vec3<double>& getA() {
            return a;
        }
        const Vec3<double>& getB() {
            return b;
        }
        const Vec3<double>& getNormal() {
            return normal;
        }
        #endif
    private:
        Vec3<double> point;
        Vec3<double> a, b;
        Vec3<double> normal;
    };
    #ifdef USE_AMP

    struct g_Plane {
    public:
        g_Plane() restrict(cpu, amp) : point{ 0.0, 0.0, 0.0 }, normal{ 0.0, 1.0, 0.0 }, matte{}, active{ ActiveMaterial::ActiveMatte } { }
        g_Plane(const Vec3<double>& point, const Vec3<double>& normal, const Matte& material) restrict(cpu, amp) : point{ point },
                                                                                normal{ normal },
                                                                                matte{ material },
                                                                                active{ ActiveMaterial::ActiveMatte } { }
        g_Plane(const Vec3<double>& point, const Vec3<double>& normal, const Glossy& material) restrict(cpu, amp) : point{ point },
                                                                                normal{ normal },
                                                                                glossy{ material },
                                                                                active{ ActiveMaterial::ActiveGlossy } { }
        Vec3<double> point;
        Vec3<double> normal;
        ActiveMaterial active;
        Matte matte;
        Glossy glossy;
    };

    struct g_Sphere {
        g_Sphere() restrict(amp) : center{ 0.0, 0.0, 0.0 }, radius{ 1.0 }, matte{}, active{ ActiveMaterial::ActiveMatte } { }
        g_Sphere(const Vec3<double>& center, double radius, const Matte& material) restrict(cpu, amp) : center{center},
                                                                                radius{radius},
                                                                                matte{material},
                                                                                active{ ActiveMaterial::ActiveMatte } { }
        g_Sphere(const Vec3<double>& center, double radius, const Glossy& material) restrict(cpu, amp) : center{ center },
                                                                                radius{ radius },
                                                                                glossy{ material },
                                                                                active{ ActiveMaterial::ActiveGlossy } { }
        ActiveMaterial active;
        Vec3<double> center;
        double radius;
        Matte matte;
        Glossy glossy;
    };

    struct g_Rectangle {
        g_Rectangle() restrict(cpu, amp) : point{0.0, 0.0, 0.0}, a{5.0, 0.0, 0.0}, b{0.0, -5.0, 0.0}, normal{0.0, 0.0, 1.0} { }
        g_Rectangle(const Vec3<double>& point, const Vec3<double>& a, const Vec3<double>& b, const Vec3<double>& normal, const Matte& matte) restrict(cpu, amp) : point{ point }, a{ a }, b{ b }, normal{ normal }, matte{ matte }, active{ ActiveMaterial::ActiveMatte } { }
        g_Rectangle(const Vec3<double>& point, const Vec3<double>& a, const Vec3<double>& b, const Vec3<double>& normal, const Glossy& glossy) restrict(cpu, amp) : point{ point }, a{ a }, b{ b }, normal{ normal }, glossy{ glossy }, active{ ActiveMaterial::ActiveGlossy } { }
        g_Rectangle(const g_Rectangle& other) restrict(cpu, amp) : point{ other.point }, a{ other.a }, b{ other.b }, normal{ other.normal }, matte{ other.matte }, glossy{ other.glossy }, active{ other.active } { }
        g_Rectangle(g_Rectangle&& other) restrict(cpu, amp) : point{static_cast<Vec3<double>&&>(other.point)},
                                                              a{static_cast<Vec3<double>&&>(other.a)},
                                                              b{static_cast<Vec3<double>&&>(other.b)},
                                                              normal{static_cast<Vec3<double>&&>(other.normal)},
                                                              matte{static_cast<Matte&&>(other.matte)},
                                                              glossy{ static_cast<Glossy&&>(other.glossy)},
                                                              active{ static_cast<ActiveMaterial&&>(other.active) } { }
        g_Rectangle& operator=(const g_Rectangle& other) restrict(cpu, amp) {
            point = other.point;
            a = other.a;
            b = other.b;
            normal = other.normal;
            active = other.active;
            return *this;
        }

        ActiveMaterial active;
        Vec3<double> point;
        Vec3<double> a, b;
        Vec3<double> normal;
        Matte matte;
        Glossy glossy;
    };

    struct g_RayHit {
        g_RayHit() restrict(amp) : hasHit{false} { }
        g_RayHit(double tMin, const Vec3<double>& normal, ActiveMaterial active) restrict(amp) : tMin{ tMin }, normal{ normal }, hasHit{ true }, active{ active } { }

        operator bool() const restrict(amp) {
            return hasHit;
        }

        Ray ray;
        int depth;
        ActiveMaterial active;
        Vec3<double> hitPoint;
        Vec3<double> normal;
        Matte matte;
        Glossy glossy;
        double tMin;
        bool hasHit;
    };

    struct g_ShadowRayHit {
        g_ShadowRayHit() restrict(amp) : hasHit{false} { }
        g_ShadowRayHit(float t) restrict(amp) : t{t}, hasHit{true} { }

        operator bool() const restrict(amp) {
            return hasHit;
        }

        float t;
        bool hasHit;
    };

    namespace OnRayCastAspect {
        static float const GetEpsilon() restrict(cpu, amp) {
            return 0.0001F;
        }

        g_RayHit onRayCast(const g_Plane& plane, const Ray& ray) restrict(amp) {
            auto t = (plane.point - ray.origin) * plane.normal / (ray.direction * plane.normal);
            if (t > GetEpsilon()) {
                g_RayHit hit{t, plane.normal, plane.active};
                return hit;
            }
            // Didn't hit
            return {};
        }

        g_ShadowRayHit onShadowRayCast(const g_Plane& plane, const Ray& ray) restrict(amp) {
            auto t = static_cast<float>((plane.point - ray.origin) * plane.normal / (ray.direction * plane.normal));
            if (t > GetEpsilon()) {
                return {t};
            }
            return {};
        }

        g_RayHit onRayCast(const g_Sphere& sphere, const Ray& ray) restrict(amp) {
            auto temp = ray.origin - sphere.center;
            auto a = ray.direction * ray.direction;
            auto b = ray.direction * (2.0 * temp);
            auto c = temp * temp - sphere.radius * sphere.radius;
            auto discriminant = b * b - (4.0 * a * c);

            // Didn't hit
            if (discriminant < 0.0) return {};

            auto e = Concurrency::fast_math::sqrt(static_cast<float>(discriminant));
            auto quadraticDenominator = 2.0 * a;

            auto finalize = [](double tMin, const Vec3<double>& normal, const g_Sphere& sphere) restrict(amp) {
                return g_RayHit{tMin, normal, sphere.active};
            };

            auto t = (-b - e) / quadraticDenominator;
            if (t > GetEpsilon()) return finalize(t, (temp + t * ray.direction).normalizeAndReturn(), sphere);

            t = (-b + e) / quadraticDenominator;
            if (t > GetEpsilon()) return finalize(t, (temp + t * ray.direction).normalizeAndReturn(), sphere);

            // Didn't hit
            return {};
        }

        g_ShadowRayHit onShadowRayCast(const g_Sphere& sphere, const Ray& ray) restrict(amp) {
            auto temp = ray.origin - sphere.center;
            auto a = ray.direction * ray.direction;
            auto b = ray.direction * (2.0 * temp);
            auto c = temp * temp - sphere.radius * sphere.radius;
            auto discriminant = b * b - (4.0 * a * c);

            // Didn't hit
            if (discriminant < 0.0) return {};

            auto e = Concurrency::fast_math::sqrt(static_cast<float>(discriminant));
            auto quadraticDenominator = 2.0 * a;

            auto t = static_cast<float>((-b - e) / quadraticDenominator);
            if (t > GetEpsilon()) return {t};

            t = static_cast<float>((-b + e) / quadraticDenominator);
            if (t > GetEpsilon()) return {t};

            // Didn't hit
            return {};
        }

        g_RayHit onRayCast(const g_Rectangle& rect, const Ray& ray) restrict(amp) {
            double t = (rect.point - ray.origin) * rect.normal / (ray.direction * rect.normal);

            // Didn't hit
            if (t <= 0) return {};

            auto temp = ray.origin + t * ray.direction;
            auto tempDir = temp - rect.point;

            auto tempDirDotSide = tempDir * rect.a;
            if (tempDirDotSide > rect.a.lengthSquared() || tempDirDotSide < 0.0) {
                // Didn't hit
                return {};
            }

            tempDirDotSide = tempDir * rect.b;
            if (tempDirDotSide > rect.b.lengthSquared() || tempDirDotSide < 0.0) {
                // Didn't hit
                return {};
            }

            return {t, rect.normal, rect.active};
        }

        g_ShadowRayHit onShadowRayCast(const g_Rectangle& rect, const Ray& ray) restrict(amp) {
            double t = (rect.point - ray.origin) * rect.normal / (ray.direction * rect.normal);

            // Didn't hit
            if (t <= 0) return {};

            auto temp = ray.origin + t * ray.direction;
            auto tempDir = temp - rect.point;

            auto tempDirDotSide = tempDir * rect.a;
            if (tempDirDotSide > rect.a.lengthSquared() || tempDirDotSide < 0.0) {
                // Didn't hit
                return {};
            }

            tempDirDotSide = tempDir * rect.b;
            if (tempDirDotSide > rect.b.lengthSquared() || tempDirDotSide < 0.0) {
                // Didn't hit
                return {};
            }

            return {static_cast<float>(t)};
        }
    } // namespace OnRayCastAspect
    #endif
} // namespace Smurf


// Copyright (c) 2012-2017 VideoStitch SAS
// Copyright (c) 2018 stitchEm

#ifndef __JACOBIANS__HPP
#define __JACOBIANS__HPP

#include <Eigen/Dense>

namespace VideoStitch {
namespace Calibration {

/**
@brief Compute the jacobian of the function which computes a rotation matrix from yaw pitch and roll wrt yaw pitch roll
@param J the output jacobian matrix
@param yaw the yaw rotation input parameter
@param pitch the pitch rotation input parameter
@param roll the roll rotation input parameter
*/
void getJacobianRotationWrtYawPitchRoll(Eigen::Matrix<double, 9, 3>& J, double yaw, double pitch, double roll);

/**
@brief Compute the jacobian of the function which computes yaw pitch roll from a rotation matrix wrt the rotation matrix
@param J the output jacobian
@param r the input rotation matrix
*/
void getJacobianYawPitchRollWrtRotation(Eigen::Matrix<double, 3, 9>& J, const Eigen::Matrix3d& r);

/**
@brief Compute the jacobian of the logSO3 wrt the input rotation matrix
@param J the output jacobian
@param R the input rotation matrix
*/
void getJacobianAxisAngleWrtRotation(Eigen::Matrix<double, 3, 9>& J, const Eigen::Matrix3d& R);

/**
@brief Compute the jacobian of matrix multiplication A*B.t() wrt A
@param J the result jacobian
@param A the first operand
@param B the second operand
*/
void computedABtdA(Eigen::Matrix<double, 9, 9>& J, const Eigen::Matrix3d& A, const Eigen::Matrix3d& B);

/**
@brief Compute the jacobian of matrix multiplication A*B.t() wrt B
@param J the result jacobian
@param A the first operand
@param B the second operand
*/
void computedABtdB(Eigen::Matrix<double, 9, 9>& J, const Eigen::Matrix3d& A, const Eigen::Matrix3d& B);

}  // namespace Calibration
}  // namespace VideoStitch

#endif


// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: http://www.viva64.com

#include <iostream>
#include <cstdint>
#include <cmath>
#include <cassert>
#include <bitset>
#include <fstream>
#include <regex>
#include <boost/algorithm/string/replace.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/float128.hpp>
#include "kyiv.h"
#include "asm_disasm.h"
typedef uint64_t addr_t;
typedef uint64_t word_t;
typedef int64_t  signed_word_t;
typedef uint32_t opcode_t;
typedef boost::multiprecision::int128_t mul_word_t;

constexpr addr3_t word_to_addr3(word_t w){
    constexpr word_t Addr_1_mask_shift = (40-6-11)+1;
    constexpr word_t Addr_1_mask = 0b11'111'111'111ULL << (Addr_1_mask_shift);  // also was -1
    constexpr word_t Addr_2_mask_shift = (40-6-12-11)+1;
    constexpr word_t Addr_2_mask = 0b11'111'111'111ULL << (Addr_2_mask_shift);  // also was -1
    constexpr word_t Addr_3_mask_shift = 0; // Для одноманітності
    constexpr word_t Addr_3_mask = 0b11'111'111'111ULL;

    addr3_t res;
    res.source_1    = (w & Addr_1_mask) >> Addr_1_mask_shift;
    res.source_2    = (w & Addr_2_mask) >> Addr_2_mask_shift;
    res.destination = (w & Addr_3_mask);
    return res;
}

constexpr opcode_t word_to_opcode(word_t w) {
    constexpr word_t op_code_shift = (40-5)+1;
    constexpr word_t op_code_mask = 0b11'111ULL << (op_code_shift);
    opcode_t opcode = (w & op_code_mask) >> op_code_shift;
    return opcode;
}

static constexpr word_t mask_40_bits = (1ULL << 40) - 1; // 0b111...11 -- 40 1-bits, std::pow(2, 41) === 1 << 41
static constexpr word_t mask_41_bit = (1ULL << 40);      // 0b1000...00 -- 40 zeros after the 1

constexpr bool is_negative(word_t w){
    return w & mask_41_bit; // 0 - додатнє, не нуль -- від'ємне
}

constexpr word_t to_negative(word_t w){
    return w | mask_41_bit;
}

constexpr word_t to_positive(word_t w){
    return w & (~mask_41_bit);
}

constexpr uint16_t leftmost_one(word_t w){
    uint16_t ct = 0;
    while (w > 1) {
        ct++;
        w = w >> 1;
    }
    return ct;
}

//hz looks like kostyl but whatever
uint16_t leftmost_one(mul_word_t w){
    uint16_t ct = 0;
    while (w > 1) {
        ct++;
        w = w >> 1;
    }
    return ct;
}

signed_word_t get_absolute(word_t w){
    return static_cast<signed_word_t>(w & mask_40_bits);
}

signed_word_t word_to_number(word_t w){
    signed_word_t sign1 = (is_negative(w) ? -1 : 1);
    signed_word_t abs_val1 = get_absolute(w);
    return sign1 * abs_val1;
}

constexpr bool get_A1(word_t w){
    constexpr word_t A_1_mask = 1ULL << (40-5);
    return w & A_1_mask;
}

constexpr bool get_A2(word_t w){
    constexpr word_t A_2_mask = 1ULL << (40-5-12);
    return w & A_2_mask;
}

constexpr bool get_A3(word_t w){
    constexpr uint64_t A_3_mask = 1ULL << (40-5-24);
    return w & A_3_mask;
}

constexpr addr3_t shift_addr3_byA(addr3_t addr3, uint64_t offset, word_t w){
    if(get_A1(w))
        addr3.source_1 += offset;
    if(get_A2(w))
        addr3.source_2 += offset;
    if(get_A3(w))
        addr3.destination += offset;
    return  addr3;
}

//! Returns: True -- continue, false -- ситуація останову.
bool Kyiv_t::execute_opcode(){
    K_reg = kmem.read_memory(C_reg);
    std::cout << K_reg << std::endl;
    opcode_t opcode = word_to_opcode(K_reg);

    if (opcode == 0) {
        ++C_reg;

    }
//    std::cout << "opcode: " << opcode << std::endl;
    addr3_t addr3 = word_to_addr3(K_reg); // Парі команд потрібна
    std::cout << "A_reg_2: " << A_reg << std::endl;
    std::cout << "Cycle_reg" << Loop_reg << std::endl;
    addr3_t addr3_shifted = shift_addr3_byA(addr3, A_reg, K_reg); // Решта використовують цю змінну

    std::cout << "source 1: " << addr3_shifted.source_1 << std::endl;
    std::cout << "source 2: " << addr3_shifted.source_2 << std::endl;

    disassembly(K_reg, kmem, addr3_shifted);
    //! Ймовірно, потім це діло треба буде відрефакторити -- відчуваю, но де буде проблема - поки не знаю :+)
    switch(opcode){

        //TODO: Тестував лише opcode_add !!! -- решта вважайте невірними, поки не буде тестів. opcode_div
        case arythm_operations_t::opcode_div: [[fallthrough]];
        case arythm_operations_t::opcode_norm: [[fallthrough]];
        case arythm_operations_t::opcode_add: [[fallthrough]];
        case arythm_operations_t::opcode_sub: [[fallthrough]];
        case arythm_operations_t::opcode_addcmd: [[fallthrough]];
        case arythm_operations_t::opcode_subabs: [[fallthrough]];
        case arythm_operations_t::opcode_mul: [[fallthrough]];
        case arythm_operations_t::opcode_addcyc: [[fallthrough]];
        case arythm_operations_t::opcode_mul_round:
            opcode_arythm(addr3_shifted, opcode);
            break;
            //==========================================================================================================
        case flow_control_operations_t::opcode_jmp_less_or_equal: [[fallthrough]];
        case flow_control_operations_t::opcode_jmp_abs_less_or_equal: [[fallthrough]];
        case flow_control_operations_t::opcode_jmp_equal: [[fallthrough]];
        case flow_control_operations_t::opcode_fork_negative: [[fallthrough]];
        case flow_control_operations_t::opcode_call_negative: [[fallthrough]];
        case flow_control_operations_t::opcode_ret: [[fallthrough]];
        case flow_control_operations_t::opcode_group_op_begin: [[fallthrough]];
        case flow_control_operations_t::opcode_group_op_end: [[fallthrough]];
        case flow_control_operations_t::opcode_F: [[fallthrough]];
        case flow_control_operations_t::opcode_stop:
            opcode_flow_control(addr3_shifted, opcode, addr3);
            break;
//==========================================================================================================
        case logic_operations_t::opcode_log_shift:{
            //! TODO: Мені не повністю зрозуміло з обох книг, величина зсуву береться із RAM за адресою,
            //! чи закодована в команді? Швидше перше -- але перевірити!
            word_t shift = kmem.read_memory(addr3_shifted.source_1) ; // Глушко-Ющенко, стор 12, сверджує: "на число разрядов,
            // равное абсолютной величине константы сдвига, размещаемой в шести младших разрядах ячейки а1"
            // 2^6 -- 64, тому решта бітів справді просто дадуть нуль на виході, але все рівно маскую, щоб
            // не було невизначеної поведінки С. Та й зразу знак викидаємо
            std::cout << "shift: " << shift << std::endl;
            shift &= 0b111'111;
            std::cout << "shift after some and: " << shift << std::endl;
            if(is_negative(kmem.read_memory(addr3_shifted.source_1))) {
                kmem.write_memory(addr3_shifted.destination, kmem.read_memory(addr3_shifted.source_2) >> shift);
            } else {
                kmem.write_memory(addr3_shifted.destination , kmem.read_memory(addr3_shifted.source_2) << shift);
                kmem.write_memory(addr3_shifted.destination,  kmem.read_memory(addr3_shifted.destination) & (mask_40_bits | mask_41_bit)); // Зануляємо зайві біти
            }
            ++C_reg;
        }
            break;
        case logic_operations_t::opcode_log_or:{
            kmem.write_memory(addr3_shifted.destination, kmem.read_memory(addr3_shifted.source_1) | kmem.read_memory(addr3_shifted.source_2));
            ++C_reg;
            std::cout << "orr" << kmem.read_memory(addr3_shifted.destination) << std::endl;
        }
            break;
        case logic_operations_t::opcode_log_and:{

            kmem.write_memory(addr3_shifted.destination, kmem.read_memory(addr3_shifted.source_1) & kmem.read_memory(addr3_shifted.source_2));

            ++C_reg;
        }
            break;
        case logic_operations_t::opcode_log_xor:{
            kmem.write_memory(addr3_shifted.destination, kmem.read_memory(addr3_shifted.source_1) ^ kmem.read_memory(addr3_shifted.source_2));
            ++C_reg;
        }
            break;
//==========================================================================================================
        case IO_operations_t::opcode_read_perfo_data:{
                std::ifstream punch_cards;
                std::string line;
                std::string perfo;
                std::vector<std::string> argv;
                signed_word_t number;

                punch_cards.open("../../mem/punch_cards_in.txt");

                int counter = 0;
                int num_counter = 0;
                bool flag;

                while (punch_cards) {
                    std::getline(punch_cards, line);
                    if (counter == perfo_num) {
                        perfo = line.substr(addr3_shifted.destination, line.size());
                        boost::split(argv, perfo, boost::is_any_of(" "), boost::algorithm::token_compress_off);
                        for(auto num : argv){
                            if(num_counter == addr3_shifted.source_2 - addr3_shifted.source_1){
                                flag = true;
                                break;
                            }
                            number = std::stol(num);
                            if(number >= 0){
                                kmem.write_memory(addr3_shifted.source_1 + num_counter, number);
                            }else{
                                kmem.write_memory(addr3_shifted.source_1 + num_counter, to_negative(std::abs(number)));
                            }
                            num_counter++;
                        }
                    }else if(counter > num_counter){
                        perfo = line;
                        boost::split(argv, perfo, boost::is_any_of(" "), boost::algorithm::token_compress_off);
                        for(auto num : argv){
                            if(num_counter == addr3_shifted.source_2 - addr3_shifted.source_1){
                                flag = true;
                                break;
                            }
                            number = std::stoi(num);
                            if (number >= 0) {
                                kmem.write_memory(addr3_shifted.source_1 + num_counter, number);
                            } else {
                                kmem.write_memory(addr3_shifted.source_1 + num_counter, to_negative(std::abs(number)));
                            }
                            num_counter ++;
                        }
                    }
                    if(flag == true){
                        break;
                    }
                    counter++;
                }
                punch_cards.close();
        }
            break;

        case IO_operations_t::opcode_read_perfo_binary:{
            std::ifstream punch_cards;
            std::ifstream heads;
            std::string head;
            std::string line;

            punch_cards.open("../punched_tape.txt");
            heads.open("../heads.txt");

            std::getline(heads, head);
            std::getline(heads, head);
            // int num = std::stoi(head);
            size_t num = h;
            int counter = 0;
            int com_counter = 0;
            bool flag = false;

            while(punch_cards){
                std::getline(punch_cards, line);
                int pos = 0;
                if(counter == num){
                    if(com_counter < addr3_shifted.source_2 - addr3_shifted.source_1){
                        kmem.write_memory(addr3_shifted.source_1 + com_counter, std::stol(line, 0, 8));
                        com_counter++;
                    }else{
                        flag = true;
                        break;
                    }
                }else if(counter > num){
                    if(com_counter < addr3_shifted.source_2 - addr3_shifted.source_1){
                        kmem.write_memory(addr3_shifted.source_1 + com_counter, std::stol(line, 0, 8));
                        com_counter ++;
                    }else{
                        flag = true;
                        break;
                    }

                }
                if(flag){
                    break;
                }
            }
            h += com_counter;
        }
            break;

        case IO_operations_t::opcode_read_magnetic_drum:{
            std::ifstream magnetic_drum;
            std::string line;
            std::string data;
            std::vector<std::string> argv;
            signed_word_t number;

            magnetic_drum.open("../../mem/drum_in.txt");

            int counter = 0;
            int num_counter = 0;
            bool flag;
            while (magnetic_drum) {
                std::getline(magnetic_drum, line);
                if(counter == drum_num_read){
                    data = line.substr(drum_zone_read, line.size());
                    boost::split(argv, data, boost::is_any_of(" "), boost::algorithm::token_compress_off);
                    for(const auto& num : argv){
                        if(num_counter == addr3_shifted.source_2 - addr3_shifted.source_1){
                            flag = true;
                            break;
                        }
                        number = std::stoi(num);
                        if(number >= 0){
                            kmem.write_memory(addr3_shifted.source_1 + num_counter, number);
                        }else{
                            kmem.write_memory(addr3_shifted.source_1 + num_counter, to_negative(std::abs(number)));
                        }
                        num_counter ++;
                    }
                }else if(counter > drum_num_read){
                    data = line;
                    boost::split(argv, data, boost::is_any_of(" "), boost::algorithm::token_compress_off);
                    for(const auto& num : argv){
                        if(num_counter == addr3_shifted.source_2 - addr3_shifted.source_1){
                            flag = true;
                            break;
                        }
                        number = std::stoi(num);
                        if(number >= 0){
                            kmem.write_memory(addr3_shifted.source_1 + num_counter, number);
                        }else{
                            kmem.write_memory(addr3_shifted.source_1 + num_counter, to_negative(std::abs(number)));
                        }
                        num_counter ++;
                    }
                }
                if(flag){
                    break;
                }
                counter ++;
            }

            magnetic_drum.close();
        }
            break;

        case IO_operations_t::opcode_write_perfo_binary:{
            std::ofstream myfile;
            myfile.open("../punc_cards_out.txt");
            if (myfile.is_open())
            {
                for(uint64_t i = 0; i <= addr3_shifted.source_2; i++){
                    myfile << word_to_number(kmem.read_memory(addr3_shifted.source_1 + i));
                    myfile << ' ';
                }
                myfile.close();
            }else {
                std::cout << "Unable to open file";
            }
            C_reg = addr3_shifted.destination;
            K_reg = kmem.read_memory(C_reg);
        }

        case IO_operations_t::opcode_write_magnetic_drum:{
            std::ofstream myfile;
            myfile.open("../magnetic_drum.txt");
            if (myfile.is_open())
            {
                for(uint64_t i = 0; i <= addr3_shifted.source_2; i++){
                    myfile << word_to_number(kmem.read_memory(addr3_shifted.source_1 + i));
                    myfile << ' ';
                }
                myfile.close();
            }else {
                std::cout << "Unable to open file";
            }
            C_reg = addr3_shifted.destination;
            K_reg = kmem.read_memory(C_reg);
        }
            break;

        case IO_operations_t::opcode_init_magnetic_drum:{
            if (addr3_shifted.source_1 == 0) {
                drum_num_read = addr3_shifted.source_2;
                drum_zone_read = addr3_shifted.destination;
            } else if (addr3_shifted.source_1 == 1) {
                drum_num_write = addr3_shifted.source_2;
                drum_zone_write = addr3_shifted.destination;
            }
        }
            break;
//==========================================================================================================
        default:
            T_reg = true; // ! TODO: Не пам'ятаю, яка там точно реакція на невідому команду
    }
    return !T_reg;
}

void Kyiv_t::opcode_arythm(const addr3_t& addr3, opcode_t opcode){
    //! TODO: Додати перевірку на можливість запису. Що робила машина при спробі запису в ПЗП?
    //! TODO: Додати перевірку на вихід за границю пам'яті -- воно ніби зациклювалося при тому
    //! (зверталося до байта add mod 2^11, в сенсі), але точно не знаю.
    signed_word_t sign1 = (is_negative(kmem.read_memory(addr3.source_1)) ? -1 : 1);
    signed_word_t sign2 = (is_negative(kmem.read_memory(addr3.source_2)) ? -1 : 1);
    std::cout << "sign 2" << sign2 << std::endl;
    std::cout << "gfuigfalhf: " << addr3.source_2 << std::endl;
    word_t abs_val1 = static_cast<signed_word_t>(kmem.read_memory(addr3.source_1) & mask_40_bits);
    word_t abs_val2 = static_cast<signed_word_t>(kmem.read_memory(addr3.source_2) & mask_40_bits);;
    signed_word_t res = sign1 * (signed_word_t) abs_val1;

    signed_word_t res_for_norm;
    mul_word_t res_mul;
    uint16_t power = 40 - leftmost_one(abs_val1) -1;

    std::cout << sign1 * (signed_word_t) abs_val1 << "gifhdaflvdjasbh\t" << sign2 * (signed_word_t) abs_val2 << std::endl;
    switch(opcode){
        case arythm_operations_t::opcode_add:
            res += sign2 * (signed_word_t) abs_val2;
            break;
        case arythm_operations_t::opcode_sub:
            res -= sign2 * (signed_word_t) abs_val2;
            break;
        case arythm_operations_t::opcode_addcmd:
            res += (signed_word_t) abs_val2;
            break;
        case arythm_operations_t::opcode_subabs:
            res = (signed_word_t) abs_val1 - (signed_word_t) abs_val2;
            break;
        case arythm_operations_t::opcode_addcyc:
            res += sign2 * (signed_word_t) abs_val2; // Те ж, що і для opcode_add, але подальша обробка інша
            break;
        case arythm_operations_t::opcode_mul: [[fallthrough]];
        case arythm_operations_t::opcode_mul_round:
            res_mul = sign1 * (mul_word_t) abs_val1 * sign2 * (mul_word_t) abs_val2;
            std::cout << "H : " << res_mul << std::endl;
            break;
        case arythm_operations_t::opcode_norm: {
            res_for_norm = sign1 * (abs_val1 << power);
        }
            break;
        case arythm_operations_t::opcode_div: {
            if ((abs_val2 == 0) || (abs_val2 < abs_val1)) {
                T_reg = true;
                ++C_reg;
                return;
            }
            res_mul = ((mul_word_t) abs_val1 << 40) / (mul_word_t) abs_val2;
            // std::cout << "Div " << res_mul << std::endl;
        }
            break;

        default:
            assert(false && "Should never been here!");
    }

    if(opcode == arythm_operations_t::opcode_add ||
       opcode == arythm_operations_t::opcode_sub ||
       opcode == arythm_operations_t::opcode_subabs     //! TODO: Я не плутаю, результат може мати знак?
            ) {
        //! TODO: До речі, а якщо переповнення, воно кінцевий регістр змінювало до останову, чи ні?
        // Тут я зробив, ніби ні -- але ХЗ, могло. Щоб точно знати -- треба моделювати на рівні схем ;=) --
        // як ви і поривалися. Але це не має бути важливим.
        bool is_negative = (res < 0);
        if (is_negative)
            res = -res;
        assert(res >= 0);
        if (res & mask_41_bit) { // if sum & CPU1.mask_41_bit == 1 -- overflow to sign bit
            T_reg = true;
            ++C_reg;
            return;
        }
        kmem.write_memory(addr3.destination, static_cast<uint64_t>(res) & mask_40_bits);
        // std::cout << -1 * res << std::endl;
        if (is_negative)
            kmem.write_memory(addr3.destination, kmem.read_memory(addr3.destination) | mask_41_bit);
        //! "Нуль, получаемый как разность двух равных чисел, имеет отрицательный знак" -- стор. 13 Глушко-Ющенко, опис УПЧ
        if(opcode == arythm_operations_t::opcode_sub && res == 0
           && abs_val2 == 0 //! TODO: Моє припущення -- перевірити!
                ){
            kmem.write_memory(addr3.destination, res | mask_41_bit);
            std::cout << "NEGATIVE 0" << (res | mask_41_bit) << std::endl;
        }
    } else if(opcode == arythm_operations_t::opcode_addcmd){
        kmem.write_memory(addr3.destination, static_cast<uint64_t>(res) & mask_40_bits);
        kmem.write_memory(addr3.destination, kmem.read_memory(addr3.destination) | (kmem.read_memory(addr3.source_2) & mask_41_bit)); // Копіюємо біт знаку з source_2 // edited тут наче так має бути
    } else if(opcode == arythm_operations_t::opcode_addcyc){
        //! TODO: Вияснити, а як ця команда функціонує.
        // "Отличается от обычного сложения лишь тем, что  в нем отсутствует блокировка при выходе
        // из разполагаемого числа разрядов. Перенос из знакового разряда поступает в младший разряд
        // сумматора".
        // Питання (нумеруючи біти з 1 до 41):
        // 1. Перенос із 40 в 41 біт тут можливий? З фрази виглядає, що так.
        // 2. Якщо додавання переносу до молодшого біту виникло переповнення, що далі?
        //    Так виглядає, що воно не може виникнути, але чи я не помилився? -- не може, десь через переніс буде 0
        bool is_negative = (res < 0);
        // std::cout << (res) << std::endl;
        if (is_negative)
            res = -res;
        assert(res >= 0);

        // std::cout << std::bitset<41> (res) << std::endl;
        if(res & mask_41_bit){
            res += 1; // Маємо перенос із знакового біту
        }

        kmem.write_memory(addr3.destination, static_cast<uint64_t>(res) & mask_40_bits);
        if (is_negative)
            kmem.write_memory(addr3.destination, kmem.read_memory(addr3.destination) | mask_41_bit);
    } else if(opcode == arythm_operations_t::opcode_mul ||
              opcode == arythm_operations_t::opcode_mul_round
            ) {
        bool is_negative = (res_mul < 0);
        //std::cout << res_mul << std::endl;
        if (is_negative)
            res_mul = -res_mul;
        assert(res_mul >= 0);

        uint16_t leftmost = leftmost_one(res_mul);

        if (opcode == arythm_operations_t::opcode_mul_round) {
            res_mul += 1ULL << 41;
        }
        res_mul = res_mul >> 40;

        kmem.write_memory(addr3.destination, static_cast<uint64_t>(res_mul) & mask_40_bits);
        std::cout << "DEBUUUUUUUUG2 " << static_cast<uint64_t>(res_mul) << std::endl;
        // std::cout << is_negative << std::endl;
        if (is_negative)
            kmem.write_memory(addr3.destination, kmem.read_memory(addr3.destination) | mask_41_bit);
        std::cout << "DEBUUUUUUUUG " << word_to_number(kmem.read_memory(addr3.destination)) << std::endl;
//        std::cout << std::bitset<41>(kmem[addr3.destination]) << std::endl;
//        std::cout << "Mult res: " << word_to_number(kmem[addr3.destination]) << std::endl;

    } else if (opcode == arythm_operations_t::opcode_norm) {
        bool is_negative = (res_for_norm < 0);

        if (is_negative)
            res_for_norm = -res_for_norm;
        assert(res_for_norm >= 0);
//
//        std::cout << "norm_val: " << (res_for_norm) << std::endl;
//        std::cout << "norm_power: " << (power) << std::endl;
//        std::cout << "norm_val_64: " << std::bitset<64>(res_for_norm) << std::endl;
//        std::cout << "norm_val_41: " << std::bitset<41>(res_for_norm) << std::endl;

        kmem.write_memory(addr3.source_2, power);
        kmem.write_memory(addr3.destination, static_cast<uint64_t>(res_for_norm) & mask_40_bits);
        if (is_negative)
            kmem.write_memory(addr3.destination, kmem.read_memory(addr3.destination) | mask_41_bit);
//
//        std::cout << "norm_val_mem: " << kmem[addr3.destination] << std::endl;
//        std::cout << "norm_val_pow: " << kmem[addr3.source_2] << std::endl;
    } else if (opcode == arythm_operations_t::opcode_div) {
        kmem.write_memory(addr3.destination, static_cast<uint64_t>(res_mul) & mask_40_bits);
        if ((sign1 * sign2) == -1)
            kmem.write_memory(addr3.destination, kmem.read_memory(addr3.destination) | mask_41_bit);
//        std::cout << "Div res: " << word_to_number(kmem[addr3.destination]) << std::endl;
    }
    ++C_reg;
}

void Kyiv_t::opcode_flow_control(const addr3_t& addr3_shifted, opcode_t opcode, const addr3_t &addr3){
    signed_word_t sign1 = (is_negative(kmem.read_memory(addr3_shifted.source_1)) ? -1 : 1);
    signed_word_t sign2 = (is_negative(kmem.read_memory(addr3_shifted.source_2)) ? -1 : 1);
    signed_word_t abs_val1 = static_cast<signed_word_t>(kmem.read_memory(addr3_shifted.source_1) & mask_40_bits);
    signed_word_t abs_val2 = static_cast<signed_word_t>(kmem.read_memory(addr3_shifted.source_2) & mask_40_bits);;

    switch (opcode) {
        case flow_control_operations_t::opcode_jmp_less_or_equal: {
            if((sign1 * abs_val1) <= (sign2 * abs_val2)){
                C_reg = addr3_shifted.destination;
            } else {
                ++C_reg;
            }
        }
            break;
        case flow_control_operations_t::opcode_jmp_abs_less_or_equal: {
//            std::cout << "Num1 " << get_absolute(kmem[addr3_shifted.source_1]) << std::endl;
//            std::cout << "Num2 " << get_absolute(kmem[addr3_shifted.source_2]) << std::endl;
            if (abs_val1 <= abs_val2) {
                C_reg = addr3_shifted.destination;
            } else {
                ++C_reg;
            }
        }
            break;
        case flow_control_operations_t::opcode_jmp_equal: {
            if( (sign1 * abs_val1) == (sign2 * abs_val2)){
                C_reg = addr3_shifted.destination;
            } else {
                ++C_reg;
            }
        }
            break;
        case flow_control_operations_t::opcode_fork_negative: {
            if( is_negative(kmem.read_memory(addr3_shifted.source_1)) ){
                C_reg = addr3_shifted.destination;
            }else{
                C_reg = addr3_shifted.source_2;
            }
        }
            break;
        case flow_control_operations_t::opcode_call_negative:{
            if( is_negative(kmem.read_memory(addr3_shifted.source_1)) ){
                P_reg = addr3_shifted.source_2; //! TODO: Згідно тексту стор 342 (пункт 18) Гнеденко-Королюк-Ющенко-1961
                //! Глушков-Ющенко, стор 13, УПП не до кінця однозначна -- a1 без штриха, це зрозуміло,
                //! але з врахуванням A і біта модифікатора, чи без?
                //! Виглядає, що в таблиці на стор 180 -- помилка ('A2 => P -- зайвий штрих точно помилка,
                //! чи помилка, що, немає зсуву на А?).
                //! Однак, в  Гнеденко-Королюк-Ющенко-1961 опкод (32) суперечить опкоду в Глушко-Ющенко.
                //! Перевірити!
                C_reg = addr3_shifted.destination;
            }else{
                ++C_reg; //! Тут P_reg не мала б змінювати
            }
        }
            break;
        case flow_control_operations_t::opcode_ret:{
            C_reg = P_reg;
        }
            break;
        case flow_control_operations_t::opcode_group_op_begin:{
            // У книжці Глушков-Ющенко на ст. 14, ймовірно, помилка, бо навіть словами пояснено,
            // що береться значення а1 і а2, але разом із тим наголошено, що береться не 'а1 чи 'а2,
            // а саме а1 і а2. В кінці цієї книжки та у Гнеденко-Королюк-Ющенко пише,
            // ніби беруться значення, тому тут реалізовано саме так.
            Loop_reg =  addr3_shifted.source_1; //word_to_number(kmem.read_memory(addr3_shifted.source_1));
            A_reg = addr3_shifted.source_2; // word_to_number(kmem.read_memory(addr3_shifted.source_2));
            if (A_reg == Loop_reg) {
                C_reg = addr3_shifted.destination;
            } else {
                ++C_reg;
            }

            std::cout << "A_reg: " << A_reg << std::endl;
        }
            break;
        case flow_control_operations_t::opcode_group_op_end:{
            // Такий самий прикол, як з НГО
            // not a value
            A_reg += addr3.source_1;// word_to_number(kmem.read_memory(addr3_shifted.source_1));
            if (A_reg == Loop_reg) {
                C_reg = word_to_number(addr3_shifted.destination);
            } else {
                C_reg = (addr3_shifted.source_2);
            }
        }
            break;
        case flow_control_operations_t::opcode_F:{
            // Якщо я правильно розібралася з 2 попередними командами, то тут все зрозуміло і немає суперечностей
            A_reg = word_to_addr3(kmem.read_memory(addr3_shifted.source_1)).source_2;
            word_t res = kmem.read_memory(A_reg);
            kmem.write_memory(addr3_shifted.destination, res);
            ++C_reg;
        }
            break;
        case flow_control_operations_t::opcode_stop:{ //! TODO: Вона враховує стан кнопки на пульті?
            // From Glushkov-Iushchenko p. 55
            // If B_tumb == 0 -> neutral mode -> full stop
            // If B_tumb > 0 -> just skip one command without full stop
            // From Glushkov-Iushchenko pp. 163-164
            // If B_tumb == 1 -> stop by 3d address
            // If B_tumb == 2 -> stop by command number
            // I'm not sure what to do with 1st and 2nd B_tumb (maybe that should be handled in main???)
            if (!B_tumb) {
                T_reg = true;
                ++C_reg;
            }
            else {
                C_reg += 2;
            }
        }
            break;
    }
}


/*
 * symb_reg_basic_eval.cpp
 * Date: 2013-01-28
 * Author: Karsten Ahnert (karsten.ahnert@gmx.de)
 */

#define FUSION_MAX_VECTOR_SIZE 20

#include <gpcxx/tree.hpp>
#include <gpcxx/generate.hpp>
#include <gpcxx/operator.hpp>
#include <gpcxx/eval.hpp>
#include <gpcxx/evolve.hpp>
#include <gpcxx/io.hpp>
#include <gpcxx/stat.hpp>
#include <gpcxx/app.hpp>

#include <boost/fusion/include/make_vector.hpp>

#include <iostream>
#include <random>
#include <vector>
#include <functional>

const std::string tab = "\t";

namespace pl = std::placeholders;
namespace fusion = boost::fusion;

int main( int argc , char *argv[] )
{
    typedef std::mt19937 rng_type ;

    typedef gpcxx::basic_tree< char > tree_type;
    typedef gpcxx::regression_context< double , 3 > context_type;

    auto eval = gpcxx::make_static_eval< double , char , context_type >(
        fusion::make_vector(
            fusion::make_vector( '1' , []( context_type const& t ) { return 1.0; } )
          , fusion::make_vector( '2' , []( context_type const& t ) { return 2.0; } )
          , fusion::make_vector( '3' , []( context_type const& t ) { return 3.0; } )
          , fusion::make_vector( '4' , []( context_type const& t ) { return 4.0; } )
          , fusion::make_vector( '5' , []( context_type const& t ) { return 5.0; } )
          , fusion::make_vector( '6' , []( context_type const& t ) { return 6.0; } )
          , fusion::make_vector( '7' , []( context_type const& t ) { return 7.0; } )
          , fusion::make_vector( '8' , []( context_type const& t ) { return 8.0; } )
          , fusion::make_vector( '9' , []( context_type const& t ) { return 9.0; } )
          , fusion::make_vector( 'x' , []( context_type const& t ) { return t[0]; } )
          , fusion::make_vector( 'y' , []( context_type const& t ) { return t[1]; } )
          , fusion::make_vector( 'z' , []( context_type const& t ) { return t[2]; } )
          ) ,
        fusion::make_vector(
            fusion::make_vector( 's' , []( double v ) -> double { return std::sin( v ); } )
          , fusion::make_vector( 'c' , []( double v ) -> double { return std::cos( v ); } )
          ) ,
        fusion::make_vector(
            fusion::make_vector( '+' , std::plus< double >() )
          , fusion::make_vector( '-' , std::minus< double >() )
          , fusion::make_vector( '*' , std::multiplies< double >() )
          , fusion::make_vector( '/' , std::divides< double >() )
          ) );
    typedef decltype( eval ) eval_type;

    size_t population_size = 812;
    size_t number_elite = 1;
    double mutation_rate = 0.2;
    double crossover_rate = 0.6;
    double reproduction_rate = 0.3;
    size_t min_tree_height = 4 , max_tree_height = 12;
    size_t tournament_size = 15;

    rng_type rng;
    auto node_generator = eval.get_node_generator< rng_type >();
    node_generator.set_weight( 0 , 1.0 );
    node_generator.set_weight( 1 , 1.0 );
    node_generator.set_weight( 2 , 1.0 );
    auto tree_generator = gpcxx::make_ramp( rng , node_generator , min_tree_height , max_tree_height , 0.5 );

    typedef std::vector< tree_type > population_type;
    typedef std::vector< double > fitness_type;
    typedef gpcxx::static_pipeline< population_type , fitness_type , rng_type > evolver_type;

    evolver_type evolver( number_elite , mutation_rate , crossover_rate , reproduction_rate , rng );

    auto fitness_f = gpcxx::regression_fitness< eval_type >( eval );
    evolver.mutation_function() = gpcxx::make_mutation(
        gpcxx::make_point_mutation( rng , tree_generator , max_tree_height , 20 ) ,
        gpcxx::make_tournament_selector( rng , tournament_size ) );
    evolver.crossover_function() = gpcxx::make_crossover(
        gpcxx::make_one_point_crossover_strategy( rng , 10 ) ,
        gpcxx::make_tournament_selector( rng , tournament_size ) );
    evolver.reproduction_function() = gpcxx::make_reproduce( gpcxx::make_tournament_selector( rng , tournament_size ) );

    auto c = gpcxx::generate_normal_distributed_test_data< 3 >( rng , 1024 , 0.0 , 1.0 , []( double x1 , double x2 , double x3 )
            { return  x1 * x1 * x1 + 1.0 / 10.0 * x2 * x2 - 3.0 / 4.0 * x3 + 1.0 ; } );

    std::vector< double > fitness( population_size , 0.0 );
    std::vector< tree_type > population( population_size );

    // initialize population with random trees and evaluate fitness
    for( size_t i=0 ; i<population.size() ; ++i )
    {
        tree_generator( population[i] );
        fitness[i] = fitness_f( population[i] , c );
    }

    std::cout << "Best individuals" << std::endl << gpcxx::best_individuals( population , fitness ) << std::endl;
    std::cout << "Statistics : " << gpcxx::calc_population_statistics( population ) << std::endl;
    std::cout << std::endl << std::endl;

    for( size_t i=0 ; i<10 ; ++i )
    {
        evolver.next_generation( population , fitness );
        for( size_t i=0 ; i<population.size() ; ++i )
            fitness[i] = fitness_f( population[i] , c );

        std::cout << "Iteration " << i << std::endl;
        std::cout << "Best individuals" << std::endl << gpcxx::best_individuals( population , fitness , 1 ) << std::endl;
        std::cout << "Statistics : " << gpcxx::calc_population_statistics( population ) << std::endl << std::endl;
    }

    return 0;
}


/*
 * system_propagator.hpp
 *
 * Created on: Mar 21, 2018 15:15
 * Description: a convenience wrapper to propogate system model
 *            with given control input
 * Note: this propagator doesn't consider any additional constraints
 *            to system states, see comments below
 *
 * Copyright (c) 2018 Ruixiang Du (rdu)
 */

#ifndef SYSTEM_PROPAGATOR_HPP
#define SYSTEM_PROPAGATOR_HPP

#include <cstdint>
#include <vector>

#include <boost/numeric/odeint.hpp>

namespace robosw {
template <typename Model, typename Input>
class SystemPropagator {
 public:
  typename Model::state_type Propagate(typename Model::state_type init_state,
                                       Input u, double t0, double tf,
                                       double dt) {
    typename Model::state_type x = init_state;
    boost::numeric::odeint::integrate_const(
        boost::numeric::odeint::runge_kutta4<typename Model::state_type>(),
        Model(u), x, t0, tf, dt);
    return x;
  }
};
}  // namespace robosw

#endif /* SYSTEM_PROPAGATOR_HPP */


#include <iostream>
#include <string>
#include <boost/algorithm/string.hpp>

const std::string input = R"(cpy 1 a
cpy 1 b
cpy 26 d
jnz c 2
jnz 1 5
cpy 7 c
inc d
dec c
jnz c -2
cpy a c
inc a
dec b
jnz b -2
cpy c b
dec d
jnz d -6
cpy 14 c
cpy 14 d
inc a
dec d
jnz d -2
dec c
jnz c -5)";

int64_t run(const std::initializer_list<int64_t> &init_vals)
{
    std::vector<std::string> lines;
    boost::split(lines, input, boost::is_any_of("\n"));

    std::vector<int64_t> registers(init_vals);

    size_t pc = 0;

    while (pc < lines.size())
    {
        //std::cout << pc << ": " << registers[0] << ", " << registers[1] << ", "
        //    << registers[2] << ", " << registers[3] << ": " << lines[pc] << std::endl;

        const std::string &line = lines[pc];

        if (line.substr(0, 4) == "cpy ")
        {
            int64_t copy_val = 0;
            size_t space_pos = line.find(' ', 4);

            if ((line[4] >= 'a') && (line[4] <= 'd'))
                copy_val = registers[line[4] - 'a'];
            else
                copy_val = atoi(line.substr(4, space_pos - 4).c_str());

            registers[line.back() - 'a'] = copy_val;
            pc += 1;
        }
        else if (line.substr(0, 4) == "inc ")
        {
            registers[line.back() - 'a'] += 1;
            pc += 1;
        }
        else if (line.substr(0, 4) == "dec ")
        {
            registers[line.back() - 'a'] -= 1;
            pc += 1;
        }
        else if (line.substr(0, 4) == "jnz ")
        {
            int64_t test_val = 0;
            size_t space_pos = line.find(' ', 4);

            if ((line[4] >= 'a') && (line[4] <= 'd'))
                test_val = registers[line[4] - 'a'];
            else
                test_val = atoi(line.substr(4, space_pos - 4).c_str());

            if (test_val != 0)
            {
                pc += atoi(line.substr(space_pos + 1).c_str());
            }
            else
            {
                pc += 1;
            }
        }
    }

    return registers[0];
}

int main(int argc, char *argv[])
{
    int64_t answer1 = run({ 0, 0, 0, 0 });
    int64_t answer2 = run({ 0, 0, 1, 0 });

    std::cout << "Answer #1: " << answer1 << std::endl;
    std::cout << "Answer #2: " << answer2 << std::endl;
}


/* Author: Wolfgang Bangerth, Texas A&M University, 2006 */

/*    $Id: step-23.cc 27657 2012-11-21 13:19:08Z bangerth $       */
/*    Version: $Name:  $                                          */
/*                                                                */
/*    Copyright (C) 2006-2009, 2011-2012 by the deal.II authors */
/*                                                                */
/*    This file is subject to QPL and may not be  distributed     */
/*    without copyright and license information. Please refer     */
/*    to the file deal.II/doc/license.html for the  text  and     */
/*    further information on this license.                        */

// @sect3{Include files}

// We start with the usual assortment of include files that we've seen in so
// many of the previous tests:
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>

#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/constraint_matrix.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>

#include <deal.II/numerics/data_out.h>

#include <fstream>
#include <iostream>

// Here are the only three include files of some new interest: The first one
// is already used, for example, for the
// VectorTools::interpolate_boundary_values and
// VectorTools::apply_boundary_values functions. However, we here use another
// function in that class, VectorTools::project to compute our initial values
// as the $L^2$ projection of the continuous initial values. Furthermore, we
// use VectorTools::create_right_hand_side to generate the integrals
// $(f^n,\phi^n_i)$. These were previously always generated by hand in
// assemble_system or similar functions in application
// code. However, we're too lazy to do that here, so simply use a library
// function:
#include <deal.II/numerics/vector_tools.h>

// In a very similar vein, we are also too lazy to write the code to assemble
// mass and Laplace matrices, although it would have only taken copying the
// relevant code from any number of previous tutorial programs. Rather, we
// want to focus on the things that are truly new to this program and
// therefore use the MatrixTools::create_mass_matrix and
// MatrixTools::create_laplace_matrix functions. They are declared here:
#include <deal.II/numerics/matrix_tools.h>

// Finally, here is an include file that contains all sorts of tool functions
// that one sometimes needs. In particular, we need the
// Utilities::int_to_string class that, given an integer argument, returns a
// string representation of it. It is particularly useful since it allows for
// a second parameter indicating the number of digits to which we want the
// result padded with leading zeros. We will use this to write output files
// that have the form solution-XXX.gnuplot where XXX
// denotes the number of the time step and always consists of three digits
// even if we are still in the single or double digit time steps.
#include <deal.II/base/utilities.h>

// The last step is as in all previous programs:
namespace Step23
{
  using namespace dealii;

  // @sect3{The WaveEquation class}

  // Next comes the declaration of the main class. It's public interface of
  // functions is like in most of the other tutorial programs. Worth
  // mentioning is that we now have to store four matrices instead of one: the
  // mass matrix $M$, the Laplace matrix $A$, the matrix $M+k^2\theta^2A$ used
  // for solving for $U^n$, and a copy of the mass matrix with boundary
  // conditions applied used for solving for $V^n$. Note that it is a bit
  // wasteful to have an additional copy of the mass matrix around. We will
  // discuss strategies for how to avoid this in the section on possible
  // improvements.
  //
  // Likewise, we need solution vectors for $U^n,V^n$ as well as for the
  // corresponding vectors at the previous time step, $U^{n-1},V^{n-1}$. The
  // system_rhs will be used for whatever right hand side vector
  // we have when solving one of the two linear systems in each time
  // step. These will be solved in the two functions solve_u and
  // solve_v.
  //
  // Finally, the variable theta is used to indicate the
  // parameter $\theta$ that is used to define which time stepping scheme to
  // use, as explained in the introduction. The rest is self-explanatory.
  template <int dim>
  class WaveEquation
  {
  public:
    WaveEquation ();
    void run ();

  private:
    void setup_system ();
    void solve_u ();
    void solve_v ();
    void output_results () const;

    Triangulation<dim>   triangulation;
    FE_Q<dim>            fe;
    DoFHandler<dim>      dof_handler;

    ConstraintMatrix constraints;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> mass_matrix;
    SparseMatrix<double> laplace_matrix;
    SparseMatrix<double> matrix_u;
    SparseMatrix<double> matrix_v;

    Vector<double>       solution_u, solution_v;
    Vector<double>       old_solution_u, old_solution_v;
    Vector<double>       system_rhs;

    double time, time_step;
    unsigned int timestep_number;
    const double theta;
  };

  // @sect3{Equation data}

  // Before we go on filling in the details of the main class, let us define
  // the equation data corresponding to the problem, i.e. initial and boundary
  // values for both the solution $u$ and its time derivative $v$, as well as
  // a right hand side class. We do so using classes derived from the Function
  // class template that has been used many times before, so the following
  // should not be a surprise.
  //
  // Let's start with initial values and choose zero for both the value $u$ as
  // well as its time derivative, the velocity $v$:
  template <int dim>
  class InitialValuesU : public Function<dim>
  {
  public:
    InitialValuesU () : Function<dim>() {}

    virtual double value (const Point<dim>   &p,
                          const unsigned int  component = 0) const;
  };

  template <int dim>
  class InitialValuesV : public Function<dim>
  {
  public:
    InitialValuesV () : Function<dim>() {}

    virtual double value (const Point<dim>   &p,
                          const unsigned int  component = 0) const;
  };

  template <int dim>
  double InitialValuesU<dim>::value (const Point<dim>  & /*p*/,
                                     const unsigned int component) const
  {
    Assert (component == 0, ExcInternalError());
    return 0;
  }

  template <int dim>
  double InitialValuesV<dim>::value (const Point<dim>  & /*p*/,
                                     const unsigned int component) const
  {
    Assert (component == 0, ExcInternalError());
    return 0;
  }

  // Secondly, we have the right hand side forcing term. Boring as we are, we
  // choose zero here as well:
  template <int dim>
  class RightHandSide : public Function<dim>
  {
  public:
    RightHandSide () : Function<dim>() {}

    virtual double value (const Point<dim>   &p,
                          const unsigned int  component = 0) const;
  };

  template <int dim>
  double RightHandSide<dim>::value (const Point<dim>  & /*p*/,
                                    const unsigned int component) const
  {
    Assert (component == 0, ExcInternalError());
    return 0;
  }

  // Finally, we have boundary values for $u$ and $v$. They are as described
  // in the introduction, one being the time derivative of the other:
  template <int dim>
  class BoundaryValuesU : public Function<dim>
  {
  public:
    BoundaryValuesU () : Function<dim>() {}

    virtual double value (const Point<dim>   &p,
                          const unsigned int  component = 0) const;
  };

  template <int dim>
  class BoundaryValuesV : public Function<dim>
  {
  public:
    BoundaryValuesV () : Function<dim>() {}

    virtual double value (const Point<dim>   &p,
                          const unsigned int  component = 0) const;
  };

  template <int dim>
  double BoundaryValuesU<dim>::value (const Point<dim> &p,
                                      const unsigned int component) const
  {
    Assert (component == 0, ExcInternalError());

    if ((this->get_time() <= 0.5) &&
        (p[0] < 0) &&
        (p[1] < 1./3) &&
        (p[1] > -1./3))
      return std::sin (this->get_time() * 4 * numbers::PI);
    else
      return 0;
  }

  template <int dim>
  double BoundaryValuesV<dim>::value (const Point<dim> &p,
                                      const unsigned int component) const
  {
    Assert (component == 0, ExcInternalError());

    if ((this->get_time() <= 0.5) &&
        (p[0] < 0) &&
        (p[1] < 1./3) &&
        (p[1] > -1./3))
      return (std::cos (this->get_time() * 4 * numbers::PI) *
              4 * numbers::PI);
    else
      return 0;
  }

  // @sect3{Implementation of the WaveEquation class}

  // The implementation of the actual logic is actually fairly short, since we
  // relegate things like assembling the matrices and right hand side vectors
  // to the library. The rest boils down to not much more than 130 lines of
  // actual code, a significant fraction of which is boilerplate code that can
  // be taken from previous example programs (e.g. the functions that solve
  // linear systems, or that generate output).
  //
  // Let's start with the constructor (for an explanation of the choice of
  // time step, see the section on Courant, Friedrichs, and Lewy in the
  // introduction):
  template <int dim>
  WaveEquation<dim>::WaveEquation () :
    fe (1),
    dof_handler (triangulation),
    time_step (1./64),
    theta (0.5)
  {}

  // @sect4{WaveEquation::setup_system}

  // The next function is the one that sets up the mesh, DoFHandler, and
  // matrices and vectors at the beginning of the program, i.e. before the
  // first time step. The first few lines are pretty much standard if you've
  // read through the tutorial programs at least up to step-6:
  template <int dim>
  void WaveEquation<dim>::setup_system ()
  {
    GridGenerator::hyper_cube (triangulation, -1, 1);
    triangulation.refine_global (7);

    std::cout << "Number of active cells: "
              << triangulation.n_active_cells()
              << std::endl;

    dof_handler.distribute_dofs (fe);

    std::cout << "Number of degrees of freedom: "
              << dof_handler.n_dofs()
              << std::endl
              << std::endl;

    sparsity_pattern.reinit (dof_handler.n_dofs(),
                             dof_handler.n_dofs(),
                             dof_handler.max_couplings_between_dofs());
    DoFTools::make_sparsity_pattern (dof_handler, sparsity_pattern);
    sparsity_pattern.compress();

    // Then comes a block where we have to initialize the 3 matrices we need
    // in the course of the program: the mass matrix, the laplace matrix, and
    // the matrix $M+k^2\theta^2A$ used when solving for $U^n$ in each time
    // step.
    //
    // When setting up these matrices, note that they all make use of the same
    // sparsity pattern object. Finally, the reason why matrices and sparsity
    // patterns are separate objects in deal.II (unlike in many other finite
    // element or linear algebra classes) becomes clear: in a significant
    // fraction of applications, one has to hold several matrices that happen
    // to have the same sparsity pattern, and there is no reason for them not
    // to share this information, rather than re-building and wasting memory
    // on it several times.
    //
    // After initializing all of these matrices, we call library functions
    // that build the Laplace and mass matrices. All they need is a DoFHandler
    // object and a quadrature formula object that is to be used for numerical
    // integration. Note that in many respects these functions are better than
    // what we would usually do in application programs, for example because
    // they automatically parallelize building the matrices if multiple
    // processors are available in a machine. The matrices for solving linear
    // systems will be filled in the run() method because we need to re-apply
    // boundary conditions every time step.
    mass_matrix.reinit (sparsity_pattern);
    laplace_matrix.reinit (sparsity_pattern);
    matrix_u.reinit (sparsity_pattern);
    matrix_v.reinit (sparsity_pattern);

    MatrixCreator::create_mass_matrix (dof_handler, QGauss<dim>(3),
                                       mass_matrix);
    MatrixCreator::create_laplace_matrix (dof_handler, QGauss<dim>(3),
                                          laplace_matrix);

    // The rest of the function is spent on setting vector sizes to the
    // correct value. The final line closes the hanging node constraints
    // object. Since we work on a uniformly refined mesh, no constraints exist
    // or have been computed (i.e. there was no need to call
    // DoFTools::make_hanging_node_constraints as in other programs), but we
    // need a constraints object in one place further down below anyway.
    solution_u.reinit (dof_handler.n_dofs());
    solution_v.reinit (dof_handler.n_dofs());
    old_solution_u.reinit (dof_handler.n_dofs());
    old_solution_v.reinit (dof_handler.n_dofs());
    system_rhs.reinit (dof_handler.n_dofs());

    constraints.close ();
  }

  // @sect4{WaveEquation::solve_u and WaveEquation::solve_v}

  // The next two functions deal with solving the linear systems associated
  // with the equations for $U^n$ and $V^n$. Both are not particularly
  // interesting as they pretty much follow the scheme used in all the
  // previous tutorial programs.
  //
  // One can make little experiments with preconditioners for the two matrices
  // we have to invert. As it turns out, however, for the matrices at hand
  // here, using Jacobi or SSOR preconditioners reduces the number of
  // iterations necessary to solve the linear system slightly, but due to the
  // cost of applying the preconditioner it is no win in terms of run-time. It
  // is not much of a loss either, but let's keep it simple and just do
  // without:
  template <int dim>
  void WaveEquation<dim>::solve_u ()
  {
    SolverControl           solver_control (1000, 1e-8*system_rhs.l2_norm());
    SolverCG<>              cg (solver_control);

    cg.solve (matrix_u, solution_u, system_rhs,
              PreconditionIdentity());

    std::cout << "   u-equation: " << solver_control.last_step()
              << " CG iterations."
              << std::endl;
  }

  template <int dim>
  void WaveEquation<dim>::solve_v ()
  {
    SolverControl           solver_control (1000, 1e-8*system_rhs.l2_norm());
    SolverCG<>              cg (solver_control);

    cg.solve (matrix_v, solution_v, system_rhs,
              PreconditionIdentity());

    std::cout << "   v-equation: " << solver_control.last_step()
              << " CG iterations."
              << std::endl;
  }

  // @sect4{WaveEquation::output_results}

  // Likewise, the following function is pretty much what we've done
  // before. The only thing worth mentioning is how here we generate a string
  // representation of the time step number padded with leading zeros to 3
  // character length using the Utilities::int_to_string function's second
  // argument.
  template <int dim>
  void WaveEquation<dim>::output_results () const
  {
    DataOut<dim> data_out;

    data_out.attach_dof_handler (dof_handler);
    data_out.add_data_vector (solution_u, "U");
    data_out.add_data_vector (solution_v, "V");

    data_out.build_patches ();

    const std::string filename = "solution-" +
                                 Utilities::int_to_string (timestep_number, 3) +
                                 ".gnuplot";
    std::ofstream output (filename.c_str());
    data_out.write_gnuplot (output);
  }

  // @sect4{WaveEquation::run}

  // The following is really the only interesting function of the program. It
  // contains the loop over all time steps, but before we get to that we have
  // to set up the grid, DoFHandler, and matrices. In addition, we have to
  // somehow get started with initial values. To this end, we use the
  // VectorTools::project function that takes an object that describes a
  // continuous function and computes the $L^2$ projection of this function
  // onto the finite element space described by the DoFHandler object. Can't
  // be any simpler than that:
  template <int dim>
  void WaveEquation<dim>::run ()
  {
    setup_system();

    VectorTools::project (dof_handler, constraints, QGauss<dim>(3),
                          InitialValuesU<dim>(),
                          old_solution_u);
    VectorTools::project (dof_handler, constraints, QGauss<dim>(3),
                          InitialValuesV<dim>(),
                          old_solution_v);

    // The next thing is to loop over all the time steps until we reach the
    // end time ($T=5$ in this case). In each time step, we first have to
    // solve for $U^n$, using the equation $(M^n + k^2\theta^2 A^n)U^n =$
    // $(M^{n,n-1} - k^2\theta(1-\theta) A^{n,n-1})U^{n-1} + kM^{n,n-1}V^{n-1}
    // +$ $k\theta \left[k \theta F^n + k(1-\theta) F^{n-1} \right]$. Note
    // that we use the same mesh for all time steps, so that $M^n=M^{n,n-1}=M$
    // and $A^n=A^{n,n-1}=A$. What we therefore have to do first is to add up
    // $MU^{n-1} - k^2\theta(1-\theta) AU^{n-1} + kMV^{n-1}$ and the forcing
    // terms, and put the result into the system_rhs vector. (For
    // these additions, we need a temporary vector that we declare before the
    // loop to avoid repeated memory allocations in each time step.)
    //
    // The one thing to realize here is how we communicate the time variable
    // to the object describing the right hand side: each object derived from
    // the Function class has a time field that can be set using the
    // Function::set_time and read by Function::get_time. In essence, using
    // this mechanism, all functions of space and time are therefore
    // considered functions of space evaluated at a particular time. This
    // matches well what we typically need in finite element programs, where
    // we almost always work on a single time step at a time, and where it
    // never happens that, for example, one would like to evaluate a
    // space-time function for all times at any given spatial location.
    Vector<double> tmp (solution_u.size());
    Vector<double> forcing_terms (solution_u.size());

    for (timestep_number=1, time=time_step;
         time<=5;
         time+=time_step, ++timestep_number)
      {
        std::cout << "Time step " << timestep_number
                  << " at t=" << time
                  << std::endl;

        mass_matrix.vmult (system_rhs, old_solution_u);

        mass_matrix.vmult (tmp, old_solution_v);
        system_rhs.add (time_step, tmp);

        laplace_matrix.vmult (tmp, old_solution_u);
        system_rhs.add (-theta * (1-theta) * time_step * time_step, tmp);

        RightHandSide<dim> rhs_function;
        rhs_function.set_time (time);
        VectorTools::create_right_hand_side (dof_handler, QGauss<dim>(2),
                                             rhs_function, tmp);
        forcing_terms = tmp;
        forcing_terms *= theta * time_step;

        rhs_function.set_time (time-time_step);
        VectorTools::create_right_hand_side (dof_handler, QGauss<dim>(2),
                                             rhs_function, tmp);

        forcing_terms.add ((1-theta) * time_step, tmp);

        system_rhs.add (theta * time_step, forcing_terms);

        // After so constructing the right hand side vector of the first
        // equation, all we have to do is apply the correct boundary
        // values. As for the right hand side, this is a space-time function
        // evaluated at a particular time, which we interpolate at boundary
        // nodes and then use the result to apply boundary values as we
        // usually do. The result is then handed off to the solve_u()
        // function:
        {
          BoundaryValuesU<dim> boundary_values_u_function;
          boundary_values_u_function.set_time (time);

          std::map<unsigned int,double> boundary_values;
          VectorTools::interpolate_boundary_values (dof_handler,
                                                    0,
                                                    boundary_values_u_function,
                                                    boundary_values);

          // The matrix for solve_u() is the same in every time steps, so one
          // could think that it is enough to do this only once at the
          // beginning of the simulation. However, since we need to apply
          // boundary values to the linear system (which eliminate some matrix
          // rows and columns and give contributions to the right hand side),
          // we have to refill the matrix in every time steps before we
          // actually apply boundary data. The actual content is very simple:
          // it is the sum of the mass matrix and a weighted Laplace matrix:
          matrix_u.copy_from (mass_matrix);
          matrix_u.add (theta * theta * time_step * time_step, laplace_matrix);
          MatrixTools::apply_boundary_values (boundary_values,
                                              matrix_u,
                                              solution_u,
                                              system_rhs);
        }
        solve_u ();

        // The second step, i.e. solving for $V^n$, works similarly, except
        // that this time the matrix on the left is the mass matrix (which we
        // copy again in order to be able to apply boundary conditions, and
        // the right hand side is $MV^{n-1} - k\left[ \theta A U^n +
        // (1-\theta) AU^{n-1}\right]$ plus forcing terms. %Boundary values
        // are applied in the same way as before, except that now we have to
        // use the BoundaryValuesV class:
        laplace_matrix.vmult (system_rhs, solution_u);
        system_rhs *= -theta * time_step;

        mass_matrix.vmult (tmp, old_solution_v);
        system_rhs += tmp;

        laplace_matrix.vmult (tmp, old_solution_u);
        system_rhs.add (-time_step * (1-theta), tmp);

        system_rhs += forcing_terms;

        {
          BoundaryValuesV<dim> boundary_values_v_function;
          boundary_values_v_function.set_time (time);

          std::map<unsigned int,double> boundary_values;
          VectorTools::interpolate_boundary_values (dof_handler,
                                                    0,
                                                    boundary_values_v_function,
                                                    boundary_values);
          matrix_v.copy_from (mass_matrix);
          MatrixTools::apply_boundary_values (boundary_values,
                                              matrix_v,
                                              solution_v,
                                              system_rhs);
        }
        solve_v ();

        // Finally, after both solution components have been computed, we
        // output the result, compute the energy in the solution, and go on to
        // the next time step after shifting the present solution into the
        // vectors that hold the solution at the previous time step. Note the
        // function SparseMatrix::matrix_norm_square that can compute
        // $\left<V^n,MV^n\right>$ and $\left$ in one step,
        // saving us the expense of a temporary vector and several lines of
        // code:
        output_results ();

        std::cout << "   Total energy: "
                  << (mass_matrix.matrix_norm_square (solution_v) +
                      laplace_matrix.matrix_norm_square (solution_u)) / 2
                  << std::endl;

        old_solution_u = solution_u;
        old_solution_v = solution_v;
      }
  }
}

// @sect3{The main function}

// What remains is the main function of the program. There is nothing here
// that hasn't been shown in several of the previous programs:
int main ()
{
  try
    {
      using namespace dealii;
      using namespace Step23;

      deallog.depth_console (0);

      WaveEquation<2> wave_equation_solver;
      wave_equation_solver.run ();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


#include "k52/optimization/steepest_descent_method.h"

#ifdef BUILD_WITH_MPI

#include <boost/mpi.hpp>
#include <k52/parallel/mpi/constants.h>

#endif

#include <iostream>
#include <cmath>

#include <k52/optimization/params/i_continuous_parameters.h>

namespace
{
/// @brief Golden ratio value for GoldenSectionSearch method
/// @link https://en.wikipedia.org/wiki/Golden_ratio
	const double kAlpha = (1.0 + sqrt(5.0)) / 2.0;
}

using ::std::vector;

namespace k52
{
namespace optimization
{

SteepestDescentMethod::SteepestDescentMethod(
    double increment_of_the_argument,
    size_t max_iteration_number,
    double precision)
    : increment_of_the_argument_(increment_of_the_argument)
    , max_iteration_number_(max_iteration_number)
    , precision_(precision)
{
}

SteepestDescentMethod* SteepestDescentMethod::Clone() const
{
    return new SteepestDescentMethod(
        increment_of_the_argument_,
        max_iteration_number_,
        precision_);
}

std::string SteepestDescentMethod::get_name() const
{
    return "Steepest Descent Method";
}

#ifdef BUILD_WITH_MPI
void SteepestDescentMethod::Send(boost::mpi::communicator* communicator, int target) const
 {
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, increment_of_the_argument_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, max_iteration_number_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, precision_);
}

void SteepestDescentMethod::Receive(boost::mpi::communicator* communicator, int source)
{
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, increment_of_the_argument_);
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, max_iteration_number_);
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, precision_);
}
#endif

vector<double> SteepestDescentMethod::FindOptimalParameters(const vector<double>& initial_parameters)
{
    vector<double> parameters(initial_parameters);
    size_t counter = 0;
    double lambda = 1;

    vector<double> next_step_parameters(parameters);
    do
    {
        parameters = next_step_parameters;
        counter++;
        vector<double> gradient = CalculateGradient(parameters);
        vector<double> invert_gradiend = InvertVector(gradient);

        lambda = GoldenSectionSearch(parameters, lambda, invert_gradiend);

        for (size_t i = 0; i < next_step_parameters.size(); ++i)
        {
            next_step_parameters[i] = parameters[i] + lambda * invert_gradiend[i];
        }

    } while (!IsExitCriteriaFulfilled(next_step_parameters)
        && !(counter == max_iteration_number_));

    return parameters;
}

bool SteepestDescentMethod::IsExitCriteriaFulfilled(
    const vector<double>& steps_array) const
{
    double temp = 0;
    vector<double> gradient = CalculateGradient(steps_array);
    for (size_t i = 0; i < steps_array.size(); ++i)
    {
        temp += gradient[i] * gradient[i];
    }
    if (sqrt(temp) > precision_)
    {
        return false;
    }
    return true;
}
/// @TODO: needs to replaced copy-pasted method to the method from Math
/// @Source: conjugate_gradient_method.cpp
/// @link https://github.com/PavelKovalets/k52/blob/develop/src/optimization/conjugate_gradient_method.cpp#L159
vector<double> SteepestDescentMethod::CalculateGradient(
    const vector<double>& parameters) const
{
    vector<double> gradient(parameters.size());

    for (size_t i = 0; i<parameters.size(); ++i)
    {
        gradient[i] = CalculateDerivative(parameters, i);
    }

    return gradient;
}

/// @TODO: needs to replaced copy-pasted method to the method from Math
/// @Source: conjugate_gradient_method.cpp
/// @link https://github.com/PavelKovalets/k52/blob/develop/src/optimization/conjugate_gradient_method.cpp#L126
double SteepestDescentMethod::CalculateDerivative(
    const vector<double>& parameters,
    const size_t& index) const
{
    double half = 0.5;
    vector<double> decrement_function(parameters);
    vector<double> increment_function(parameters);
    decrement_function[index] = parameters[index] - increment_of_the_argument_ * half;
    increment_function[index] = parameters[index] + increment_of_the_argument_ * half;
    double increment_function_value = CountObjectiveFunctionValueToMinimize(increment_function);
    double decrement_function_value = CountObjectiveFunctionValueToMinimize(decrement_function);
    return (increment_function_value - decrement_function_value) / increment_of_the_argument_;
}

/// @TODO: needs to replaced to the method from Math
vector<double> SteepestDescentMethod::InvertVector(
    const vector<double>& parameters) const
{
    vector<double> new_parameters(parameters);
    for (size_t i = 0; i < new_parameters.size(); ++i)
    {
        new_parameters[i] *= -1;
    }
    return new_parameters;
}

double SteepestDescentMethod::GoldenSectionSearch(
    const vector<double>& init_point,
    const double& step,
    const vector<double>& direction) const
{
    vector<double> next_point(init_point.size());
    double lambda = 0;
    double step_for_borders = 0;

    step_for_borders = Localization(init_point, step, direction);

    lambda = FindStepLength(init_point, step_for_borders, direction);

    return lambda;
}

double SteepestDescentMethod::FindStepLength(
    const vector<double>& init_parameters,
    const double& step_for_borders,
    const vector<double>& direction) const
{
    vector<double> right_border_parameters(init_parameters.size());
    vector<double> next_left_border_point(init_parameters.size());
    vector<double> next_right_border_point(init_parameters.size());
    double left_border = 0;
    double next_left_border = 0;
    double next_right_border = 0;
    double constriction_number = 0.382;
    double right_border = step_for_borders;

    for (size_t i = 0; i < init_parameters.size(); ++i)
    {
        right_border_parameters[i] = init_parameters[i] + step_for_borders*direction[i];
    }

    next_left_border = left_border + constriction_number*(right_border - left_border);
    next_right_border = right_border - constriction_number*(right_border - left_border);

    double last_left_border = 0;
    double last_right_border = 0;
    double next_left_border_value = 0;
    double next_right_border_value = 0;
    do
    {
        for (size_t i = 0; i < init_parameters.size(); ++i)
        {
            next_left_border_point[i] = init_parameters[i] + next_left_border*direction[i];
            next_right_border_point[i] = init_parameters[i] + next_right_border*direction[i];
        }
        next_left_border_value = CountObjectiveFunctionValueToMinimize(next_left_border_point);
        next_right_border_value = CountObjectiveFunctionValueToMinimize(next_right_border_point);

        last_left_border = left_border;
        last_right_border = right_border;

        if (next_left_border < next_right_border)
        {
            if (next_left_border_value > next_right_border_value)
            {
                left_border = next_left_border;
                next_left_border = next_right_border;
                next_right_border = right_border - constriction_number * (right_border - left_border);
            }
            else
            {
                right_border = next_right_border;
                next_right_border = next_left_border;
                next_left_border = left_border + constriction_number * (right_border - left_border);
            }
        }
        else
        {
            if (next_right_border_value > next_left_border_value)
            {
                left_border = next_right_border;
                next_right_border = next_left_border;
                next_left_border = right_border - constriction_number * (right_border - left_border);
            }
            else
            {
                right_border = next_left_border;
                next_left_border = next_right_border;
                next_right_border = left_border + constriction_number * (right_border - left_border);
            }
        }
        if ((left_border == last_left_border && right_border == last_right_border)) {
            break;
        }

    } while ((abs(left_border - right_border) > precision_));

    return (left_border + right_border) / 2;
}

double SteepestDescentMethod::Localization(
    const vector<double>& init_parameters,
    const double& init_step,
    const vector<double>& direction) const
{
    vector<double> point(init_parameters.size());
    double step = init_step*2;

    do {
		step /= 2;
        for (size_t i = 0; i < point.size(); ++i)
        {
            point[i] = init_parameters[i] + step*direction[i];
        }
    } while ((CountObjectiveFunctionValueToMinimize(point) > CountObjectiveFunctionValueToMinimize(init_parameters))
        && (abs(step) > precision_));

    vector<double> next_step_point(point);
    do
    {
        point = next_step_point;
        step *= kAlpha;
        for (size_t i = 0; i < point.size(); ++i)
        {
        next_step_point[i] = init_parameters[i] + step*direction[i];
        }

    } while (CountObjectiveFunctionValueToMinimize(next_step_point) <= CountObjectiveFunctionValueToMinimize(point));

    double final_step = 0;
    for (size_t i = 0; i < next_step_point.size(); ++i)
    {
        final_step += (next_step_point[i] - init_parameters[i])*(next_step_point[i] - init_parameters[i]);
    }

    return sqrt(final_step);
}

}//optimization
}//k52


#include <cmath>
#include <boost/math/special_functions/fpclassify.hpp> // isnan, isinf
#include <ecf/ECF.h>
#include "SymbRegEvalOp.h"
#include "ReadData.h"
#include "infixTree.h"

// from WriteBest.h
extern bool evaluateVerbose;

void SymbRegEvalOp::registerParameters(StateP state)
{
	state->getRegistry()->registerEntry("input_file", (voidP)(new std::string), ECF::STRING);
	state->getRegistry()->registerEntry("linear_scaling", (voidP) (new uint(0)), ECF::UINT);
	state->getRegistry()->registerEntry("error_weights_file", (voidP) (new std::string), ECF::STRING);
	state->getRegistry()->registerEntry("error_metric", (voidP) (new std::string), ECF::STRING);
	state->getRegistry()->registerEntry("data_skip", (voidP) (new uint(1)), ECF::UINT);
	state->getRegistry()->registerEntry("data_offset", (voidP) (new uint(0)), ECF::UINT);
}

void SymbRegEvalOp::createTerminals(StateP state)
{
	static std::string configTerminals = *((std::string*) state->getGenotypes()[0]->getParameterValue(state, "terminalset").get());
	//configTerminals = *((std::string*) state->getRegistry()->getEntry("Tree.terminalset").get());

	varNames.clear();
	std::string terminalSet;
	for(uint i = 0; i < nVariables; i++) {
		varNames.push_back ("x" + uint2str(i + 1));
		terminalSet += varNames[i] + " ";
	}

	// set terminal names
	terminalSet = configTerminals + " " + terminalSet;
	state->getGenotypes()[0]->setParameterValue(state, "terminalset", (voidP) new std::string(terminalSet));
	//state->getRegistry()->modifyEntry("Tree.terminalset", (voidP) new std::string(terminalSet));

	// reinitialize population with updated terminals
	Tree::Tree* hometree = (Tree::Tree*) state->getGenotypes()[0].get();
	hometree->primitiveSet_ = Tree::PrimitiveSetP();
	state->getPopulation()->initialize(state);
}

bool SymbRegEvalOp::initialize(StateP state)
{
	state_ = state;
	recordResults = false;
	results.clear();

	voidP sptr = state->getRegistry()->getEntry("data_skip");
	uint skip = *((uint*)sptr.get());
	sptr = state->getRegistry()->getEntry("data_offset");
	uint offset = *((uint*)sptr.get());
	sptr = state->getRegistry()->getEntry("linear_scaling");
	uint scaling = *((uint*)sptr.get());
	linearScaling = false;
	if(scaling != 0)
		linearScaling = true;

	std::string dataFile = *((std::string*) state->getRegistry()->getEntry("input_file").get());
	// read from file into Evaluator data
	if (!readDataFromFile(data, dataFile, offset, skip)) {
		return false;
	}

	nSamples = data.size();
	nVariables = data[0].size() - 1;
	results.resize(nSamples);

	// a) create terminal names for use with canonical GP
	createTerminals(state);

	// b) read terminal names from config
	//varNames.clear();
	//std::string terminals = *((std::string*) state->getGenotypes()[0]->getParameterValue(state, "terminalset").get());
	//std::stringstream ss(terminals);
	//std::string terminal;
	//for(uint i = 0; i < nVariables; i++) {
	//	ss >> terminal;
	//	varNames.push_back (terminal);
	//}

	// initialize evaluator
	eval.data = &data;
	eval.initialize();
	for(uint i = 0; i < nVariables; i++) {
		eval.addTerminal(varNames[i]);
	}

	return true;
}

FitnessP SymbRegEvalOp::evaluate(IndividualP individual)
{
	if(linearScaling == true)
		return evaluateUsingLinearScaling(individual);

	// we try to minimize the function value, so we use FitnessMin fitness (for minimization problems)
	FitnessP fitness (new FitnessMin);

	// get the genotype we defined in the configuration file
	Tree::Tree* tree = (Tree::Tree*) individual->getGenotype().get();

	// output tree expression to string
	std::stringstream sValue;
	for(uint i = 0; i < tree->size(); i++) {
		sValue << (*tree)[i]->primitive_->getName() << " ";
	}
	// send expression to evaluator
	eval.parseExpression(sValue.str(), tree->size());

	double value = 0;
	double result;
	// evaluating data from input file
	for (uint i = 0; i < nSamples; i++) {
		//// set only defined variables (x1, x2, ...)
		//for(uint term = 0; term < nVariables; term++) {
		//	tree->setTerminalValue(varNames[term], &data[i][term]);
		//}

		//// get the f value of the current tree
		//tree->execute(&result);

		result = eval.executeParsedExpression(i);

		// clip the value to [0, 255]
		result = (int) abs(result);
		if(result > 255)
			result = 255;

		if(recordResults)
			results[i] = result;

		// add the absolute difference
		//value += fabs(eval.data[i][nVariables] - result);
		// or squared error
		double error = fabs(data[i][nVariables] - result) * fabs(data[i][nVariables] - result);

		value += error;
	}

	value /= nSamples;	   // MSE
	value = sqrt(value);   // RMSE

	fitness->setValue(value);

	if(evaluateVerbose) {
		stringstream ss;
		string infix;
		showTree(infix, tree);
		ss << infix << endl;
		ECF_LOG(state_, 1, ss.str());
	}

	return fitness;
}

FitnessP SymbRegEvalOp::evaluateUsingLinearScaling(IndividualP individual)
{
	FitnessP fitness (new FitnessMin);

	// get the genotype we defined in the configuration file
	Tree::Tree* tree = (Tree::Tree*) individual->getGenotype().get();

	// output tree expression to string
	std::stringstream sValue;
	for(uint i = 0; i < tree->size(); i++) {
		sValue << (*tree)[i]->primitive_->getName() << " ";
	}
	// send expression to evaluator
	eval.parseExpression(sValue.str(), tree->size());

	//average value of y and t
	double mean_y = 0, mean_t = 0, sum_yt = 0, sum_sqr_y = 0;

	for (uint i = 0 ; i < nSamples ; i++)
	{
		//// set only defined variables (x1, x2, ...)
		//for(uint term = 0; term < nVariables; term++) {
		//	tree->setTerminalValue(varNames[term], &data[i][term]);
		//}

		// get the y value of the current tree
		double y;
		//tree->execute(&y);
		y = eval.executeParsedExpression(i);

		// clip the value to [0, 255]
		y = (int) abs(y);
		if(y > 255)
			y = 255;

		mean_y += y;
		double t = data[i][nVariables];
		mean_t += t;
		sum_yt += t*y;
		sum_sqr_y += y*y;
	}

	mean_y /= nSamples;
	mean_t /= nSamples;

	//coefficients for linear regression
	double b = (sum_yt - nSamples*mean_y*mean_t)/(sum_sqr_y - nSamples*mean_y*mean_y);
	double a = mean_t - b*mean_y;

	bool isNaN = boost::math::isnan(a) || boost::math::isnan(b);
	bool isInf = boost::math::isinf(a) || boost::math::isinf(b);
	if (isNaN || isInf) {
		a = 0;
		b = 1;
//		fitness->setValue(1e14);
//		return fitness;
	}

	double value = 0;
	double var_of_y = 0; //variance of variable result

	for(uint i = 0; i < nSamples; i++) {
		//// set only defined variables (x1, x2, ...)
		//for(uint term = 0; term < nVariables; term++) {
		//	tree->setTerminalValue(varNames[term], &data[i][term]);
		//}

		double y;
		//tree->execute(&y);
		y = eval.executeParsedExpression(i);

		// clip the value to [0, 255]
		y = (int) abs(y);
		if(y > 255)
			y = 255;

		var_of_y += (y-mean_y)*(y-mean_y);
		double result = a + b*y;
		result = (int) result;

		if(recordResults)
			results[i] = result;

		// add the absolute difference
		//value += fabs(eval.data[i][nVariables] - result);
		// or squared error
		double error = fabs(data[i][nVariables] - result) * fabs(data[i][nVariables] - result);
		value += error;
	}

	value /= nSamples;	   // MSE
	value = sqrt(value);   // RMSE

	var_of_y /= nSamples-1;

	if (evaluateVerbose) {
		stringstream ss;
		string infix;
		showTree(infix, tree);
		ss << infix << endl;
		ss << "Linear scaling parameters: scale=" << b << " offset=" << a << std::endl;
		ECF_LOG(state_, 1, ss.str());
	}

	fitness->setValue(value);

	return fitness;
}