// 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 */


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/integral.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/list/instance.hpp>
#include <boost/hana/orderable/laws.hpp>
using namespace boost::hana;

template <typename T, typename U>
void test() {
    BOOST_HANA_CONSTANT_ASSERT(Orderable::laws::check(
        list(
            integral<T, 0>, integral<U, 0>,
            integral<T, 1>, integral<U, 1>,
            integral<T, 2>, integral<U, 2>
        )
    ));
}

int main() {
    test<int, int>();
    test<int, unsigned long long>();
}


/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2012 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

#ifndef MAPNIK_IMAGE_FILTER_HPP
#define MAPNIK_IMAGE_FILTER_HPP

//mapnik
#include <mapnik/image_filter_types.hpp>
#include <mapnik/util/hsl.hpp>

// boost
#include <boost/variant/static_visitor.hpp>
#include <boost/gil/gil_all.hpp>
#include <boost/concept_check.hpp>
#include <boost/foreach.hpp>

// agg
#include "agg_basics.h"
#include "agg_rendering_buffer.h"
#include "agg_pixfmt_rgba.h"
#include "agg_scanline_u.h"
#include "agg_blur.h"
#include "agg_gradient_lut.h"
// stl
#include <cmath>

// 8-bit YUV
//Y = ( (  66 * R + 129 * G +  25 * B + 128) >> 8) +  16
//U = ( ( -38 * R -  74 * G + 112 * B + 128) >> 8) + 128
//V = ( ( 112 * R -  94 * G -  18 * B + 128) >> 8) + 128

//bits_type x_gradient = (0.125f*c2 + 0.25f*c5 + 0.125f*c8)
//    - (0.125f*c0 + 0.25f*c3 + 0.125f*c6);
//bits_type y_gradient = (0.125f*c0 + 0.25f*c1 + 0.125f*c2)
//    - (0.125f*c6 + 0.25f*c7 + 0.125f*c8);

// c0 c1 c2
// c3 c4 c5
// c6 c7 c8

//sharpen
//  0 -1  0
// -1  5 -1
//  0 -1  0
//bits_type out_value = -c1 - c3 + 5.0*c4 - c5 - c7;

// edge detect
//  0  1  0
//  1 -4  1
//  0  1  0
//bits_type out_value = c1 + c3 - 4.0*c4 + c5 + c7;

//
//if (out_value < 0) out_value = 0;
//if (out_value > 255) out_value = 255;

// emboss
// -2 -1  0
// -1  1  1
//  0  1  2

// bits_type out_value = -2*c0 - c1 - c3 + c4 + c5 + c7 +  2*c8;

// blur
//float out_value = (0.1f*c0 + 0.1f*c1 + 0.1f*c2 +
//                   0.1f*c3 + 0.1f*c4 + 0.1f*c5 +
//                  0.1f*c6 + 0.1f*c7 + 0.1f*c8);

//float out_value  = std::sqrt(std::pow(x_gradient,2) + std::pow(y_gradient,2));
//float theta = std::atan2(x_gradient,y_gradient);
//if (out_value < 0.0) out_value = 0.0;
//if (out_value < 1.0) out_value = 1.0;

//float conv_matrix[]={1/3.0,1/3.0,1/3.0};

//float gaussian_1[]={0.00022923296f,0.0059770769f,0.060597949f,0.24173197f,0.38292751f,
//                    0.24173197f,0.060597949f,0.0059770769f,0.00022923296f};

//float gaussian_2[]={
//    0.00048869418f,0.0024031631f,0.0092463447f,
//   0.027839607f,0.065602221f,0.12099898f,0.17469721f,
//   0.19744757f,
//   0.17469721f,0.12099898f,0.065602221f,0.027839607f,
//   0.0092463447f,0.0024031631f,0.00048869418f
//};

//  kernel_1d_fixed<float,9> kernel(conv,4);

// color_converted_view<rgb8_pixel_t>(src_view);
//typedef kth_channel_view_type< 0, const rgba8_view_t>::type view_t;

//view_t red = kth_channel_view<0>(const_view(src_view));

//kernel_1d_fixed<float,3> kernel(sharpen,0);
//convolve_rows_fixed<rgba32f_pixel_t>(src_view,kernel,src_view);
// convolve_cols_fixed<rgba32f_pixel_t>(src_view,kernel,dst_view);

namespace mapnik {  namespace filter { namespace detail {

static const float blur_matrix[] = {0.1111f,0.1111f,0.1111f,0.1111f,0.1111f,0.1111f,0.1111f,0.1111f,0.1111f};
static const float emboss_matrix[] = {-2,-1,0,-1,1,1,0,1,2};
static const float sharpen_matrix[] = {0,-1,0,-1,5,-1,0,-1,0 };
static const float edge_detect_matrix[] = {0,1,0,1,-4,1,0,1,0 };

}

using boost::gil::rgba8_image_t;
using boost::gil::rgba8_view_t;

template <typename Image>
boost::gil::rgba8_view_t rgba8_view(Image & img)
{
    using boost::gil::interleaved_view;
    using boost::gil::rgba8_pixel_t;
    return interleaved_view(img.width(), img.height(),
                            reinterpret_cast<rgba8_pixel_t*>(img.raw_data()),
                            img.width() * sizeof(rgba8_pixel_t));
}

template <typename Image>
struct double_buffer
{
    boost::gil::rgba8_image_t   dst_buffer;
    boost::gil::rgba8_view_t    dst_view;
    boost::gil::rgba8_view_t    src_view;

    explicit double_buffer(Image & src)
        : dst_buffer(src.width(), src.height())
        , dst_view(view(dst_buffer))
        , src_view(rgba8_view(src)) {}

    ~double_buffer()
    {
        copy_pixels(dst_view, src_view);
    }
};

template <typename Src, typename Dst, typename Conv>
void process_channel_impl (Src const& src, Dst & dst, Conv const& k)
{
    using boost::gil::bits32f;

    bits32f out_value =
        k[0]*src[0] + k[1]*src[1] + k[2]*src[2] +
        k[3]*src[3] + k[4]*src[4] + k[5]*src[5] +
        k[6]*src[6] + k[7]*src[7] + k[8]*src[8]
        ;
    if (out_value < 0) out_value = 0;
    if (out_value > 255) out_value = 255;
    dst = out_value;
}

template <typename Src, typename Dst, typename Conv>
void process_channel (Src const& src, Dst & dst, Conv const& k)
{
    boost::ignore_unused_variable_warning(src);
    boost::ignore_unused_variable_warning(dst);
    boost::ignore_unused_variable_warning(k);
}

template <typename Src, typename Dst>
void process_channel (Src const& src, Dst & dst, mapnik::filter::blur)
{
    process_channel_impl(src,dst,mapnik::filter::detail::blur_matrix);
}

template <typename Src, typename Dst>
void process_channel (Src const& src, Dst & dst, mapnik::filter::emboss)
{
    process_channel_impl(src,dst,mapnik::filter::detail::emboss_matrix);
}

template <typename Src, typename Dst>
void process_channel (Src const& src, Dst & dst, mapnik::filter::sharpen)
{
    process_channel_impl(src,dst,mapnik::filter::detail::sharpen_matrix);
}

template <typename Src, typename Dst>
void process_channel (Src const& src, Dst & dst, mapnik::filter::edge_detect)
{
    process_channel_impl(src,dst,mapnik::filter::detail::edge_detect_matrix);
}

template <typename Src, typename Dst>
void process_channel (Src const& src, Dst & dst, mapnik::filter::sobel)
{
    using boost::gil::bits32f;

    bits32f x_gradient = (src[2] + 2*src[5] + src[8])
        - (src[0] + 2*src[3] + src[6]);

    bits32f y_gradient = (src[0] + 2*src[1] + src[2])
        - (src[6] + 2*src[7] + src[8]);

    bits32f  out_value  = std::sqrt(std::pow(x_gradient,2) + std::pow(y_gradient,2));
    //bts32f theta = std::atan2(x_gradient,y_gradient);
    if (out_value < 0) out_value = 0;
    if (out_value > 255) out_value = 255;
    dst = out_value;
}

template <typename Src, typename Dst, typename Filter>
void apply_convolution_3x3(Src const& src_view, Dst & dst_view, Filter const& filter)
{
    using boost::gil::bits32f;
    using boost::gil::point2;

    // p0 p1 p2
    // p3 p4 p5
    // p6 p7 p8

    typename Src::xy_locator src_loc = src_view.xy_at(0,0);
    typename Src::xy_locator::cached_location_t loc00 = src_loc.cache_location(-1,-1);
    typename Src::xy_locator::cached_location_t loc10 = src_loc.cache_location( 0,-1);
    typename Src::xy_locator::cached_location_t loc20 = src_loc.cache_location( 1,-1);
    typename Src::xy_locator::cached_location_t loc01 = src_loc.cache_location(-1, 0);
    typename Src::xy_locator::cached_location_t loc11 = src_loc.cache_location( 0, 0);
    typename Src::xy_locator::cached_location_t loc21 = src_loc.cache_location( 1, 0);
    typename Src::xy_locator::cached_location_t loc02 = src_loc.cache_location(-1, 1);
    typename Src::xy_locator::cached_location_t loc12 = src_loc.cache_location( 0, 1);
    typename Src::xy_locator::cached_location_t loc22 = src_loc.cache_location( 1, 1);

    typename Src::x_iterator dst_it = dst_view.row_begin(0);

    // top row
    for (int x = 0 ; x < src_view.width(); ++x)
    {
        *dst_it = src_loc[loc11];
        for (int i = 0; i < 3; ++i)
        {
            bits32f p[9];

            p[4] = src_loc[loc11][i];
            p[7] = src_loc[loc12][i];

            if (x == 0)
            {
                p[3] = p[4];
                p[6] = p[7];
            }
            else
            {
                p[3] = src_loc[loc01][i];
                p[6] = src_loc[loc02][i];
            }

            if ( x == src_view.width()-1)
            {
                p[5] = p[4];
                p[8] = p[7];
            }
            else
            {
                p[5] = src_loc[loc21][i];
                p[8] = src_loc[loc22][i];
            }

            p[0] = p[6];
            p[1] = p[7];
            p[2] = p[8];

            process_channel(p, (*dst_it)[i], filter);
        }
        ++src_loc.x();
        ++dst_it;
    }
    // carrige-return
    src_loc += point2<std::ptrdiff_t>(-src_view.width(),1);

    // 1... height-1 rows
    for (int y = 1; y<src_view.height()-1; ++y)
    {
        for (int x = 0; x < src_view.width(); ++x)
        {
            *dst_it = src_loc[loc11];
            for (int i = 0; i < 3; ++i)
            {
                bits32f p[9];

                p[1] = src_loc[loc10][i];
                p[4] = src_loc[loc11][i];
                p[7] = src_loc[loc12][i];

                if (x == 0)
                {
                    p[0] = p[1];
                    p[3] = p[4];
                    p[6] = p[7];
                }
                else
                {
                    p[0] = src_loc[loc00][i];
                    p[3] = src_loc[loc01][i];
                    p[6] = src_loc[loc02][i];
                }

                if ( x == src_view.width() - 1)
                {
                    p[2] = p[1];
                    p[5] = p[4];
                    p[8] = p[7];
                }
                else
                {
                    p[2] = src_loc[loc20][i];
                    p[5] = src_loc[loc21][i];
                    p[8] = src_loc[loc22][i];
                }
                process_channel(p, (*dst_it)[i], filter);
            }
            ++dst_it;
            ++src_loc.x();
        }
        // carrige-return
        src_loc += point2<std::ptrdiff_t>(-src_view.width(),1);
    }

    // bottom row
    //src_loc = src_view.xy_at(0,src_view.height()-1);
    for (int x = 0 ; x < src_view.width(); ++x)
    {
        *dst_it = src_loc[loc11];
        for (int i = 0; i < 3; ++i)
        {
            bits32f p[9];

            p[1] = src_loc[loc10][i];
            p[4] = src_loc[loc11][i];

            if (x == 0)
            {
                p[0] = p[1];
                p[3] = p[4];
            }
            else
            {
                p[0] = src_loc[loc00][i];
                p[3] = src_loc[loc01][i];
            }

            if ( x == src_view.width()-1)
            {
                p[2] = p[1];
                p[5] = p[4];

            }
            else
            {
                p[2] = src_loc[loc20][i];
                p[5] = src_loc[loc21][i];
            }

            p[6] = p[0];
            p[7] = p[1];
            p[8] = p[2];

            process_channel(p, (*dst_it)[i], filter);
        }
        ++src_loc.x();
        ++dst_it;
    }
}

template <typename Src, typename Filter>
void apply_filter(Src & src, Filter const& filter)
{
    double_buffer<Src> tb(src);
    apply_convolution_3x3(tb.src_view, tb.dst_view, filter);
}

template <typename Src>
void apply_filter(Src & src, agg_stack_blur const& op)
{
    agg::rendering_buffer buf(src.raw_data(),src.width(),src.height(), src.width() * 4);
    agg::pixfmt_rgba32 pixf(buf);
    agg::stack_blur_rgba32(pixf,op.rx,op.ry);
}

template <typename Src>
void apply_filter(Src & src, colorize_alpha const& op)
{
    using namespace boost::gil;

    agg::gradient_lut<agg::color_interpolator<agg::rgba8> > grad_lut;
    grad_lut.remove_all();
    std::size_t size = op.size();
    if (size < 2) return;

    double step = 1.0/(size-1);
    double offset = 0.0;
    BOOST_FOREACH( mapnik::filter::color_stop const& stop, op)
    {
        mapnik::color const& c = stop.color;
        double stop_offset = stop.offset;
        if (stop_offset == 0)
        {
            stop_offset = offset;
        }
        grad_lut.add_color(stop_offset, agg::rgba(c.red()/256.0,
                                                  c.green()/256.0,
                                                  c.blue()/256.0,
                                                  c.alpha()/256.0));
        offset += step;
    }
    grad_lut.build_lut();

    rgba8_view_t src_view = rgba8_view(src);
    for (int y=0; y<src_view.height(); ++y)
    {
        rgba8_view_t::x_iterator src_it = src_view.row_begin(y);
        for (int x=0; x<src_view.width(); ++x)
        {
            uint8_t & r = get_color(src_it[x], red_t());
            uint8_t & g = get_color(src_it[x], green_t());
            uint8_t & b = get_color(src_it[x], blue_t());
            uint8_t & a = get_color(src_it[x], alpha_t());
            if ( a > 0)
            {
                agg::rgba8 c = grad_lut[a];
                r = (c.r * a + 255) >> 8;
                g = (c.g * a + 255) >> 8;
                b = (c.b * a + 255) >> 8;
#if 0
                // rainbow
                r = 0;
                g = 0;
                b = 0;
                if (a < 64)
                {
                    g = a * 4;
                    b = 255;
                }
                else if (a >= 64 && a < 128)
                {
                    g = 255;
                    b = 255 - ((a - 64) * 4);
                }
                else if (a >= 128 && a < 192)
                {
                    r = (a - 128) * 4;
                    g = 255;
                }
                else // >= 192
                {
                    r = 255;
                    g = 255 - ((a - 192) * 4);
                }
                r = (r * a + 255) >> 8;
                g = (g * a + 255) >> 8;
                b = (b * a + 255) >> 8;
#endif
            }
        }
    }
}

/*
template <typename Src>
void apply_filter(Src & src, hsla const& transform)
{
    using namespace boost::gil;
    bool tinting = !transform.is_identity();
    bool set_alpha = !transform.is_alpha_identity();
    // todo - filters be able to report if they
    // should be run to avoid overhead of temp buffer
    if (tinting || set_alpha)
    {
        rgba8_view_t src_view = rgba8_view(src);
        for (int y=0; y<src_view.height(); ++y)
        {
            rgba8_view_t::x_iterator src_it = src_view.row_begin(y);
            for (int x=0; x<src_view.width(); ++x)
            {
                uint8_t & r = get_color(src_it[x], red_t());
                uint8_t & g = get_color(src_it[x], green_t());
                uint8_t & b = get_color(src_it[x], blue_t());
                uint8_t & a = get_color(src_it[x], alpha_t());
                double a2 = a/255.0;
                double a1 = a2;
                if (set_alpha && a2 > 0.01)
                {
                    a2 = transform.a0 + (a2 * (transform.a1 - transform.a0));
                    a = static_cast<unsigned>(std::floor((a2 * 255.0) +.5));
                    if (a > 255) a = 255;
                    if (a < 0) a = 0;
                }
                if (tinting && a2 > 0.01)
                {
                    double h;
                    double s;
                    double l;
                    // demultiply
                    if (a1 <= 0.0)
                    {
                        r = g = b = 0;
                        continue;
                    }
                    else if (a1 < 1)
                    {
                        r /= a1;
                        g /= a1;
                        b /= a1;
                    }
                    rgb2hsl(r,g,b,h,s,l);
                    double h2 = transform.h0 + (h * (transform.h1 - transform.h0));
                    double s2 = transform.s0 + (s * (transform.s1 - transform.s0));
                    double l2 = transform.l0 + (l * (transform.l1 - transform.l0));
                    if (h2 > 1) { std::clog << "h2: " << h2 << "\n"; h2 = 1; }
                    else if (h2 < 0) { std::clog << "h2: " << h2 << "\n"; h2 = 0; }
                    if (s2 > 1) { std::clog << "h2: " << h2 << "\n"; s2 = 1; }
                    else if (s2 < 0) { std::clog << "s2: " << s2 << "\n"; s2 = 0; }
                    if (l2 > 1) { std::clog << "h2: " << h2 << "\n"; l2 = 1; }
                    else if (l2 < 0) { std::clog << "l2: " << l2 << "\n"; l2 = 0; }
                    hsl2rgb(h2,s2,l2,r,g,b);
                    // premultiply
                    // we only work with premultiplied source,
                    // thus all color values must be <= alpha
                    r *= a2;
                    g *= a2;
                    b *= a2;
                }
                else
                {
                    // demultiply
                    if (a1 <= 0.0)
                    {
                        r = g = b = 0;
                        continue;
                    }
                    else if (a1 < 1)
                    {
                        r /= a1;
                        g /= a1;
                        b /= a1;
                    }
                    // premultiply
                    // we only work with premultiplied source,
                    // thus all color values must be <= alpha
                    r *= a2;
                    g *= a2;
                    b *= a2;
                }
            }
        }
    }
}
*/

template <typename Src>
void apply_filter(Src & src, gray const& op)
{
    using namespace boost::gil;

    rgba8_view_t src_view = rgba8_view(src);

    for (int y=0; y<src_view.height(); ++y)
    {
        rgba8_view_t::x_iterator src_it = src_view.row_begin(y);
        for (int x=0; x<src_view.width(); ++x)
        {
            // formula taken from boost/gil/color_convert.hpp:rgb_to_luminance
            uint8_t & r = get_color(src_it[x], red_t());
            uint8_t & g = get_color(src_it[x], green_t());
            uint8_t & b = get_color(src_it[x], blue_t());
            uint8_t   v = uint8_t((4915 * r + 9667 * g + 1802 * b + 8192) >> 14);
            r = g = b = v;
        }
    }
}

template <typename Src, typename Dst>
void x_gradient_impl(Src const& src_view, Dst const& dst_view)
{
    for (int y=0; y<src_view.height(); ++y)
    {
        typename Src::x_iterator src_it = src_view.row_begin(y);
        typename Dst::x_iterator dst_it = dst_view.row_begin(y);

        dst_it[0][0] = 128 + (src_it[0][0] - src_it[1][0]) / 2;
        dst_it[0][1] = 128 + (src_it[0][1] - src_it[1][1]) / 2;
        dst_it[0][2] = 128 + (src_it[0][2] - src_it[1][2]) / 2;

        dst_it[dst_view.width()-1][0] = 128 + (src_it[src_view.width()-2][0] - src_it[src_view.width()-1][0]) / 2;
        dst_it[dst_view.width()-1][1] = 128 + (src_it[src_view.width()-2][1] - src_it[src_view.width()-1][1]) / 2;
        dst_it[dst_view.width()-1][2] = 128 + (src_it[src_view.width()-2][2] - src_it[src_view.width()-1][2]) / 2;

        dst_it[0][3] = dst_it[src_view.width()-1][3] = 255;

        for (int x=1; x<src_view.width()-1; ++x)
        {
            dst_it[x][0] = 128 + (src_it[x-1][0] - src_it[x+1][0]) / 2;
            dst_it[x][1] = 128 + (src_it[x-1][1] - src_it[x+1][1]) / 2;
            dst_it[x][2] = 128 + (src_it[x-1][2] - src_it[x+1][2]) / 2;
            dst_it[x][3] = 255;
        }
    }
}

template <typename Src>
void apply_filter(Src & src, x_gradient const& op)
{
    double_buffer<Src> tb(src);
    x_gradient_impl(tb.src_view, tb.dst_view);
}

template <typename Src>
void apply_filter(Src & src, y_gradient const& op)
{
    double_buffer<Src> tb(src);
    x_gradient_impl(rotated90ccw_view(tb.src_view),
                    rotated90ccw_view(tb.dst_view));
}

template <typename Src>
void apply_filter(Src & src, invert const& op)
{
    using namespace boost::gil;

    rgba8_view_t src_view = rgba8_view(src);

    for (int y=0; y<src_view.height(); ++y)
    {
        rgba8_view_t::x_iterator src_it = src_view.row_begin(y);
        for (int x=0; x<src_view.width(); ++x)
        {
            // we only work with premultiplied source,
            // thus all color values must be <= alpha
            uint8_t   a = get_color(src_it[x], alpha_t());
            uint8_t & r = get_color(src_it[x], red_t());
            uint8_t & g = get_color(src_it[x], green_t());
            uint8_t & b = get_color(src_it[x], blue_t());
            r = a - r;
            g = a - g;
            b = a - b;
        }
    }
}

template <typename Src>
struct filter_visitor : boost::static_visitor<void>
{
    filter_visitor(Src & src)
    : src_(src) {}

    template <typename T>
    void operator () (T const& filter)
    {
        apply_filter(src_, filter);
    }

    Src & src_;
};

}}

#endif // MAPNIK_IMAGE_FILTER_HPP


/* ----------------------------------------------------------------------------
 * GTDynamics Copyright 2020, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 * @file  main.cpp
 * @brief Quadruped trajectory optimization with pre-specified footholds.
 * @Author: Alejandro Escontrela
 */

#include <gtdynamics/dynamics/DynamicsGraph.h>
#include <gtdynamics/dynamics/OptimizerSetting.h>
#include <gtdynamics/factors/MinTorqueFactor.h>
#include <gtdynamics/factors/PointGoalFactor.h>
#include <gtdynamics/universal_robot/Robot.h>
#include <gtdynamics/universal_robot/sdf.h>
#include <gtdynamics/utils/initialize_solution_utils.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtParams.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>

#include <algorithm>
#include <boost/algorithm/string/join.hpp>
#include <boost/optional.hpp>
#include <cmath>
#include <fstream>
#include <iostream>
#include <utility>

#define GROUND_HEIGHT -0.191839

using std::string;
using std::vector;

using gtsam::Point3;
using gtsam::Pose3;
using gtsam::Rot3;
using gtsam::Values;
using gtsam::Vector;
using gtsam::Vector3;
using gtsam::Vector6;
using gtsam::noiseModel::Isotropic;

using namespace gtdynamics;

int main(int argc, char** argv) {
  // Load the quadruped. Based on the vision 60 quadruped by Ghost robotics:
  // https://youtu.be/wrBNJKZKg10
  auto robot =
      gtdynamics::CreateRobotFromFile(kUrdfPath + string("/vision60.urdf"));

  double sigma_dynamics = 1e-6;    // std of dynamics constraints.
  double sigma_objectives = 1e-4;  // std of additional objectives.

  // Noise models.
  auto dynamics_model_6 = Isotropic::Sigma(6, sigma_dynamics),
       dynamics_model_3 = Isotropic::Sigma(3, sigma_dynamics),
       dynamics_model_1 = Isotropic::Sigma(1, sigma_dynamics),
       objectives_model_6 = Isotropic::Sigma(6, sigma_objectives),
       objectives_model_3 = Isotropic::Sigma(3, sigma_objectives),
       objectives_model_1 = Isotropic::Sigma(1, sigma_objectives);

  // Initialize graph builder with desired dynamics constraint stds.
  // TODO(aescontrela): Make a constructor for OptimizerSetting that
  //     initializes all noise models with the same sigma.
  auto opt = gtdynamics::OptimizerSetting();
  opt.bp_cost_model = dynamics_model_6;
  opt.bv_cost_model = dynamics_model_6;
  opt.ba_cost_model = dynamics_model_6;
  opt.p_cost_model = dynamics_model_6;
  opt.v_cost_model = dynamics_model_6;
  opt.a_cost_model = dynamics_model_6;
  opt.f_cost_model = dynamics_model_6;
  opt.fa_cost_model = dynamics_model_6;
  opt.t_cost_model = dynamics_model_1;
  opt.cp_cost_model = dynamics_model_1;
  opt.cfriction_cost_model = dynamics_model_1;
  opt.cv_cost_model = dynamics_model_3;
  opt.ca_cost_model = dynamics_model_3;
  opt.planar_cost_model = dynamics_model_3;
  opt.prior_q_cost_model = dynamics_model_1;
  opt.prior_qv_cost_model = dynamics_model_1;
  opt.prior_qa_cost_model = dynamics_model_1;
  opt.prior_t_cost_model = dynamics_model_1;
  opt.q_col_cost_model = dynamics_model_1;
  opt.v_col_cost_model = dynamics_model_1;
  opt.time_cost_model = dynamics_model_1;
  auto graph_builder = gtdynamics::DynamicsGraph(opt);

  // Env parameters.
  Vector3 gravity(0, 0, -9.8);
  double mu = 1.0;

  // All contacts.
  auto c0 = ContactPoint{Point3(0.14, 0, 0), 0};  // Front left.
  auto c1 = ContactPoint{Point3(0.14, 0, 0), 0};  // Hind left.
  auto c2 = ContactPoint{Point3(0.14, 0, 0), 0};  // Front right.
  auto c3 = ContactPoint{Point3(0.14, 0, 0), 0};  // Hind right.

  // Contact points for each phase. First move one leg at a time then switch
  // to a more dynamic gait with two legs in swing per phase.
  using CPs = ContactPoints;
  // Initially stationary.
  CPs p0 = {{"lower0", c0}, {"lower1", c1}, {"lower2", c2}, {"lower3", c3}};
  CPs t01 = {{"lower1", c1}, {"lower2", c2}, {"lower3", c3}};
  // Front right leg swing.
  CPs p1 = {{"lower1", c1}, {"lower2", c2}, {"lower3", c3}};
  CPs t12 = {{"lower2", c2}, {"lower3", c3}};
  // Front left leg swing.
  CPs p2 = {{"lower0", c0}, {"lower2", c2}, {"lower3", c3}};
  CPs t23 = {{"lower0", c0}, {"lower3", c3}};
  // Hind right swing.
  CPs p3 = {{"lower0", c0}, {"lower1", c1}, {"lower3", c3}};
  CPs t34 = {{"lower0", c0}, {"lower1", c1}};
  // Hind left swing.
  CPs p4 = {{"lower0", c0}, {"lower1", c1}, {"lower2", c2}};
  CPs t45 = {{"lower1", c1}, {"lower2", c2}};
  // Front left leg and hind right swing.
  CPs p5 = {{"lower1", c1}, {"lower2", c2}};
  CPs t56 = {};
  // Front right leg and hind left swing.
  CPs p6 = {{"lower0", c0}, {"lower3", c3}};

  // Define contact points for each phase, transition contact points,
  // and phase durations.
  vector<CPs> phase_cps = {p0, p1, p0, p2, p0, p3, p0, p4,
                           p0, p1, p0, p2, p0, p3, p0, p4};
  vector<CPs> trans_cps = {p1, p1, p2, p2, p3, p3, p4, p4,
                           p1, p1, p2, p2, p3, p3, p4};
  vector<int> phase_steps = {50, 60, 50, 60, 50, 60, 50, 60,
                             50, 60, 50, 60, 50, 60, 50, 60};

  // Define the cumulative phase steps.
  vector<int> cum_phase_steps;
  for (int i = 0; i < phase_steps.size(); i++) {
    int cum_val =
        i == 0 ? phase_steps[0] : phase_steps[i] + cum_phase_steps[i - 1];
    cum_phase_steps.push_back(cum_val);
    std::cout << cum_val << std::endl;
  }
  int t_f = cum_phase_steps[cum_phase_steps.size() - 1];  // Final timestep.
  double dt_des = 1. / 240.;  // Desired timestep duration.

  // Robot model for each phase.
  vector<Robot> robots(phase_cps.size(), robot);

  // Collocation scheme.
  auto collocation = CollocationScheme::Euler;

  // Graphs for transition between phases + their initial values.
  vector<gtsam::NonlinearFactorGraph> transition_graphs;
  vector<Values> transition_graph_init;
  double gaussian_noise = 1e-5;  // Add gaussian noise to initial values.
  for (int p = 1; p < phase_cps.size(); p++) {
    transition_graphs.push_back(graph_builder.dynamicsFactorGraph(
        robot, cum_phase_steps[p - 1], trans_cps[p - 1], mu));
    transition_graph_init.push_back(ZeroValues(
        robot, cum_phase_steps[p - 1], gaussian_noise, trans_cps[p - 1]));
  }

  // Construct the multi-phase trajectory factor graph.
  auto graph = graph_builder.multiPhaseTrajectoryFG(
      robot, phase_steps, transition_graphs, collocation, phase_cps, mu);

  // Build the objective factors.
  gtsam::NonlinearFactorGraph objective_factors;
  auto base_link = robot.link("body");
  vector<string> links = {"lower0", "lower1", "lower2", "lower3"};
  std::map<string, gtdynamics::LinkSharedPtr> link_map;
  for (auto&& link : links)
    link_map.insert(std::make_pair(link, robot.link(link)));

  // Compute the centroid of the contacts.
  auto centroid = Point3(0, 0, 0);
  for (auto&& link : links)
    centroid +=
        (link_map[link]->wTcom() * Pose3(Rot3(), c0.point)).translation();
  centroid = centroid / 4;

  std::map<string, double> centroid_contact_dist;
  std::map<string, double> prev_theta;
  for (auto&& link : links) {
    auto cp = (link_map[link]->wTcom() * Pose3(Rot3(), c0.point)).translation();
    auto delta = cp - centroid;
    centroid_contact_dist.insert(std::make_pair(link, delta.norm()));
    prev_theta.insert(std::make_pair(link, std::atan2(delta.y(), delta.x())));
  }

  // Distance to move contact point during swing.
  double theta_inc = M_PI / 6;

  // Add contact point objectives to factor graph.
  for (int p = 0; p < phase_cps.size(); p++) {
    // Phase start and end timesteps.
    int t_p_i = cum_phase_steps[p] - phase_steps[p];
    if (p != 0) t_p_i += 1;
    int t_p_f = cum_phase_steps[p];

    // Obtain the contact links and swing links for this phase.
    vector<string> phase_contact_links;
    for (auto&& kv : phase_cps[p]) {
      phase_contact_links.push_back(kv.first);
    }

    vector<string> phase_swing_links;
    for (auto&& l : links) {
      if (std::find(phase_contact_links.begin(), phase_contact_links.end(),
                    l) == phase_contact_links.end())
        phase_swing_links.push_back(l);
    }

    // Update the goal point for the swing links.
    for (auto&& psl : phase_swing_links)
      prev_theta[psl] = prev_theta[psl] + theta_inc;

    for (int t = t_p_i; t <= t_p_f; t++) {
      // Normalized phase progress.
      double t_normed = (double)(t - t_p_i) / (double)(t_p_f - t_p_i);
      for (auto&& pcl : phase_contact_links) {
        // TODO(aescontrela): Use correct contact point for each link.
        auto new_cp = Point3(centroid.x() + centroid_contact_dist[pcl] *
                                                std::cos(prev_theta[pcl]),
                             centroid.y() + centroid_contact_dist[pcl] *
                                                std::sin(prev_theta[pcl]),
                             GROUND_HEIGHT - 0.03);

        objective_factors.add(gtdynamics::PointGoalFactor(
            internal::PoseKey(link_map[pcl]->id(), t), objectives_model_3,
            c0.point, new_cp));
      }

      double h = GROUND_HEIGHT +
                 0.05 * std::pow(t_normed, 1.1) * std::pow(1 - t_normed, 0.7);

      for (auto&& psl : phase_swing_links) {
        auto new_cp = Point3(centroid.x() + centroid_contact_dist[psl] *
                                                std::cos(prev_theta[psl]),
                             centroid.y() + centroid_contact_dist[psl] *
                                                std::sin(prev_theta[psl]),
                             h);

        objective_factors.add(gtdynamics::PointGoalFactor(
            internal::PoseKey(link_map[psl]->id(), t), objectives_model_3,
            c0.point, new_cp));
      }
    }
  }

  // Add base goal objectives to the factor graph.
  auto base_pose_goal =
      Pose3(Rot3::Rz(theta_inc * phase_cps.size() / 8.), Point3(0, 0, 0.13));
  auto base_pose_model = gtsam::noiseModel::Diagonal::Sigmas(
      (Vector(6) << sigma_objectives * 3, sigma_objectives * 3,
       sigma_objectives * 100, 10000, sigma_objectives, sigma_objectives)
          .finished());
  for (int t = 0; t <= t_f; t++)
    objective_factors.addPrior(internal::PoseKey(base_link->id(), t),
                               base_pose_goal, base_pose_model);

  // Add link boundary conditions to FG.
  for (auto&& link : robot.links()) {
    // Initial link pose, twists.
    objective_factors.addPrior(internal::PoseKey(link->id(), 0), link->wTcom(),
                               dynamics_model_6);
    objective_factors.addPrior<Vector6>(internal::TwistKey(link->id(), 0),
                                        Vector6::Zero(), dynamics_model_6);

    // Final link twists, accelerations.
    objective_factors.addPrior<Vector6>(internal::TwistKey(link->id(), t_f),
                                        Vector6::Zero(), objectives_model_6);
    objective_factors.addPrior<Vector6>(
        internal::TwistAccelKey(link->id(), t_f), Vector6::Zero(),
        objectives_model_6);
  }

  // Add joint boundary conditions to FG.
  for (auto&& joint : robot.joints()) {
    objective_factors.addPrior(internal::JointAngleKey(joint->id(), 0), 0.0,
                               dynamics_model_1);
    objective_factors.addPrior(internal::JointVelKey(joint->id(), 0), 0.0,
                               dynamics_model_1);

    objective_factors.addPrior(internal::JointVelKey(joint->id(), t_f), 0.0,
                               objectives_model_1);
    objective_factors.addPrior(internal::JointAccelKey(joint->id(), t_f), 0.0,
                               objectives_model_1);
  }

  // Add prior factor constraining all Phase keys to have duration of 1 / 240.
  for (int phase = 0; phase < phase_steps.size(); phase++) {
    objective_factors.addPrior(PhaseKey(phase), dt_des,
                               gtsam::noiseModel::Isotropic::Sigma(1, 1e-30));
  }

  // Add min torque objectives.
  for (int t = 0; t <= t_f; t++) {
    for (auto&& joint : robot.joints())
      objective_factors.add(gtdynamics::MinTorqueFactor(
          internal::TorqueKey(joint->id(), t),
          gtsam::noiseModel::Gaussian::Covariance(gtsam::I_1x1)));
  }
  graph.add(objective_factors);

  // Initialize solution.
  gtsam::Values init_vals;
  init_vals = gtdynamics::MultiPhaseZeroValuesTrajectory(
      robot, phase_steps, transition_graph_init, dt_des, gaussian_noise,
      phase_cps);

  // Optimize!
  gtsam::LevenbergMarquardtParams params;
  params.setVerbosityLM("SUMMARY");
  params.setlambdaInitial(1e0);
  params.setlambdaLowerBound(1e-7);
  params.setlambdaUpperBound(1e10);
  gtsam::LevenbergMarquardtOptimizer optimizer(graph, init_vals, params);
  gtsam::Values results = optimizer.optimize();

  // Log the joint angles, velocities, accels, torques, and current goal pose.
  vector<string> jnames;
  for (auto&& joint : robot.joints()) jnames.push_back(joint->name());
  string jnames_str = boost::algorithm::join(jnames, ",");
  std::ofstream traj_file;
  traj_file.open("traj.csv");
  // angles, vels, accels, torques, time.
  traj_file << jnames_str << "," << jnames_str << "," << jnames_str << ","
            << jnames_str << ",t"
            << "\n";
  int t = 0;
  for (int phase = 0; phase < phase_steps.size(); phase++) {
    for (int phase_step = 0; phase_step < phase_steps[phase]; phase_step++) {
      vector<string> vals;
      for (auto&& joint : robot.joints())
        vals.push_back(std::to_string(JointAngle(results, joint->id(), t)));
      for (auto&& joint : robot.joints())
        vals.push_back(std::to_string(JointVel(results, joint->id(), t)));
      for (auto&& joint : robot.joints())
        vals.push_back(std::to_string(JointAccel(results, joint->id(), t)));
      for (auto&& joint : robot.joints())
        vals.push_back(std::to_string(Torque(results, joint->id(), t)));

      vals.push_back(std::to_string(results.atDouble(PhaseKey(phase))));

      t++;
      string vals_str = boost::algorithm::join(vals, ",");
      traj_file << vals_str << "\n";
    }
  }
  traj_file.close();

  return 0;
}


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

/// \copyright Copyright 2021 the Autoware Foundation
/// All rights reserved.
/// \file
/// \brief This file defines a class for a generic state vector representation.

#ifndef KALMAN_FILTER__GENERIC_STATE_HPP_
#define KALMAN_FILTER__GENERIC_STATE_HPP_

#include <common/type_traits.hpp>
#include <common/types.hpp>
#include <helper_functions/type_name.hpp>
#include <kalman_filter/common_variables.hpp>
#include <kalman_filter/visibility_control.hpp>

#include <Eigen/Core>

#include <tuple>

namespace autoware
{
namespace prediction
{

/// Forward-declare is_state trait.
template<typename StateT>
struct is_state;

///
/// @brief      A representation of a generic state vectors with specified variables.
///
/// @tparam     VariableTs  Variables which this state consists of.
///
template<typename ScalarT, typename ... VariableTs>
class KALMAN_FILTER_PUBLIC GenericState
{
  static_assert(
    common::type_traits::conjunction<is_variable<VariableTs>...>::value,
    "\n\nState can only be generated from variables, i.e. types that inherit from Variable.\n\n");
  static_assert(
    std::is_arithmetic<ScalarT>::value, "\n\nThe provided scalar type is not arithmetic.\n\n");
  static_assert(
    sizeof...(VariableTs) > 0, "\n\nCannot create state without variables.\n\n");
  // Hide this under private to make sure it is never ODR-used.
  constexpr static std::int32_t kSize = sizeof...(VariableTs);

public:
  using Variables = std::tuple<VariableTs...>;
  using Vector = Eigen::Matrix<ScalarT, kSize, 1>;
  using Matrix = Eigen::Matrix<ScalarT, kSize, kSize>;
  using Scalar = ScalarT;

  ///
  /// @brief      Default constructor.
  ///
  GenericState() = default;

  ///
  /// @brief      Constructs a new instance from an Eigen vector
  ///
  /// @param[in]  vector  An Eigen vector.
  ///
  explicit GenericState(const Vector & vector)
  : m_state{vector} {}

  ///
  /// @brief      Get underlying Eigen vector.
  ///
  /// @return     A reference to the underlying Eigen vector.
  ///
  inline Vector & vector() noexcept {return m_state;}
  ///
  /// @brief      Get underlying Eigen vector.
  ///
  /// @return     A const reference to the underlying Eigen vector.
  ///
  inline const Vector & vector() const noexcept {return m_state;}

  ///
  /// @brief      Get an element of the vector.
  ///
  /// @param[in]  idx   The index of the vector element.
  ///
  /// @return     A reference to the vector element.
  ///
  inline ScalarT & operator[](const Eigen::Index idx) {return m_state[idx];}
  ///
  /// @brief      Get an element of the vector.
  ///
  /// @param[in]  idx   The index of the vector element.
  ///
  /// @return     A reference to the vector element.
  ///
  inline const ScalarT & operator[](const Eigen::Index idx) const {return m_state[idx];}

  ///
  /// @brief      Get vector element at a given variable.
  ///
  /// @tparam     VariableT  A variable from the state.
  ///
  /// @return     A reference to the element representing this variable.
  ///
  template<typename VariableT>
  inline ScalarT & at() noexcept
  {
    static_assert(is_variable<VariableT>::value, "Type is not a Variable.");
    return m_state[common::type_traits::index<VariableT, Variables>::value];
  }
  ///
  /// @brief      Get vector element at a given variable.
  ///
  /// @tparam     VariableT  A variable from the state.
  ///
  /// @return     A reference to the element representing this variable.
  ///
  template<typename VariableT>
  inline ScalarT & at(const VariableT) noexcept
  {
    static_assert(is_variable<VariableT>::value, "Type is not a Variable.");
    return m_state[common::type_traits::index<VariableT, Variables>::value];
  }
  ///
  /// @brief      Get vector element at a given variable.
  ///
  /// @tparam     VariableT  A variable from the state.
  ///
  /// @return     A reference to the element representing this variable.
  ///
  template<typename VariableT>
  inline const ScalarT & at() const noexcept
  {
    static_assert(is_variable<VariableT>::value, "Type is not a Variable.");
    return m_state[common::type_traits::index<VariableT, Variables>::value];
  }
  ///
  /// @brief      Get vector element at a given variable.
  ///
  /// @tparam     VariableT  A variable from the state.
  ///
  /// @return     A reference to the element representing this variable.
  ///
  template<typename VariableT>
  inline const ScalarT & at(const VariableT) const noexcept
  {
    static_assert(is_variable<VariableT>::value, "Type is not a Variable.");
    return m_state[common::type_traits::index<VariableT, Variables>::value];
  }

  ///
  /// @brief      Copy values of this state into another state.
  ///
  ///             This function copies (a subset of) variables from the current state into another
  ///             state. If `other_state` is provided, it is used as a "donor" state, i.e., the
  ///             values already in this `other_state` are unchanged unless overwritten by the ones
  ///             present in the current state. If no `other_state` is given, a new zero-initialized
  ///             instance of OtherStateT will be created and this state will be used for the
  ///             further update using the variables from the current state.
  ///
  ///             This function can be used in a couple different situations:
  ///             - when a smaller current state must be copied into a bigger other state,
  ///               overwriting some of that bigger state's variables;
  ///             - when a reduction of the current state onto a smaller state is required;
  ///             - when a rearrangement of the variables is needed.
  ///
  ///             Please see tests for examples of usage.
  ///
  /// @note       This function only is correctly compiled if either StateT is a subset of
  ///             OtherStateT, or OtherStateT is a subset of the StateT. Otherwise, a compilation
  ///             error will be issued.
  ///
  /// @param[in]  other_state  Other state.
  ///
  /// @tparam     OtherStateT  Type of the other state.
  ///
  /// @return     The updated other state.
  ///
  template<typename OtherStateT>
  inline OtherStateT copy_into(OtherStateT other_state = OtherStateT{}) const noexcept
  {
    static_assert(
      is_state<OtherStateT>::value,
      "\n\nOtherStateT is not a GenericState.\n\n");
    auto copy_variables = [&other_state, this](auto variable) {
        other_state.at(variable) = this->at(variable);
      };
    const auto & variables_tuple =
      std::conditional_t<(OtherStateT::size() > kSize), GenericState, OtherStateT>::variables();
    common::type_traits::visit(variables_tuple, copy_variables);
    return other_state;
  }

  ///
  /// @brief      Get an index of the variable in the state.
  ///
  /// @tparam     VariableT  A variable from the state.
  ///
  /// @return     An index of the given variable in the state vector.
  ///
  template<typename VariableT>
  inline constexpr static Eigen::Index index_of() noexcept
  {
    static_assert(is_variable<VariableT>::value, "Type is not a Variable.");
    return common::type_traits::index<VariableT, Variables>::value;
  }

  ///
  /// @brief      Get the size of the state.
  ///
  /// @note       This can also be called on an instance of the state.
  ///
  /// @return     Number of variables in this state.
  ///
  inline constexpr static Eigen::Index size() noexcept {return kSize;}

  /// @brief      Get a variables tuple used for iteration over all variables.
  inline constexpr static Variables variables() noexcept{return Variables{};}

  /// @brief      An equality operator.
  friend constexpr bool operator==(const GenericState & lhs, const GenericState & rhs)
  {
    return lhs.m_state.isApprox(rhs.m_state);
  }

  /// @brief      Allow using << operator with this class.
  ///
  ///             This function adds the variables to the output stream along with their names.
  friend std::ostream & operator<<(std::ostream & out, const GenericState & state)
  {
    out << "State:";
    auto print = [&out, &state](auto element) {
        out << "\n  " << helper_functions::get_type_name(element) << ": " << state.at(element);
      };
    common::type_traits::visit(GenericState::variables(), print);
    return out;
  }

private:
  /// Underlying Eigen vector.
  Vector m_state{Vector::Zero()};
};

///
/// @brief      Check if a given type is a state.
///
/// @tparam     StateT  A query potential state type.
///
template<typename StateT>
struct KALMAN_FILTER_PUBLIC is_state : public std::false_type {};

///
/// @brief      A specialization of this trait for GenericState type.
///
/// @tparam     ScalarT     A scalar type of a state.
/// @tparam     VariableTs  Variable types.
///
template<typename ScalarT, typename ... VariableTs>
struct KALMAN_FILTER_PUBLIC is_state<GenericState<ScalarT, VariableTs...>>
  : public std::true_type {};

/// A typedef for the 32 bit floating point state.
template<typename ... Ts>
using FloatState = GenericState<common::types::float32_t, Ts...>;

/// A typedef for the 64 bit floating point state.
template<typename ... Ts>
using DoubleState = GenericState<common::types::float64_t, Ts...>;

}  // namespace prediction
}  // namespace autoware

#endif  // KALMAN_FILTER__GENERIC_STATE_HPP_


/**
 * @author     : n-jing (siliuhe@sina.com)
 * @date       : 2020-07-23
 * @description: Simulate coupling between PBD fluid and elastic bodies with different algorithm backends
 * @version    : 1.0
 *
 * @author     : Zhu Fei (feizhu@pku.edu.cn)
 * @date       : 2021-07-17
 * @description: poslish code
 * @version    : 1.1
 */

#include <memory>
#include <boost/property_tree/json_parser.hpp>

#include "Framework/Framework/SceneGraph.h"
#include "Framework/Framework/Log.h"
#include "Framework/Mapping/PointSetToPointSet.h"
#include "Dynamics/ParticleSystem/PositionBasedFluidModel.h"
#include "Dynamics/ParticleSystem/ParticleFluid.h"
#include "Dynamics/ParticleSystem/ParticleElasticBody.h"
#include "Dynamics/ParticleSystem/ElasticityModule.h"
#include "Dynamics/ParticleSystem/StaticBoundary.h"
#include "Dynamics/ParticleSystem/SolidFluidInteraction.h"
#include "Dynamics/EmbeddedMethod/EmbeddedFiniteElement.h"
#include "Dynamics/EmbeddedMethod/EmbeddedMassSpring.h"
#include "Rendering/SurfaceMeshRender.h"
#include "Rendering/PointRenderModule.h"
#include "GUI/GlutGUI/GLApp.h"

using namespace std;
using namespace PhysIKA;

/**
 * setup scene: dambreak fluid simulated with PBD, 3 elastic bodies simulated with Embedded FEM, embedded Mass-spring,
 * and projective-peridynamics respectively
 */
void createScene()
{
    SceneGraph& scene = SceneGraph::getInstance();

    std::shared_ptr<StaticBoundary<DataType3f>> root = scene.createNewScene<StaticBoundary<DataType3f>>();
    root->loadCube(Vector3f(0), Vector3f(1), 0.015f, true);  //scene boundary

    //PBD fluid
    std::shared_ptr<ParticleFluid<DataType3f>> fluid = std::make_shared<ParticleFluid<DataType3f>>();
    root->addParticleSystem(fluid);
    fluid->loadParticles(Vector3f(0), Vector3f(0.5, 1.0, 1.0), 0.015f);
    fluid->setMass(10);
    std::shared_ptr<PositionBasedFluidModel<DataType3f>> pbd = std::make_shared<PositionBasedFluidModel<DataType3f>>();
    pbd->setSmoothingLength(0.02);
    fluid->setNumericalModel(pbd);
    fluid->currentPosition()->connect(&pbd->m_position);
    fluid->currentVelocity()->connect(&pbd->m_velocity);
    fluid->currentForce()->connect(&pbd->m_forceDensity);
    auto ptRender = std::make_shared<PointRenderModule>();
    ptRender->setColor(Vector3f(0, 0, 1));
    ptRender->setColorRange(0, 1);
    fluid->addVisualModule(ptRender);

    //sfi
    std::shared_ptr<SolidFluidInteraction<DataType3f>> sfi = std::make_shared<SolidFluidInteraction<DataType3f>>();
    sfi->setInteractionDistance(0.02);
    root->addChild(sfi);
    sfi->addParticleSystem(fluid);

    //elastic bodies
    for (int i = 0; i < 3; i++)
    {
        if (i % 3 == 0)
        {
            //elastic bunny simulated with embedded FEM
            std::shared_ptr<EmbeddedFiniteElement<DataType3f>> bunny = std::make_shared<EmbeddedFiniteElement<DataType3f>>();
            root->addParticleSystem(bunny);
            bunny->setMass(1.0);
            bunny->loadParticles("../../Media/bunny/sparse_bunny_points.obj");
            bunny->loadSurface("../../Media/bunny/sparse_bunny_mesh.obj");
            bunny->translate(Vector3f(0.75, 0.2, 0.4 + i * 0.3));
            bunny->setVisible(false);
            bunny->getElasticitySolver()->inHorizon()->setValue(0.03);
            bunny->getTopologyMapping()->setSearchingRadius(0.05);
            bunny->getElasticitySolver()->setIterationNumber(10);
            auto sRender = std::make_shared<SurfaceMeshRender>();
            bunny->getSurfaceNode()->addVisualModule(sRender);
            sRender->setColor(Vector3f(1, 1, 0));
            boost::property_tree::ptree pt;
            const std::string           jsonfile_path = "../../Media/bunny/embedded_finite_element_sparse.json";
            read_json(jsonfile_path, pt);
            bunny->init_problem_and_solver(pt);
            sfi->addParticleSystem(bunny);
        }
        else if (i % 3 == 1)
        {
            //elastic bunny simulated with projective-peridynamics
            std::shared_ptr<ParticleElasticBody<DataType3f>> bunny = std::make_shared<ParticleElasticBody<DataType3f>>();
            root->addParticleSystem(bunny);
            bunny->setMass(1.0);
            bunny->loadParticles("../../Media/bunny/sparse_bunny_points.obj");
            bunny->loadSurface("../../Media/bunny/sparse_bunny_mesh.obj");
            bunny->translate(Vector3f(0.75, 0.2, 0.4 + i * 0.3));
            bunny->setVisible(false);
            bunny->getElasticitySolver()->setIterationNumber(10);
            bunny->getElasticitySolver()->inHorizon()->setValue(0.03);
            bunny->getTopologyMapping()->setSearchingRadius(0.05);
            auto sRender = std::make_shared<SurfaceMeshRender>();
            bunny->getSurfaceNode()->addVisualModule(sRender);
            sRender->setColor(Vector3f(1, 0, 1));
            sfi->addParticleSystem(bunny);
        }
        else
        {
            //elastic bunny simulated with embedded mass spring method
            std::shared_ptr<EmbeddedMassSpring<DataType3f>> bunny = std::make_shared<EmbeddedMassSpring<DataType3f>>();
            root->addParticleSystem(bunny);
            bunny->setMass(1.0);
            bunny->loadParticles("../../Media/bunny/sparse_bunny_points.obj");
            bunny->loadSurface("../../Media/bunny/sparse_bunny_mesh.obj");
            bunny->translate(Vector3f(0.75, 0.2, 0.4 + i * 0.3));
            bunny->setVisible(false);
            bunny->getElasticitySolver()->inHorizon()->setValue(0.03);
            bunny->getTopologyMapping()->setSearchingRadius(0.05);
            auto sRender = std::make_shared<SurfaceMeshRender>();
            bunny->getSurfaceNode()->addVisualModule(sRender);
            sRender->setColor(Vector3f(0, 1, 1));
            boost::property_tree::ptree pt;
            const std::string           jsonfile_path = "../../Media/bunny/embedded_mass_spring_sparse.json";
            read_json(jsonfile_path, pt);
            bunny->init_problem_and_solver(pt);
            sfi->addParticleSystem(bunny);
        }
    }
}

int main()
{
    createScene();

    Log::setOutput("console_log.txt");
    Log::setLevel(Log::Info);
    Log::sendMessage(Log::Info, "Simulation begin");

    GLApp window;
    window.createWindow(1024, 768);
    window.mainLoop();

    Log::sendMessage(Log::Info, "Simulation end!");
    return 0;
}


//  Copyright (c) 2019 Robert Ramey
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <boost/safe_numerics/safe_integer.hpp>
#include <boost/safe_numerics/automatic.hpp>
#include "test_left_shift_automatic_results.hpp"

template <class T>
using safe_t = boost::safe_numerics::safe<
    T,
    boost::safe_numerics::automatic
>;

#include "test_left_shift_constexpr.hpp"

using namespace boost::mp11;

template<typename First, typename Second>
struct test_pair {
    static const std::size_t i = First();
    static const std::size_t j = Second();
    constexpr static const bool value = test_left_shift_constexpr(
        mp_at_c<test_values, i>()(),
        mp_at_c<test_values, j>()(),
        test_left_shift_automatic_result[i][j]
    );
};

#include <boost/mp11/list.hpp>
#include <boost/mp11/algorithm.hpp>

int main(){
    using namespace boost::mp11;

    using value_indices = mp_iota_c<mp_size<test_values>::value>;

    static_assert(
        mp_all_of<
            mp_product<
                test_pair,
                value_indices,
                value_indices
            >,
            mp_to_bool
        >(),
        "all values for all integer types correctly shifted to the left"
    );
    return 0;
}


#include <boost/math/special_functions/relative_difference.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;
}


/*
 [auto_generated]
 libs/numeric/odeint/test/bulirsch_stoer.cpp

 [begin_description]
 This file tests the Bulirsch-Stoer stepper.
 [end_description]

 Copyright 2009-2012 Karsten Ahnert
 Copyright 2009-2012 Mario Mulansky

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

// disable checked iterator warning for msvc
#include <boost/config.hpp>
#ifdef BOOST_MSVC
    #pragma warning(disable:4996)
#endif

#define BOOST_TEST_MODULE odeint_bulirsch_stoer

#include <utility>
#include <iostream>

#include <boost/array.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/numeric/odeint/stepper/bulirsch_stoer.hpp>
#include <boost/numeric/odeint/stepper/bulirsch_stoer_dense_out.hpp>

#include <boost/numeric/odeint/integrate/integrate_adaptive.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;

typedef double value_type;
typedef boost::array< value_type , 3 > state_type;

const double sigma = 10.0;
const double R = 28.0;
const double b = 8.0 / 3.0;

struct lorenz
{
    template< class State , class Deriv >
    void operator()( const State &x , Deriv &dxdt , double t ) const
    {
        dxdt[0] = sigma * ( x[1] - x[0] );
        dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
        dxdt[2] = -b * x[2] + x[0] * x[1];
    }
};

struct const_system
{
    template< class State , class Deriv >
    void operator()( const State &x , Deriv &dxdt , double t ) const
    {
        dxdt[0] = 1.0;
        dxdt[1] = 1.0;
        dxdt[2] = 1.0;
    }
};

struct sin_system
{
    template< class State , class Deriv >
    void operator()( const State &x , Deriv &dxdt , double t ) const
    {
        dxdt[0] = sin( x[0] );
        dxdt[1] = cos( x[1] );
        dxdt[2] = sin( x[2] ) + cos( x[2] );
    }
};

BOOST_AUTO_TEST_SUITE( bulirsch_stoer_test )

BOOST_AUTO_TEST_CASE( test_bulirsch_stoer )
{
    typedef bulirsch_stoer< state_type > stepper_type;
    stepper_type stepper( 1E-9 , 1E-9 , 1.0 , 0.0 );

    state_type x;
    x[0] = 10.0 ; x[1] = 10.0 ; x[2] = 5.0;

    double dt = 0.1;

    //stepper.try_step( lorenz() , x , t , dt );

    std::cout << "starting integration..." << std::endl;

    size_t steps = integrate_adaptive( stepper , lorenz() , x , 0.0 , 10.0 , dt );

    std::cout << "required steps: " << steps << std::endl;

    bulirsch_stoer_dense_out< state_type > bs_do( 1E-9 , 1E-9 , 1.0 , 0.0 );
    x[0] = 10.0 ; x[1] = 10.0 ; x[2] = 5.0;
    double t = 0.0;
    dt = 1E-1;
    bs_do.initialize( x , t , dt );
    bs_do.do_step( sin_system() );
    std::cout << "one step successful, new time: " << bs_do.current_time() << " (" << t << ")" << std::endl;

    x = bs_do.current_state();
    std::cout << "x( " << bs_do.current_time() << " ) = [ " << x[0] << " , " << x[1] << " , " << x[2] << " ]" << std::endl;

    bs_do.calc_state( bs_do.current_time()/3 , x );
    std::cout << "x( " << bs_do.current_time()/3 << " ) = [ " << x[0] << " , " << x[1] << " , " << x[2] << " ]" << std::endl;

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

    x[0] = 10.0 ; x[1] = 10.0 ; x[2] = 5.0;
    t = 0.0; dt /= 3;
    bs_do.initialize( x , t , dt );
    bs_do.do_step( sin_system() );
    x = bs_do.current_state();
    std::cout << "x( " << bs_do.current_time() << " ) = [ " << x[0] << " , " << x[1] << " , " << x[2] << " ]" << std::endl;

    t = dt;
    bs_do.initialize( x , t , dt );
    bs_do.do_step( sin_system() );
    x = bs_do.current_state();

    t = 2*dt;
    bs_do.initialize( x , t , dt );
    bs_do.do_step( sin_system() );
    x = bs_do.current_state();

    std::cout << "x( " << bs_do.current_time() << " ) = [ " << x[0] << " , " << x[1] << " , " << x[2] << " ]" << std::endl;
}

BOOST_AUTO_TEST_SUITE_END()