#define EIGEN_STACK_ALLOCATION_LIMIT 512000

#include <Lodestar/blocks/std/ConstantBlock.hpp>
#include <Lodestar/blocks/std/SumBlock.hpp>
#include <Lodestar/blocks/std/GainBlock.hpp>
#include <Lodestar/blocks/std/MinMaxIdxBlock.hpp>
#include <Lodestar/blocks/std/UnaryAbsBlock.hpp>
#include <Lodestar/blocks/std/UnaryMeanBlock.hpp>
#include <Lodestar/blocks/std/BufferBlock.hpp>
#include <Lodestar/blocks/std/SignumBlock.hpp>
#include <Lodestar/blocks/std/SaturationBlock.hpp>
#include <Lodestar/blocks/std/DeadzoneBlock.hpp>
#include <Lodestar/blocks/std/FunctionBlock.hpp>
#include <Lodestar/blocks/std/SwitchBlock.hpp>
#include <Lodestar/blocks/std/SimplePIDBlock.hpp>
#include <Lodestar/blocks/std/DemuxBlock.hpp>
#include <Lodestar/blocks/std/MuxBlock.hpp>
#include <Lodestar/blocks/std/DelayBlock.hpp>

#include <Lodestar/blocks/BlockUtilities.hpp>
#include <Lodestar/blocks/aux/Executor.hpp>

#include <iostream>

#include "ThermographerBlock.hpp"
#include "PandaArmBlock.hpp"
#include "PandaArmVelBlock.hpp"

#include <Eigen/Dense>

#include <matplot/matplot.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>

#include <chrono>

#include "FuzzyPIDExample.hpp"

using namespace ls::blocks;
using namespace ls::blocks::std;

void torqueControl()
{
    const int kJointNo = 2;
    PandaArmBlock pandaBlk;
    pandaBlk.ipAddress() = KF_CONTROL_IP;

    ::std::function<Eigen::Vector<double, 7>(PandaState)> jointStateFunc = [](
            PandaState ps) -> Eigen::Vector<double, 7> {
        return ps.robot.jointPosition;
    };

    auto jointStateBlk = FunctionBlock<>::make(jointStateFunc);

    DemuxBlock<Eigen::Vector<double, 7>> demuxBlk;

    demuxBlk.o<6>();

    SimplePIDBlock<double> pidBlk;
    pidBlk.pGain() = 100000;
    pidBlk.iGain() = 10000;
    pidBlk.dGain() = 0;
    pidBlk.samplingPeriod() = 1e-3;

    SumBlock<double, 2> sumBlk;
    sumBlk.setOperators(decltype(sumBlk)::Plus, decltype(sumBlk)::Minus);

    ConstantBlock<double> constBlk{0.65}, zeroBlk{0};

    SaturationBlock<double> satBlk;
    satBlk.lower() = -25;
    satBlk.upper() = 25;

    MuxBlock<Eigen::Vector<double, 7>> muxBlk;

    connect(pandaBlk.o<0>(), jointStateBlk.i<0>());
    connect(jointStateBlk.o<0>(), demuxBlk.i<0>());
    connect(constBlk.o<0>(), sumBlk.i<0>());
    connect(demuxBlk.o<kJointNo>(), sumBlk.i<1>());
    connect(sumBlk.o<0>(), pidBlk.i<0>());
    connect(pidBlk.o<0>(), satBlk.i<0>());

    connect(satBlk.o<0>(), muxBlk.i<kJointNo>());
    connect(zeroBlk.o<0>(), muxBlk.i<0>());
    connect(zeroBlk.o<0>(), muxBlk.i<1>());
//    connect(zeroBlk.o<0>(), muxBlk.i<2>());
    connect(zeroBlk.o<0>(), muxBlk.i<3>());
    connect(zeroBlk.o<0>(), muxBlk.i<4>());
    connect(zeroBlk.o<0>(), muxBlk.i<5>());
    connect(zeroBlk.o<0>(), muxBlk.i<6>());

    connect(muxBlk.o<0>(), pandaBlk.i<0>());

    BlockPack bp{pandaBlk, jointStateBlk, demuxBlk, pidBlk, constBlk, sumBlk, satBlk, zeroBlk};

    aux::Executor ex{bp};
    ex.resolveExecutionOrder();

    ::std::chrono::steady_clock::time_point begin = ::std::chrono::steady_clock::now();
    ::std::chrono::steady_clock::time_point end = ::std::chrono::steady_clock::now();
    long sleepTime = 0;

    for (int i = 0; i < 10000; i++) {
//        if (i < 500) {
//            pandaBlk.i<0>().object << 5, 0, 0, 0, 0, 0, 0;
//            pandaBlk.i<0>().propagate();
//        } else {
//            pandaBlk.i<0>().object.setZero();
//            pandaBlk.i<0>().propagate();
//        }

        begin = ::std::chrono::steady_clock::now();
        ex.trigger();

        ::std::cout << "[" << i << "] dt: " << pandaBlk.o<0>().object.robot.duration.toSec() << ::std::endl;
        ::std::cout << "Error: " << sumBlk.o<0>() << ::std::endl;
        ::std::cout << "Control: " << satBlk.o<0>() << ::std::endl;
        ::std::cout << "Joints: " << pandaBlk.o<0>().object.robot.jointPosition.transpose() << ::std::endl;
        end = ::std::chrono::steady_clock::now();

        sleepTime = 500000 - ::std::chrono::duration_cast<::std::chrono::nanoseconds>(end - begin).count();
        if (sleepTime > 0) {
            ::std::this_thread::sleep_for(::std::chrono::nanoseconds(sleepTime));
        }

    }
}

void torqueControlFull()
{
    PandaArmBlock pandaBlk;
    pandaBlk.ipAddress() = KF_CONTROL_IP;

    ::std::function<Eigen::Vector<double, 7>(PandaState)> jointStateFunc = [](
            PandaState ps) -> Eigen::Vector<double, 7> {
        return ps.robot.jointPosition;
    };

    auto jointStateBlk = FunctionBlock<>::make(jointStateFunc);

    DemuxBlock<Eigen::Vector<double, 7>> demuxBlk;

    SimplePIDBlock<Eigen::Vector<double, 7>, ls::blocks::std::SimplePIDBlockOperator::Elementwise> pidBlk;
    pidBlk.pGain() << 10, 25, 10, 50, 10, 25, 10;
    pidBlk.iGain() << 0.2, 0.4, 0.4, 0.6, 0.4, 0.6, 0.4;
    pidBlk.dGain() << 0.1, -0.2, -0.1, -0.1, -0.1, -0.1, -0.1;
    pidBlk.dGain() *= 0.1;
    pidBlk.samplingPeriod() = 1e-4;

    SumBlock<Eigen::Vector<double, 7>, 2> sumBlk, sumBlk2;
    sumBlk.setOperators(decltype(sumBlk)::Plus, decltype(sumBlk)::Minus);
    sumBlk.zero().setZero();
    sumBlk2.zero().setZero();

    ConstantBlock<Eigen::Vector<double, 7>>
            constBlk{{0.2 * M_PI, 0.1 * M_PI, 0, -0.1 * M_PI, 0, M_PI / 2, -0.75 * M_PI}},
//    constBlk{{0,0,0,0,0,0,0}},
    biasBlk{{0, 0, 0, 0, 0, 0, 0}};
//    biasBlk{{-0.170114, -19.7043, -4.7674, 24.7622, -2.80476, 1.16146, 0.28083}};

    SaturationBlock<Eigen::Vector<double, 7>> satBlk;
    satBlk.lower() = -15;
    satBlk.upper() = 15;

    MuxBlock<Eigen::Vector<double, 7>> muxBlk;

    connect(pandaBlk.o<0>(), jointStateBlk.i<0>());
    connect(constBlk.o<0>(), sumBlk.i<0>());
    connect(jointStateBlk.o<0>(), sumBlk.i<1>());
    connect(sumBlk.o<0>(), pidBlk.i<0>());
//    connect(pidBlk.o<0>(), satBlk.i<0>());
    connect(biasBlk.o<0>(), sumBlk2.i<0>());
    connect(pidBlk.o<0>(), sumBlk2.i<1>());
    connect(sumBlk2.o<0>(), satBlk.i<0>());

    connect(satBlk.o<0>(), pandaBlk.i<0>());

    BlockPack bp{pandaBlk, jointStateBlk, demuxBlk, pidBlk, constBlk, sumBlk, satBlk, sumBlk2};

    aux::Executor ex{bp};
    ex.resolveExecutionOrder();

    ::std::chrono::steady_clock::time_point begin = ::std::chrono::steady_clock::now();
    ::std::chrono::steady_clock::time_point begin0 = ::std::chrono::steady_clock::now();
    ::std::chrono::steady_clock::time_point end = ::std::chrono::steady_clock::now();
    long sleepTime = 0;

    for (int i = 0; i < 100000000; i++) {
        if (i < 150) {
            pandaBlk.trigger();
            continue;
        } else if (i == 150) {
            begin0 = ::std::chrono::steady_clock::now();
        }
//        if (i < 500) {
//            pandaBlk.i<0>().object << 5, 0, 0, 0, 0, 0, 0;
//            pandaBlk.i<0>().propagate();
//        } else {
//            pandaBlk.i<0>().object.setZero();
//            pandaBlk.i<0>().propagate();
//        }

        begin = ::std::chrono::steady_clock::now();
        ex.trigger();

//        ::std::cout << "[" << i << "] dt: " << pandaBlk.o<0>().object.robot.duration.toSec() << ::std::endl;
//        ::std::cout << "Error: " << sumBlk.o<0>().object.transpose() << ::std::endl;
//        ::std::cout << "PID: " << sumPID.o<0>().object.transpose() << ::std::endl;
//        ::std::cout << "Control: " << satBlk.o<0>().object.transpose() << ::std::endl;
//        ::std::cout << "Joints: " << pandaBlk.o<0>().object.robot.jointPosition.transpose() << ::std::endl;
//        ::std::cout << "Torques: " << pandaBlk.o<0>().object.robot.jointTorque.transpose() << ::std::endl;
        if (i%1000 == 0) {
            end = ::std::chrono::steady_clock::now();

            ::std::cout << ::std::chrono::duration_cast<::std::chrono::milliseconds>(end - begin0).count() << " "
                        << sumBlk.o<0>().object[0] << " "
                        << sumBlk.o<0>().object[1] << " "
                        << sumBlk.o<0>().object[2] << " "
                        << sumBlk.o<0>().object[3] << " "
                        << sumBlk.o<0>().object[4] << " "
                        << sumBlk.o<0>().object[5] << " "
                        << sumBlk.o<0>().object[6] << ::std::endl;
        }

        end = ::std::chrono::steady_clock::now();
        sleepTime = 10000 - ::std::chrono::duration_cast<::std::chrono::nanoseconds>(end - begin).count();
        if (sleepTime > 0) {
            ::std::this_thread::sleep_for(::std::chrono::nanoseconds(sleepTime));
        }

    }
}

void torqueControlFullAntiwindup()
{
    PandaArmBlock pandaBlk;
    pandaBlk.ipAddress() = KF_CONTROL_IP;

    ::std::function<Eigen::Vector<double, 7>(PandaState)> jointStateFunc = [](
            PandaState ps) -> Eigen::Vector<double, 7> {
        return ps.robot.jointPosition;
    };

    auto jointStateBlk = FunctionBlock<>::make(jointStateFunc);

    GainBlock<Eigen::Vector<double, 7>, Eigen::Matrix<double, 7, 7>> Kp;
    GainBlock<Eigen::Vector<double, 7>, Eigen::Matrix<double, 7, 7>> Ki;
    GainBlock<Eigen::Vector<double, 7>, Eigen::Matrix<double, 7, 7>> Kd;
    GainBlock<Eigen::Vector<double, 7>, double> periodInv{1e4};
    GainBlock<Eigen::Vector<double, 7>, double> period{1e-4};

    DelayBlock<Eigen::Vector<double, 7>, 1> delayDiff, delayInt;

    SaturationBlock<Eigen::Vector<double, 7>> satInt;

    satInt.lower() = -150;
    satInt.upper() = 150;

    Kp.gain().diagonal() << 10, 25, 10, 50, 10, 25, 10;
    Ki.gain().diagonal() << 0.2, 0.4, 0.4, 0.6, 0.4, 0.6, 0.4;
    Kd.gain().diagonal() << 0.1, -0.2, -0.1, -0.1, -0.1, -0.1, -0.1;
    Kd.gain() *= 0.1;

    SumBlock<Eigen::Vector<double, 7>, 2> sumBlk, sumBlk2, sumDiff, sumInt;
    sumBlk.setOperators(decltype(sumBlk)::Plus, decltype(sumBlk)::Minus);
    sumDiff.setOperators(decltype(sumDiff)::Plus, decltype(sumDiff)::Minus);
    sumBlk.zero().setZero();
    sumBlk2.zero().setZero();
    sumDiff.zero().setZero();
    sumInt.zero().setZero();

    SumBlock<Eigen::Vector<double, 7>, 3> sumPID;
    sumPID.zero().setZero();

    ConstantBlock<Eigen::Vector<double, 7>>
            constBlk{{0.2 * M_PI, 0.1 * M_PI, 0, -0.1 * M_PI, 0, M_PI / 2, -0.75 * M_PI}},
//    constBlk{{0,0,0,0,0,0,0}},
    biasBlk{{0, 0, 0, 0, 0, 0, 0}};
//    biasBlk{{-0.170114, -19.7043, -4.7674, 24.7622, -2.80476, 1.16146, 0.28083}};

    SaturationBlock<Eigen::Vector<double, 7>> satBlk;
    satBlk.lower() = -15;
    satBlk.upper() = 15;

    // Proportional
    connect(sumBlk.o<0>(), Kp.i<0>());
    connect(Kp.o<0>(), sumPID.i<0>());

    // Integral
    connect(sumInt.o<0>(), satInt.i<0>());
    connect(satInt.o<0>(), delayInt.i<0>());
    connect(delayInt.o<0>(), sumInt.i<0>());
    connect(sumBlk.o<0>(), period.i<0>());
    connect(period.o<0>(), sumInt.i<1>());
    connect(satInt.o<0>(), Ki.i<0>());
    connect(Ki.o<0>(), sumPID.i<1>());

    // Differential
    connect(sumBlk.o<0>(), sumDiff.i<0>());
    connect(sumBlk.o<0>(), delayDiff.i<0>());
    connect(delayDiff.o<0>(), sumDiff.i<1>());
    connect(sumDiff.o<0>(), periodInv.i<0>());
    connect(periodInv.o<0>(), Kd.i<0>());
    connect(Kd.o<0>(), sumPID.i<2>());

    connect(sumPID.o<0>(), satBlk.i<0>());

    connect(pandaBlk.o<0>(), jointStateBlk.i<0>());
    connect(constBlk.o<0>(), sumBlk.i<0>());
    connect(jointStateBlk.o<0>(), sumBlk.i<1>());

    connect(satBlk.o<0>(), pandaBlk.i<0>());

    BlockPack bp{pandaBlk, jointStateBlk, constBlk, sumBlk, satBlk, sumBlk2, sumDiff, sumInt, sumPID, satInt, Kp, Ki,
                 Kd, period, periodInv, delayInt, delayDiff};

    aux::Executor ex{bp};
    ex.resolveExecutionOrder();

    ::std::chrono::steady_clock::time_point begin = ::std::chrono::steady_clock::now();
    ::std::chrono::steady_clock::time_point begin0 = ::std::chrono::steady_clock::now();
    ::std::chrono::steady_clock::time_point end = ::std::chrono::steady_clock::now();
    long sleepTime = 0;

    for (int i = 0; i < 100000000; i++) {
        if (i < 150) {
            pandaBlk.trigger();
            continue;
        } else if (i == 150) {
            begin0 = ::std::chrono::steady_clock::now();
        }
//        if (i < 500) {
//            pandaBlk.i<0>().object << 5, 0, 0, 0, 0, 0, 0;
//            pandaBlk.i<0>().propagate();
//        } else {
//            pandaBlk.i<0>().object.setZero();
//            pandaBlk.i<0>().propagate();
//        }

        begin = ::std::chrono::steady_clock::now();
        ex.trigger();

//        ::std::cout << "[" << i << "] dt: " << pandaBlk.o<0>().object.robot.duration.toSec() << ::std::endl;
//        ::std::cout << "Error: " << sumBlk.o<0>().object.transpose() << ::std::endl;
//        ::std::cout << "PID: " << sumPID.o<0>().object.transpose() << ::std::endl;
//        ::std::cout << "Control: " << satBlk.o<0>().object.transpose() << ::std::endl;
//        ::std::cout << "Joints: " << pandaBlk.o<0>().object.robot.jointPosition.transpose() << ::std::endl;
//        ::std::cout << "Torques: " << pandaBlk.o<0>().object.robot.jointTorque.transpose() << ::std::endl;
//        if (i%100 == 0) {
//            end = ::std::chrono::steady_clock::now();
//
//            ::std::cout << ::std::chrono::duration_cast<::std::chrono::milliseconds>(end - begin0).count() << " "
//                        << sumBlk.o<0>().object[0] << " "
//                        << sumBlk.o<0>().object[1] << " "
//                        << sumBlk.o<0>().object[2] << " "
//                        << sumBlk.o<0>().object[3] << " "
//                        << sumBlk.o<0>().object[4] << " "
//                        << sumBlk.o<0>().object[5] << " "
//                        << sumBlk.o<0>().object[6] << ::std::endl;
//        }

        end = ::std::chrono::steady_clock::now();
        sleepTime = 1000 - ::std::chrono::duration_cast<::std::chrono::nanoseconds>(end - begin).count();
        if (sleepTime > 0) {
            ::std::this_thread::sleep_for(::std::chrono::nanoseconds(sleepTime));
        }

    }
}

void fuzzy()
{
    PandaArmBlock pandaBlk;
    pandaBlk.ipAddress() = KF_CONTROL_IP;

    ::std::function<Eigen::Vector<double, 7>(PandaState)> jointStateFunc = [](
            PandaState ps) -> Eigen::Vector<double, 7> {
        return ps.robot.jointPosition;
    };

    auto jointStateBlk = FunctionBlock<>::make(jointStateFunc);

    DemuxBlock<Eigen::Vector<double, 7>> demuxBlk;

    using TScalar = double;
    using TType = Eigen::Vector<TScalar, 7>;
    using TMatrix = Eigen::Matrix<TScalar, 7, 7>;

    TScalar T = 1e-4;

    GainBlock<TType, TMatrix> gainK, gainKp, gainKd, gainKi, gainKuPD, gainKuI;
    GainBlock<TType, TScalar> gainTinv1{1.f/T}, gainTinv2{1.f/T};
    DelayBlock<TType, 1> delay1, delay2, delay3, delay4, delay5;
    delay1.clear(TType::Zero());
    delay2.clear(TType::Zero());
    delay3.clear(TType::Zero());
    delay4.clear(TType::Zero());
    delay5.clear(TType::Zero());
    SumBlock<TType, 2> sum1, sum2, sum3, sum4, sum5, sum6, sum7, fuzzyPDSum;

    sum1.setOperators(decltype(sum1)::Plus, decltype(sum1)::Minus);
    sum2.setOperators(decltype(sum2)::Plus, decltype(sum2)::Minus);
    sum5.setOperators(decltype(sum5)::Plus, decltype(sum5)::Minus);

    auto &sp = sum1.i<0>();
    auto &e = sum1.o<0>();
    auto &d = gainTinv2.o<0>();
    auto &r = gainTinv1.o<0>();
    auto &duPD = fuzzyPDSum.o<0>();
    auto &duI = sum4.o<0>();
    auto &uPD = sum5.o<0>();
    auto &uI = sum6.o<0>();
    auto &uPID = sum7.o<0>();

    connect(e, delay1.i<0>());
    connect(delay1.o<0>(), gainKi.i<0>());
    connect(gainKi.o<0>(), sum4.i<0>());

    connect(e, delay2.i<0>());
    connect(delay2.o<0>(), sum3.i<0>());
    connect(e, sum3.i<1>());
    connect(sum3.o<0>(), gainTinv2.i<0>());
    connect(d, gainKp.i<0>());

    connect(e, sum2.i<0>());
    connect(e, delay3.i<0>());
    connect(delay3.o<0>(), sum2.i<1>());
    connect(sum2.o<0>(), gainTinv1.i<0>());

    connect(r, gainK.i<0>());
    connect(r, gainKd.i<0>());
    connect(gainK.o<0>(), sum4.i<1>());

    connect(gainKp.o<0>(), fuzzyPDSum.i<0>());
    connect(gainKd.o<0>(), fuzzyPDSum.i<1>());
    connect(duPD, gainKuPD.i<0>());
    connect(gainKuPD.o<0>(), sum5.i<0>());
    connect(uPD, delay4.i<0>());
    connect(delay4.o<0>(), sum5.i<1>());

    connect(sum4.o<0>(), gainKuI.i<0>());
    connect(gainKuI.o<0>(), sum6.i<0>());
    connect(uI, delay5.i<0>());
    connect(delay5.o<0>(), sum6.i<1>());

    connect(uPD, sum7.i<0>());
    connect(uI, sum7.i<1>());

    gainKp.gain().diagonal() << 10, 25, 10, 50, 10, 25, 10;
    gainKi.gain().diagonal() << 0.2, 0.4, 0.4, 0.6, 0.4, 0.6, 0.4;
    gainKd.gain().diagonal() << 0.1, -0.2, -0.1, -0.1, -0.1, -0.1, -0.1;
    gainKd.gain() *= 0.1;

//    gainK.gain().diagonal() << 1, 1, 1, 1, 1, 1, 1;
//    gainKuPD.gain().diagonal() << 1, 1, 1, 1, 1, 1, 1;
//    gainKuI.gain().diagonal() << 1, 1, 1, 1, 1, 1, 1;

    gainK.gain().setZero();
    gainKuPD.gain().setZero();
    gainKuI.gain().setZero();

    SumBlock<TType, 2> sumBlk;
    sumBlk.setOperators(decltype(sumBlk)::Plus, decltype(sumBlk)::Minus);
    sumBlk.zero().setZero();
    sum1.zero().setZero();
    sum2.zero().setZero();
    sum3.zero().setZero();
    sum4.zero().setZero();
    sum5.zero().setZero();
    sum6.zero().setZero();
    sum7.zero().setZero();
    fuzzyPDSum.zero().setZero();

    SimplePIDBlock<Eigen::Vector<double, 7>, ls::blocks::std::SimplePIDBlockOperator::Elementwise> pidBlk;
    pidBlk.pGain() << 10, 25, 10, 50, 10, 25, 10;
    pidBlk.iGain() << 0.2, 0.4, 0.4, 0.6, 0.4, 0.6, 0.4;
    pidBlk.dGain() << 0.1, -0.2, -0.1, -0.1, -0.1, -0.1, -0.1;
    pidBlk.dGain() *= 0.1;
    pidBlk.samplingPeriod() = 1e-4;

    SumBlock<Eigen::Vector<double, 7>, 2> sumBlk2;
    sumBlk.setOperators(decltype(sumBlk)::Plus, decltype(sumBlk)::Minus);
    sumBlk.zero().setZero();
    sumBlk2.zero().setZero();

    ConstantBlock<Eigen::Vector<double, 7>>
            constBlk{{0.2 * M_PI, 0.1 * M_PI, 0, -0.1 * M_PI, 0, M_PI / 2, -0.75 * M_PI}},
//    constBlk{{0,0,0,0,0,0,0}},
    biasBlk{{0, 0, 0, 0, 0, 0, 0}};
//    biasBlk{{-0.170114, -19.7043, -4.7674, 24.7622, -2.80476, 1.16146, 0.28083}};

    SaturationBlock<Eigen::Vector<double, 7>> satBlk;
    satBlk.lower() = -15;
    satBlk.upper() = 15;

    MuxBlock<Eigen::Vector<double, 7>> muxBlk;

    connect(pandaBlk.o<0>(), jointStateBlk.i<0>());
    connect(constBlk.o<0>(), sum1.i<0>());
    connect(jointStateBlk.o<0>(), sum1.i<1>());
//    connect(pidBlk.o<0>(), satBlk.i<0>());
    connect(biasBlk.o<0>(), sumBlk2.i<0>());
    connect(uPID, sumBlk2.i<1>());
    connect(sumBlk2.o<0>(), satBlk.i<0>());

    connect(satBlk.o<0>(), pandaBlk.i<0>());

    BlockPack bp{pandaBlk, jointStateBlk, demuxBlk, pidBlk, constBlk, satBlk, sumBlk2,
                 gainK, gainKp, gainKd, gainKi, gainKuPD, gainKuI, gainTinv1, gainTinv2, delay1, delay2, delay3, delay4, delay5, sum1, sum2, sum3, sum4, sum5, sum6, sum7, fuzzyPDSum};
    bp.makeGraph();
    aux::Executor ex{bp};
    ex.resolveExecutionOrder();

    ::std::chrono::steady_clock::time_point begin = ::std::chrono::steady_clock::now();
    ::std::chrono::steady_clock::time_point begin0 = ::std::chrono::steady_clock::now();
    ::std::chrono::steady_clock::time_point end = ::std::chrono::steady_clock::now();
    long sleepTime = 0;

    for (int i = 0; i < 100000000; i++) {
        if (i < 150) {
            pandaBlk.trigger();
            continue;
        } else if (i == 150) {
            begin0 = ::std::chrono::steady_clock::now();
        }
//        if (i < 500) {
//            pandaBlk.i<0>().object << 5, 0, 0, 0, 0, 0, 0;
//            pandaBlk.i<0>().propagate();
//        } else {
//            pandaBlk.i<0>().object.setZero();
//            pandaBlk.i<0>().propagate();
//        }

//        while (!pandaBlk.done) {}
        begin = ::std::chrono::steady_clock::now();
        ex.trigger();

//        ::std::cout << "[" << i << "] dt: " << pandaBlk.o<0>().object.robot.duration.toSec() << ::std::endl;
//        ::std::cout << "Error: " << sumBlk.o<0>().object.transpose() << ::std::endl;
//        ::std::cout << "PID: " << sumPID.o<0>().object.transpose() << ::std::endl;
//        ::std::cout << "Control: " << satBlk.o<0>().object.transpose() << ::std::endl;
//        ::std::cout << "Joints: " << pandaBlk.o<0>().object.robot.jointPosition.transpose() << ::std::endl;
//        ::std::cout << "Torques: " << pandaBlk.o<0>().object.robot.jointTorque.transpose() << ::std::endl;
        if (i%1000 == 0) {
            end = ::std::chrono::steady_clock::now();

            ::std::cout << ::std::chrono::duration_cast<::std::chrono::milliseconds>(end - begin0).count() << " "
                        << sum1.o<0>().object[0] << " "
                        << sum1.o<0>().object[1] << " "
                        << sum1.o<0>().object[2] << " "
                        << sum1.o<0>().object[3] << " "
                        << sum1.o<0>().object[4] << " "
                        << sum1.o<0>().object[5] << " "
                        << sum1.o<0>().object[6] << ::std::endl;
            ::std::cout << "Control "
                        << uPID.object[0] << " "
                        << uPID.object[1] << " "
                        << uPID.object[2] << " "
                        << uPID.object[3] << " "
                        << uPID.object[4] << " "
                        << uPID.object[5] << " "
                        << uPID.object[6] << ::std::endl;
        }

//        end = ::std::chrono::steady_clock::now();
//        sleepTime = 500000 - ::std::chrono::duration_cast<::std::chrono::nanoseconds>(end - begin).count();
//        if (sleepTime > 0) {
//            ::std::this_thread::sleep_for(::std::chrono::nanoseconds(sleepTime));
//        }

    }
}

void torqueControlFullFuzzy()
{
    PandaArmBlock pandaBlk;
    pandaBlk.ipAddress() = KF_CONTROL_IP;

    ::std::function<Eigen::Vector<double, 7>(PandaState)> jointStateFunc = [](
            PandaState ps) -> Eigen::Vector<double, 7> {
        return ps.robot.jointPosition;
    };

    auto jointStateBlk = FunctionBlock<>::make(jointStateFunc);

    using TScalar = double;
    using TType = Eigen::Vector<TScalar, 7>;
    using TMatrix = Eigen::Matrix<TScalar, 7, 7>;

    TScalar T = 1e-4;

    GainBlock<TType, TMatrix> gainK, gainKp, gainKd, gainKi, gainKuPD, gainKuI;
    GainBlock<TType, TScalar> gainTinv1{1.f/T}, gainTinv2{1.f/T};
    DelayBlock<TType> delay1, delay2, delay3, delay4, delay5;
    SumBlock<TType, 2> sum1, sum2, sum3, sum4, sum5, sum6, sum7, fuzzyPDSum;

    sum1.setOperators(decltype(sum1)::Plus, decltype(sum1)::Minus);
    sum2.setOperators(decltype(sum2)::Plus, decltype(sum2)::Minus);
    sum5.setOperators(decltype(sum5)::Plus, decltype(sum5)::Minus);

    auto &sp = sum1.i<0>();
    auto &e = sum1.o<0>();
    auto &d = gainTinv2.o<0>();
    auto &r = gainTinv1.o<0>();
    auto &duPD = fuzzyPDSum.o<0>();
    auto &duI = sum4.o<0>();
    auto &uPD = sum5.o<0>();
    auto &uI = sum6.o<0>();
    auto &uPID = sum7.o<0>();

    connect(e, delay1.i<0>());
    connect(delay1.o<0>(), gainKi.i<0>());
    connect(gainKi.o<0>(), sum4.i<0>());

    connect(e, delay2.i<0>());
    connect(delay2.o<0>(), sum3.i<0>());
    connect(e, sum3.i<1>());
    connect(sum3.o<0>(), gainTinv2.i<0>());
    connect(d, gainKp.i<0>());

    connect(e, sum2.i<0>());
    connect(e, delay3.i<0>());
    connect(delay3.o<0>(), sum2.i<1>());
    connect(sum2.o<0>(), gainTinv1.i<0>());

    connect(r, gainK.i<0>());
    connect(r, gainKd.i<0>());
    connect(gainK.o<0>(), sum4.i<1>());

    connect(gainKp.o<0>(), fuzzyPDSum.i<0>());
    connect(gainKd.o<0>(), fuzzyPDSum.i<1>());
    connect(duPD, gainKuPD.i<0>());
    connect(gainKuPD.o<0>(), sum5.i<0>());
    connect(uPD, delay4.i<0>());
    connect(delay4.o<0>(), sum5.i<1>());

    connect(sum4.o<0>(), gainKuI.i<0>());
    connect(gainKuI.o<0>(), sum6.i<0>());
    connect(uI, delay5.i<0>());
    connect(delay5.o<0>(), sum6.i<1>());

    connect(uPD, sum7.i<0>());
    connect(uI, sum7.i<1>());

    gainKp.gain().diagonal() << 10, 25, 10, 50, 10, 25, 10;
    gainKi.gain().diagonal() << 0.2, 0.4, 0.4, 0.6, 0.4, 0.6, 0.4;
    gainKd.gain().diagonal() << 0.1, -0.2, -0.1, -0.1, -0.1, -0.1, -0.1;
    gainKd.gain() *= 0.1;

    gainK.gain().diagonal() << 1, 1, 1, 1, 1, 1, 1;
    gainKuPD.gain().diagonal() << 1, 1, 1, 1, 1, 1, 1;
    gainKuI.gain().diagonal() << 1, 1, 1, 1, 1, 1, 1;

    SumBlock<TType, 2> sumBlk;
    sumBlk.setOperators(decltype(sumBlk)::Plus, decltype(sumBlk)::Minus);
    sumBlk.zero().setZero();
    sum1.zero().setZero();
    sum2.zero().setZero();
    sum3.zero().setZero();
    sum4.zero().setZero();
    sum5.zero().setZero();
    sum6.zero().setZero();
    sum7.zero().setZero();
    fuzzyPDSum.zero().setZero();

    ConstantBlock<TType>
            constBlk{{0.2 * M_PI, 0.1 * M_PI, 0, -0.1 * M_PI, 0, M_PI / 2, -0.75 * M_PI}},
//    constBlk{{0,0,0,0,0,0,0}},
    biasBlk{{0, 0, 0, 0, 0, 0, 0}};
//    biasBlk{{-0.170114, -19.7043, -4.7674, 24.7622, -2.80476, 1.16146, 0.28083}};

    SaturationBlock<TType> satBlk;
    satBlk.lower() = -15;
    satBlk.upper() = 15;

    connect(sumBlk.o<0>(), sum1.i<0>());
    connect(sum7.o<0>(), satBlk.i<0>());

    connect(pandaBlk.o<0>(), jointStateBlk.i<0>());
    connect(constBlk.o<0>(), sumBlk.i<0>());
    connect(jointStateBlk.o<0>(), sumBlk.i<1>());

    connect(satBlk.o<0>(), pandaBlk.i<0>());

    BlockPack bp{pandaBlk, delay1, delay2, delay5, sum1, sum2, sum3, sum4, sum5, biasBlk, sum6, sum7, fuzzyPDSum, gainKp,
                 constBlk, sumBlk, delay4, satBlk, delay3, jointStateBlk, gainK, gainKi, gainKd, gainKuPD, gainKuI, gainTinv1, gainTinv2};

    aux::Executor ex{bp};
    ex.executionOrder = bp.blocks;
    ex.resolveExecutionOrder();

    ::std::chrono::steady_clock::time_point begin = ::std::chrono::steady_clock::now();
    ::std::chrono::steady_clock::time_point begin0 = ::std::chrono::steady_clock::now();
    ::std::chrono::steady_clock::time_point end = ::std::chrono::steady_clock::now();
    long sleepTime = 0;

    for (int i = 0; i < 100000000; i++) {
        if (i < 150) {
            pandaBlk.trigger();
            continue;
        } else if (i == 150) {
            begin0 = ::std::chrono::steady_clock::now();
        }
//        if (i < 500) {
//            pandaBlk.i<0>().object << 5, 0, 0, 0, 0, 0, 0;
//            pandaBlk.i<0>().propagate();
//        } else {
//            pandaBlk.i<0>().object.setZero();
//            pandaBlk.i<0>().propagate();
//        }

        begin = ::std::chrono::steady_clock::now();
        ex.trigger();

//        ::std::cout << "[" << i << "] dt: " << pandaBlk.o<0>().object.robot.duration.toSec() << ::std::endl;
//        ::std::cout << "Error: " << sumBlk.o<0>().object.transpose() << ::std::endl;
//        ::std::cout << "PID: " << sumPID.o<0>().object.transpose() << ::std::endl;
//        ::std::cout << "Control: " << satBlk.o<0>().object.transpose() << ::std::endl;
//        ::std::cout << "Joints: " << pandaBlk.o<0>().object.robot.jointPosition.transpose() << ::std::endl;
//        ::std::cout << "Torques: " << pandaBlk.o<0>().object.robot.jointTorque.transpose() << ::std::endl;
        if (i%100 == 0) {
            end = ::std::chrono::steady_clock::now();

            ::std::cout << ::std::chrono::duration_cast<::std::chrono::milliseconds>(end - begin0).count() << " "
                        << sumBlk.o<0>().object[0] << " "
                        << sumBlk.o<0>().object[1] << " "
                        << sumBlk.o<0>().object[2] << " "
                        << sumBlk.o<0>().object[3] << " "
                        << sumBlk.o<0>().object[4] << " "
                        << sumBlk.o<0>().object[5] << " "
                        << sumBlk.o<0>().object[6] << ::std::endl;
        }

        end = ::std::chrono::steady_clock::now();
        sleepTime = 10000 - ::std::chrono::duration_cast<::std::chrono::nanoseconds>(end - begin).count();
        if (sleepTime > 0) {
            ::std::this_thread::sleep_for(::std::chrono::nanoseconds(sleepTime));
        }

    }
}

//void velControlFull()
//{
//    PandaArmVelBlock pandaBlk;
//    pandaBlk.ipAddress() = KF_CONTROL_IP;
//
//    ::std::function<Eigen::Vector<double, 6>(PandaState)> eePosFunc = [] (PandaState ps) -> Eigen::Vector<double, 6> {
//        Eigen::Vector<double, 6> v = Eigen::Vector<double, 6>::Zero();
//        v.block<3,1>(0,0) = ps.robot.endEffectorPose_Base.block<3,3>(0,0) * ps.robot.endEffectorPose_Base.block<3,1>(0,3);
//        return v;
//    };
//
//    auto eePosBlk = FunctionBlock<>::make(eePosFunc);
//
////    ::std::function<Eigen::Vector<double, 6>(Eigen::Vector<double, 3>, Eigen::Vector<double, 3>)> stackVectorsFunc = [] (Eigen::Vector<double, 3> a, Eigen::Vector<double, 3> b) -> Eigen::Vector<double, 6> {
////        Eigen::Vector<double, 6> v;
////        v << a, b;
////
////        return v;
////    };
////
////    auto stackVectorsBlk = FunctionBlock<>::make(stackVectorsFunc);
//
//    DemuxBlock<Eigen::Vector<double, 6>> demuxBlk;
//
//    SimplePIDBlock<Eigen::Vector<double, 6>, ls::blocks::std::SimplePIDBlockOperator::Elementwise> pidBlk;
//    pidBlk.pGain() << 0, 0, 0, 0, 0, 0;
//    pidBlk.iGain() << 1, 1, 1, 0, 0, 0;
//    pidBlk.dGain() << 0, 0, 0, 0, 0, 0;
//    pidBlk.iGain() *= 0.1;
//    pidBlk.samplingPeriod() = 1e-3;
//
//    SumBlock<Eigen::Vector<double, 6>, 2> sumBlk, sumBlk2;
//    sumBlk.setOperators(decltype(sumBlk)::Plus, decltype(sumBlk)::Minus);
//    sumBlk.zero().setZero();
//    sumBlk2.zero().setZero();
//
//    ConstantBlock<Eigen::Vector<double, 6>>
//            constBlk{{0.1, 0, 0, 0, 0, 0}},
////    constBlk{{0,0,0,0,0,0,0}},
//    biasBlk{{0,0,0,0,0,0}};
////    biasBlk{{-0.170114, -19.7043, -4.7674, 24.7622, -2.80476, 1.16146, 0.28083}};
//
//    ConstantBlock<Eigen::Vector<double, 3>> zeroBlk{{0,0,0}};
//
//    SaturationBlock<Eigen::Vector<double, 6>> satBlk;
//    satBlk.lower() = -0.05;
//    satBlk.upper() = 0.05;
//
//    MuxBlock<Eigen::Vector<double, 6>> muxBlk;
//
//    connect(pandaBlk.o<0>(), eePosBlk.i<0>());
//    connect(constBlk.o<0>(), sumBlk.i<0>());
////    connect(eePosBlk.o<0>(), stackVectorsBlk.i<0>());
////    connect(zeroBlk.o<0>(), stackVectorsBlk.i<1>());
//    connect(eePosBlk.o<0>(), sumBlk.i<1>());
//    connect(sumBlk.o<0>(), pidBlk.i<0>());
////    connect(pidBlk.o<0>(), satBlk.i<0>());
//    connect(biasBlk.o<0>(), sumBlk2.i<0>());
//    connect(pidBlk.o<0>(), sumBlk2.i<1>());
//    connect(sumBlk2.o<0>(), satBlk.i<0>());
//
//    connect(satBlk.o<0>(), pandaBlk.i<0>());
//
//    BlockPack bp{pandaBlk, eePosBlk, demuxBlk, pidBlk, constBlk, sumBlk, satBlk, sumBlk2, zeroBlk};
//
//    aux::Executor ex{bp};
//    ex.resolveExecutionOrder();
//
//    ::std::chrono::steady_clock::time_point begin = ::std::chrono::steady_clock::now();
//    ::std::chrono::steady_clock::time_point end = ::std::chrono::steady_clock::now();
//    long sleepTime = 0;
//
//    static bool init  = false;
//
//
//    for (int i=0; i<1000000; i++) {
//        if (i < 500) {
//            pandaBlk.trigger();
//            ::std::this_thread::sleep_for(::std::chrono::nanoseconds(100000));
//            continue;
//        }
//
////        if (i < 500) {
////            pandaBlk.i<0>().object << 5, 0, 0, 0, 0, 0, 0;
////            pandaBlk.i<0>().propagate();
////        } else {
////            pandaBlk.i<0>().object.setZero();
////            pandaBlk.i<0>().propagate();
////        }
//
//        if (!init) {
//            eePosBlk.trigger();
//            Eigen::Vector3d pos0 = eePosBlk.o<0>().object.block<3,1>(0,0);
//            auto ps = pandaBlk.o<0>().object;
//            constBlk.constant().block<3,1>(0,0) << pos0 + ps.robot.endEffectorPose_Base.block<3,3>(0,0) * Eigen::Vector3d{0.2,0,0};
////            constBlk.o<0>().object = constBlk.constant();
////            constBlk.o<0>().propagate();
//            constBlk.trigger();
//            init = true;
//        }
//
////        begin = ::std::chrono::steady_clock::now();
//        ex.trigger();
//
////        ::std::cout << "[" << i << "] dt: " << pandaBlk.o<0>().object.robot.duration.toSec() << ::std::endl;
//        ::std::cout << "State: " << eePosBlk.o<0>().object.transpose() << ::std::endl;
//        ::std::cout << "Error: " << sumBlk.o<0>().object.transpose() << ::std::endl;
//        ::std::cout << "Set point: " << constBlk.o<0>().object.transpose() << ::std::endl;
//        ::std::cout << "Control: " << satBlk.o<0>().object.transpose() << ::std::endl;
////        ::std::cout << "Joints: " << pandaBlk.o<0>().object.robot.jointPosition.transpose() << ::std::endl;
////        ::std::cout << "Torques: " << pandaBlk.o<0>().object.robot.jointTorque.transpose() << ::std::endl;
////        end = ::std::chrono::steady_clock::now();
////
////        sleepTime = 1000 - ::std::chrono::duration_cast<::std::chrono::nanoseconds>(end - begin).count();
////        if (sleepTime > 0) {
////            ::std::this_thread::sleep_for(::std::chrono::nanoseconds(sleepTime));
////        }
//
//    }
//}

void velControlFull2()
{
    PandaArmVelBlock pandaBlk;
    pandaBlk.ipAddress() = KF_CONTROL_IP;

    ::std::function<Eigen::Vector<double, 3>(PandaState)> eePosFunc = [](PandaState ps) -> Eigen::Vector<double, 3> {
//        return ps.robot.endEffectorPose_Base.block<3,3>(0,0) * ps.robot.endEffectorPose_Base.block<3,1>(0,3);
        return ps.robot.endEffectorPose_Base.block<3, 1>(0, 3);
    };

    auto eePosBlk = FunctionBlock<>::make(eePosFunc);

//    ::std::function<Eigen::Vector<double, 6>(Eigen::Vector<double, 3>, Eigen::Vector<double, 3>)> stackVectorsFunc = [] (Eigen::Vector<double, 3> a, Eigen::Vector<double, 3> b) -> Eigen::Vector<double, 6> {
//        Eigen::Vector<double, 6> v;
//        v << a, b;
//
//        return v;
//    };
//
//    auto stackVectorsBlk = FunctionBlock<>::make(stackVectorsFunc);

    SimplePIDBlock<Eigen::Vector<double, 3>, ls::blocks::std::SimplePIDBlockOperator::Elementwise> pidBlk;
    pidBlk.pGain() << 0, 0, 0;
    pidBlk.iGain() << 1, 1, 1;
    pidBlk.dGain() << -0.001, -0.001, -0.001;
    pidBlk.iGain() *= 0.01;
    pidBlk.samplingPeriod() = 1e-5;

    SumBlock<Eigen::Vector<double, 3>, 3> sumBlk;
    sumBlk.setOperators(decltype(sumBlk)::Plus, decltype(sumBlk)::Plus, decltype(sumBlk)::Minus);
    sumBlk.zero().setZero();

    ConstantBlock<Eigen::Vector<double, 3>>
            constBlk{{0.1, 0, 0}},
//    constBlk{{0,0,0,0,0,0,0}},
    biasBlk{{0, 0, 0}};
//    biasBlk{{-0.170114, -19.7043, -4.7674, 24.7622, -2.80476, 1.16146, 0.28083}};

    ConstantBlock<Eigen::Vector<double, 3>> setpointBlk{{0.1, 0, 0}};

    SaturationBlock<Eigen::Vector<double, 3>> satBlk;
    satBlk.lower() = -0.05;
    satBlk.upper() = 0.05;

    connect(pandaBlk.o<0>(), eePosBlk.i<0>());
    connect(constBlk.o<0>(), sumBlk.i<0>());
    connect(setpointBlk.o<0>(), sumBlk.i<1>());
    connect(eePosBlk.o<0>(), sumBlk.i<2>());
    connect(sumBlk.o<0>(), pidBlk.i<0>());
//    connect(pidBlk.o<0>(), satBlk.i<0>());
//    connect(biasBlk.o<0>(), sumBlk2.i<0>());
//    connect(pidBlk.o<0>(), sumBlk2.i<1>());
    connect(pidBlk.o<0>(), satBlk.i<0>());
    connect(satBlk.o<0>(), pandaBlk.i<0>());

    BlockPack bp{pandaBlk, eePosBlk, pidBlk, constBlk, sumBlk, satBlk, setpointBlk};

    aux::Executor ex{bp};
    ex.resolveExecutionOrder();

    ::std::chrono::steady_clock::time_point begin = ::std::chrono::steady_clock::now();
    ::std::chrono::steady_clock::time_point end = ::std::chrono::steady_clock::now();
    long sleepTime = 0;

    static bool init = false;

    for (int i = 0; i < 100000000; i++) {
        if (i < 500) {
            pandaBlk.trigger();
            ::std::this_thread::sleep_for(::std::chrono::nanoseconds(100000));
            continue;
        }

//        if (i < 500) {
//            pandaBlk.i<0>().object << 5, 0, 0, 0, 0, 0, 0;
//            pandaBlk.i<0>().propagate();
//        } else {
//            pandaBlk.i<0>().object.setZero();
//            pandaBlk.i<0>().propagate();
//        }

        if (!init) {
            eePosBlk.trigger();
            constBlk.constant() = eePosBlk.o<0>().object;;
//            constBlk.o<0>().object = constBlk.constant();
//            constBlk.o<0>().propagate();
            constBlk.trigger();
            init = true;
        }

        begin = ::std::chrono::steady_clock::now();
        ex.trigger();
        end = ::std::chrono::steady_clock::now();

        ::std::cout << ::std::chrono::duration_cast<::std::chrono::milliseconds>(end - begin).count() << " " << sumBlk.o<0>().object.transpose() << ::std::endl;

//        ::std::cout << "[" << i << "] dt: " << pandaBlk.o<0>().object.robot.duration.toSec() << ::std::endl;
//        ::std::cout << "State: " << eePosBlk.o<0>().object.transpose() << ::std::endl;
//        ::std::cout << "Error: " << sumBlk.o<0>().object.transpose() << ::std::endl;
//        ::std::cout << "Set point: " << constBlk.o<0>().object.transpose() << ::std::endl;
//        ::std::cout << "Control: " << satBlk.o<0>().object.transpose() << ::std::endl;
//        ::std::cout << "Joints: " << pandaBlk.o<0>().object.robot.jointPosition.transpose() << ::std::endl;
//        ::std::cout << "Torques: " << pandaBlk.o<0>().object.robot.jointTorque.transpose() << ::std::endl;
//        end = ::std::chrono::steady_clock::now();
//        ::std::cout << ::std::chrono::duration_cast<::std::chrono::microseconds>(end - begin).count() << ::std::endl;
//
//        sleepTime = 1000 - ::std::chrono::duration_cast<::std::chrono::nanoseconds>(end - begin).count();
//        if (sleepTime > 0) {
//            ::std::this_thread::sleep_for(::std::chrono::nanoseconds(sleepTime));
//        }

    }
}

void autofocus()
{
    const unsigned int NBuffer = 100;

    ThermographerBlock<float> blkT;
//    left 21074216.xml
//    right 20114198.xml
    blkT.serial() = "/home/advcc/20104321.xml";
//    blkT.serial() = "/home/advcc/21074216.xml";
    blkT.i<0>() = 0;

    GainBlock<decltype(blkT)::Matrix, Eigen::Matrix<float, 5, 7>, GainBlockOperator::Convolution> blkConvX, blkConvX2;
    GainBlock<decltype(blkT)::Matrix, Eigen::Matrix<float, 7, 5>, GainBlockOperator::Convolution> blkConvY, blkConvY2;

    SumBlock<decltype(blkT)::Matrix, 2> blkSum;
    blkSum.zero().setZero();

    SumBlock<float, 2> blkSumSharpness, blkSumPID;
    blkSumSharpness.zero() = 0;
    blkSumPID.zero() = 0;
    blkSumSharpness.setOperators(decltype(blkSumSharpness)::Plus, decltype(blkSumSharpness)::Minus);

    UnaryAbsBlock<decltype(blkT)::Matrix> blkAbs;

    MinMaxIdxBlock<decltype(blkT)::Matrix, ls::blocks::std::MinMaxIdxBlockOperator::Max> blkMaxIdx;

    UnaryMeanBlock<decltype(blkT)::Matrix> blkMean;

    BufferBlock<float, NBuffer> blkBuffer;
    blkBuffer.clear(0);

    SignumBlock<float, float> blkSignum;

    DeadzoneBlock<float> blkDeadzone, blkDeadzonePID;
    blkDeadzone.lower() = -3e-2;
    blkDeadzone.upper() = 3e-2;

    SaturationBlock<float> blkSaturation;
    blkSaturation.lower() = 0;
    blkSaturation.upper() = 60;

    //------------------- ANRA

//    auto nrBwdDiffLambda = [](
//            decltype(blkT)::Matrix T0,
//            decltype(blkT)::Matrix T1,
//            decltype(blkT)::Matrix T2,
//            decltype(blkT)::Matrix T3,
//            decltype(blkT)::Matrix T4,
//            decltype(blkT)::Matrix T5,
//            decltype(blkT)::Matrix T6,
//            decltype(blkT)::Matrix T7) -> decltype(blkT)::Matrix {
//        return (1.f / (64.f / 27.f)) * (T0.array() + 5 * T1.array() + 9 * T2.array() + 5 * T3.array() - 5 * T4.array() - 9 * T5.array() - 5 * T6.array() - T7.array());
//    };
//
//    FunctionBlock<decltype(blkT)::Matrix,
//            decltype(blkT)::Matrix,
//            decltype(blkT)::Matrix,
//            decltype(blkT)::Matrix,
//            decltype(blkT)::Matrix,
//            decltype(blkT)::Matrix,
//            decltype(blkT)::Matrix,
//            decltype(blkT)::Matrix,
//    decltype(blkT)::Matrix>::Function nrBwdDiffFunc = nrBwdDiffLambda;

//    FunctionBlock<decltype(blkT)::Matrix,
//            decltype(blkT)::Matrix,
//            decltype(blkT)::Matrix,
//            decltype(blkT)::Matrix,
//            decltype(blkT)::Matrix,
//            decltype(blkT)::Matrix,
//            decltype(blkT)::Matrix,
//            decltype(blkT)::Matrix,
//            decltype(blkT)::Matrix> blkNrBwdDiff;

//    auto blkNrBwdDiff = FunctionBlock<>::make(nrBwdDiffFunc);
//
//    BufferBlock<decltype(blkT)::Matrix, 8> blkTBuffer;
//    blkTBuffer.clear(decltype(blkT)::Matrix::Zero());
//
//    GainBlock<decltype(blkT)::Matrix, float> blkLaplacianScale{1.f / ((90e-6) * (90e-6))};

//    ::std::function<decltype(blkT)::Matrix(decltype(blkT)::Matrix)> rtcFunc = [](decltype(blkT)::Matrix Tt) -> decltype(blkT)::Matrix {
//        decltype(blkT)::Matrix RTC = Tt.unaryExpr([](float x) { return ::std::exp(::std::abs(x)); });
//        RTC.array() -= RTC.minCoeff();
//        RTC.array() *= (1.f / RTC.maxCoeff());
//
//        return RTC;
//    };
//
//    auto blkRTC = FunctionBlock<>::make(rtcFunc);

//    ::std::function<float(float)> focus2pxFunc = [] (float focus) -> float {
//        return 81e-6 + 18e-6 * focus / 100.f;
//    };
//
//    auto blkFocus2Px = FunctionBlock<>::make(focus2pxFunc);
//
//    ::std::function<float(float, decltype(blkT)::Matrix, decltype(blkT)::Matrix, decltype(blkT)::Matrix)> anraFunc = [](float h, decltype(blkT)::Matrix RTC, decltype(blkT)::Matrix Tt, decltype(blkT)::Matrix TxxTyy) -> float {
//        decltype(blkT)::Matrix a = h*h * Tt.array() / TxxTyy.array();
//        return (a.array() * RTC.array()).sum();
//    };
//
//    auto blkANRA = FunctionBlock<>::make(anraFunc);
//
//    connect(blkSum.o<0>(), blkANRA.i<3>());
//    connect(blkT.o<0>(), blkTBuffer.i<0>());
//    connect(blkTBuffer.o<0>(), blkNrBwdDiff.i<0>());
//    connect(blkTBuffer.o<1>(), blkNrBwdDiff.i<1>());
//    connect(blkTBuffer.o<2>(), blkNrBwdDiff.i<2>());
//    connect(blkTBuffer.o<3>(), blkNrBwdDiff.i<3>());
//    connect(blkTBuffer.o<4>(), blkNrBwdDiff.i<4>());
//    connect(blkTBuffer.o<5>(), blkNrBwdDiff.i<5>());
//    connect(blkTBuffer.o<6>(), blkNrBwdDiff.i<6>());
//    connect(blkTBuffer.o<7>(), blkNrBwdDiff.i<7>());
//    connect(blkNrBwdDiff.o<0>(), blkANRA.i<2>());
//    connect(blkNrBwdDiff.o<0>(), blkRTC.i<0>());
//    connect(blkRTC.o<0>(), blkANRA.i<1>());
//    connect(blkT.o<1>(), blkFocus2Px.i<0>());
//    connect(blkFocus2Px.o<0>(), blkANRA.i<0>());

    //------------------- ANRA

    ::std::function<float(float, float)> controlFunc = [](float sign, float focus) -> float {
//        static bool isInit = false;
//        static float f;
//        if (!isInit) {
//            f = focus;
//            isInit = true;
//        }

//        focus -= 2.5f*sign;
        focus -= 1.f * sign;

        return focus;
    };

    auto blkControlFunc = FunctionBlock<>::make(controlFunc);

    SimplePIDBlock<float, ls::blocks::std::SimplePIDBlockOperator::Scalar, ls::blocks::std::SimplePIDBlockParameter::AdditionalInput> blkPID;
    blkPID.samplingPeriod() = 5e-3;
    blkPID.pGain() = -15;
    blkPID.iGain() = 5;
    blkPID.dGain() = 0;
    blkDeadzonePID.lower() = -1e-1;
    blkDeadzonePID.upper() = 1e-1;

    SwitchBlock<float, SwitchBlockParameter::AdditionalInput> blkSwitch, blkSwitch2;

    Eigen::Matrix<float, 5, 7> kNRFx;
    Eigen::Matrix<float, 7, 5> kNRFy;

    kNRFx << -1, -4, -5, 0, 5, 4, 1,
            -4, -16, -20, 0, 20, 16, 4,
            -6, -24, -30, 0, 30, 24, 6,
            -4, -16, -20, 0, 20, 16, 4,
            -1, -4, -5, 0, 5, 4, 1;
    kNRFx *= -(1.f / 512);

    kNRFy = kNRFx.transpose();

    blkConvX.gain() = kNRFx;
    blkConvX2.gain() = kNRFx;

    blkConvY.gain() = kNRFy;
    blkConvY2.gain() = kNRFy;

    connect(blkT.o<0>(), blkConvX.i<0>());
    connect(blkT.o<0>(), blkConvY.i<0>());
    connect(blkConvX.o<0>(), blkConvX2.i<0>());
    connect(blkConvY.o<0>(), blkConvY2.i<0>());
    connect(blkConvX2.o<0>(), blkSum.i<0>());
    connect(blkConvY2.o<0>(), blkSum.i<1>());
    connect(blkSum.o<0>(), blkAbs.i<0>());
    connect(blkAbs.o<0>(), blkMaxIdx.i<0>());
    connect(blkAbs.o<0>(), blkMean.i<0>());
    connect(blkMean.o<0>(), blkSwitch.i<0>());
    connect(blkMaxIdx.o<2>(), blkSwitch.i<1>());
    connect(blkSwitch.o<0>(), blkBuffer.i<0>());
    connect(blkBuffer.o<0>(), blkSumSharpness.i<0>());
    connect(blkBuffer.o<NBuffer - 1>(), blkSumSharpness.i<1>());
    connect(blkSumSharpness.o<0>(), blkDeadzone.i<0>());
    connect(blkDeadzone.o<0>(), blkSignum.i<0>());
    connect(blkSignum.o<0>(), blkControlFunc.i<0>());
    connect(blkT.o<1>(), blkControlFunc.i<1>());
    connect(blkControlFunc.o<0>(), blkSwitch2.i<0>());
    connect(blkSumSharpness.o<0>(), blkPID.i<0>());
    connect(blkPID.o<0>(), blkDeadzonePID.i<0>());
    connect(blkDeadzonePID.o<0>(), blkSumPID.i<0>());
    connect(blkT.o<1>(), blkSumPID.i<1>());
    connect(blkSumPID.o<0>(), blkSwitch2.i<1>());
    connect(blkSwitch2.o<0>(), blkSaturation.i<0>());
    connect(blkSaturation.o<0>(), blkT.i<0>());

    BlockPack bp{blkT, blkConvX, blkConvX2, blkConvY, blkConvY2, blkSum, blkAbs, blkMaxIdx, blkBuffer, blkSumSharpness,
                 blkSignum, blkDeadzone, blkControlFunc, blkDeadzonePID, blkSaturation, blkMean, blkSwitch, blkSwitch2,
                 blkPID, blkSumPID};
//    BlockPack bp{blkT, blkConvX, blkConvX2, blkConvY, blkConvY2, blkSum, blkAbs, blkMaxIdx, blkBuffer, blkSumSharpness,
//                 blkSignum, blkDeadzone, blkControlFunc, blkDeadzonePID, blkSaturation, blkMean, blkSwitch, blkSwitch2,
//                 blkPID, blkSumPID,
//                 blkTBuffer, blkANRA, blkFocus2Px, blkNrBwdDiff, blkRTC};

    aux::Executor ex{bp};
    ex.resolveExecutionOrder();

    auto getTemperature = [&](double x, double y) -> double {
        if ((y >= 288 || y < 0) || (x >= 382 || x < 0))
            return 0;

        return blkT.o<0>().object((int) round(y), (int) round(x));
    };

    cv::Mat M(288, 382, CV_32FC1), MLaplacian(288, 382, CV_32FC1);
    Eigen::Map<Eigen::Matrix<float, 288, 382, Eigen::RowMajor>> M_Eigen(M.ptr<float>(), M.rows, M.cols);
    Eigen::Map<Eigen::Matrix<float, 288, 382, Eigen::RowMajor>> MLaplacian_Eigen(MLaplacian.ptr<float>(),
                                                                                MLaplacian.rows, MLaplacian.cols);
    M_Eigen.setZero();
    MLaplacian_Eigen.setZero();

//    blkT.o<0>() = M_Eigen;

//    M.resize(382*288);
//    M.reshape(382, 288);

//    ::std::stringstream ss;
//    ex.makeDotFile(ss, true, true);

    // Toggle between mean and max focus.
    blkSwitch.toggle();

    // Toggle between custom control function and PID.
    blkSwitch2.toggle();

    ::std::chrono::steady_clock::time_point begin, end;

    for (int i = 0; i < 10000; i++) {
        if (i < 250) {
            blkT.i<0>() = 0;
            blkT.trigger();
            continue;
        } else if (i == 250) {
            begin = ::std::chrono::steady_clock::now();
        }

//        blkT.trigger();
        ex.trigger();

        if (i > 5) {
//            matplot::gca()->xlim({0, 382});
//            matplot::gca()->ylim({0, 288});
//            matplot::fcontour(getTemperature)->filled(true).colormap_line_when_filled(true);
//
//            matplot::show();

            M_Eigen << blkT.o<0>().object;
            M_Eigen *= (1.0 / M_Eigen.maxCoeff());

            MLaplacian_Eigen << blkAbs.o<0>().object;
            MLaplacian_Eigen.unaryExpr([&](float a) -> float { return a - MLaplacian_Eigen.minCoeff(); });
            MLaplacian_Eigen *= (1.0 / MLaplacian_Eigen.maxCoeff());
//        cv::eigen2cv((blkT.o<0>().object/blkT.o<0>().object.maxCoeff()).eval(), M);
            cv::namedWindow("Temperature field", cv::WINDOW_AUTOSIZE);
            cv::imshow("Temperature field", M);
            cv::namedWindow("Laplacian field", cv::WINDOW_AUTOSIZE);
            cv::imshow("Laplacian field", MLaplacian);
            cv::waitKey(5);
        }

//        ::std::cout << "[" << i << "] sharpness change: " << blkSignum.o<0>().object << " | "
//                    << blkSumSharpness.o<0>().object << ::std::endl;
//        ::std::cout << "[" << i << "] setpoint (" << blkSwitch2.o<0>().object << "): " << blkSaturation.o<0>().object
//                    << ::std::endl;

//        ::std::cout << "[" << i << "] mean temperature: " << blkT.o<0>().object.mean() << ", max: " << blkT.o<0>().object.maxCoeff() << ::std::endl;
//        ::std::cout << "[" << i << "] mean deriv x: " << blkConvX.o<0>().object.mean() << ", max: " << blkConvX.o<0>().object.maxCoeff() << ::std::endl;
//        ::std::cout << "[" << i << "] mean deriv x 2: " << blkConvX2.o<0>().object.mean() << ", max: " << blkConvX2.o<0>().object.maxCoeff() << ::std::endl;
//        ::std::cout << "[" << i << "] mean deriv y: " << blkConvY.o<0>().object.mean() << ", max: " << blkConvY.o<0>().object.maxCoeff() << ::std::endl;
//        ::std::cout << "[" << i << "] mean deriv y 2: " << blkConvY2.o<0>().object.mean() << ", max: " << blkConvY2.o<0>().object.maxCoeff() << ::std::endl;
//        ::std::cout << "[" << i << "] mean laplacian: " << blkSum.o<0>().object.mean() << ", max: " << blkMaxIdx.o<2>().object << " at (" << blkMaxIdx.o<0>().object << ", " << blkMaxIdx.o<1>().object << ")" << ::std::endl;
//        ::std::cout << "[" << i << "] focus: " << blkT.o<1>().object << ::std::endl;
//        ::std::cout << "[" << i << "] diffusivity: " << blkANRA.o<0>().object << ::std::endl;
        end = ::std::chrono::steady_clock::now();
        ::std::cout << ::std::chrono::duration_cast<::std::chrono::milliseconds>(end - begin).count() << ", " << blkMaxIdx.o<2>().object << ", " << blkT.o<1>().object << ::std::endl;
    }
}

void autofocusFuzzy()
{
    using TScalar = float;
    using TType = float;
    using TMatrix = float;

    TScalar T = 5e-3;

    GainBlock<TType, TMatrix> gainK, gainKp, gainKd, gainKi, gainKuPD, gainKuI;
    GainBlock<TType, TScalar> gainTinv1{1.f / T}, gainTinv2{1.f / T};
    DelayBlock<TType> delay1, delay2, delay3, delay4, delay5;
    SumBlock<TType, 2> sum1, sum2, sum3, sum4, sum5, sum6, sum7, fuzzyPDSum;

    sum1.setOperators(decltype(sum1)::Plus, decltype(sum1)::Minus);
    sum2.setOperators(decltype(sum2)::Plus, decltype(sum2)::Minus);
    sum5.setOperators(decltype(sum5)::Plus, decltype(sum5)::Minus);

    connect(sum1.o<0>(), delay1.i<0>());
    connect(delay1.o<0>(), gainKi.i<0>());
    connect(gainKi.o<0>(), sum4.i<0>());

    connect(sum1.o<0>(), delay2.i<0>());
    connect(delay2.o<0>(), sum3.i<0>());
    connect(sum1.o<0>(), sum3.i<1>());
    connect(sum3.o<0>(), gainTinv2.i<0>());
    connect(gainTinv2.o<0>(), gainKp.i<0>());

    connect(sum1.o<0>(), sum2.i<0>());
    connect(sum1.o<0>(), delay3.i<0>());
    connect(delay3.o<0>(), sum2.i<1>());
    connect(sum2.o<0>(), gainTinv1.i<0>());

    connect(gainTinv1.o<0>(), gainK.i<0>());
    connect(gainTinv1.o<0>(), gainKd.i<0>());
    connect(gainK.o<0>(), sum4.i<1>());

    connect(gainKp.o<0>(), fuzzyPDSum.i<0>());
    connect(gainKd.o<0>(), fuzzyPDSum.i<1>());
    connect(fuzzyPDSum.o<0>(), gainKuPD.i<0>());
    connect(gainKuPD.o<0>(), sum5.i<0>());
    connect(sum5.o<0>(), delay4.i<0>());
    connect(delay4.o<0>(), sum5.i<1>());

    connect(sum4.o<0>(), gainKuI.i<0>());
    connect(gainKuI.o<0>(), sum6.i<0>());
    connect(sum6.o<0>(), delay5.i<0>());
    connect(delay5.o<0>(), sum6.i<1>());

    connect(sum5.o<0>(), sum7.i<0>());
    connect(sum6.o<0>(), sum7.i<1>());

    gainKp.gain() = -15;
    gainKi.gain() = 5;
    gainKd.gain() = 0;

    gainK.gain() = 1;
    gainKuPD.gain() = 1;
    gainKuI.gain() = 1;

    const unsigned int NBuffer = 100;

    ThermographerBlock<float> blkT;
//    left 21074216.xml
//    right 20114198.xml
    blkT.serial() = "/home/advcc/20104321.xml";
//    blkT.serial() = "/home/advcc/21074216.xml";
    blkT.i<0>() = 0;

    GainBlock<decltype(blkT)::Matrix, Eigen::Matrix<float, 5, 7>, GainBlockOperator::Convolution> blkConvX, blkConvX2;
    GainBlock<decltype(blkT)::Matrix, Eigen::Matrix<float, 7, 5>, GainBlockOperator::Convolution> blkConvY, blkConvY2;

    SumBlock<decltype(blkT)::Matrix, 2> blkSum;
    blkSum.zero().setZero();

    SumBlock<float, 2> blkSumSharpness, blkSumPID;
    blkSumSharpness.zero() = 0;
    blkSumPID.zero() = 0;
    blkSumSharpness.setOperators(decltype(blkSumSharpness)::Plus, decltype(blkSumSharpness)::Minus);

    UnaryAbsBlock<decltype(blkT)::Matrix> blkAbs;

    MinMaxIdxBlock<decltype(blkT)::Matrix, ls::blocks::std::MinMaxIdxBlockOperator::Max> blkMaxIdx;

    UnaryMeanBlock<decltype(blkT)::Matrix> blkMean;

    BufferBlock<float, NBuffer> blkBuffer;
    blkBuffer.clear(0);

    SignumBlock<float, float> blkSignum;

    DeadzoneBlock<float> blkDeadzone, blkDeadzonePID;
    blkDeadzone.lower() = -3e-2;
    blkDeadzone.upper() = 3e-2;

    SaturationBlock<float> blkSaturation;
    blkSaturation.lower() = 0;
    blkSaturation.upper() = 60;

    ::std::function<float(float, float)> controlFunc = [](float sign, float focus) -> float {
//        static bool isInit = false;
//        static float f;
//        if (!isInit) {
//            f = focus;
//            isInit = true;
//        }

//        focus -= 2.5f*sign;
        focus -= 1.f * sign;

        return focus;
    };

    auto blkControlFunc = FunctionBlock<>::make(controlFunc);

    blkDeadzonePID.lower() = -1e-1;
    blkDeadzonePID.upper() = 1e-1;

    SwitchBlock<float, SwitchBlockParameter::AdditionalInput> blkSwitch, blkSwitch2;

    Eigen::Matrix<float, 5, 7> kNRFx;
    Eigen::Matrix<float, 7, 5> kNRFy;

    kNRFx << -1, -4, -5, 0, 5, 4, 1,
            -4, -16, -20, 0, 20, 16, 4,
            -6, -24, -30, 0, 30, 24, 6,
            -4, -16, -20, 0, 20, 16, 4,
            -1, -4, -5, 0, 5, 4, 1;
    kNRFx *= -(1.f / 512);

    kNRFy = kNRFx.transpose();

    blkConvX.gain() = kNRFx;
    blkConvX2.gain() = kNRFx;

    blkConvY.gain() = kNRFy;
    blkConvY2.gain() = kNRFy;

    connect(blkT.o<0>(), blkConvX.i<0>());
    connect(blkT.o<0>(), blkConvY.i<0>());
    connect(blkConvX.o<0>(), blkConvX2.i<0>());
    connect(blkConvY.o<0>(), blkConvY2.i<0>());
    connect(blkConvX2.o<0>(), blkSum.i<0>());
    connect(blkConvY2.o<0>(), blkSum.i<1>());
    connect(blkSum.o<0>(), blkAbs.i<0>());
    connect(blkAbs.o<0>(), blkMaxIdx.i<0>());
    connect(blkAbs.o<0>(), blkMean.i<0>());
    connect(blkMean.o<0>(), blkSwitch.i<0>());
    connect(blkMaxIdx.o<2>(), blkSwitch.i<1>());
    connect(blkSwitch.o<0>(), blkBuffer.i<0>());
    connect(blkBuffer.o<0>(), blkSumSharpness.i<0>());
    connect(blkBuffer.o<NBuffer - 1>(), blkSumSharpness.i<1>());
    connect(blkSumSharpness.o<0>(), blkDeadzone.i<0>());
    connect(blkDeadzone.o<0>(), blkSignum.i<0>());
    connect(blkSignum.o<0>(), blkControlFunc.i<0>());
    connect(blkT.o<1>(), blkControlFunc.i<1>());
    connect(blkControlFunc.o<0>(), blkSwitch2.i<0>());
    connect(blkSumSharpness.o<0>(), sum1.i<0>());
    connect(sum7.o<0>(), blkDeadzonePID.i<0>());
    connect(blkDeadzonePID.o<0>(), blkSumPID.i<0>());
    connect(blkT.o<1>(), blkSumPID.i<1>());
    connect(blkSumPID.o<0>(), blkSwitch2.i<1>());
    connect(blkSwitch2.o<0>(), blkSaturation.i<0>());
    connect(blkSaturation.o<0>(), blkT.i<0>());

    BlockPack bp{blkT, blkConvX, blkConvX2, blkConvY, blkConvY2, blkSum, blkAbs, blkMaxIdx, blkBuffer, blkSumSharpness,
                 blkSignum, blkDeadzone, blkControlFunc, blkDeadzonePID, blkSaturation, blkMean, blkSwitch, blkSwitch2,
                 blkSumPID,
                 gainK, gainKp, gainKd, gainKi, gainKuPD, gainKuI, gainTinv1, gainTinv2, delay1,
                 delay2, delay3, delay4, delay5, sum1, sum2, sum3, sum4, sum5, sum6, sum7, fuzzyPDSum};
//    BlockPack bp{blkT, blkConvX, blkConvX2, blkConvY, blkConvY2, blkSum, blkAbs, blkMaxIdx, blkBuffer, blkSumSharpness,
//                 blkSignum, blkDeadzone, blkControlFunc, blkDeadzonePID, blkSaturation, blkMean, blkSwitch, blkSwitch2,
//                 blkPID, blkSumPID,
//                 blkTBuffer, blkANRA, blkFocus2Px, blkNrBwdDiff, blkRTC};

    aux::Executor ex{bp};
    ex.resolveExecutionOrder();

    auto getTemperature = [&](double x, double y) -> double {
        if ((y >= 288 || y < 0) || (x >= 382 || x < 0))
            return 0;

        return blkT.o<0>().object((int) round(y), (int) round(x));
    };

    cv::Mat M(288, 382, CV_32FC1), MLaplacian(288, 382, CV_32FC1);
    Eigen::Map<Eigen::Matrix<float, 288, 382, Eigen::RowMajor>> M_Eigen(M.ptr<float>(), M.rows, M.cols);
    Eigen::Map<Eigen::Matrix<float, 288, 382, Eigen::RowMajor>> MLaplacian_Eigen(MLaplacian.ptr<float>(),
                                                                                MLaplacian.rows, MLaplacian.cols);
    M_Eigen.setZero();
    MLaplacian_Eigen.setZero();

//    blkT.o<0>() = M_Eigen;

//    M.resize(382*288);
//    M.reshape(382, 288);

//    ::std::stringstream ss;
//    ex.makeDotFile(ss, true, true);

// Toggle between mean and max focus.
    blkSwitch.toggle();

// Toggle between custom control function and PID.
    blkSwitch2.toggle();

    ::std::chrono::steady_clock::time_point begin, end;

    for (int i = 0; i < 10000; i++) {
        if (i < 250) {
            blkT.i<0>() = 0;
            blkT.trigger();
            continue;
        } else if (i == 250) {
            begin = ::std::chrono::steady_clock::now();
        }

//        blkT.trigger();
        ex.trigger();

        if (i > 5) {
//            matplot::gca()->xlim({0, 382});
//            matplot::gca()->ylim({0, 288});
//            matplot::fcontour(getTemperature)->filled(true).colormap_line_when_filled(true);
//
//            matplot::show();

            M_Eigen << blkT.o<0>().object;
            M_Eigen *= (1.0 / M_Eigen.maxCoeff());

            MLaplacian_Eigen << blkAbs.o<0>().object;
            MLaplacian_Eigen.unaryExpr([&](float a) -> float { return a - MLaplacian_Eigen.minCoeff(); });
            MLaplacian_Eigen *= (1.0 / MLaplacian_Eigen.maxCoeff());
//        cv::eigen2cv((blkT.o<0>().object/blkT.o<0>().object.maxCoeff()).eval(), M);
            cv::namedWindow("Temperature field", cv::WINDOW_AUTOSIZE);
            cv::imshow("Temperature field", M);
            cv::namedWindow("Laplacian field", cv::WINDOW_AUTOSIZE);
            cv::imshow("Laplacian field", MLaplacian);
            cv::waitKey(5);
        }

//        ::std::cout << "[" << i << "] sharpness change: " << blkSignum.o<0>().object << " | "
//                    << blkSumSharpness.o<0>().object << ::std::endl;
//        ::std::cout << "[" << i << "] setpoint (" << blkSwitch2.o<0>().object << "): " << blkSaturation.o<0>().object
//                    << ::std::endl;

//        ::std::cout << "[" << i << "] mean temperature: " << blkT.o<0>().object.mean() << ", max: " << blkT.o<0>().object.maxCoeff() << ::std::endl;
//        ::std::cout << "[" << i << "] mean deriv x: " << blkConvX.o<0>().object.mean() << ", max: " << blkConvX.o<0>().object.maxCoeff() << ::std::endl;
//        ::std::cout << "[" << i << "] mean deriv x 2: " << blkConvX2.o<0>().object.mean() << ", max: " << blkConvX2.o<0>().object.maxCoeff() << ::std::endl;
//        ::std::cout << "[" << i << "] mean deriv y: " << blkConvY.o<0>().object.mean() << ", max: " << blkConvY.o<0>().object.maxCoeff() << ::std::endl;
//        ::std::cout << "[" << i << "] mean deriv y 2: " << blkConvY2.o<0>().object.mean() << ", max: " << blkConvY2.o<0>().object.maxCoeff() << ::std::endl;
//        ::std::cout << "[" << i << "] mean laplacian: " << blkSum.o<0>().object.mean() << ", max: " << blkMaxIdx.o<2>().object << " at (" << blkMaxIdx.o<0>().object << ", " << blkMaxIdx.o<1>().object << ")" << ::std::endl;
//        ::std::cout << "[" << i << "] focus: " << blkT.o<1>().object << ::std::endl;
//        ::std::cout << "[" << i << "] diffusivity: " << blkANRA.o<0>().object << ::std::endl;
        end = ::std::chrono::steady_clock::now();
        ::std::cout << ::std::chrono::duration_cast<::std::chrono::milliseconds>(end - begin).count() << ", " << blkMaxIdx.o<2>().object << ", " << blkT.o<1>().object << ::std::endl;
    }
}

int main()
{
//    autofocus();
//    autofocusFuzzy();
//    torqueControlFull();
    torqueControlFullAntiwindup();
//    fuzzy();
//    torqueControlFullFuzzy();
//    velControlFull2();

//    FuzzyPIDExample::example();
//    FuzzyPIDExample::exampleSL();

    return 0;
}


// Copyright (C) 2011 by the BEM++ Authors
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include "../check_arrays_are_close.hpp"
#include "../type_template.hpp"

#include "assembly/assembly_options.hpp"
#include "assembly/discrete_boundary_operator.hpp"
#include "assembly/boundary_operator.hpp"
#include "assembly/context.hpp"
#include "assembly/modified_helmholtz_3d_single_layer_boundary_operator.hpp"
#include "assembly/numerical_quadrature_strategy.hpp"

#include "common/boost_make_shared_fwd.hpp"

#include "grid/grid_factory.hpp"
#include "grid/grid.hpp"

#include "space/piecewise_linear_continuous_scalar_space.hpp"
#include "space/piecewise_constant_scalar_space.hpp"

#include <algorithm>
#include "common/armadillo_fwd.hpp"
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/version.hpp>
#include <complex>

namespace Bempp
{

namespace
{

template <typename T> T initWaveNumber();
template <> float initWaveNumber() { return 1.2f; }
template <> double initWaveNumber(){ return 1.2; }
template <> std::complex<float> initWaveNumber()
{ return std::complex<float>(1.2f, 0.7f); }
template <> std::complex<double> initWaveNumber()
{ return std::complex<double>(1.2, 0.7); }

}

}

// Tests

using namespace Bempp;

BOOST_AUTO_TEST_SUITE(ModifiedHelmholtz3dSingleLayerBoundaryOperator)

BOOST_AUTO_TEST_CASE_TEMPLATE(interpolated_matches_noniterpolated,
                              BasisFunctionType, basis_function_types)
{
    typedef BasisFunctionType BFT;
    typedef typename Fiber::ScalarTraits<BFT>::ComplexType RT;
    typedef typename Fiber::ScalarTraits<BFT>::ComplexType KT;
    typedef typename Fiber::ScalarTraits<BFT>::RealType CT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
                params, "meshes/two_disjoint_triangles.msh",
                false /* verbose */);

    PiecewiseLinearContinuousScalarSpace<BFT> pwiseLinears(grid);
    PiecewiseConstantScalarSpace<BFT> pwiseConstants(grid);

    AssemblyOptions assemblyOptions;
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setAbsoluteQuadratureOrder(5);
    accuracyOptions.doubleSingular.setAbsoluteQuadratureOrder(5);
    NumericalQuadratureStrategy<BFT, RT> quadStrategy(accuracyOptions);

    Context<BFT, RT> context(make_shared_from_ref(quadStrategy), assemblyOptions);

    const KT waveNumber(3.23, 0.31);

    BoundaryOperator<BFT, RT> opNoninterpolated =
            modifiedHelmholtz3dSingleLayerBoundaryOperator<BFT, KT, RT>(
                make_shared_from_ref(context),
                make_shared_from_ref(pwiseLinears),
                make_shared_from_ref(pwiseLinears),
                make_shared_from_ref(pwiseLinears),
                waveNumber,
                "", NO_SYMMETRY,
                false);
    BoundaryOperator<BFT, RT> opInterpolated =
            modifiedHelmholtz3dSingleLayerBoundaryOperator<BFT, KT, RT>(
                make_shared_from_ref(context),
                make_shared_from_ref(pwiseLinears),
                make_shared_from_ref(pwiseLinears),
                make_shared_from_ref(pwiseLinears),
                waveNumber,
                "", NO_SYMMETRY,
                true);

    arma::Mat<RT> matNoninterpolated = opNoninterpolated.weakForm()->asMatrix();
    arma::Mat<RT> matInterpolated = opInterpolated.weakForm()->asMatrix();

    const CT eps = std::numeric_limits<CT>::epsilon();
    BOOST_CHECK(check_arrays_are_close<RT>(
                    matNoninterpolated, matInterpolated, 100 * eps));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(symmetric_matches_nonsymmetric_in_aca_mode,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename Fiber::ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    if (boost::is_same<RT, std::complex<float> >::value) {
        // The AHMED support for single-precision complex symmetric matrices
        // is broken
        BOOST_CHECK(true);
        return;
    }

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
                params, "../../meshes/sphere-h-0.4.msh",
                false /* verbose */);

    PiecewiseLinearContinuousScalarSpace<BFT> pwiseLinears(grid);
    PiecewiseConstantScalarSpace<BFT> pwiseConstants(grid);

    AssemblyOptions assemblyOptions;
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    AcaOptions acaOptions;
    acaOptions.minimumBlockSize = 4;
    assemblyOptions.switchToAcaMode(acaOptions);
    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(4);
    accuracyOptions.doubleSingular.setRelativeQuadratureOrder(2);
    NumericalQuadratureStrategy<BFT, RT> quadStrategy(accuracyOptions);

    Context<BFT, RT> context(make_shared_from_ref(quadStrategy), assemblyOptions);

    const RT waveNumber = initWaveNumber<RT>();

    BoundaryOperator<BFT, RT> opNonsymmetric =
            modifiedHelmholtz3dSingleLayerBoundaryOperator<BFT, RT, RT>(
                make_shared_from_ref(context),
                make_shared_from_ref(pwiseLinears),
                make_shared_from_ref(pwiseConstants),
                make_shared_from_ref(pwiseLinears),
                waveNumber,
                "", NO_SYMMETRY);
    BoundaryOperator<BFT, RT> opSymmetric =
            modifiedHelmholtz3dSingleLayerBoundaryOperator<BFT, RT, RT>(
                make_shared_from_ref(context),
                make_shared_from_ref(pwiseLinears),
                make_shared_from_ref(pwiseConstants),
                make_shared_from_ref(pwiseLinears),
                waveNumber,
                "", SYMMETRIC);

#   ifdef WITH_AHMED
        arma::Mat<RT> matNonsymmetric = opNonsymmetric.weakForm()->asMatrix();
        arma::Mat<RT> matSymmetric = opSymmetric.weakForm()->asMatrix();

        BOOST_CHECK(check_arrays_are_close<RT>(
                        matNonsymmetric, matSymmetric, 2 * acaOptions.eps));
#   else
        BOOST_CHECK(true);
#   endif
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef PYTHONIC_INCLUDE_NUMPY_ARCSIN_HPP
#define PYTHONIC_INCLUDE_NUMPY_ARCSIN_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"

#include <boost/simd/function/asin.hpp>

PYTHONIC_NS_BEGIN

namespace numpy
{

  namespace wrapper
  {
    double arcsin(long const &v);
    template <class T>
    auto arcsin(T const &v) -> decltype(boost::simd::asin(v));
  }
#define NUMPY_NARY_FUNC_NAME arcsin
#define NUMPY_NARY_FUNC_SYM wrapper::arcsin
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


/*
 * protonReconstruct.cpp
 *
 *  Created on: Dec 22, 2011
 *      Author: u051747
 */
#include <iostream>
#include "parameterparser.h"
#include "cuNDArray.h"
#include "hoCuNDArray.h"
#include "cuNDArray_math.h"

#include "splineBackprojectionOperator.h"
#include "hoNDArray_fileio.h"
#include "check_CUDA.h"

#include "hoCuGPBBSolver.h"
#include "hoCuPartialDerivativeOperator.h"
#include "hoCuNDArray_blas.h"
#include "hoCuNDArray_elemwise.h"

#include "osSARTSolver.h"
#include "hoOSGPBBSolver.h"
#include "hoOSCGSolver.h"
#include "hoOSCGPBBSolver.h"
//#include "hoABIBBSolver.h"
#include "hdf5_utils.h"

#include "encodingOperatorContainer.h"
#include "hoCuOperator.h"
#include "hoImageOperator.h"
#include "identityOperator.h"
#include <boost/program_options.hpp>
#include "vector_td_io.h"

#include "hoCuTvOperator.h"
#include "hoCuTvPicsOperator.h"
#include "projectionSpaceOperator.h"
#include "hoCuNCGSolver.h"
#include "FilteredProton.h"

using namespace std;
using namespace Gadgetron;
typedef float _real;

typedef solver<hoCuNDArray<_real>, hoCuNDArray<_real> > baseSolver;
namespace po = boost::program_options;
int main( int argc, char** argv)
{

	//
	// Parse command line
	//

	_real background =  0.00106;
	std::string dataName;
	std::string outputFile;
	vector_td<int,3> dimensions;
	vector_td<float,3> physical_dims;
	vector_td<float,3> origin;
	int iterations;
	int device;
	int subsets;
	bool use_hull,use_weights, estimate_missing;
	po::options_description desc("Allowed options");
	desc.add_options()
					("help", "produce help message")
					("data,D", po::value<std::string>(&dataName)->default_value("data.hdf5"), "HDF5 file containing projections and splines")
					("dimensions,d", po::value<vector_td<int,3> >(&dimensions)->default_value(vector_td<int,3>(512,512,1)), "Pixel dimensions of the image")
					("size,S", po::value<vector_td<float,3> >(&physical_dims)->default_value(vector_td<float,3>(20,20,5)), "Dimensions of the image")
					("center,c", po::value<vector_td<float,3> >(&origin)->default_value(vector_td<float,3>(0,0,0)), "Center of the reconstruction")
					("output,f", po::value<std::string>(&outputFile)->default_value("image.hdf5"), "Output filename")
					("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
					("use_hull",po::value<bool>(&use_hull)->default_value(true),"Use hull estimate")
					("use_weights",po::value<bool>(&use_weights)->default_value(false),"Use weights if available. Always true if variance is set")
					("estimate_missing",po::value<bool>(&estimate_missing)->default_value(true),"Estimate missing voxels in projection")
					;

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << "\n";
		return 1;
	}
	std::cout << "Command line options:" << std::endl;
	for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
		boost::any a = it->second.value();
		std::cout << it->first << ": ";
		if (a.type() == typeid(std::string)) std::cout << it->second.as<std::string>();
		else if (a.type() == typeid(int)) std::cout << it->second.as<int>();
		else if (a.type() == typeid(bool)) std::cout << it->second.as<bool>();
		else if (a.type() == typeid(float)) std::cout << it->second.as<float>();
		else if (a.type() == typeid(vector_td<float,3>)) std::cout << it->second.as<vector_td<float,3> >();
		else if (a.type() == typeid(vector_td<int,3>)) std::cout << it->second.as<vector_td<int,3> >();
		else std::cout << "Unknown type" << std::endl;
		std::cout << std::endl;
	}
	cudaSetDevice(device);
	//cudaDeviceReset();

	std::vector<size_t> rhs_dims(&dimensions[0],&dimensions[3]); //Quick and dirty vector_td to vector

	auto  data = boost::make_shared<protonDataset<hoCuNDArray>>(dataName,use_weights);
	if (use_hull) //If we don't estimate the hull, we should use a larger volume
		data->preprocess(rhs_dims,physical_dims,use_hull,background);
	else {
		floatd3 physical_dims2 = physical_dims*sqrt(2.0f);
		data->preprocess(rhs_dims,physical_dims2,use_hull,background);
	}
	FilteredProton<hoCuNDArray> E;

	if (data->get_weights())
		*data->get_projections() *= *data->get_weights(); //Have to scale projection data by weights before handing it to the solver. Write up the cost function and see why.

	boost::shared_ptr< hoCuNDArray<_real> > result;
	{
		GPUTimer tim("Reconstruction time:");
		result = E.calculate(rhs_dims,physical_dims,data,estimate_missing);
	}

/*
	splineBackprojectionOperator<hoCuNDArray> op(data,physical_dims);

	hoCuNDArray<float> tmp(*data->get_projections());
	op.mult_M(result.get(),&tmp);
	tmp -= *data->get_projections();

	std::cout << "Residual: " << dot(&tmp,&tmp) << std::endl;
*/
	//Calculate correct scaling factor because someone cannot be bother to calculate it by hand...
	//float s = dot(data->get_projections().get(),&tmp)/dot(&tmp,&tmp);
	//*result *= s;

	std::cout << "Calculation done, saving " << std::endl;
	//write_nd_array<_real>(result.get(), (char*)parms.get_parameter('f')->get_string_value());
	std::stringstream ss;
	for (int i = 0; i < argc; i++){
		ss << argv[i] << " ";
	}

	std::cout << "Image norm " << asum(result.get())/result->get_number_of_elements() << std::endl;
	saveNDArray2HDF5<3>(result.get(),outputFile,physical_dims,origin,ss.str(), -1);

}


#include <iostream>
#include <string>
#include <vector>
#include <unordered_map>
#include <Eigen/Dense>

#ifndef BLOCK_HPP
#define BLOCK_HPP

using namespace Eigen;

class TetrisBlock
{
public:
  std::string this_name;
  int this_value;
  Vector2f this_pivot;
  Matrix2f this_rotation_matrix;
  Matrix4f this_mask;

  TetrisBlock(std::string name, int value, Matrix4f mask, Vector2f pivot);

  Vector2f rotate_point(Vector2f point);
  Matrix4f rotate_mask();
  void rotate();
};

std::ostream & operator<<(std::ostream & flux, TetrisBlock const & block );

bool block_position_possible(int x, int y, const TetrisBlock &block, const MatrixXf &grid);
void add_block_to_grid(int x, int y, const TetrisBlock &block, MatrixXf &grid);
MatrixXf get_visualGrid(int x, int y, int visualNrows, int visualNcols, const TetrisBlock &block, MatrixXf grid, int top_padding, int left_padding);

int next_one_index(int index, VectorXf col);
bool is_line(VectorXf row);
std::vector<int> find_lines(const MatrixXf &grid, int padding);
VectorXf refactor_col(int line_index, VectorXf col);
void delete_line(int line_index, MatrixXf &grid, int padding);

std::unordered_map<std::string, TetrisBlock> get_block_map();

#endif


#include <iostream>
//
#include <polyvec/api.hpp>
#include <polyvec/core/log.hpp>
#include <polyvec/misc.hpp>
#include <polyvec/io/vtk_curve_writer.hpp>
//
#include <dev/pdf.hpp>
//
#include <polyvec/mc/get_bounding_box.hpp>
#include <polyvec/mc/get_pixel_regions.hpp>
#include <polyvec/mc/junction.hpp>
#include <polyvec/mc/random_color.hpp>
#include <polyvec/mc/raster_image_connectivity.hpp>
//
#include <Eigen/Core>
#include <Eigen/Geometry>

NAMESPACE_BEGIN()
void draw_raster_closed(const Eigen::Matrix2Xd& raster,
                        const Eigen::Vector4d& color) {
  ::polyvec::vec<::polyvec::real2> points;
  for (int i = 0; i < raster.cols(); ++i) {
    points.emplace_back(raster.col(i));
  }

  ::polyvec::draw::polygon(points, ::polyvec::Style::fill(color.head<3>()));
}  // end of draw_raster_closed()
NAMESPACE_END()

NAMESPACE_BEGIN(polyvectest)
NAMESPACE_BEGIN(mc)

int junctions(int, char**) {
  std::vector<std::string> image_addrs;
  image_addrs.emplace_back(POLYVEC_TEST_PATH "/mc/abc.png");
  image_addrs.emplace_back(POLYVEC_TEST_PATH "/mc/holes001.png");
  // image_addrs.emplace_back(POLYVEC_TEST_PATH "/mc/chess.png");

  printf("==== Testing raster image connectivity \n");

  polyfit::Log::open("test_dump/log-export-multicolor-traces.txt");

  for (int i = 0; i < (int)image_addrs.size(); ++i) {
    std::vector<Eigen::Matrix2Xi> boundaries;
    std::vector<Eigen::Vector4d> colors;
    polyfit::mc::get_pixel_regions_from_bmp(image_addrs[i], boundaries, colors);

    // Get the bounding box of the whole picture
    Eigen::AlignedBox<int, 2> bbox = polyfit::mc::get_bounding_box(boundaries);

    // Can you build a connectivity
    polyfit::mc::RasterImageConnectivity raster_conn =
        polyfit::mc::RasterImageConnectivity::build(boundaries);
    const bool is_mesh_okay =
        raster_conn.connectivity().check_sanity_slowly(true);
    printf("Mesh okay status: %d \n", (int)is_mesh_okay);

    std::unique_ptr<::polyvec::DevicePDF> pdf;

    //
    // Draw the image by random colors
    //
    pdf.reset(new ::polyvec::DevicePDF(
        ::polyvec::misc::sfmt("test_dump/junctions%02d_%04d.pdf", i, 0).c_str(),
        1, 1));
    //
    std::vector<int> by_area = raster_conn.regions_sorted_by_area();
    //
    for (int j = 0; j < (int)raster_conn.n_regions(); ++j) {
      const int region_id = by_area[j];
      std::vector<int> holes;
      int outer_boundary;
      raster_conn.region_to_polygon(region_id, outer_boundary, holes);
      assert_break(outer_boundary == region_id);

      draw_raster_closed(
          raster_conn.get_polygon_points(outer_boundary).cast<double>(),
          j == 0 ? Eigen::Vector4d(1, 1, 1, 1) : ::polyfit::mc::random_color());

      for (int h : holes) {
        Eigen::Matrix2Xi bdry = raster_conn.get_polygon_points(h);
        bdry.rowwise().reverseInPlace();
        draw_raster_closed(bdry.cast<double>(), Eigen::Vector4d(1, 1, 1, 1));
      }
    }  // end of regions
    // pdf->draw(0, 0);

    //
    // Now get the junction types and draw them
    //

    std::vector<polyfit::mc::JunctionType> junction_types;
    polyfit::mc::find_junctions(raster_conn, junction_types);

    // Draw the junctions
    for (int vid = 0; vid < raster_conn.n_pixel_vertices(); ++vid) {
      if (junction_types[vid] == ::polyfit::mc::JUNCTION_INVALID) {
        continue;
      }

      // Now draw
      Eigen::Vector2i pos = raster_conn.pixel_vertex_index_to_pos(vid);
      auto __draw = [&](const std::string& ss) {
        ::polyvec::draw::text(pos.cast<double>(), ss.c_str(),
                              ::polyvec::draw::font_pdf,
                              ::polyvec::Style::fill(::polyvec::colors::black));
      };

      switch (junction_types[vid]) {
        case polyfit::mc::JUNCTION_3_SHARP:
          __draw("3");
          break;
        case polyfit::mc::JUNCTION_4_SHARP:
          __draw("4");
          break;
        case polyfit::mc::JUNCTION_KOPF:
          __draw("K");
          break;
        default:;
      }
    }  // end of vertices
    pdf->draw(0, 0);
  }  // end of images

  return EXIT_FAILURE;
}

NAMESPACE_END(mc)
NAMESPACE_END(polyvectest)


#include "../src/params.h"
#include "../src/r1_proof.h"
#include "../src/r1_proof_generator.h"
#include "../src/r1_proof_verifier.h"

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

#include <stdlib.h>

bool test(secp_primitives::GroupElement& g, std::vector<secp_primitives::GroupElement>& h_, std::vector<secp_primitives::Scalar>& b, int n, int m){

    secp_primitives::Scalar r;
    r.randomize();
    sigma::R1ProofGenerator<secp_primitives::Scalar, secp_primitives::GroupElement> prover(g, h_, b, r, n, m);
    sigma::R1Proof<secp_primitives::Scalar, secp_primitives::GroupElement> proof;
    prover.proof(proof);

    sigma::R1ProofVerifier<secp_primitives::Scalar, secp_primitives::GroupElement> verifier(g,h_,prover.get_B(), n, m);
    return verifier.verify(proof);
}

BOOST_AUTO_TEST_SUITE(sigma_R1_test)

BOOST_AUTO_TEST_CASE(serialize_deserialize_proof)
{
    auto params = sigma::Params::get_default();
    int n = 4;
    int m = 16;
    secp_primitives::GroupElement g;
    g.randomize();
    std::vector<secp_primitives::GroupElement> h_;
    std::vector<secp_primitives::Scalar> b;
    secp_primitives::GroupElement h;
    for(int i = 0; i < m; ++i) {
        h.randomize();
        h_.push_back(h);
        b.push_back(secp_primitives::Scalar(unsigned(1)));
        for(int j = 1; j < n; ++j){
            h.randomize();
            h_.push_back(h);
            b.push_back(secp_primitives::Scalar(unsigned(0)));

        }
    }

    secp_primitives::Scalar r;
    r.randomize();
    sigma::R1ProofGenerator<secp_primitives::Scalar, secp_primitives::GroupElement> prover(g, h_, b, r, n, m);
    sigma::R1Proof<secp_primitives::Scalar, secp_primitives::GroupElement> proof;
    prover.proof(proof);

    sigma::R1ProofVerifier<secp_primitives::Scalar, secp_primitives::GroupElement> verifier(g,h_,prover.get_B(), n, m);
    BOOST_CHECK(verifier.verify(proof));

    unsigned char buffer [proof.memoryRequired(n, m)];
    proof.serialize(buffer);

    sigma::R1Proof<secp_primitives::Scalar, secp_primitives::GroupElement> resulted;
    resulted.deserialize(buffer,n, m);

    BOOST_CHECK(verifier.verify(resulted));
}

BOOST_AUTO_TEST_CASE(fixed_size_test)
{
    auto params = sigma::Params::get_default();
    int n = 4;
    int m = 16;
    secp_primitives::GroupElement g;
    g.randomize();
    std::vector<secp_primitives::GroupElement> h_;
    std::vector<secp_primitives::Scalar> b;
    secp_primitives::GroupElement h;
    for(int i = 0; i < m; ++i) {
        h.randomize();
        h_.push_back(h);
        b.push_back(secp_primitives::Scalar(unsigned(1)));
        for(int j = 1; j < n; ++j){
            h.randomize();
            h_.push_back(h);
            b.push_back(secp_primitives::Scalar(unsigned(0)));

        }
    }

    BOOST_CHECK(test(g, h_, b, n, m));
}

BOOST_AUTO_TEST_CASE(random_size_test)
{
    auto params = sigma::Params::get_default();
    int n = rand() % 64 + 16;
    int m = rand() % 32 + 16;
    secp_primitives::GroupElement g;
    g.randomize();
    std::vector<secp_primitives::GroupElement> h_;
    std::vector<secp_primitives::Scalar> b;
    for(int i = 0; i < m; ++i) {
        secp_primitives::GroupElement h;
        h.randomize();
        h_.push_back(h);
        b.push_back(secp_primitives::Scalar(unsigned(1)));
        for(int j = 1; j < n; ++j){
            h.randomize();
            h_.push_back(h);
            b.push_back(secp_primitives::Scalar(unsigned(0)));

        }
    }

    BOOST_CHECK(test(g, h_, b, n, m));
}

BOOST_AUTO_TEST_CASE(all_positions)
{
    auto params = sigma::Params::get_default();
    int n = 32;
    int m = 16;
    secp_primitives::GroupElement g;
    std::vector <secp_primitives::GroupElement> h_;
    std::vector <secp_primitives::Scalar> b;
    secp_primitives::GroupElement h;
    for(int k = 0; k < n; ++k){
        g.randomize();
        for(int i = 0;i< m ;++i) {
            for(int j = 0;j < n;++j){
                h.randomize();
                h_.push_back(h);
                if(j == k)
                    b.push_back(secp_primitives::Scalar(unsigned(1)));
                else
                    b.push_back(secp_primitives::Scalar(unsigned(0)));
            }
        }
        BOOST_CHECK(test(g, h_, b, n, m));
        h_.clear();
        b.clear();
    }
}

BOOST_AUTO_TEST_CASE(one_in_random_position)
{
    auto params = sigma::Params::get_default();
    int n = 32;
    int m = 16;
    int k = rand() % n;
    secp_primitives::GroupElement g;
    std::vector <secp_primitives::GroupElement> h_;
    std::vector <secp_primitives::Scalar> b;
    secp_primitives::GroupElement h;
    g.randomize();
    for(int i = 0;i < m;++i) {
        k = rand() % n;
        for(int j = 0;j < n;++j){
            h.randomize();
            h_.push_back(h);
            if(j == k)
                b.push_back(secp_primitives::Scalar(unsigned(1)));
            else
                b.push_back(secp_primitives::Scalar(unsigned(0)));
        }
    }
    BOOST_CHECK(test(g, h_, b, n, m));
}

BOOST_AUTO_TEST_CASE(all_0s_in_random_row)
{
    auto params = sigma::Params::get_default();
    int n = 32;
    int m = 16;
    int k = rand() % m;
    secp_primitives::GroupElement g;
    std::vector <secp_primitives::GroupElement> h_;
    std::vector <secp_primitives::Scalar> b;
    secp_primitives::GroupElement h;
    g.randomize();
    for(int i = 0;i < m;++i) {
        for(int j = 0;j < n;++j){
            h.randomize();
            h_.push_back(h);
            if(i != k)
                b.push_back(secp_primitives::Scalar(unsigned(1)));
            else
                b.push_back(secp_primitives::Scalar(unsigned(0)));
        }
    }
    BOOST_CHECK(!test(g, h_, b, n, m)); // expect false
}

BOOST_AUTO_TEST_CASE(all_1s_in_random_row)
{
    auto params = sigma::Params::get_default();
    int n = 32;
    int m = 16;
    int k = rand() % m;
    secp_primitives::GroupElement g;
    std::vector <secp_primitives::GroupElement> h_;
    std::vector <secp_primitives::Scalar> b;
    secp_primitives::GroupElement h;
    g.randomize();
    for(int i = 0;i < m;++i) {
        for(int j = 0;j < n;++j){
            h.randomize();
            h_.push_back(h);
            if(i == k)
                b.push_back(secp_primitives::Scalar(unsigned(1)));
            else
                b.push_back(secp_primitives::Scalar(unsigned(0)));
        }
    }
    BOOST_CHECK(!test(g, h_, b, n, m)); // expect false
}

BOOST_AUTO_TEST_CASE(two_1s_in_random_row)
{
    auto params = sigma::Params::get_default();
    int n = 32;
    int m = 16;
    int k = rand() % n;
    secp_primitives::GroupElement g;
    std::vector <secp_primitives::GroupElement> h_;
    std::vector <secp_primitives::Scalar> b;
    secp_primitives::GroupElement h;
    g.randomize();
    for(int i = 0;i < m;++i) {
        for(int j = 0;j < n;++j){
            h.randomize();
            h_.push_back(h);
            if(j == k){
                b.push_back(secp_primitives::Scalar(unsigned(1)));
                b.push_back(secp_primitives::Scalar(unsigned(1)));
                h.randomize();
                h_.push_back(h);
                ++j;
            }
            else
                b.push_back(secp_primitives::Scalar(unsigned(0)));
        }
    }
    BOOST_CHECK(!test(g, h_, b, n, m)); // expect false
}

BOOST_AUTO_TEST_CASE(one_all_0s_element)
{
    auto params = sigma::Params::get_default();
    int n = 32;
    int m = 16;
    int r;
    secp_primitives::GroupElement g;
    std::vector <secp_primitives::GroupElement> h_;
    std::vector <secp_primitives::Scalar> b;
    secp_primitives::GroupElement h;
    for(int k = 0; k < n; ++k){
        g.randomize();
        r = rand() % m;
        for(int i = 0;i< m ;++i) {
            for(int j = 0;j < n;++j){
                h.randomize();
                h_.push_back(h);
                if(j == k && i != r)
                    b.push_back(secp_primitives::Scalar(unsigned(1)));
                else
                    b.push_back(secp_primitives::Scalar(unsigned(0)));
            }
        }
        BOOST_CHECK(!test(g, h_, b, n, m)); // expect false
        h_.clear();
        b.clear();
    }
}

BOOST_AUTO_TEST_SUITE_END()


// #include "FSM_States/FSM_State_Loco.h"
#include <Utilities/Timer.h>
#include <Controllers/WBC_Ctrl/LocomotionCtrl/LocomotionCtrl.hpp>
//#include <rt/rt_interface_lcm.h>

#include <cmath>
#include <iostream>
#include <fstream>
#include <json/writer.h>
#include <json/value.h>
#include <json/reader.h>
#include <cppTypes.h>
#include <boost/format.hpp>
#include "Controllers/LegController.h"
#include "Dynamics/MiniCheetah.h"
#include "Dynamics/DynamicsSimulator.h"
#include "Dynamics/FloatingBaseModel.h"
#include "Dynamics/Quadruped.h"
#include "Utilities/utilities.h"

class DataProcessor {
 public:
  DataProcessor();
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  Eigen::VectorXd ComputeJointVelocities(float t);
  Vec12<double> ComputeJointTorques(float t);
  void WriteAsDatFile(Eigen::MatrixXd* result, int i);

private:
    FloatingBaseModel<double> cheetahModel;
    FBModelState<double> x;
    Vec12<double> jointTorques;
    DVec<double> genForce;
    DVec<double> genForceInterp;
    SVec<double> fReturn;
    Json::Value opt;
    Json::Reader reader;
    float t; // current timestamp
    float time;
    // std::ofstream file_in;
    // std::ofstream file_out;
    Vec12<double> footForces;
    Eigen::MatrixXd* result;
};


#define BOOST_TEST_MODULE "test_debye_huckel_potential"

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

#include <test/util/utility.hpp>

#include <mjolnir/forcefield/global/DebyeHuckelPotential.hpp>
#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/util/make_unique.hpp>
#include <mjolnir/util/logger.hpp>

BOOST_AUTO_TEST_CASE(DH_double)
{
    using real_type = double;
    using potential_type = mjolnir::DebyeHuckelPotential<real_type>;
    using parameter_type = potential_type::parameter_type;
    using traits_type = mjolnir::SimulatorTraits<real_type, mjolnir::UnlimitedBoundary>;
    using boundary_type = typename traits_type::boundary_type;

    mjolnir::LoggerManager::set_default_logger("test_debye_huckel_potential.log");
    {
        // to make the value realistic, we need to modify the unit system.
        using phys_type = mjolnir::physics::constants<real_type>;
        using unit_type = mjolnir::unit::constants<real_type>;

        phys_type::set_kB(unit_type::boltzmann_constant() *
                          1e-3 * unit_type::J_to_cal() * unit_type::avogadro_constant());
        phys_type::set_NA(unit_type::avogadro_constant());
        phys_type::set_eps0((unit_type::vacuum_permittivity() /
            unit_type::elementary_charge()) / unit_type::elementary_charge() *
            (1e+3 / unit_type::J_to_cal() / unit_type::avogadro_constant()) *
            (1.0 / unit_type::m_to_angstrom()));

        phys_type::set_m_to_length(1e-10);
        phys_type::set_length_to_m(1e+10);
        phys_type::set_L_to_volume(1e+27);
        phys_type::set_volume_to_L(1e-27);
    }

    constexpr std::size_t N = 10000;
    constexpr real_type   h = 1e-6;
    constexpr real_type tol = 1e-6;

    mjolnir::System<traits_type> sys(0, boundary_type{});
    sys.attribute("temperature")    = 300.0; // [K]
    sys.attribute("ionic_strength") =   0.2; // [M]

    potential_type potential(5.5);
    potential.initialize(sys);

    parameter_type params{1.0};

    const real_type x_min = 0.5 * potential.debye_length();
    const real_type x_max = 5.5 * potential.debye_length();

    mjolnir::test::check_potential(potential, params, x_min, x_max, tol, h, N);
}
BOOST_AUTO_TEST_CASE(DH_float)
{
    using real_type = float;
    using potential_type = mjolnir::DebyeHuckelPotential<real_type>;
    using parameter_type = potential_type::parameter_type;
    using traits_type = mjolnir::SimulatorTraits<real_type, mjolnir::UnlimitedBoundary>;
    using boundary_type = typename traits_type::boundary_type;

    mjolnir::LoggerManager::set_default_logger("test_debye_huckel_potential.log");
    {
        // to make the value realistic, we need to modify the unit system.
        using phys_type = mjolnir::physics::constants<real_type>;
        using unit_type = mjolnir::unit::constants<real_type>;

        phys_type::set_kB(unit_type::boltzmann_constant() *
                          1e-3 * unit_type::J_to_cal() * unit_type::avogadro_constant());
        phys_type::set_NA(unit_type::avogadro_constant());
        phys_type::set_eps0((unit_type::vacuum_permittivity() /
            unit_type::elementary_charge()) / unit_type::elementary_charge() *
            (1e+3 / unit_type::J_to_cal() / unit_type::avogadro_constant()) *
            (1.0 / unit_type::m_to_angstrom()));

        phys_type::set_m_to_length(1e-10);
        phys_type::set_length_to_m(1e+10);
        phys_type::set_L_to_volume(1e+27);
        phys_type::set_volume_to_L(1e-27);
    }

    constexpr static std::size_t N = 1000;
    constexpr static real_type   h = 0.002f;
    constexpr static real_type tol = 0.004f;

    mjolnir::System<traits_type> sys(0, boundary_type{});
    sys.attribute("temperature")    = 300.0; // [K]
    sys.attribute("ionic_strength") =   0.2; // [M]

    potential_type potential(5.5f);
    potential.initialize(sys);

    parameter_type params{1.0f};

    const real_type x_min = 0.5f * potential.debye_length();
    const real_type x_max = 5.5f * potential.debye_length();

    mjolnir::test::check_potential(potential, params, x_min, x_max, tol, h, N);
}

BOOST_AUTO_TEST_CASE(DH_cutoff_length)
{
    using real_type = double;
    using traits_type = mjolnir::SimulatorTraits<real_type, mjolnir::UnlimitedBoundary>;
    using boundary_type = typename traits_type::boundary_type;
    using potential_type = mjolnir::DebyeHuckelPotential<real_type>;

    using parameter_list_type  = mjolnir::DebyeHuckelParameterList<traits_type>;
    using parameter_type       = parameter_list_type::parameter_type;
    using ignore_molecule_type = parameter_list_type::ignore_molecule_type;
    using ignore_group_type    = parameter_list_type::ignore_group_type;

    mjolnir::LoggerManager::set_default_logger("test_debye_huckel_potential.log");
    {
        // to make the value realistic, we need to modify the unit system.
        using phys_type = mjolnir::physics::constants<real_type>;
        using unit_type = mjolnir::unit::constants<real_type>;

        phys_type::set_kB(unit_type::boltzmann_constant() *
                          1e-3 * unit_type::J_to_cal() * unit_type::avogadro_constant());
        phys_type::set_NA(unit_type::avogadro_constant());
        phys_type::set_eps0((unit_type::vacuum_permittivity() /
            unit_type::elementary_charge()) / unit_type::elementary_charge() *
            (1e+3 / unit_type::J_to_cal() / unit_type::avogadro_constant()) *
            (1.0 / unit_type::m_to_angstrom()));

        phys_type::set_m_to_length(1e-10);
        phys_type::set_length_to_m(1e+10);
        phys_type::set_L_to_volume(1e+27);
        phys_type::set_volume_to_L(1e-27);
    }

    mjolnir::System<traits_type> sys(10, boundary_type{});
    sys.attribute("temperature")    = 300.0; // [K]
    sys.attribute("ionic_strength") =   0.2; // [M]

    potential_type potential(5.5f);
    potential.initialize(sys);

    // generate a parameter
    std::vector<std::pair<std::size_t, parameter_type>> parameters(10);
    for(std::size_t i=0; i<parameters.size(); ++i)
    {
        parameters.at(i).first          = i;
        parameters.at(i).second.charge  = (i % 2 == 0) ? 1.0 : -1.0;
    }
    parameter_list_type parameter_list(parameters, {}, ignore_molecule_type{"Nothing"}, ignore_group_type({}));

    // estimated maximum cutoff (from per-particle parameter)
    const auto max_cutoff = potential.max_cutoff(parameter_list.parameters().begin(), parameter_list.parameters().end());

    // secure maximum cutoff calculated from per-pair parameter
    // This requires O(N^2), so it will not calculated in usual cases
    real_type max_abs_cutoff = 0.0;
    for(std::size_t i=0; i<parameters.size(); ++i)
    {
        for(std::size_t j=0; j<parameters.size(); ++j)
        {
            max_abs_cutoff = std::max(max_abs_cutoff, potential.absolute_cutoff(parameter_list.prepare_params(i, j)));
        }
    }

    // max cutoff estimated from per-particle parameter should be the same or larger than the secure max cutoff
    BOOST_TEST(max_abs_cutoff <= max_cutoff);

    const real_type safety = 1.0 + std::numeric_limits<real_type>::epsilon();
    for(std::size_t i=0; i<parameters.size(); ++i)
    {
        for(std::size_t j=0; j<parameters.size(); ++j)
        {
            BOOST_TEST(potential.potential(max_abs_cutoff * safety, parameter_list.prepare_params(i, j)) == 0.0);
        }
    }
}


// Copyright (c) Facebook, Inc. and its affiliates.

// This source code is licensed under the MIT license found in the
// LICENSE file in the root directory of this source tree.
#ifndef TORCHROT_H
#define TORCHROT_H

#include <assert.h>
#include <fstream>
#include <iostream>
#include <string>

#include "dtt.h"
#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <torch/script.h>
#include <torch/torch.h>

Eigen::Quaternionf tensor4ToQuat(torch::Tensor T);
Eigen::Matrix3f tensor33ToMatrix(torch::Tensor T);
Eigen::AngleAxisf tensor3ToAngleAxis(torch::Tensor T);
Eigen::Vector4f quatToVector(Eigen::Quaternionf q);

torch::Tensor normalizeQuaternion(torch::Tensor q);
torch::Tensor invertQuaternion(torch::Tensor q);
torch::Tensor quatToAxis(torch::Tensor q);
torch::Tensor quatToAngle(torch::Tensor q);
torch::Tensor quatToMatrix(torch::Tensor q);
torch::Tensor quatToRotvec(torch::Tensor q);
torch::Tensor matrixToQuat(torch::Tensor m);
torch::Tensor rotvecToQuat(torch::Tensor r);
torch::Tensor quaternionMultiply(torch::Tensor q1, torch::Tensor q2);

#endif