CropFilter.hpp

#ifndef CLOUDLIB_CROPFILTER_HPP_
#define CLOUDLIB_CROPFILTER_HPP_

#include <pcl/common/transforms.h>
#include <pcl/point_types.h>

#include "ofxGui.h"
#include "ofxOsc.h"
#include "ofxTimeMeasurements.h"

#include "ofEasingParameterFloat.hpp"

#include "ofxCloudLib/ParsedParameterGroup.hpp"

namespace ofxCloudLib
{

class CropFilterParameterGroup : public ofxCloudLib::ParsedParameterGroup
{
  public:
    CropFilterParameterGroup() : ParsedParameterGroup()
    {
        setName("Crop Filter");
        add(enabled_.set("enable", false));
        add(pos_.set("position", glm::vec3(-10), glm::vec3(-10), glm::vec3(10)));
        add(dim_.set("dimension", glm::vec3(.01), glm::vec3(.01), glm::vec3(10)));
        add(ease_dim_.set("ease dim", glm::vec3(.01), glm::vec3(.01), glm::vec3(10)));
        add(slide_down_.set("filter (down)", .5, 0, 1));
        add(capacity_.set("capacity", 200, 1, 5000));
        add(count_.set("count", 0, 0, 1000));
        add(rms_.set("rms", 0, 0, 1));
        add(height_.set("height", 0, 0, 1));
        add(thresh_.set("thresh", 0.06, 0.06, .5));

        dim_.addListener(this, &CropFilterParameterGroup::ease);

        add_to_string_mapping(&(ofAbstractParameter &)pos_);
        add_to_string_mapping(&(ofAbstractParameter &)dim_);
    }

    auto ease(glm::vec3 &to) -> void
    {
        
        ease_dim_.ease(to);
    }

    void force_trigger(ofEvent<ofAbstractParameter> event)
    {
        ofNotifyEvent(event, pos_);
        ofNotifyEvent(event, dim_);
    }

    void reset()
    {
        pos_        = glm::vec3(0, 0, .5);
        dim_.set(glm::vec3(.5, .5, 1));
        capacity_   = 200;
        slide_down_ = .5;
        count_      = 0;
        enabled_    = false;
    }

    ofParameter<float> slide_down_;
    ofEasingParameterVec3 ease_dim_;
    ofParameter<glm::vec3> pos_;
    ofParameter<glm::vec3> dim_;
    ofParameter<bool> state_;
    ofParameter<bool> newstate_;
    ofParameter<float> thresh_;
    ofParameter<bool> enabled_;
    ofParameter<float> capacity_;
    ofParameter<int> count_;
    ofParameter<float> rms_;
    ofParameter<float> height_;
};

class CropFilter
{
  public:
    CropFilter(int i, int total = 8)
    {
        index_ = i;
        result_.reset(new pcl::PointCloud<pcl::PointXYZ>);
        color_.setHsb((float(i) / float(total)) * 255, 255, 128, 200);
        material_.setAmbientColor(ofColor(20, 20, 20, 0));
        material_.setSpecularColor(ofColor(0, 0, 0, 0));
    }

    float get_fill()
    {
        auto fill = ofClamp(parameters_.count_.get() / parameters_.capacity_.get(), 0, 1);
        fill_ = fill;
        if (fill < 0.02) {
            fill_ = 0;
        } else {
            fill_ = fill;
        }
        return fill_;

    }

    float get_pan() {
        return ofMap(parameters_.pos_->x, -4, 4, 0, 1);
    }

    void draw_floor(ofColor color, float size, std::string label)
    {
        ofPushStyle();
        ofSetColor(color);
        ofDrawBox(parameters_.pos_.get().x, 0, parameters_.pos_.get().z, size, .01, size);
        ofSetColor(ofColor::white);
        ofDrawBitmapString(label, parameters_.pos_.get().x, .25, parameters_.pos_.get().z);
        ofPopStyle();
    }

    virtual void draw()                       = 0;
    virtual bool apply_rule(pcl::PointXYZ &p) = 0;

    void perform(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud)
    {
        if (parameters_.enabled_ && input_cloud->size() > 0) {

            result_->clear();
            parameters_.height_ = 0;
            float new_height    = 0;

            // apply_rule from here

            // perhaps only on param_changes? (setup_rules()?)
            min_ = parameters_.pos_.get() - (parameters_.ease_dim_.get() / 2);
            max_ = parameters_.pos_.get() + (parameters_.ease_dim_.get() / 2);
            // end perhaps

            for (int i = 0; i < input_cloud->size(); i++) {
                auto p = input_cloud->points[i];
                if (apply_rule(p)) {
                    result_->push_back(p);

                    // perhaps another loop outside apply_rule?
                    if (p.y > new_height) new_height = ofMap(p.y, min_.y, max_.y, 0, 1, true);
                }
            }
            // end apply_rule

            if (new_height > parameters_.height_) {
                parameters_.height_ = new_height;
            } else {
                parameters_.height_ = new_height * parameters_.slide_down_ + parameters_.height_ * (1.0 - parameters_.slide_down_);
            }

            if (!parameters_.state_) {
                if (get_fill() > parameters_.thresh_ + .03) {
                    parameters_.newstate_ = true;
                }
            } else {
                if (get_fill() < parameters_.thresh_ - .03) {
                    parameters_.newstate_ = false;
                    parameters_.height_   = 0;
                }
            }

            if (result_->size() > parameters_.count_) {
                parameters_.count_ = result_->size();
            } else {
                parameters_.count_ = result_->size() * parameters_.slide_down_ + parameters_.count_ * (1.0 - parameters_.slide_down_);
            }

        } else {
            parameters_.newstate_ = false;
            parameters_.count_    = 0;
            parameters_.height_   = 0;
        }
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr get()
    {
        return result_;
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr result_;
    CropFilterParameterGroup parameters_;
    glm::vec3 min_, max_;
    ofColor color_;
    ofParameter<float> fill_;
    size_t index_;
    float distance_ = 10000;
    ofMaterial material_;
};

class CropBoxFilter : public CropFilter
{
  public:
    CropBoxFilter(int i, int total = 8) : CropFilter(i, total){};

    void draw() override
    {
        glm::vec3 size = parameters_.ease_dim_.get();

        ofPushStyle();
        ofEnableBlendMode(OF_BLENDMODE_ADD);
        ofDisableDepthTest();
        if (parameters_.enabled_) {

            ofSetColor(color_);
            ofNoFill();
            ofDrawBox(parameters_.pos_, size.x, size.y, size.z);
            ofFill();

            auto c = color_;
            c.a    = ofMap(get_fill(), 0, .25, 10, 50, true);

            if (parameters_.count_ > 10) {
                glm::vec3 fillsize = parameters_.ease_dim_.get() * glm::vec3(1, get_fill(), 1);
                glm::vec3 fillpos  = parameters_.pos_.get() - glm::vec3(0, (size.y * (1.0 - get_fill())) / 2, 0);
                ofSetColor(c);
                ofDrawBox(fillpos, fillsize.x, fillsize.y, fillsize.z);
            }
            /*
            if (parameters_.rms_ > 0) {
                glm::vec3 fillsize = parameters_.dim_.get() * glm::vec3(1, parameters_.rms_, 1);
                glm::vec3 fillpos  = parameters_.pos_.get() - glm::vec3(0, (size.y * (1.0 - parameters_.rms_)) / 2, 0);
                ofSetColor(255, 0, 0, ofMap(parameters_.rms_, 0, .25, 10, 150, true));
                ofDrawBox(fillpos, fillsize.x, fillsize.y, fillsize.z);
            }
            */
            if (parameters_.state_) {
                c.a = 128;
            } else {
                c.a = 64;
            }
            ofSetColor(c);
            ofDrawBox(parameters_.pos_->x, 0, parameters_.pos_->z, size.x, .01, size.z);


        } else {
            ofSetColor(200, 200, 200, 128);
            ofNoFill();
            ofDrawBox(parameters_.pos_, size.x, size.y, size.z);
            ofFill();
        }
        ofPopStyle();
    }

    bool apply_rule(pcl::PointXYZ &p) override
    {
        return (p.x > min_[0] && p.y > min_[1] && p.z > min_[2] &&
                p.x < max_[0] && p.y < max_[1] && p.z < max_[2]);
    }
};

class CropCylinderFilter : public CropFilter
{
  public:
    CropCylinderFilter(int i, int total = 8) : CropFilter(i, total){};

    void draw() override
    {
        glm::vec3 size = parameters_.ease_dim_.get();

        ofPushStyle();
        ofEnableBlendMode(OF_BLENDMODE_ADD);
        ofDisableDepthTest();
        if (parameters_.enabled_) {
            ofSetColor(color_);
            ofNoFill();
            // ofDrawBox(parameters_.pos_, size.x, size.y, size.z);
            ofDrawCylinder(parameters_.pos_->x, parameters_.pos_->y, parameters_.pos_->z, size.x * .707, size.y);
            ofFill();

            auto c = color_;
            c.a    = 128; //ofMap(get_fill(), 0, .1, 0, 1, true);


            if (parameters_.count_ > 0) {
                glm::vec3 fillsize = parameters_.ease_dim_.get() * glm::vec3(1, get_fill(), 1);
                glm::vec3 fillpos  = parameters_.pos_.get() - glm::vec3(0, (size.y * (1.0 - get_fill())) / 2, 0);
                ofSetColor(c);
                //  ofDrawBox(fillpos, fillsize.x, fillsize.y, fillsize.z);
                material_.setDiffuseColor(c);
                material_.begin();
                ofDrawCylinder(fillpos, fillsize.x * .707, fillsize.y);
                material_.end();
            }
            if (parameters_.rms_ > 0) {
                /*
                glm::vec3 fillsize = parameters_.dim_.get() * glm::vec3(1, parameters_.rms_, 1);
                glm::vec3 fillpos  = parameters_.pos_.get() - glm::vec3(0, (size.y * (1.0 - parameters_.rms_)) / 2, 0);
                ofSetColor(255, 0, 0, ofMap(parameters_.rms_, 0, .25, 10, 150, true));
                ofDrawBox(fillpos, fillsize.x, fillsize.y, fillsize.z);
                */
            }
            if (parameters_.state_) {
                ofSetColor(color_);
                material_.setDiffuseColor(color_);

            } else {
                auto c = color_;
                c.a    = 128;
                ofSetColor(color_);
            }
            material_.setDiffuseColor(color_);
            material_.begin();
            ofDrawCylinder(parameters_.pos_->x, parameters_.pos_->y, parameters_.pos_->z, size.x * .707, size.y);
            material_.end();

        } else {
            /*
            //    a mettre dans un debug quelconque?
            ofSetColor(200, 200, 200, 128);
            ofNoFill();
            ofDrawBox(parameters_.pos_, size.x, size.y, size.z);
            ofFill();
            */
        }
        ofPopStyle();
    }

    bool apply_rule(pcl::PointXYZ &p) override
    {
        // THIS FOR BOX!
        return (p.x > min_[0] && p.y > min_[1] && p.z > min_[2] &&
                p.x < max_[0] && p.y < max_[1] && p.z < max_[2]);
    }
};

class FilterManager
{
  public:
    FilterManager(size_t num_filters, ofxOscSender *sender)
    {
        sender_ = sender;

        parameters_.setName("FilterManager");
        parameters_.add(enabled_.set("enabled", true));

        for (size_t i = 0; i < num_filters; i++) {

            // TODO: figure out parametrisation of type (same type for all manager?)
            filters_.push_back(new CropBoxFilter(i));
            filters_.back()->parameters_.setName("Filter #" + ofToString(i));
            parameters_.add(filters_.back()->parameters_);
        }

        // uniquement pour presets!
        zones_gui_.setup();
        zones_gui_.add(parameters_);
    }

    void save_preset_id(int num)
    {
        ofLogNotice("filter_manager") << "saving preset" << ofToString(num) << ".json";
        zones_gui_.saveToFile("presets/zones/preset" + ofToString(num) + ".json");
        ofJson json;
        ofSerialize(json, parameters_);
        ofSaveJson("presets/zones/preset" + ofToString(num) + "-group.json", json);
    }

    void load_preset_id(int num)
    {
        ofLogNotice("filter_manager") << "loading preset" << ofToString(num) << ".json";
        zones_gui_.loadFromFile("presets/zones/preset" + ofToString(num) + ".json");


        for (const auto &filter : filters_) {
            zones_gui_.getGroup(parameters_.getName()).getGroup(filter->parameters_.getName()).minimize();
        }
    }

    void clear_preset_id(int num) {
        ofFile::removeFile("presets/zones/preset" + ofToString(num) + ".json");
    }

    bool is_there_such_a_preset_id(int num) {
        return ofFile::doesFileExist("presets/zones/preset" + ofToString(num) + ".json");
    }

    void update(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud)
    {

        if (enabled_) {
            TS_START("FilterManager::update()");

            ofxOscMessage m, m_height;
            m.setAddress("/filter_manager/counts");
            m_height.setAddress("/filter_manager/height");
            for (const auto &filter : filters_) {
                filter->perform(input_cloud);

                if (filter->parameters_.state_ != filter->parameters_.newstate_) {
                    excitated_ = true;
                    ofxOscMessage m2;
                    m2.setAddress("/filter_manager/event/state");
                    m2.addIntArg(filter->index_);
                    m2.addIntArg(filter->parameters_.newstate_);
                    sender_->sendMessage(m2);
                    filter->parameters_.state_ = filter->parameters_.newstate_;

                    // // exception hard-codee: si une zone ext est released avant la int, releasera la int
                    // if (filter->index_ > =8 && filter->parameters_.state_ == false) {
                    //     filters_[filter->index_-1].newstate_ = false;
                    // }
                }

                //      m.addIntArg(filter->parameters_.count_);
                if (excitated_) {
                    m_height.addFloatArg(filter->parameters_.height_);
                }
                m.addFloatArg(filter->get_fill());
            }
            sender_->sendMessage(m);
            //      sender_->sendMessage(m_height);
            TS_STOP("FilterManager::update()");
        }
    }

    void process_osc(ofxOscMessage &m)
    {
        if (m.getAddress() == "/filter_manager/filter/set") {
            int filter_num = m.getArgAsInt(0);
            if (filter_num >= 0 && filter_num < filters_.size()) {
                auto filter                     = filters_[filter_num];
                filter->parameters_.pos_        = glm::vec3(m.getArgAsFloat(1), m.getArgAsFloat(2), m.getArgAsFloat(3));
                filter->parameters_.dim_.set(glm::vec3(m.getArgAsFloat(4), m.getArgAsFloat(5), m.getArgAsFloat(6)));
                filter->parameters_.slide_down_ = m.getArgAsFloat(7);
                filter->parameters_.thresh_     = m.getArgAsFloat(8);
                filter->parameters_.capacity_   = m.getArgAsInt(9);
            }
        } else if (m.getAddress() == "/filter_manager/filter/enable") {
            int filter_num = m.getArgAsInt(0);

            if (filter_num >= 0 && filter_num < filters_.size()) {
                auto filter                  = filters_[filter_num];
                filter->parameters_.enabled_ = m.getArgAsBool(1);
            }

        } else if (m.getAddress() == "/filter_manager/rms") {

            for (int i = 0; i < 4; i++) {
                auto filter              = filters_[i];
                filter->parameters_.rms_ = 5 * (m.getArgAsFloat(i));
            }

        } else if (m.getAddress() == "/filter_manager/enable") {
            ofLogNotice("crop_manager") << "filter enable";

            enabled_ = m.getArgAsInt(0);
        } else if (m.getAddress() == "/filter_manager/save") {
            save_preset_id(m.getArgAsInt(0));
        } else if (m.getAddress() == "/filter_manager/load") {
            load_preset_id(m.getArgAsInt(0));
        }
    }
    std::vector<CropFilter *> &get_filters() { return filters_; }

    void draw()
    {
        if (enabled_) {

            ofPushMatrix();
            ofPopStyle();
            ofEnableLighting();
            for (const auto &filter : filters_) {
                filter->draw();
            }
            ofDisableLighting();

            ofPopStyle();
            ofPopMatrix();
        }
    }

    void draw_floor(size_t index, ofColor color, float size, std::string label)
    {
        filters_[index]->draw_floor(color, size, label);
    }
    void draw_gui()
    {
        zones_gui_.draw();
    }

    ofParameterGroup parameters_;
    ofParameter<bool> enabled_;
    ofxPanel zones_gui_;

  private:
    bool excitated_ = false;
    std::vector<CropFilter *> filters_;
    ofxOscSender *sender_;
};
}
#endif // CLOUDLIB_CROPFILTER_HPP_