/****************************************************************************
*
*    Copyright (c) 2019 Vivante Corporation
*
*    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.
*
*****************************************************************************/

#pragma once

#include <backendsCommon/CpuTensorHandle.hpp>
#include <backendsCommon/Workload.hpp>
#include <backendsCommon/WorkloadData.hpp>
#include <boost/log/trivial.hpp>
#include "TNpuWorkloads.hpp"

namespace armnn {
template <typename armnn::DataType... DataTypes>
class NpuNormalizationWorkload : public TNpuWorkload<NormalizationQueueDescriptor, DataTypes...> {
   public:
    using base_type = TNpuWorkload<NormalizationQueueDescriptor, DataTypes...>;
    explicit NpuNormalizationWorkload(const NormalizationQueueDescriptor& descriptor,
                                      const WorkloadInfo& info)
        : TNpuWorkload<NormalizationQueueDescriptor, DataTypes...>(descriptor, info),
          m_NormChannelType(descriptor.m_Parameters.m_NormChannelType),
          m_NormMethodType(descriptor.m_Parameters.m_NormMethodType),
          m_NormSize(descriptor.m_Parameters.m_NormSize),
          m_Alpha(descriptor.m_Parameters.m_Alpha),
          m_Beta(descriptor.m_Parameters.m_Beta),
          m_K(descriptor.m_Parameters.m_K),
          m_DataLayout(descriptor.m_Parameters.m_DataLayout) {
        std::vector<uint32_t> inOperandIds;

        // Only 1 input tensor
        NpuTensorHandler* inputTensorHandle =
            dynamic_cast<NpuTensorHandler*>(descriptor.m_Inputs[0]);
        uint32_t inputOperandId = this->AddOperandAndSetValue(
            inputTensorHandle->GetTensorInfo(), inputTensorHandle->GetShape(), nullptr);
        inOperandIds.push_back(inputOperandId);

        // order is important
        // Add norm size(radius) operand
        // Strong Assumption on rounding Mode
        inOperandIds.push_back(this->AddOperandAndSetValue(m_NormSize / 2u));

        // Add kappa(bias) operand
        inOperandIds.push_back(this->AddOperandAndSetValue(m_K));

        // Add alpha operand
        inOperandIds.push_back(this->AddOperandAndSetValue(m_Alpha));

        // Add beta oparand
        inOperandIds.push_back(this->AddOperandAndSetValue(m_Beta));

        // Add channel type operand
        inOperandIds.push_back(this->AddOperandAndSetValue(uint32_t(m_NormChannelType)));

        // Add method type operand
        inOperandIds.push_back(this->AddOperandAndSetValue(uint32_t(m_NormMethodType)));

        // Add layout operand
        int32_t layoutCode = m_DataLayout == armnn::DataLayout::NCHW
                                 ? int32_t(nnrt::DataLayout::NCHW)
                                 : int32_t(nnrt::DataLayout::NHWC);
        inOperandIds.push_back(this->AddOperandAndSetValue(layoutCode));

        std::vector<uint32_t> outOperandIds;
        NpuTensorHandler* outputTensorHandle =
            dynamic_cast<NpuTensorHandler*>(descriptor.m_Outputs[0]);
        uint32_t outputTensorId = this->AddOperandAndSetValue(
            outputTensorHandle->GetTensorInfo(), outputTensorHandle->GetShape(), nullptr);
        outOperandIds.push_back(outputTensorId);

        this->AddOperation(nnrt::OperationType::LOCAL_RESPONSE_NORMALIZATION,
                           inOperandIds.size(),
                           inOperandIds.data(),
                           outOperandIds.size(),
                           outOperandIds.data());
    }

   private:
    /// Normalization channel algorithm to use (Across, Within).
    NormalizationAlgorithmChannel m_NormChannelType;
    /// Normalization method algorithm to use (LocalBrightness, LocalContrast).
    NormalizationAlgorithmMethod m_NormMethodType;
    /// Depth radius value.
    uint32_t m_NormSize;
    /// Alpha value for the normalization equation.
    float m_Alpha;
    /// Beta value for the normalization equation.
    float m_Beta;
    /// Kappa value used for the across channel normalization equation.
    float m_K;
    /// The data layout to be used (NCHW, NHWC).
    DataLayout m_DataLayout;
};
using NpuNormalizationFloat32Workload = NpuNormalizationWorkload<armnn::DataType::Float32>;
using NpuNormalizationFloat16Workload = NpuNormalizationWorkload<armnn::DataType::Float16>;
using NpuNormalizationUint8Workload = NpuNormalizationWorkload<armnn::DataType::QuantisedAsymm8>;
}  // namespace armnn


#ifndef GUNEROMERKLETREE_H_
#define GUNEROMERKLETREE_H_

#include <deque>
#include <boost/optional.hpp>
#include <boost/static_assert.hpp>
#include <libff/common/utils.hpp>
#include <libsnark/common/default_types/r1cs_gg_ppzksnark_pp.hpp>
#include <libsnark/common/default_types/r1cs_gg_ppzksnark_pp.hpp>
#include <libsnark/relations/constraint_satisfaction_problems/r1cs/examples/r1cs_examples.hpp>
#include <libsnark/zk_proof_systems/ppzksnark/r1cs_gg_ppzksnark/r1cs_gg_ppzksnark.hpp>
#include <libsnark/zk_proof_systems/ppzksnark/r1cs_ppzksnark/r1cs_ppzksnark.hpp>
#include <libsnark/zk_proof_systems/ppzksnark/uscs_ppzksnark/uscs_ppzksnark.hpp>
#include <libff/algebra/fields/field_utils.hpp>
#include <libff/algebra/scalar_multiplication/multiexp.hpp>
#include <libsnark/common/default_types/r1cs_ppzksnark_pp.hpp>
#include <libsnark/zk_proof_systems/ppzksnark/r1cs_ppzksnark/r1cs_ppzksnark.hpp>
#include <libff/algebra/curves/edwards/edwards_pp.hpp>
#include <libff/algebra/curves/mnt/mnt4/mnt4_pp.hpp>
#include <libff/algebra/curves/mnt/mnt6/mnt6_pp.hpp>
#include <libff/common/utils.hpp>

#include <libsnark/gadgetlib1/gadgets/merkle_tree/merkle_tree_check_read_gadget.hpp>
#include <libsnark/gadgetlib1/gadgets/merkle_tree/merkle_tree_check_update_gadget.hpp>

using namespace libsnark;

namespace gunero {

/**
 * A Merkle tree is maintained as two maps:
 * - a map from addresses to values, and
 * - a map from addresses to hashes.
 *
 * The second map maintains the intermediate hashes of a Merkle tree
 * built atop the values currently stored in the tree (the
 * implementation admits a very efficient support for sparse
 * trees). Besides offering methods to load and store values, the
 * class offers methods to retrieve the root of the Merkle tree and to
 * obtain the authentication paths for (the value at) a given address.
 */

typedef libff::bit_vector gunero_merkle_authentication_node;
typedef std::vector<gunero_merkle_authentication_node> gunero_merkle_authentication_path;

std::ostream& operator<<(std::ostream &out, const gunero_merkle_authentication_node& node);
std::istream& operator>>(std::istream &in, gunero_merkle_authentication_node& node);
std::ostream& operator<<(std::ostream &out, const std::vector<gunero_merkle_authentication_node>& M_account);
std::istream& operator>>(std::istream &in, std::vector<gunero_merkle_authentication_node>& M_account);

template<typename HashT>
class gunero_merkle_tree {
private:

    typedef typename HashT::hash_value_type hash_value_type;
    typedef typename HashT::merkle_authentication_path_type gunero_merkle_authentication_path_type;

public:

    std::vector<hash_value_type> hash_defaults;
    std::map<size_t, libff::bit_vector> values;
    std::map<size_t, hash_value_type> hashes;

    size_t depth;
    size_t value_size;
    size_t digest_size;

    gunero_merkle_tree(const size_t depth, const size_t value_size);
    gunero_merkle_tree(const size_t depth, const size_t value_size, const std::vector<libff::bit_vector> &contents_as_vector);
    gunero_merkle_tree(const size_t depth, const size_t value_size, const std::map<size_t, libff::bit_vector> &contents);

    libff::bit_vector get_value(const libff::bit_vector address) const;
    void set_value(const libff::bit_vector address, const libff::bit_vector &value);

    hash_value_type get_root() const;
    gunero_merkle_authentication_path_type get_path(const libff::bit_vector address) const;

    void dump() const;
};

template<typename FieldT, typename HashT>
class gunero_merkle_authentication_path_variable : public gadget<FieldT> {
public:

    const size_t tree_depth;
    std::vector<digest_variable<FieldT> > left_digests;
    std::vector<digest_variable<FieldT> > right_digests;

    gunero_merkle_authentication_path_variable(protoboard<FieldT> &pb,
                                        const size_t tree_depth,
                                        const std::string &annotation_prefix) :
        gadget<FieldT>(pb, annotation_prefix),
        tree_depth(tree_depth)
    {
        for (size_t i = 0; i < tree_depth; ++i)
        {
            left_digests.emplace_back(digest_variable<FieldT>(pb, HashT::get_digest_len(), FMT(annotation_prefix, " left_digests_%zu", i)));
            right_digests.emplace_back(digest_variable<FieldT>(pb, HashT::get_digest_len(), FMT(annotation_prefix, " right_digests_%zu", i)));
        }
    }

    void generate_r1cs_constraints()
    {
        for (size_t i = 0; i < tree_depth; ++i)
        {
            left_digests[i].generate_r1cs_constraints();
            right_digests[i].generate_r1cs_constraints();
        }
    }

    void generate_r1cs_witness(const libff::bit_vector address, const gunero_merkle_authentication_path &path)
    {
        assert(address.size() == tree_depth);
        assert(path.size() == tree_depth);

        for (size_t i = 0; i < tree_depth; ++i)
        {
            //if (address & (1ul << (tree_depth-1-i)))
            //if (address.at(tree_depth-1-i))
            if (address.at(i))
            {
                left_digests[i].generate_r1cs_witness(path[i]);
            }
            else
            {
                right_digests[i].generate_r1cs_witness(path[i]);
            }
        }
    }
    gunero_merkle_authentication_path get_authentication_path(const libff::bit_vector address) const;
};

template<typename FieldT, typename HashT>
class gunero_merkle_tree_check_read_gadget : public gadget<FieldT> {
private:

    std::vector<HashT> hashers;
    std::vector<block_variable<FieldT> > hasher_inputs;
    std::vector<digest_selector_gadget<FieldT> > propagators;
    std::vector<digest_variable<FieldT> > internal_output;

    std::shared_ptr<digest_variable<FieldT> > computed_root;
    std::shared_ptr<bit_vector_copy_gadget<FieldT> > check_root;

public:

    const size_t digest_size;
    const size_t tree_depth;
    pb_linear_combination_array<FieldT> address_bits;
    digest_variable<FieldT> leaf;
    digest_variable<FieldT> root;
    gunero_merkle_authentication_path_variable<FieldT, HashT> path;
    pb_linear_combination<FieldT> read_successful;

    gunero_merkle_tree_check_read_gadget(protoboard<FieldT> &pb,
                                  const size_t tree_depth,
                                  const pb_linear_combination_array<FieldT> &address_bits,
                                  const digest_variable<FieldT> &leaf_digest,
                                  const digest_variable<FieldT> &root_digest,
                                  const gunero_merkle_authentication_path_variable<FieldT, HashT> &path,
                                  const pb_linear_combination<FieldT> &read_successful,
                                  const std::string &annotation_prefix) :
        gadget<FieldT>(pb, annotation_prefix),
        digest_size(HashT::get_digest_len()),
        tree_depth(tree_depth),
        address_bits(address_bits),
        leaf(leaf_digest),
        root(root_digest),
        path(path),
        read_successful(read_successful)
    {
        /*
        The tricky part here is ordering. For Merkle tree
        authentication paths, path[0] corresponds to one layer below
        the root (and path[tree_depth-1] corresponds to the layer
        containing the leaf), while address_bits has the reverse order:
        address_bits[0] is LSB, and corresponds to layer containing the
        leaf, and address_bits[tree_depth-1] is MSB, and corresponds to
        the subtree directly under the root.
        */
        assert(tree_depth > 0);
        assert(tree_depth == address_bits.size());

        for (size_t i = 0; i < tree_depth-1; ++i)
        {
            internal_output.emplace_back(digest_variable<FieldT>(pb, digest_size, FMT(this->annotation_prefix, " internal_output_%zu", i)));
        }

        computed_root.reset(new digest_variable<FieldT>(pb, digest_size, FMT(this->annotation_prefix, " computed_root")));

        for (size_t i = 0; i < tree_depth; ++i)
        {
            block_variable<FieldT> inp(pb, path.left_digests[i], path.right_digests[i], FMT(this->annotation_prefix, " inp_%zu", i));
            hasher_inputs.emplace_back(inp);
            hashers.emplace_back(HashT(pb, 2*digest_size, inp, (i == 0 ? *computed_root : internal_output[i-1]),
                                    FMT(this->annotation_prefix, " load_hashers_%zu", i)));
        }

        for (size_t i = 0; i < tree_depth; ++i)
        {
            /*
            The propagators take a computed hash value (or leaf in the
            base case) and propagate it one layer up, either in the left
            or the right slot of authentication_path_variable.
            */
            propagators.emplace_back(digest_selector_gadget<FieldT>(pb, digest_size, i < tree_depth - 1 ? internal_output[i] : leaf,
                                                                    address_bits[tree_depth-1-i], path.left_digests[i], path.right_digests[i],
                                                                    FMT(this->annotation_prefix, " digest_selector_%zu", i)));
        }

        check_root.reset(new bit_vector_copy_gadget<FieldT>(pb, computed_root->bits, root.bits, read_successful, FieldT::capacity(), FMT(annotation_prefix, " check_root")));
    }

    void generate_r1cs_constraints()
    {
        /* ensure correct hash computations */
        for (size_t i = 0; i < tree_depth; ++i)
        {
            // Note that we check root outside and have enforced booleanity of path.left_digests/path.right_digests outside in path.generate_r1cs_constraints
            hashers[i].generate_r1cs_constraints(false);
        }

        /* ensure consistency of path.left_digests/path.right_digests with internal_output */
        for (size_t i = 0; i < tree_depth; ++i)
        {
            propagators[i].generate_r1cs_constraints();
        }

        check_root->generate_r1cs_constraints(false, false);
    }

    void generate_r1cs_witness()
    {
        /* do the hash computations bottom-up */
        for (int i = tree_depth-1; i >= 0; --i)
        {
            /* propagate previous input */
            propagators[i].generate_r1cs_witness();

            /* compute hash */
            hashers[i].generate_r1cs_witness();
        }

        check_root->generate_r1cs_witness();
    }

    static size_t root_size_in_bits();
    /* for debugging purposes */
    static size_t expected_constraints(const size_t tree_depth);
};

} // end namespace `gunero`

#endif /* GUNEROMERKLETREE_H_ */


#include <ros/ros.h>
#include <iostream>
#include <signal.h>
#include <stdio.h>
#include <boost/bind.hpp>
#include <dark_msgs/Detect.h>
#include <dark_msgs/DetectArray.h>

#include "dark_yolo.h"

darknet::Yolo yolo_detector_;
darknet::Classify classifier_;

ros::Publisher detect_publish;

namespace darknet
{
    uint32_t Yolo::get_network_height()
    {
        return darknet_network_->h;
    }
    uint32_t Yolo::get_network_width()
    {
        return darknet_network_->w;
    }
    void Yolo::load(std::string& in_model_file, std::string& in_trained_file, double in_min_confidence, double in_nms_threshold)
    {
        min_confidence_ = in_min_confidence;
        nms_threshold_ = in_nms_threshold;
        darknet_network_ = parse_network_cfg(&in_model_file[0]);
        load_weights(darknet_network_, &in_trained_file[0]);
        set_batch_network(darknet_network_, 1);

        layer output_layer = darknet_network_->layers[darknet_network_->n - 1];
        darknet_boxes_.resize(output_layer.w * output_layer.h * output_layer.n);
    }

    Yolo::~Yolo()
    {
        free_network(darknet_network_);
    }

    std::vector< RectClassScore<float> > Yolo::detect(image& in_darknet_image)
    {
        return forward(in_darknet_image);
    }

    image Yolo::convert_image(const sensor_msgs::ImageConstPtr& msg)
    {
        if (msg->encoding != sensor_msgs::image_encodings::BGR8)
        {
            ROS_ERROR("Unsupported encoding");
            exit(-1);
        }

        auto data = msg->data;
        uint32_t height = msg->height, width = msg->width, offset = msg->step - 3 * width;
        uint32_t i = 0, j = 0;
        image im = make_image(width, height, 3);

        for (uint32_t line = height; line; line--)
        {
            for (uint32_t column = width; column; column--)
            {
                for (uint32_t channel = 0; channel < 3; channel++)
                    im.data[i + width * height * channel] = data[j++] / 255.;
                i++;
            }
            j += offset;
        }

        if (darknet_network_->w == (int) width && darknet_network_->h == (int) height)
        {
            return im;
        }
        image resized = resize_image(im, darknet_network_->w, darknet_network_->h);
        free_image(im);
        return resized;
    }

    std::vector< RectClassScore<float> > Yolo::forward(image& in_darknet_image)
    {
        float * in_data = in_darknet_image.data;
        float *prediction = network_predict(darknet_network_, in_data);
        layer output_layer = darknet_network_->layers[darknet_network_->n - 1];

        output_layer.output = prediction;
        int nboxes = 0;
        int num_classes = output_layer.classes;
        detection *darknet_detections = get_network_boxes(darknet_network_, darknet_network_->w, darknet_network_->h, min_confidence_, .5, NULL, 0, &nboxes);

        do_nms_sort(darknet_detections, nboxes, num_classes, nms_threshold_);

        std::vector< RectClassScore<float> > detections;

        for (int i = 0; i < nboxes; i++)
        {
            int class_id = -1;
            float score = 0.f;
            //find the class
            for(int j = 0; j < num_classes; ++j){
                if (darknet_detections[i].prob[j] >= min_confidence_){
                    if (class_id < 0) {
                        class_id = j;
                        score = darknet_detections[i].prob[j];
                    }
                }
            }
            //if class found
            if (class_id >= 0)
            {
                RectClassScore<float> detection;

                detection.x = darknet_detections[i].bbox.x - darknet_detections[i].bbox.w/2;
                detection.y = darknet_detections[i].bbox.y - darknet_detections[i].bbox.h/2;
                detection.w = darknet_detections[i].bbox.w;
                detection.h = darknet_detections[i].bbox.h;
                detection.score = score;
                detection.class_type = class_id;
                //std::cout << detection.toString() << std::endl;

                detections.push_back(detection);
            }
        }
        //std::cout << std::endl;
        return detections;
    }

    uint32_t Classify::get_network_height()
    {
        return classify_network_->h;
    }
    uint32_t Classify::get_network_width()
    {
        return classify_network_->w;
    }
    void Classify::load(std::string& in_model_file, std::string& in_trained_file)
    {
        classify_network_ = parse_network_cfg(&in_model_file[0]);
        // load_weights(classify_network_, &in_trained_file[0]);
        // set_batch_network(classify_network_, 1);

        // layer output_layer = classify_network_->layers[classify_network_->n - 1];
        // darknet_boxes_.resize(output_layer.w * output_layer.h * output_layer.n);
    }

    Classify::~Classify()
    {
        free_network(classify_network_);
    }

    void Classify::classify_image(image& in_darknet_image)
    {
        return forward(in_darknet_image);
    }

    image Classify::convert_image(const sensor_msgs::ImageConstPtr& msg)
    {
        if (msg->encoding != sensor_msgs::image_encodings::BGR8)
        {
            ROS_ERROR("Unsupported encoding");
            exit(-1);
        }

        auto data = msg->data;
        uint32_t height = msg->height, width = msg->width, offset = msg->step - 3 * width;
        uint32_t i = 0, j = 0;
        image im = make_image(width, height, 3);

        for (uint32_t line = height; line; line--)
        {
            for (uint32_t column = width; column; column--)
            {
                for (uint32_t channel = 0; channel < 3; channel++)
                    im.data[i + width * height * channel] = data[j++] / 255.;
                i++;
            }
            j += offset;
        }

        if (classify_network_->w == (int) width && classify_network_->h == (int) height)
        {
            return im;
        }
        image resized = resize_image(im, classify_network_->w, classify_network_->h);
        free_image(im);
        return resized;
    }

    void Classify::forward(image& in_darknet_image)
    {
        float * in_data = in_darknet_image.data;
        float *prediction = network_predict(classify_network_, in_data);

        if(classify_network_->hierarchy) hierarchy_predictions(prediction, classify_network_->outputs, classify_network_->hierarchy, 1, 1);
        // layer output_layer = classify_network_->layers[classify_network_->n - 1];
        //
        // output_layer.output = prediction;
        // int nboxes = 0;
        // int num_classes = output_layer.classes;
        // detection *darknet_detections = get_network_boxes(classify_network_, classify_network_->w, classify_network_->h, min_confidence_, .5, NULL, 0, &nboxes);
        //
        // do_nms_sort(darknet_detections, nboxes, num_classes, nms_threshold_);
        //
        // std::vector< RectClassScore<float> > detections;
        //
        // for (int i = 0; i < nboxes; i++)
        // {
        //     int class_id = -1;
        //     float score = 0.f;
        //     //find the class
        //     for(int j = 0; j < num_classes; ++j){
        //         if (darknet_detections[i].prob[j] >= min_confidence_){
        //             if (class_id < 0) {
        //                 class_id = j;
        //                 score = darknet_detections[i].prob[j];
        //             }
        //         }
        //     }
        //     //if class found
        //     if (class_id >= 0)
        //     {
        //         RectClassScore<float> detection;
        //
        //         detection.x = darknet_detections[i].bbox.x - darknet_detections[i].bbox.w/2;
        //         detection.y = darknet_detections[i].bbox.y - darknet_detections[i].bbox.h/2;
        //         detection.w = darknet_detections[i].bbox.w;
        //         detection.h = darknet_detections[i].bbox.h;
        //         detection.score = score;
        //         detection.class_type = class_id;
        //         //std::cout << detection.toString() << std::endl;
        //
        //         detections.push_back(detection);
        //     }
        // }
        // //std::cout << std::endl;
        // return detections;
    }
}  // namespace darknet

void rgbgr_image_y(image& im)
{
    int i;
    for(i = 0; i < im.w*im.h; ++i)
    {
        float swap = im.data[i];
        im.data[i] = im.data[i+im.w*im.h*2];
        im.data[i+im.w*im.h*2] = swap;
    }
}

image convert_ipl_to_image(const sensor_msgs::ImageConstPtr& msg)
{

    double image_ratio_;
    uint32_t image_top_bottom_border_;//black strips added to the input image to maintain aspect ratio while resizing it to fit the network input size
    uint32_t image_left_right_border_;

    cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(msg, "bgr8");//toCvCopy(image_source, sensor_msgs::image_encodings::BGR8);
    cv::Mat mat_image = cv_image->image;

    uint32_t network_input_width = yolo_detector_.get_network_width();
    uint32_t network_input_height = yolo_detector_.get_network_height();

    uint32_t image_height = msg->height,
            image_width = msg->width;

    IplImage ipl_image;
    cv::Mat final_mat;

    if (network_input_width!=image_width
        || network_input_height != image_height)
    {
        //final_mat = cv::Mat(network_input_width, network_input_height, CV_8UC3, cv::Scalar(0,0,0));
        image_ratio_ = (double ) network_input_width /  (double)mat_image.cols;

        cv::resize(mat_image, final_mat, cv::Size(), image_ratio_, image_ratio_);
        image_top_bottom_border_ = abs(final_mat.rows-network_input_height)/2;
        image_left_right_border_ = abs(final_mat.cols-network_input_width)/2;
        cv::copyMakeBorder(final_mat, final_mat,
                           image_top_bottom_border_, image_top_bottom_border_,
                           image_left_right_border_, image_left_right_border_,
                           cv::BORDER_CONSTANT, cv::Scalar(0,0,0));

    }
    else
        final_mat = mat_image;

    ipl_image = final_mat;

    unsigned char *data = (unsigned char *)ipl_image.imageData;
    int h = ipl_image.height;
    int w = ipl_image.width;
    int c = ipl_image.nChannels;
    int step = ipl_image.widthStep;
    int i, j, k;

    image darknet_image = make_image(w, h, c);

    for(i = 0; i < h; ++i){
        for(k= 0; k < c; ++k){
            for(j = 0; j < w; ++j){
                darknet_image.data[k*w*h + i*w + j] = data[i*step + j*c + k]/255.;
            }
        }
    }
    rgbgr_image_y(darknet_image);
    return darknet_image;
}

int maximum_in_array(float* array, int N){
  float big = array[0];
  int idx = 0;
  for(int i = 1; i < N; i++){
    if(big < array[i]){
      big = array[i];
      idx = i;
    }
  }
  return idx;
}

void image_callback(const sensor_msgs::ImageConstPtr& in_image_message)
{
    std::vector< RectClassScore<float> > detections;
    image darknet_image_ = {};
    darknet_image_ = convert_ipl_to_image(in_image_message);

    detections = yolo_detector_.detect(darknet_image_);

    //Check the if the network is able to score_threshold
    // float score_array[detections.size()];
    // for (unsigned int i = 0; i < detections.size(); ++i)
    //   score_array[i] = detections[i].score;
    //
    // int idx_max = maximum_in_array(score_array,detections.size());
    // std::cout<<"Score: "<<detections[idx_max].score<<" "<<"Class: "<<detections[idx_max].class_type<<std::endl;

    //Output messages for detect /darknet_ros

    dark_msgs::DetectArray output_detect_array;
    output_detect_array.header = in_image_message->header;
    for (unsigned int i = 0; i < detections.size(); ++i)
    {
        if(detections.size()>0)
        {
            dark_msgs::Detect output_detect;

            output_detect.x = detections[i].x;
            output_detect.y = detections[i].y;
            output_detect.w = detections[i].w;
            output_detect.h = detections[i].h;
            if (detections[i].x < 0)
                output_detect.x = 0;
            if (detections[i].y < 0)
                output_detect.y = 0;
            if (detections[i].w < 0)
                output_detect.w = 0;
            if (detections[i].h < 0)
                output_detect.h = 0;

            output_detect.score = detections[i].score;
            output_detect.class_id = detections[i].class_type;
            //std::cout << "x "<< rect.x<< " y " << rect.y << " w "<< rect.width << " h "<< rect.height<< " s " << rect.score << " c " << in_objects[i].class_type << std::endl;

            output_detect_array.objects.push_back(output_detect);

        }
    }

    detect_publish.publish(output_detect_array);
    free(darknet_image_.data);
}

// void YoloNode::config_cb(const autoware_msgs::ConfigSsd::ConstPtr& param)
// {
//     score_threshold_ 	= param->score_threshold;
// }

void mySigintHandler(int sig)
{
  // Do some custom action.
  // For example, publish a stop message to some other nodes.

  // All the default sigint handler does is call shutdown()
  ros::shutdown();
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "dark_ros");
  ROS_INFO("dl interface for ros is now running");
  ros::NodeHandle nh("~");
  signal(SIGINT, mySigintHandler);

  int flag = 1; //temprorily - detection 0, classification 1, segmentation 2
  //Network stuff
  //Here we have to give a condition, if the network chosen in YOLO
  //parameters which are to be converted with yaml

  if(flag==0){
    std::string image_raw_topic_str = "/usb_cam/image_raw", network_definition_file = "/home/ajwahir/ros_dl/src/dl_interface/darknet/cfg/yolo.cfg" ,pretrained_model_file = "/home/ajwahir/ros_dl/src/dl_interface/darknet/data/yolo.weights" ;
    float score_threshold_ = 0.5, nms_threshold_ = 0.45;
    ros::Subscriber subscriber_image_raw_;

    ROS_INFO("Initializing the network on Darknet...");
    yolo_detector_.load(network_definition_file, pretrained_model_file, score_threshold_, nms_threshold_);
    ROS_INFO("Initialization complete.");

    ROS_INFO("Subscribing to... %s", image_raw_topic_str.c_str());
    detect_publish = nh.advertise<dark_msgs::DetectArray>("detected_objects", 1);
    subscriber_image_raw_ = nh.subscribe(image_raw_topic_str, 1, image_callback);
  }
  else if(flag==1){
    std::string image_raw_topic_str = "/usb_cam/image_raw", network_definition_file = "/home/ajwahir/ros_dl/src/dl_interface/darknet/cfg/tiny.cfg" ,pretrained_model_file = "/home/ajwahir/ros_dl/src/dl_interface/darknet/data/tiny.weights" ;
    ros::Subscriber subscriber_image_raw_;

    ROS_INFO("Initializing the network on Darknet...");
    classifier_.load(network_definition_file, pretrained_model_file);
    ROS_INFO("Initialization complete.");

    ROS_INFO("Subscribing to... %s", image_raw_topic_str.c_str());
    // detect_publish = nh.advertise<dark_msgs::DetectArray>("detected_objects", 1);
    // subscriber_image_raw_ = nh.subscribe(image_raw_topic_str, 1, image_callback);
  }

  ros::spin();

  return 0;
}


///////////////////////////////////////////////////////////////////////////////
// quant_traits.hpp
//
//  Copyright 2004 Eric Niebler. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_XPRESSIVE_DETAIL_STATIC_PRODUCTIONS_QUANT_TRAITS_HPP_EAN_10_04_2005
#define BOOST_XPRESSIVE_DETAIL_STATIC_PRODUCTIONS_QUANT_TRAITS_HPP_EAN_10_04_2005

#include <boost/mpl/or.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/xpressive/proto/proto.hpp>
#include <boost/xpressive/detail/detail_fwd.hpp>

#ifdef BOOST_MSVC
# pragma warning(push)
# pragma warning(disable: 4307) // '+' : integral constant overflow
#endif

namespace boost { namespace xpressive { namespace detail
{

    ///////////////////////////////////////////////////////////////////////////////
    // generic_quant_tag
    template<uint_t Min, uint_t Max>
    struct generic_quant_tag
      : proto::unary_tag
    {
        typedef mpl::integral_c<uint_t, Min> min_type;
        typedef mpl::integral_c<uint_t, Max> max_type;
    };

    ///////////////////////////////////////////////////////////////////////////////
    // min_type / max_type
    template<typename Tag>
    struct min_type : Tag::min_type {};

    template<>
    struct min_type<proto::unary_plus_tag> : mpl::integral_c<uint_t, 1> {};

    template<>
    struct min_type<proto::unary_star_tag> : mpl::integral_c<uint_t, 0> {};

    template<>
    struct min_type<proto::logical_not_tag> : mpl::integral_c<uint_t, 0> {};

    template<typename Tag>
    struct max_type : Tag::max_type {};

    template<>
    struct max_type<proto::unary_plus_tag> : mpl::integral_c<uint_t, UINT_MAX-1> {};

    template<>
    struct max_type<proto::unary_star_tag> : mpl::integral_c<uint_t, UINT_MAX-1> {};

    template<>
    struct max_type<proto::logical_not_tag> : mpl::integral_c<uint_t, 1> {};

    struct use_simple_repeat_predicate
    {
        template<typename Op, typename, typename>
        struct apply
          : use_simple_repeat<typename proto::arg_type<Op>::type>
        {
        };
    };

    ///////////////////////////////////////////////////////////////////////////////
    // is_greedy_quant
    template<typename Xpr>
    struct is_greedy_quant
      : mpl::false_
    {
    };

    template<typename Op, typename Tag>
    struct is_greedy_quant<proto::unary_op<Op, Tag> >
      : mpl::or_
        <
            is_same<Tag, proto::unary_plus_tag>
          , is_same<Tag, proto::unary_star_tag>
          , is_same<Tag, proto::logical_not_tag>
        >
    {
    };

    template<typename Op, uint_t Min, uint_t Max>
    struct is_greedy_quant<proto::unary_op<Op, generic_quant_tag<Min, Max> > >
      : mpl::true_
    {
    };

}}}

#ifdef BOOST_MSVC
# pragma warning(pop)
#endif

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 NUMSCALE SAS
//
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_MEMORY_FUNCTIONS_SIMD_SSE_AVX_EXTRACT_HPP_INCLUDED
#define BOOST_SIMD_MEMORY_FUNCTIONS_SIMD_SSE_AVX_EXTRACT_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_AVX_SUPPORT

#include <boost/simd/memory/functions/extract.hpp>
#include <boost/simd/sdk/simd/meta/retarget.hpp>
#include <boost/simd/sdk/meta/cardinal_of.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>
#include <boost/mpl/modulus.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( extract_
                                    , boost::simd::tag::avx_
                                    , (A0)(A1)
                                    , ((simd_< integer_<A0>, boost::simd::tag::avx_ >))
                                      (mpl_integral_< scalar_< integer_<A1> > >)
                                    )
  {
    typedef typename meta::scalar_of<A0>::type   result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1) const
    {
      typedef typename meta::retarget<A0, tag::sse_>::type ltype;
      typedef typename meta::cardinal_of<ltype>::type card;

      ltype that = _mm256_extractf128_si256(a0(), A1::value / ltype::static_size);
      return extract(that, mpl::modulus< A1, card >());
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( extract_
                                    , boost::simd::tag::avx_
                                    , (A0)(A1)
                                    , ((simd_< single_<A0>, boost::simd::tag::avx_ >))
                                      (mpl_integral_< scalar_< integer_<A1> > >)
                                    )
  {
    typedef typename meta::scalar_of<A0>::type   result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1) const
    {
      typedef typename meta::retarget<A0, tag::sse_>::type ltype;
      typedef typename meta::cardinal_of<ltype>::type card;

      ltype that = _mm256_extractf128_ps(a0(), A1::value / ltype::static_size);
      return extract(that, mpl::modulus< A1, card >());
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( extract_
                                    , boost::simd::tag::avx_
                                    , (A0)(A1)
                                    , ((simd_< double_<A0>, boost::simd::tag::avx_ >))
                                      (mpl_integral_< scalar_< integer_<A1> > >)
                                    )
  {
    typedef typename meta::scalar_of<A0>::type   result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1) const
    {
      typedef typename meta::retarget<A0, tag::sse_>::type ltype;
      typedef typename meta::cardinal_of<ltype>::type card;

      ltype that = _mm256_extractf128_pd(a0(), A1::value / ltype::static_size);
      return extract(that, mpl::modulus< A1, card >());
    }
  };
} } }

#endif
#endif


#include "abstract.hpp"

#include "t1p.h"
#include "box.h"

#include <ap_disjunction.h>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <list>
#include <Eigen/Dense>

//static Managers mans{t1p_manager_alloc(), box_manager_alloc()};

ArithExpr::ArithExpr(): expr{nullptr} {}

ArithExpr::ArithExpr(double constant):
  expr{ap_texpr0_cst_scalar_double(constant)} {}

ArithExpr::ArithExpr(int ind): expr{ap_texpr0_dim(ind)} {}

ArithExpr::ArithExpr(double lower, double upper):
  expr{ap_texpr0_cst_interval_double(lower, upper)} {}

ArithExpr::ArithExpr(ap_texpr0_t* e): expr{e} {}

ArithExpr::ArithExpr(const ArithExpr& other):
  expr{ap_texpr0_copy(other.expr)} {}

ArithExpr::ArithExpr(ArithExpr&& other): expr(other.expr) {
  other.expr = nullptr;
}

ArithExpr::~ArithExpr() {
  if (expr != nullptr) {
    ap_texpr0_free(expr);
  }
}

ArithExpr& ArithExpr::operator=(const ArithExpr& other) {
  if (expr != nullptr) {
    ap_texpr0_free(expr);
  }
  expr = ap_texpr0_copy(other.expr);
  return *this;
}

ArithExpr& ArithExpr::operator=(ArithExpr&& other) {
  if (expr != nullptr) {
    ap_texpr0_free(expr);
  }
  expr = other.expr;
  other.expr = nullptr;
  return *this;
}

ArithExpr ArithExpr::negate() const {
  return ArithExpr{ap_texpr0_unop(AP_TEXPR_NEG, ap_texpr0_copy(expr),
        AP_RTYPE_DOUBLE, AP_RDIR_NEAREST)};
}

ArithExpr ArithExpr::operator+(const ArithExpr& other) const {
  return ArithExpr{ap_texpr0_binop(AP_TEXPR_ADD,
      ap_texpr0_copy(expr),
      ap_texpr0_copy(other.expr),
      AP_RTYPE_DOUBLE, AP_RDIR_NEAREST)};
}

ArithExpr ArithExpr::operator-(const ArithExpr& other) const {
  return ArithExpr{ap_texpr0_binop(AP_TEXPR_SUB,
      ap_texpr0_copy(expr),
      ap_texpr0_copy(other.expr),
      AP_RTYPE_DOUBLE, AP_RDIR_NEAREST)};
}

ArithExpr ArithExpr::operator*(const ArithExpr& other) const {
  return ArithExpr{ap_texpr0_binop(AP_TEXPR_MUL,
      ap_texpr0_copy(expr),
      ap_texpr0_copy(other.expr),
      AP_RTYPE_DOUBLE, AP_RDIR_NEAREST)};
}

ArithExpr ArithExpr::operator/(const ArithExpr& other) const {
  return ArithExpr{ap_texpr0_binop(AP_TEXPR_DIV,
      ap_texpr0_copy(expr),
      ap_texpr0_copy(other.expr),
      AP_RTYPE_DOUBLE, AP_RDIR_NEAREST)};
}

ArithExpr ArithExpr::operator^(int power) const {
  // Apron's t1p domain does not support exponents yet, but since we only
  // allow exponents to integer powers we just expand the exponent here.
  ap_texpr0_t* ret = ap_texpr0_copy(this->expr);
  for (int i = 0; i < power - 1; i++) {
    ret = ap_texpr0_binop(AP_TEXPR_MUL,
        ap_texpr0_copy(this->expr),
        ret,
        AP_RTYPE_DOUBLE, AP_RDIR_NEAREST);
  }
  return ArithExpr{ret};
}

LinCons::LinCons(): weights{Eigen::MatrixXd(0, 0)},
  biases{Eigen::VectorXd(0)} {}

LinCons::LinCons(const Eigen::MatrixXd& ws, const Eigen::VectorXd& bs):
  weights{ws}, biases{bs} {}

double LinCons::distance_from(const Eigen::VectorXd& x) const {
  // A x <= b -> A x - b <= 0
  // distance = max(A x - b)
  // If distance is positive then it is the l_infty distance between x and
  // the constrained space. Otherwise, distance decreases as we move away
  // from the safe region.
  return (this->weights * x - this->biases).maxCoeff();
}

inline ap_manager_t* get_manager_from_domain(AbstractDomain dom, size_t size) {
  ap_manager_t* base;
  switch (dom) {
    case AbstractDomain::ZONOTOPE:
      base = t1p_manager_alloc();
      break;
    case AbstractDomain::INTERVAL:
      base = box_manager_alloc();
      break;
    case AbstractDomain::POLYHEDRA:
      base = pk_manager_alloc(false);
      break;
    default:
      throw std::runtime_error("Unrecognized domain in get_manager");
  }
  if (size <= 1) {
    return base;
  }
  return ap_disjunction_manager_alloc(base, NULL);
}

AbstractVal::AbstractVal(): man{nullptr}, value{nullptr} {}

// Note that ap_manager_copy doesn't actually create a copy of m, it only
// increments a reference count, so this copy is not expensive.
AbstractVal::AbstractVal(ap_manager_t* m, ap_abstract0_t* v):
  man{ap_manager_copy(m)}, value{v} {}

AbstractVal::AbstractVal(AbstractDomain dom,
    const std::vector<Eigen::VectorXd>& a,
    const std::vector<double>& b): domain{dom} {
  //if (dom == AbstractDomain::ZONOTOPE) {
  //  man = mans.get_t1p_manager();
  //} else {
  //  man = mans.get_box_manager();
  //}
  man = get_manager_from_domain(dom, 1);
  ap_lincons0_array_t arr = ap_lincons0_array_make(a.size());
  for (size_t i = 0; i < a.size(); i++) {
    ap_linexpr0_t* expr = ap_linexpr0_alloc(AP_LINEXPR_DENSE, a[i].size());
    for (int j = 0; j < a[i].size(); j++) {
      ap_linexpr0_set_coeff_scalar_double(expr, j, -a[i](j));
    }
    ap_linexpr0_set_cst_scalar_double(expr, b[i]);
    ap_lincons0_t cons = ap_lincons0_make(AP_CONS_SUPEQ, expr, NULL);
    arr.p[i] = cons;
  }
  value = ap_abstract0_of_lincons_array(man, 0, a[0].size(), &arr);
  ap_lincons0_array_clear(&arr);
}

AbstractVal::AbstractVal(AbstractDomain dom, const LinCons& lc) {
  man = get_manager_from_domain(dom, 1);
  ap_lincons0_array_t arr = ap_lincons0_array_make(lc.biases.size());
  for (int i = 0; i < lc.biases.size(); i++) {
    ap_linexpr0_t* expr = ap_linexpr0_alloc(AP_LINEXPR_DENSE,
        lc.weights.row(i).size());
    for (int j = 0; j < lc.weights.row(i).size(); j++) {
      ap_linexpr0_set_coeff_scalar_double(expr, j, -lc.weights(i, j));
    }
    ap_linexpr0_set_cst_scalar_double(expr, lc.biases(i));
    ap_lincons0_t cons = ap_lincons0_make(AP_CONS_SUPEQ, expr, NULL);
    arr.p[i] = cons;
  }
  value = ap_abstract0_of_lincons_array(man, 0, lc.weights.row(0).size(), &arr);
  ap_lincons0_array_clear(&arr);
}

AbstractVal::AbstractVal(AbstractDomain dom,
    const Eigen::VectorXd& lowers,
    const Eigen::VectorXd& uppers): domain{dom} {
  //if (dom == AbstractDomain::ZONOTOPE) {
  //  man = mans.get_t1p_manager();
  //} else {
  //  man = mans.get_box_manager();
  //}
  man = get_manager_from_domain(dom, 1);
  ap_interval_t** itv =
    (ap_interval_t**) malloc(lowers.size() * sizeof(ap_interval_t*));
  for (int i = 0; i < lowers.size(); i++) {
    itv[i] = ap_interval_alloc();
    ap_interval_set_double(itv[i], lowers(i), uppers(i));
  }
  value = ap_abstract0_of_box(man, 0, lowers.size(), itv);
  ap_interval_array_free(itv, lowers.size());
}

AbstractVal::AbstractVal(const AbstractVal& other) {
  man = ap_manager_copy(other.man);
  value = ap_abstract0_copy(man, other.value);
}

AbstractVal::AbstractVal(AbstractVal&& other) {
  man = other.man;
  value = other.value;
  other.man = nullptr;
  other.value = nullptr;
}

AbstractVal::~AbstractVal() {
  if (value != nullptr) {
    ap_abstract0_free(man, value);
  }
  if (man != nullptr) {
    // Internally, apron managers are reference counted. If this manager is
    // still used elsewhere (assuming it was copied with ap_manager_copy) the
    // reference count will be decremented but the manager will not be freed.
    ap_manager_free(man);
  }
}

std::unique_ptr<AbstractVal> AbstractVal::add_trailing_dimensions(
    int n) const {
  ap_dimchange_t* dimchange = ap_dimchange_alloc(0, n);
  int d = dims();
  for (int i = 0; i < n; i++) {
    dimchange->dim[i] = d;
  }
  ap_abstract0_t* res = ap_abstract0_add_dimensions(
      man, false, value, dimchange, false);
  ap_dimchange_free(dimchange);
  return this->make_new(res);
}

std::unique_ptr<AbstractVal> AbstractVal::add_leading_dimensions(
    int n) const {
  ap_dimchange_t* dimchange = ap_dimchange_alloc(0, n);
  for (int i = 0; i < n; i++) {
    dimchange->dim[i] = 0;
  }
  ap_abstract0_t* res = ap_abstract0_add_dimensions(
      man, false, value, dimchange, false);
  ap_dimchange_free(dimchange);
  return this->make_new(res);
}

std::unique_ptr<AbstractVal> AbstractVal::remove_trailing_dimensions(
    int n) const {
  // NOTE: t1p_remove_dimensions is buggy, but seems to work for removing
  // one dimension. Therefore, we remove one dimension at a time until n
  // dimensions have been removed.
  ap_abstract0_t* res = ap_abstract0_copy(man, value);
  ap_dimchange_t* dimchange = ap_dimchange_alloc(0, 1);
  for (int i = 0; i < n; i++) {
    int d = ap_abstract0_dimension(man, res).realdim;
    dimchange->dim[0] = d - 1;

    //std::cout << "remove_trailing_dimensions before" << std::endl;
    //this->print(stdout);
    res = ap_abstract0_remove_dimensions(man, true, res, dimchange);
    //std::cout << "remove_trailing_dimensions after" << std::endl;
    //ap_abstract0_fprint(stdout, man, res, NULL);
  }
  ap_dimchange_free(dimchange);
  return this->make_new(res);
}

std::unique_ptr<AbstractVal> AbstractVal::meet_linear_constraint(
    const Eigen::MatrixXd& a,
    const Eigen::VectorXd& b) const {
  int size = b.size();
  ap_lincons0_array_t arr = ap_lincons0_array_make(size);

  // a1 x1 + a2 x2 + ... + an xn <= b ==>
  // -a1 x1 - a2 x2 - ... - an xn + b >= 0
  for (int i = 0; i < size; i++) {
    ap_linexpr0_t* expr = ap_linexpr0_alloc(AP_LINEXPR_DENSE, a.cols());
    for (int j = 0; j < a.cols(); j++) {
      ap_linexpr0_set_coeff_scalar_double(expr, j, -a(i,j));
    }
    ap_linexpr0_set_cst_scalar_double(expr, b(i));
    arr.p[i] = ap_lincons0_make(AP_CONS_SUPEQ, expr, NULL);
  }

  ap_abstract0_t* v = ap_abstract0_meet_lincons_array(man, false, value, &arr);
  ap_lincons0_array_clear(&arr);

  return this->make_new(v);
}

std::unique_ptr<AbstractVal> AbstractVal::scalar_affine(
    const Eigen::MatrixXd& w,
    const Eigen::VectorXd& b) const {
  int in_size = w.cols();
  int out_size = w.rows();

  std::unique_ptr<AbstractVal> v{};
  if (out_size > in_size) {
    v = this->add_trailing_dimensions(out_size - in_size);
  } else {
    v = this->clone();
  }

  ap_dim_t* dims = (ap_dim_t*) malloc(out_size * sizeof(ap_dim_t));
  ap_linexpr0_t** update = (ap_linexpr0_t**) malloc(out_size *
      sizeof(ap_linexpr0_t*));
  for (int j = 0; j < out_size; j++) {
    dims[j] = j;
    update[j] = ap_linexpr0_alloc(AP_LINEXPR_DENSE, in_size);
    for (int k = 0; k < in_size; k++) {
      ap_linexpr0_set_coeff_scalar_double(update[j], k, w(j,k));
    }
    ap_linexpr0_set_cst_scalar_double(update[j], b(j));
  }

  ap_abstract0_t* res = ap_abstract0_assign_linexpr_array(
      man, false, v->get_value(), dims, update, out_size, NULL);

  free(dims);
  for (int j = 0; j < out_size; j++) {
    ap_linexpr0_free(update[j]);
  }
  free(update);

  std::unique_ptr<AbstractVal> ret = this->make_new(res);
  if (in_size > out_size) {
    ret = ret->remove_trailing_dimensions(in_size - out_size);
  }
  return ret;
}

std::unique_ptr<AbstractVal> AbstractVal::interval_affine(
    const Eigen::MatrixXd& wl,
    const Eigen::MatrixXd& wu,
    const Eigen::VectorXd& bl,
    const Eigen::VectorXd& bu) const {
  int in_size = wl.cols();
  int out_size = wl.rows();

  std::unique_ptr<AbstractVal> v{};
  if (out_size > in_size) {
    v = this->add_trailing_dimensions(out_size - in_size);
  } else {
    v = this->clone();
  }

  ap_dim_t* dims = (ap_dim_t*) malloc(out_size * sizeof(ap_dim_t));
  ap_linexpr0_t** update = (ap_linexpr0_t**) malloc(out_size *
      sizeof(ap_linexpr0_t*));
  for (int j = 0; j < out_size; j++) {
    dims[j] = j;
    update[j] = ap_linexpr0_alloc(AP_LINEXPR_DENSE, in_size);
    for (int k = 0; k < in_size; k++) {
      ap_linexpr0_set_coeff_interval_double(update[j], k, wl(j,k), wu(j, k));
    }
    ap_linexpr0_set_cst_interval_double(update[j], bl(j), bu(j));
  }

  ap_abstract0_t* res = ap_abstract0_assign_linexpr_array(
      man, false, v->get_value(), dims, update, out_size, NULL);

  free(dims);
  for (int j = 0; j < out_size; j++) {
    ap_linexpr0_free(update[j]);
  }
  free(update);

  std::unique_ptr<AbstractVal> ret = this->make_new(res);
  if (in_size > out_size) {
    ret = ret->remove_trailing_dimensions(in_size - out_size);
  }
  return ret;

  /*
  // Create an abstract value for the coefficients.
  ap_interval_t** arr = (ap_interval_t**) malloc((in_size + 1) * out_size *
      sizeof(ap_interval_t*));
  for (int i = 0; i < in_size * out_size; i++) {
    arr[i] = ap_interval_alloc();
    int r = i / in_size;
    int c = i % in_size;
    ap_interval_set_double(arr[i], wl(r, c), wu(r, c));
  }
  for (int i = 0; i < out_size; i++) {
    int ind = i + in_size * out_size;
    arr[ind] = ap_interval_alloc();
    ap_interval_set_double(arr[ind], bl(i), bu(i));
  }
  std::unique_ptr<AbstractVal> coeffs = this->make_new(
      ap_abstract0_of_box(
          this->man, 0, (in_size + 1) * out_size, arr));

  std::unique_ptr<AbstractVal> input = this->append(*coeffs);
  // NOTE: the size of input is in_size * out_size (for the coefficients
  // + out_size (for the biases) + in_size (for the input)

  // Construct an ArithExpr for this assignment
  std::vector<ArithExpr> exprs;
  for (int i = 0; i < out_size; i++) {
    // Start with the bias term
    ArithExpr row(in_size * (out_size + 1) + i);
    for (int j = 0; j < in_size; j++) {
      // r = r + coeff(i, j) * x(j)
      row = row + ArithExpr((i + 1) * in_size + j) * ArithExpr(j);
    }
    exprs.push_back(row);
  }

  // Perform computation
  std::unique_ptr<AbstractVal> output = input->arith_computation(exprs);
  return output->remove_trailing_dimensions(in_size * out_size + in_size);
  */
}

std::unique_ptr<AbstractVal> AbstractVal::relu() const {
  std::unique_ptr<AbstractVal> z = this->clone();
  size_t num_dims = this->dims();
  Eigen::VectorXd b{Eigen::VectorXd::Zero(1)};
  Eigen::MatrixXd relu_w = Eigen::MatrixXd::Identity(num_dims, num_dims);
  Eigen::VectorXd relu_b = Eigen::VectorXd::Zero(num_dims);
  for (size_t i = 0; i < num_dims; i++) {
    Eigen::MatrixXd alt = Eigen::MatrixXd::Zero(1, num_dims);
    Eigen::MatrixXd agt = alt;
    alt(0,i) = 1.0;
    agt(0,i) = -1.0;

    std::unique_ptr<AbstractVal> zlt = z->meet_linear_constraint(alt, b);
    z = z->meet_linear_constraint(agt, b);

    relu_w(i, i) = 0.0;
    zlt = zlt->scalar_affine(relu_w, relu_b);
    relu_w(i, i) = 1.0;

    z = z->join(*zlt);
  }

  return z;
}

// NOTE: by convention, the this and other should point to the same manager.
// The manager of this is used, so if it is not compatible with the manager of
// other I'm not sure what happens.
std::unique_ptr<AbstractVal> AbstractVal::join(const AbstractVal& other) const {
  ap_abstract0_t* res = ap_abstract0_join(man, false, value, other.get_value());
  return this->make_new(res);
}

std::unique_ptr<AbstractVal> AbstractVal::meet(const AbstractVal& other) const {
  ap_abstract0_t* res = ap_abstract0_meet(man, false, value, other.get_value());
  return this->make_new(res);
}

std::unique_ptr<AbstractVal> AbstractVal::widen(
    const AbstractVal& other) const {
  ap_abstract0_t* res = ap_abstract0_widening(man, value, other.get_value());
  return this->make_new(res);
}

bool AbstractVal::operator==(const AbstractVal& other) const {
  return ap_abstract0_is_eq(man, value, other.get_value());
}

std::unique_ptr<AbstractVal> AbstractVal::append(const AbstractVal& b) const {
  int n1 = b.dims();
  int n2 = this->dims();
  std::unique_ptr<AbstractVal> p1 = this->add_trailing_dimensions(n1);
  std::unique_ptr<AbstractVal> p2 = b.add_leading_dimensions(n2);
  // At this point p1 has n1 extra unconstrained dimensions at the end and
  // p2 has n2 extra unconstrained dimensions at the beginning. Meeting
  // these two gives the desired result.

  // Apron seems to have a problem doing this meet when the zonotopes have some
  // unconstrained dimensions.
  // return p1->meet(*p2);
  //
  // Our second attempt was to convert both abstract values to arrays of linear
  // constraints, append the two arrays, then create a new value which
  // satisfies all of the resulting constraints. This runs into a very strange
  // use-after-free bug which I haven't been able to track down.
  //
  // The current strategy is to convert only b to an array of linear
  // constraints and to meet this with the resulting array.

  ap_lincons0_array_t lc2 = ap_abstract0_to_lincons_array(
      p2->get_manager(), p2->get_value());

  ap_abstract0_t* v = ap_abstract0_meet_lincons_array(
      man, false, p1->get_value(), &lc2);

  ap_lincons0_array_clear(&lc2);

  return this->make_new(v);
}

std::unique_ptr<AbstractVal> AbstractVal::arith_computation(
    const std::vector<ArithExpr>& exprs) const {
  int in_size = this->dims();
  int out_size = exprs.size();
  std::unique_ptr<AbstractVal> inp;
  if (out_size > in_size) {
    inp = this->add_trailing_dimensions(out_size - in_size);
  } else {
    inp = this->clone();
  }
  ap_texpr0_t** arr =
    (ap_texpr0_t**) malloc(exprs.size() * sizeof(ap_texpr0_t*));
  ap_dim_t* dims = (ap_dim_t*) malloc(exprs.size() * sizeof(dims));
  for (size_t i = 0; i < exprs.size(); i++) {
    arr[i] = ap_texpr0_copy(exprs[i].get_texpr());
    dims[i] = i;
  }
  ap_abstract0_t* v = ap_abstract0_assign_texpr_array(
      man, false, inp->get_value(), dims, arr, exprs.size(), NULL);
  free(dims);
  for (size_t i = 0; i < exprs.size(); i++) {
    ap_texpr0_free(arr[i]);
  }
  free(arr);
  std::unique_ptr<AbstractVal> ret = this->make_new(v);
  if (in_size > out_size) {
    ret = ret->remove_trailing_dimensions(in_size - out_size);
  }
  return ret;
}

bool AbstractVal::contains_point(const Eigen::VectorXd& x) const {
  ap_interval_t** itv = (ap_interval_t**) malloc(
      x.size() * sizeof(ap_interval_t*));
  for (int i = 0; i < x.size(); i++) {
    itv[i] = ap_interval_alloc();
    ap_interval_set_double(itv[i], x(i), x(i));
  }
  ap_abstract0_t* point = ap_abstract0_of_box(man, 0, x.size(), itv);
  ap_interval_array_free(itv, x.size());
  ap_abstract0_t* meet = ap_abstract0_meet(man, false, value, point);
  ap_abstract0_free(man, point);
  bool bottom = ap_abstract0_is_bottom(man, meet);
  ap_abstract0_free(man, meet);
  return !bottom;
}

bool AbstractVal::contains(const AbstractVal& x) const {
  // Convert this to an array of linear constraints
  ap_lincons0_array_t arr = ap_abstract0_to_lincons_array(man, value);

  // Negate each constraint
  for (size_t i = 0; i < arr.size; i++) {
    ap_linexpr0_t* expr = arr.p[i].linexpr0;
    for (size_t j = 0; j < x.dims(); j++) {
      ap_coeff_t* c = ap_linexpr0_coeffref(expr, j);
      ap_coeff_neg(c, c);
    }
    ap_coeff_t* c = ap_linexpr0_cstref(expr);
    ap_coeff_neg(c, c);
  }

  // Meet the new constraints with x
  ap_abstract0_t* v = ap_abstract0_meet_lincons_array(man, false, value, &arr);
  ap_lincons0_array_clear(&arr);

  // Since the new constraints contain everything NOT in this, if the meet is
  // not empty, then x is not entirely contained within this.
  bool bottom = ap_abstract0_is_bottom(man, v);
  ap_abstract0_free(man, v);
  return bottom;
}

Eigen::VectorXd AbstractVal::get_center() const {
  ap_interval_t** bbox = ap_abstract0_to_box(man, value);
  Eigen::VectorXd center(dims());
  for (size_t i = 0; i < dims(); i++) {
    double l, u;
    ap_double_set_scalar(&l, bbox[i]->inf, MPFR_RNDN);
    ap_double_set_scalar(&u, bbox[i]->sup, MPFR_RNDN);
    center(i) = (l + u) / 2.0;
  }
  return center;
}

Eigen::VectorXd AbstractVal::get_contained_point() const {
  ap_interval_t** bbox = ap_abstract0_to_box(man, value);
  Eigen::VectorXd lower(this->dims());
  Eigen::VectorXd upper(this->dims());
  for (size_t i = 0; i < dims(); i++) {
    double l, u;
    ap_double_set_scalar(&l, bbox[i]->inf, MPFR_RNDN);
    ap_double_set_scalar(&u, bbox[i]->sup, MPFR_RNDN);
    lower(i) = l;
    upper(i) = u;
  }
  while (true) {
    // Choose a random point inside the bounding box of this abstract value,
    // then check to see if it is contained in this value. If it is we return
    // it, otherwise try another point.
    Eigen::VectorXd rand = Eigen::VectorXd::Random(dims());
    for (size_t i = 0; i < this->dims(); i++) {
      rand(i) = (1 + rand(i)) * (upper(i) - lower(i)) / 2.0 + lower(i);
    }
    if (this->contains_point(rand)) {
      return rand;
    }
  }
}

std::unique_ptr<AbstractVal> AbstractVal::clone() const {
  return std::make_unique<AbstractVal>(man, ap_abstract0_copy(man, value));
}

std::unique_ptr<AbstractVal> AbstractVal::bottom() const {
  return std::make_unique<AbstractVal>(man, ap_abstract0_bottom(man,
        0, this->dims()));
}

LinCons AbstractVal::get_lincons() const {
  ap_lincons0_array_t res = ap_abstract0_to_lincons_array(man, value);
  int d = dims();
  std::vector<Eigen::VectorXd> ws;
  std::vector<double> bs;
  for (int i = 0; i < res.size; i++) {
    Eigen::VectorXd r(d);
    ap_linexpr0_t* exp = res.p[i].linexpr0;
    for (int j = 0; j < d; j++) {
      ap_coeff_t* c = ap_linexpr0_coeffref(exp, j);
      if (c->discr != AP_COEFF_SCALAR) {
        throw std::runtime_error("Non-scalar coefficient in get_lincons");
      }
      double d;
      ap_double_set_scalar(&d, c->val.scalar, MPFR_RNDN);
      r(j) = d;
    }
    ap_coeff_t* c = ap_linexpr0_cstref(exp);
    if (c->discr != AP_COEFF_SCALAR) {
      throw std::runtime_error("Non-scalar coefficient in get_lincons");
    }
    double d;
    ap_double_set_scalar(&d, c->val.scalar, MPFR_RNDN);
    switch (res.p[i].constyp) {
      case AP_CONS_EQ:
        ws.push_back(r);
        bs.push_back(-d);
        ws.push_back(-r);
        bs.push_back(d);
        break;
      case AP_CONS_SUPEQ:
      case AP_CONS_SUP:
        ws.push_back(-r);
        bs.push_back(d);
        break;
      default:
        throw std::runtime_error("Unknown constraint type in get_lincons");
    }
  }
  ap_lincons0_array_clear(&res);
  Eigen::MatrixXd weights(ws.size(), d);
  Eigen::VectorXd biases(bs.size());
  for (int i = 0; i < ws.size(); i++) {
    weights.row(i) = ws[i];
    biases(i) = bs[i];
  }
  return LinCons(weights, biases);
}

std::unique_ptr<AbstractVal> AbstractVal::make_new(ap_abstract0_t* a) const {
  return std::make_unique<AbstractVal>(man, a);
}

//double distance_to_abstract0(ap_manager_t* man, ap_abstract0_t* a,
//    const Eigen::VectorXd& x) {
//  // Convert the abstract value to an array of linear constraints
//  ap_lincons0_array_t arr = ap_abstract0_to_lincons_array(man, a);
//  // Determine whether each constraint is satisfied and find the constraint
//  // which is closest to x
//  bool satisfies_all = true;
//  for (int i = 0; i < arr.size; i++) {
//    // TODO: I'm not sure what the semantics of arr.p[i].scalar are. It seems
//    // to only be used for EQMOD constraints.
//    ap_linexpr0_t* expr = arr.p[i].linexpr0;
//    Eigen::VectorXd coeffs(x.size());
//    for (int j = 0; j < x.size(); j++) {
//      ap_coeff_t* c = ap_linexpr0_coeffref(expr, j);
//      if (c->discr == AP_COEFF_SCALAR) {
//        // TODO
//      } else {
//        // TODO
//      }
//    }
//  }
//  return 0;
//}
//
//double AbstractVal::distance_to_point(const Eigen::VectorXd& x) const {
//  return distance_to_abstract0(this->man, this->value, x);
//}

typedef struct {
  ap_abstract0_t* abs;
  Eigen::VectorXd center;
} abstract_value;

Eigen::VectorXd compute_center(ap_manager_t* man, ap_abstract0_t* a) {
  ap_interval_t** itv = ap_abstract0_to_box(man, a);
  int d = ap_abstract0_dimension(man, a).realdim;
  Eigen::VectorXd center(d);
  for (int i = 0; i < d; i++) {
    double l, u;
    ap_double_set_scalar(&l, itv[i]->inf, MPFR_RNDN);
    ap_double_set_scalar(&u, itv[i]->sup, MPFR_RNDN);
    center(d) = (l + u) / 2.0;
  }
  ap_interval_array_free(itv, d);
  return center;
}

// Given a manager (this is a manager for the disjunction domain) and a
// disjunctive element, reduce the number of disjuncts to size. This is done
// in place so that after this call, the value pointed to by a is a disjunctive
// value with an appropriate number of disjuncts. Note that this function does
// not perform any checks and assumes a is a disjunction. This function
// typically returns a reference to a, but not always.
ap_abstract0_t* merge_disjuncts(ap_manager_t* man, ap_abstract0_t* a,
    size_t size) {
  ap_disjunction_t* ad = (ap_disjunction_t*) a->value;
  size_t s = ad->size;
  // Get the manager for the underlying abstract domain
  ap_disjunction_internal_t* in = (ap_disjunction_internal_t*) man->internal;
  ap_manager_t* under = in->manager;

  // Compute the center of the bounding box of each disjunct
  std::list<abstract_value> vals{};
  bool is_top = false;
  for (size_t i = 0; i < s; i++) {
    abstract_value v;
    ap_abstract0_t* abs = (ap_abstract0_t*) ad->p[i];
    if (ap_abstract0_is_top(under, abs)) {
      is_top = true;
      break;
    } else if (ap_abstract0_is_bottom(under, abs)) {
      // Don't add bottom elements to vals
      continue;
    }
    v.abs = ap_abstract0_copy(under, (ap_abstract0_t*) ad->p[i]);
    v.center = compute_center(under, v.abs);
    vals.push_back(v);
  }
  if (is_top) {
    int d = ap_abstract0_dimension(man, a).realdim;
    ap_abstract0_free(man, a);
    return ap_abstract0_top(man, 0, d);
  } else if (vals.size() == 0) {
    int d = ap_abstract0_dimension(man, a).realdim;
    ap_abstract0_free(man, a);
    return ap_abstract0_bottom(man, 0, d);
  }

  if (s <= size) {
    return a;
  }

  while (vals.size() > size) {
    // Find the two elements whose centers (computed by bounding box) are
    // closest to each other.
    std::list<abstract_value>::iterator it1;
    std::list<abstract_value>::iterator it2;
    double best_dist = std::numeric_limits<double>::max();
    for (auto i = vals.begin(); i != vals.end(); i++) {
      for (auto j = vals.begin(); j != vals.end(); j++) {
        double dist = (i->center - j->center).norm();
        if (dist < best_dist) {
          best_dist = dist;
          it1 = i;
          it2 = j;
        }
      }
    }
    // Join these two elements.
    ap_abstract0_t* a1 = it1->abs;
    ap_abstract0_t* a2 = it2->abs;
    ap_abstract0_t* join = ap_abstract0_join(under, false, a1, a2);
    abstract_value av;
    av.abs = join;
    av.center = compute_center(under, join);
    // Remove the two elements from vals and add the new one.
    vals.erase(it1);
    vals.erase(it2);
    vals.push_back(av);
  }

  // Finally, we replace the values in a with those in vals.
  for (size_t i = 0; i < s; i++) {
    ap_abstract0_free(under, (ap_abstract0_t*) ad->p[i]);
  }
  free(ad->p);
  ad->size = size;
  ad->p = (void**) malloc(size * sizeof(void*));
  int ind = 0;
  for (auto it = vals.begin(); it != vals.end(); it++) {
    ad->p[ind] = it->abs;
    ind++;
  }
  return a;
}

void AbstractVal::print(FILE* out) const {
  ap_abstract0_fprint(out, man, value, NULL);
}

Powerset::Powerset(ap_manager_t* m, ap_abstract0_t* v, size_t s):
  AbstractVal{m, v}, size{s} {}

Powerset::Powerset(const Powerset& p): AbstractVal{p}, size{p.size} {}

Powerset::Powerset(AbstractDomain dom, size_t s,
    const std::vector<Eigen::VectorXd>& a, const std::vector<double>& b) {
  //ap_manager_t* base_manager;
  //if (dom == AbstractDomain::ZONOTOPE) {
  //  base_manager = mans.get_t1p_manager();
  //} else {
  //  base_manager = mans.get_box_manager();
  //}
  //man = mans.get_disj_manager(base_manager);
  man = get_manager_from_domain(dom, s);
  size = s;
  ap_lincons0_array_t arr = ap_lincons0_array_make(a.size());
  for (size_t i = 0; i < a.size(); i++) {
    ap_linexpr0_t* expr = ap_linexpr0_alloc(AP_LINEXPR_DENSE, a[i].size());
    for (int j = 0; j < a[i].size(); j++) {
      ap_linexpr0_set_coeff_scalar_double(expr, j, -a[i](j));
    }
    ap_linexpr0_set_cst_scalar_double(expr, b[i]);
    ap_lincons0_t cons = ap_lincons0_make(AP_CONS_SUPEQ, expr, NULL);
    arr.p[i] = cons;
  }
  value = ap_abstract0_of_lincons_array(man, 0, a[0].size(), &arr);
  ap_lincons0_array_clear(&arr);
}

Powerset::Powerset(AbstractDomain dom, size_t s, const LinCons& lc) {
  man = get_manager_from_domain(dom, s);
  ap_lincons0_array_t arr = ap_lincons0_array_make(lc.biases.size());
  for (int i = 0; i < lc.biases.size(); i++) {
    ap_linexpr0_t* expr = ap_linexpr0_alloc(AP_LINEXPR_DENSE,
        lc.weights.row(i).size());
    for (int j = 0; j < lc.weights.row(i).size(); j++) {
      ap_linexpr0_set_coeff_scalar_double(expr, j, -lc.weights(i, j));
    }
    ap_linexpr0_set_cst_scalar_double(expr, lc.biases(i));
    ap_lincons0_t cons = ap_lincons0_make(AP_CONS_SUPEQ, expr, NULL);
    arr.p[i] = cons;
  }
  value = ap_abstract0_of_lincons_array(man, 0, lc.weights.row(0).size(), &arr);
  ap_lincons0_array_clear(&arr);
}

Powerset::Powerset(AbstractDomain dom, size_t s,
    const Eigen::VectorXd& lowers, const Eigen::VectorXd& uppers) {
  domain = dom;
  //ap_manager_t* base_manager;
  //if (dom == AbstractDomain::ZONOTOPE) {
  //  base_manager = mans.get_t1p_manager();
  //} else {
  //  base_manager = mans.get_box_manager();
  //}
  //man = mans.get_disj_manager(base_manager);
  man = get_manager_from_domain(dom, s);
  ap_interval_t** itv =
    (ap_interval_t**) malloc(lowers.size() * sizeof(ap_interval_t*));
  for (int i = 0; i < lowers.size(); i++) {
    itv[i] = ap_interval_alloc();
    ap_interval_set_double(itv[i], lowers(i), uppers(i));
  }
  value = ap_abstract0_of_box(man, 0, lowers.size(), itv);
  ap_interval_array_free(itv, lowers.size());
}

Powerset& Powerset::operator=(const Powerset& other) {
  size = other.size;
  man = other.man;
  value = ap_abstract0_copy(man, other.value);
  return *this;
}

std::unique_ptr<AbstractVal> Powerset::join(const AbstractVal& other) const {
  std::unique_ptr<AbstractVal> res = this->AbstractVal::join(other);
  ap_manager_t* m = res->get_manager();
  ap_abstract0_t* a = res->get_value();
  a = merge_disjuncts(m, a, size);
  ap_abstract0_t* ra = (ap_abstract0_t*) malloc(sizeof(ap_abstract0_t));
  ra->value = a;
  ra->man = m;
  return this->make_new(ra);
}

std::unique_ptr<AbstractVal> Powerset::meet(const AbstractVal& other) const {
  std::unique_ptr<AbstractVal> res = this->AbstractVal::meet(other);
  ap_manager_t* m = res->get_manager();
  ap_abstract0_t* a = res->get_value();
  a = merge_disjuncts(m, a, size);
  ap_abstract0_t* ra = (ap_abstract0_t*) malloc(sizeof(ap_abstract0_t));
  ra->value = a;
  ra->man = m;
  return this->make_new(ra);
}

std::unique_ptr<AbstractVal> Powerset::arith_computation(
    const std::vector<ArithExpr>& exprs) const {
  std::unique_ptr<AbstractVal> res =
    this->AbstractVal::arith_computation(exprs);
  ap_manager_t* m = res->get_manager();
  ap_abstract0_t* a = res->get_value();
  a = merge_disjuncts(m, a, size);
  ap_abstract0_t* ra = (ap_abstract0_t*) malloc(sizeof(ap_abstract0_t));
  ra->value = a;
  ra->man = m;
  return this->make_new(ra);
}

Eigen::VectorXd Powerset::get_contained_point() const {
  ap_disjunction_t* ad = (ap_disjunction_t*) value->value;
  size_t s = ad->size;
  // Get the manager for the underlying abstract domain
  ap_disjunction_internal_t* in = (ap_disjunction_internal_t*) man->internal;
  ap_manager_t* under = in->manager;
  if (s == 0) {
    return Eigen::VectorXd(0);
  }
  ap_abstract0_t* disjunct = (ap_abstract0_t*) ad->p[0];
  return compute_center(under, disjunct);
}

std::unique_ptr<AbstractVal> Powerset::clone() const {
  return std::make_unique<Powerset>(man, ap_abstract0_copy(man, value), size);
}

std::unique_ptr<AbstractVal> Powerset::bottom() const {
  return std::make_unique<Powerset>(man, ap_abstract0_bottom(man,
        0, this->dims()), this->size);
}

std::unique_ptr<AbstractVal> Powerset::make_new(ap_abstract0_t* a) const {
  return std::make_unique<Powerset>(man, a, size);
}