detector.cpp

/*
 * detector.cpp
 *
 *  Created on: 19.10.2016
 *      Author: pzima
 */

//#define KOMPILACJA_NA_SERWERZE

#include "detector.h"

detector::detector(detector_config cfg)
{
    this->config = cfg;
    thr = std::thread(processing, this);
}

void detector::push_task(std::string task)
{
    task_mutex.lock();

    this->tasks.push(task);

    task_mutex.unlock();
}

std::string detector::pop_task()
{
    std::string task;
    task_mutex.lock();
    if(!tasks.empty())
    {
        task = tasks.front();
        tasks.pop();
    }
    else
    {
        task = std::string("null");
    }

    task_mutex.unlock();
    return task;
}

void detector::processing(void *ptr)
{

    detector* cPtr = (detector*)ptr;

    std::string task;
    while(true)
    {
        task = cPtr->pop_task();
        if(task != std::string("null"))
        {
            std::cout << "Task : " << task << std::endl;
            std::string erf_filename(cPtr->config.echo_raw_dir + task + ".erf");
            if(cPtr->wrapper.open(erf_filename, OPEN_READ))
            {
                cPtr->HEIGHT = cPtr->wrapper.get_ping_size();
                cPtr->WIDTH  = std::min(cPtr->wrapper.get_c0Length(), cPtr->wrapper.get_c1Length());
                cPtr->RATE   = cPtr->wrapper.get_rate();

                cPtr->c_0 = new int8_t *[cPtr->WIDTH]; cPtr->c_1 = new int8_t *[cPtr->WIDTH];
                for(int i = 0; i < cPtr->WIDTH; i++)
                {
                    cPtr->c_0[i] = new int8_t[cPtr->HEIGHT];
                    cPtr->c_1[i] = new int8_t[cPtr->HEIGHT];
                }

                cPtr->wrapper.get_pings(cPtr->c_0, cPtr->c_1, 0, cPtr->WIDTH);

                cPtr->cut_background(cPtr->config.cut_off_db_thresh);
                cPtr->scan(0);
                cPtr->scan(1);

                std::cout << cPtr->objects_0.size() << std::endl;
                std::cout << cPtr->objects_1.size() << std::endl;

                cPtr->compare_objects();
                cPtr->check_objects();
                std::string png_filename(cPtr->config.echo_png_dir + task + ".png");
                cPtr->get_png(png_filename);

                if(cPtr->pairs.size() > 0)
                {
                    int id = cPtr->prepare_database();

                    cPtr->prepare_echograms(png_filename, id);
                    cPtr->prepare_videos(task, id);
                }


                cPtr->objects_0.clear();
                cPtr->objects_1.clear();
                cPtr->pairs.clear();
                cPtr->wrapper.close();
                for(int i = 0; i < cPtr->WIDTH; i++)
                {
                    delete [] cPtr->c_0[i];
                    delete [] cPtr->c_1[i];
                }
                delete [] cPtr->c_0;
                delete [] cPtr->c_1;
            }
        }
        else
        {
            std::cout << "Brak zadan..." << std::endl;
        }

        usleep(1000000);
    }
}

void detector::cut_background(int db_thresh)
{
    for(int y = config.y_0_offset; y < config.y_1_offset; y++)
    {
        float back_probe_0 = 0.0f;
        float back_probe_1 = 0.0f;

        for(int x = 0; x < WIDTH; x++)
        {
            back_probe_0 += c_0[x][y];
            back_probe_1 += c_1[x][y];
        }

        back_probe_0 /= WIDTH;
        back_probe_1 /= WIDTH;

        for(int x = 0; x < WIDTH; x++)
        {
            if(c_0[x][y] < back_probe_0 + db_thresh) c_0[x][y] = BACKGROUND;
            if(c_1[x][y] < back_probe_1 + db_thresh) c_1[x][y] = BACKGROUND;
        }
    }
}

void detector::scan(int ch)
{
    bool peaks[WIDTH][HEIGHT];

    for(int y = config.y_0_offset; y < config.y_1_offset; y++)
    {
        for(int x = 0; x < WIDTH; x++)
        {
            peaks[x][y] = false;
        }
    }

    if(ch == 0)
    {
        for(int y = config.y_0_offset; y < config.y_1_offset; y++)
        {
            for(int x = 0; x < WIDTH; x++)
            {
                if(c_0[x][y] > BACKGROUND + config.scan_db_thresh && !peaks[x][y])
                {
                    int dimensions_tab[4];
                    dimensions_tab[0] = x; dimensions_tab[1] = x;
                    dimensions_tab[2] = y; dimensions_tab[3] = y;

                    if(flood_fish(x, y, dimensions_tab, 0, BACKGROUND + config.scan_db_thresh))
                    {
                        for(int pX = dimensions_tab[0]; pX < dimensions_tab[1]; pX++)
                        {
                            for(int pY = dimensions_tab[2]; pY < dimensions_tab[3]; pY++)
                            {
                                peaks[pX][pY] = true;
                            }
                        }
                        object obj;
                        obj.x_0 = dimensions_tab[0];
                        obj.x_1 = dimensions_tab[1];
                        obj.y_0 = dimensions_tab[2];
                        obj.y_1 = dimensions_tab[3];
                        objects_0.push_back(obj);
                    }
                }
            }
        }
    }
    else if(ch == 1)
    {
        for(int y = config.y_0_offset; y < config.y_1_offset; y++)
        {
            for(int x = 0; x < WIDTH; x++)
            {
                if(c_1[x][y] > BACKGROUND + config.scan_db_thresh && !peaks[x][y])
                {
                    int dimensions_tab[4];
                    dimensions_tab[0] = x; dimensions_tab[1] = x;
                    dimensions_tab[2] = y; dimensions_tab[3] = y;

                    if(flood_fish(x, y, dimensions_tab, 1, BACKGROUND + config.scan_db_thresh))
                    {
                        for(int pX = dimensions_tab[0]; pX < dimensions_tab[1]; pX++)
                        {
                            for(int pY = dimensions_tab[2]; pY < dimensions_tab[3]; pY++)
                            {
                                peaks[pX][pY] = true;
                            }
                        }
                        object obj;
                        obj.x_0 = dimensions_tab[0];
                        obj.x_1 = dimensions_tab[1];
                        obj.y_0 = dimensions_tab[2];
                        obj.y_1 = dimensions_tab[3];
                        objects_1.push_back(obj);
                    }
                }
            }
        }
    }
}

bool detector::flood_fish(int x, int y, int *dimensions_tab, int ch, int db_thresh)
{
    if(ch == 0)
    {
        if(x < 1 || y < config.y_0_offset || x > (WIDTH - 1) || y > config.y_1_offset || c_0[x][y] < db_thresh || c_0[x][y] > -1) { return false; }
        else
        {
            c_0[x][y] = c_0[x][y] * (-1);
            if(x < dimensions_tab[0]) { dimensions_tab[0] = x; }
            if(x > dimensions_tab[1]) { dimensions_tab[1] = x; }
            if(y < dimensions_tab[2]) { dimensions_tab[2] = y; }
            if(y > dimensions_tab[3]) { dimensions_tab[3] = y; }

            for(int _x = -10; _x <= 10; _x++)
            {
                for(int _y = -5; _y <= 5; _y++)
                {
                    if((x + _x) > 0 && (x + _x) < WIDTH && (y + _y) > config.y_0_offset && (y + _y) < config.y_1_offset)
                    {
                        flood_fish(x + _x, y + _y, dimensions_tab, ch, db_thresh);
                    }
                }
            }
            return true;
        }
    }
    else if(ch == 1)
    {
        if(x < 1 || y < config.y_0_offset || x > (WIDTH - 1) || y > config.y_1_offset || c_1[x][y] < db_thresh || c_1[x][y] > -1) { return false; }
        else
        {
            c_1[x][y] = c_1[x][y] * (-1);
            if(x < dimensions_tab[0]) { dimensions_tab[0] = x; }
            if(x > dimensions_tab[1]) { dimensions_tab[1] = x; }
            if(y < dimensions_tab[2]) { dimensions_tab[2] = y; }
            if(y > dimensions_tab[3]) { dimensions_tab[3] = y; }

            for(int _x = -10; _x <= 10; _x++)
            {
                for(int _y = -5; _y <= 5; _y++)
                {
                    if((x + _x) > 0 && (x + _x) < WIDTH && (y + _y) > config.y_0_offset && (y + _y) < config.y_1_offset)
                    {
                        flood_fish(x + _x, y + _y, dimensions_tab, ch, db_thresh);
                    }
                }
            }
            return true;
        }
    }
    else return false;
}

void detector::compare_objects()
{
    /* wyrzuc zbyt male  i dwa razy wyzsze niz dluzsze*/
    objects_0.erase(std::remove_if(objects_0.begin(), objects_0.end(), [](object o){
        if(o.x_1 - o.x_0 <= 3){ return true; }
        if(o.y_1 - o.y_0 <= 2){ return true; }
        //if(fabs(o.y_1 - o.y_0) > 2 * fabs(o.x_1 - o.x_0)){ return true; }
        return false;
    }), objects_0.end());
    objects_1.erase(std::remove_if(objects_1.begin(), objects_1.end(), [](object o){
        if(o.x_1 - o.x_0 <= 3){ return true; }
        if(o.y_1 - o.y_0 <= 2){ return true; }
        //if(fabs(o.y_1 - o.y_0) > 2 * fabs(o.x_1 - o.x_0)){ return true; }
        return false;
    }), objects_1.end());
    /* wyrzuc obiekty niegeste < 15%*/
    objects_0.erase(std::remove_if(objects_0.begin(), objects_0.end(), [=](object o){
        int iter = 0;
        for(int x = o.x_0; x < o.x_1; x++)
        {
            for(int y = o.y_0; y < o.y_1; y++)
            {
                if(c_0[x][y] > 0) iter++;
            }
        }
        int WH = (o.x_1 - o.x_0) * (o.y_1 - o.y_0);
        if((float)(iter) / (float)(WH) < config.min_density) { return true; }
        return false;
    }), objects_0.end());
    objects_1.erase(std::remove_if(objects_1.begin(), objects_1.end(), [=](object o){
        int iter = 0;
        for(int x = o.x_0; x < o.x_1; x++)
        {
            for(int y = o.y_0; y < o.y_1; y++)
            {
                if(c_1[x][y] > 0) iter++;
            }
        }
        int WH = (o.x_1 - o.x_0) * (o.y_1 - o.y_0);
        if((float)(iter) / (float)(WH) < config.min_density) { return true; }
        return false;
    }), objects_1.end());

    /* zrob pary */
    if(objects_0.size() < objects_1.size() || objects_0.size() == objects_0.size())
    {
        for(uint i = 0; i < objects_0.size(); i++)
        {
            float x_0 = (objects_0[i].x_0 + objects_0[i].x_1) / 2;
            float y_0 = (objects_0[i].y_0 + objects_0[i].y_1) / 2;

            for(uint j = 0; j < objects_1.size(); j++)
            {
                float x_1 = (objects_1[j].x_0 + objects_1[j].x_1) / 2;
                float y_1 = (objects_1[j].y_0 + objects_1[j].y_1) / 2;
                float distance = std::sqrt(std::pow(x_1 - x_0, 2) + std::pow(y_1 - y_0, 2));
                if(distance < config.max_dist)
                {
                    p para;
                    para.o_0 = objects_0[i];
                    para.o_1 = objects_1[j];
                    pairs.push_back(para);
                }
            }
        }
    }
    else
    {
        for(uint i = 0; i < objects_1.size(); i++)
        {
            float x_0 = (objects_1[i].x_0 + objects_1[i].x_1) / 2;
            float y_0 = (objects_1[i].y_0 + objects_1[i].y_1) / 2;

            for(uint j = 0; j < objects_0.size(); j++)
            {
                float x_1 = (objects_0[j].x_0 + objects_0[j].x_1) / 2;
                float y_1 = (objects_0[j].y_0 + objects_0[j].y_1) / 2;
                float distance = std::sqrt(std::pow(x_1 - x_0, 2) + std::pow(y_1 - y_0, 2));
                if(distance < config.max_dist)
                {
                    p para;
                    para.o_0 = objects_0[i];
                    para.o_1 = objects_1[j];
                    pairs.push_back(para);
                }
            }
        }
    }
}

void detector::check_objects()
{
    for(uint i = 0; i < pairs.size(); i++)
    {
        /* c_0 */
        for(int x = pairs[i].o_0.x_0; x < pairs[i].o_0.x_1; x++)
        {
            c_0[x][pairs[i].o_0.y_0] = -5;
            c_0[x][pairs[i].o_0.y_1] = -5;
        }
        for(int y = pairs[i].o_0.y_0; y < pairs[i].o_0.y_1; y++)
        {
            c_0[pairs[i].o_0.x_0][y] = -5;
            c_0[pairs[i].o_0.x_1][y] = -5;
        }
        /* c_1 */
        for(int x = pairs[i].o_1.x_0; x < pairs[i].o_1.x_1; x++)
        {
            c_1[x][pairs[i].o_1.y_0] = -5;
            c_1[x][pairs[i].o_1.y_1] = -5;
        }
        for(int y = pairs[i].o_1.y_0; y < pairs[i].o_1.y_1; y++)
        {
            c_1[pairs[i].o_1.x_0][y] = -5;
            c_1[pairs[i].o_1.x_1][y] = -5;
        }
    }
}

void detector::get_png(std::string filename)
{
    const int R_PAL[120] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,17,38,56,79,100,127,151,181,208,226,233,240,248,255,255,255,255,255};
    const int G_PAL[120] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,2,6,12,19,28,37,50,62,78,87,95,102,109,117,124,131,138,146,153,160,168,175,182,189,197,204,211,219,212,197,175,157,132,108,78,54,24};
    const int B_PAL[120] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7,15,22,29,36,44,51,58,66,73,80,81,80,74,66,60,49,40,26,14,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};

    cv::Mat echogram(HEIGHT * 2, WIDTH, CV_8UC3, cv::Scalar(0,0,0));

    for(int y = 0; y < HEIGHT; y++)
    {
        for(int x = 0; x < WIDTH; x++)
        {
            int pwr;
            if(c_0[x][y] < 0){ pwr = 120 - (c_0[x][y] * (-1)); }
            else { pwr = 120 - (c_0[x][y]); }
            if(pwr < 0) pwr = 0;
            if(pwr > 120) pwr = 119;

            echogram.at<cv::Vec3b>(y,x)[0] = B_PAL[pwr];
            echogram.at<cv::Vec3b>(y,x)[1] = G_PAL[pwr];
            echogram.at<cv::Vec3b>(y,x)[2] = R_PAL[pwr];
        }
    }
    for(int y = 0; y < HEIGHT; y++)
    {
        for(int x = 0; x < WIDTH; x++)
        {
            int pwr;
            if(c_1[x][y] < 0){ pwr = 120 - (c_1[x][y] * (-1)); }
            else { pwr = 120 - (c_1[x][y]); }
            if(pwr < 0) pwr = 0;
            if(pwr > 120) pwr = 119;

            echogram.at<cv::Vec3b>(y + HEIGHT,x)[0] = B_PAL[pwr];
            echogram.at<cv::Vec3b>(y + HEIGHT,x)[1] = G_PAL[pwr];
            echogram.at<cv::Vec3b>(y + HEIGHT,x)[2] = R_PAL[pwr];
        }
    }
    cv::imwrite(filename, echogram);
}

char* detector::insert_record(std::string datetime, float width, float height, float speed, int direction)
{
    char systemCall[255];
    sprintf(systemCall, "./addons/insert2 1 1 %f %f %i %f %s 2>&1", height, width, direction, speed, datetime.c_str());

    std::cout << datetime.c_str() << " dodaje rybke. H = " << height << " W = " << width << " S = " << speed << std::endl;

    char *output = new char[255];
    FILE * stream;
    const int max_buffer = 256;
    char buffer[max_buffer];

    stream = popen(systemCall, "r");
    if(stream)
    {
        while(!feof(stream))
        {
            if(fgets(buffer, max_buffer, stream) != NULL)
            {
                sprintf(output, "%s", buffer);
            }
        }
        pclose(stream);
    }
    return output;
}

std::string detector::get_datetime(long timestamp)
{
    std::stringstream date_str;
    boost::posix_time::ptime pt_1 = boost::posix_time::from_time_t(timestamp);
    boost::gregorian::date d = pt_1.date();
    auto td = pt_1.time_of_day();
    date_str << d.year() << "-" << std::setw(2) << std::setfill('0') << d.month().as_number() << "-" << std::setw(2) << std::setfill('0') << d.day() << " " << std::setw(2) << std::setfill('0') << td.hours() << ":" << std::setw(2) << std::setfill('0') << td.minutes() << ":"  << std::setw(2) << std::setfill('0') << td.seconds();

    return date_str.str();
}

int detector::prepare_database()
{
    int firstId = 0;
    for(uint i = 0; i < pairs.size(); i++)
    {
        /* string date, float objWidth, float objHeight, float objSpeed, float objDirection */
        int center_0    = (int)((pairs[i].o_0.x_0 + pairs[i].o_0.x_1) / 2);
        int center_1    = (int)((pairs[i].o_1.x_0 + pairs[i].o_1.x_1) / 2);
        int width_0     = pairs[i].o_0.x_1 - pairs[i].o_0.x_0;
        int width_1     = pairs[i].o_1.x_1 - pairs[i].o_1.x_0;
        int direction   = 0;

        float time      = fabs((center_1 - center_0) / RATE);
        float speed     = 0.1 / time;
        float width     = (((width_0 + width_1) / 2) / RATE) * speed;
        float height    = width / 3;

        if(center_0 > center_1){ direction = 1; }

        /* licz date */
        time_t t    = std::time(NULL);
        int center  = (int)((center_0 + center_1) / 2);
        int deltaT  = (int)((WIDTH - center) / 100);
        t           = t - deltaT + 7200;
        std::string datetime = get_datetime(t);

        if(i == 0)
        {
            firstId = atoi(insert_record(datetime, width, height, speed, direction));
        }
        else
        {
            insert_record(datetime, width, height, speed, direction);
        }
    }
    return firstId;
}

void detector::prepare_echograms(std::string filename, int id)
{
    cv::Mat image;
    image = cv::imread(filename, CV_LOAD_IMAGE_COLOR);

    for(uint i = 0; i < pairs.size(); i++)
    {
        cv::Rect roi_0;
        cv::Rect roi_1;
        if(pairs[i].o_0.x_0 < 100)
        {
            roi_0.x       = 0;
            roi_0.y       = 0;
            roi_0.width   = 200;
            roi_0.height  = HEIGHT;

            roi_1.x       = 0;
            roi_1.y       = HEIGHT;
            roi_1.width   = 200;
            roi_1.height  = HEIGHT;
        }
        else if(pairs[i].o_0.x_0 > WIDTH - 100)
        {
            roi_0.x       = WIDTH - 200;
            roi_0.y       = 0;
            roi_0.width   = 200;
            roi_0.height  = HEIGHT;

            roi_1.x       = WIDTH - 200;
            roi_1.y       = HEIGHT;
            roi_1.width   = 200;
            roi_1.height  = HEIGHT;
        }
        else
        {
            roi_0.x       = pairs[i].o_0.x_0 - 100;
            roi_0.y       = 0;
            roi_0.width   = 200;
            roi_0.height  = HEIGHT;

            roi_1.x       = pairs[i].o_0.x_0 - 100;
            roi_1.y       = HEIGHT;
            roi_1.width   = 200;
            roi_1.height  = HEIGHT;
        }
        cv::Mat echo_0 = image(roi_0);
        cv::Mat echo_1 = image(roi_1);

        cv::imwrite(std::string(config.fish_echo_dir + std::to_string(i + id) + "_0.png"), echo_0);
        cv::imwrite(std::string(config.fish_echo_dir + std::to_string(i + id) + "_1.png"), echo_1);
    }
}

void detector::prepare_videos(std::string filename, int id)
{
    std::vector<std::string> videos;
    for(int i = 0; i < config.x_cams; i++)
    {
        videos.push_back(std::string(config.video_dir + filename + "_" + std::to_string(i) + ".mp4"));
    }
    int V_LEN = (int)(WIDTH / 4);

    if(pairs.size() > 0)
    {
        bool **FRAMES;
        FRAMES = new bool *[pairs.size()];
        for(uint i = 0; i < pairs.size(); i++)
        {
            FRAMES[i] = new bool[V_LEN];
        }

        for(uint i = 0; i < pairs.size(); i++)
        {
            for(int j = 0; j < V_LEN; j++)
            {
                FRAMES[i][j] = false;
            }
        }

        for(uint i = 0; i < pairs.size(); i++)
        {
            int startX = 0;
            int stopX = 0;
            if(pairs[i].o_0.x_0 < 1000)
            {
                startX  = 0;
                stopX   = 500;
            }
            else if(pairs[i].o_0.x_0 > WIDTH - 1000)
            {
                startX  = (int)((WIDTH - 2000) / 4);
                stopX   = (int)(WIDTH / 4);
            }
            else
            {
                startX = (int)((pairs[i].o_0.x_0 - 1000) / 4);
                stopX = (int)((pairs[i].o_0.x_0 + 1000) / 4);
            }
            //for(int j = startX; j < startX + 500; j++)
            for(int j = startX; j < stopX; j++)
            {
                FRAMES[i][j] = true;
            }
        }
        std::thread thr[config.x_cams];
        for(int CAM = 0; CAM < config.x_cams; CAM++)
        {
            thr[CAM] = std::thread(prepare_videos_thr, pairs.size(), id, videos[CAM], CAM, FRAMES, config, pairs);
        }
        for(int CAM = 0; CAM < config.x_cams; CAM++)
        {
            thr[CAM].join();
        }

        for(uint i = 0; i < pairs.size(); i++)
        {
            delete [] FRAMES[i];
        }
        delete [] FRAMES;
    }
}

void detector::prepare_videos_thr(uint count_obj, int id, std::string video_name, int thr_id, bool **FRAMES, detector_config cfg, std::vector<p> pairs)
{
    cv::VideoWriter *writer[count_obj];
    for(uint i = 0; i < pairs.size(); i++)
    {
        writer[i] = new cv::VideoWriter(std::string(cfg.fish_video_dir + std::to_string(i + id) + "_" + std::to_string(thr_id) + ".avi"),
                                        CV_FOURCC('M','P','4','2'), 25, cvSize(704,576), 1);
    }
    int iter = 0;

    cv::VideoCapture cap_0(video_name);
    cv::Mat frame, resized_frame, mog_frame;
#ifdef KOMPILACJA_NA_SERWERZE
    cv::Ptr<cv::BackgroundSubtractor> mog = cv::createBackgroundSubtractorMOG2(50, 16, false);
#endif
    bool frame_empty = false;
    while(!frame_empty)
    {
        iter++;
        cap_0.read(frame);
        for(uint i = 0; i < pairs.size(); i++)
        {
            if(FRAMES[i][iter])
            {
                writer[i]->write(frame);
                /* gif */
#ifdef KOMPILACJA_NA_SERWERZE
                cv::resize(frame, resized_frame, cv::Size(cfg.gif_width, cfg.gif_height));
                mog->apply(resized_frame, mog_frame);

                if(cv::countNonZero(mog_frame) > cfg.gif_threshold)
                {
                    cv::imwrite(resized_frame, std::string(cfg.gif_temp_frames_dir + std::to_string(i + id) + "/" + ));
                }
#endif
            }
        }
        if(frame.empty()) { frame_empty = true; }
    }
}