#include "Behaviors/StateMachine.h"

$nodeclass CC : doStart {
    NEW_SKETCH(cam_frame, uchar, sketchFromSeg());
    NEW_SKETCH(red_stuff, bool, visops::colormask(cam_frame, "red"));
    NEW_SKETCH(blobs, usint, visops::labelcc(red_stuff));

    for(unsigned int i = 1; i < blobs->max(); ++i) {
        NEW_SKETCH(labeli, bool, blobs == i);
        //labeli = blobs == i;
        int area = 0;
        int count = 0;
        int x_sum = 0;
        int y_sum = 0;
        int xstart = labeli->findMinPlus() % labeli.width;
        int ystart = labeli->findMinPlus() / labeli.width;
        int lbound = labeli.width;
        int rbound = xstart;
        int tbound = ystart;
        int bbound = ystart;
        for(int y = ystart; y < labeli.height; ++y) {
            for(int x = 0; x < labeli.width; ++x) {
                if(labeli(x, y)) {
                    if(x < lbound) {
                        lbound = x;
                    } else if(x > rbound) {
                        rbound = x;
                    }
                    if(y > bbound) {
                        bbound = y;
                    }
                    x_sum += x;
                    y_sum += y;
                    ++area;
                    ++count;
                }
            }
        }
        if(count == 0) {
            continue;
        }

        float major;
        float minor;
        if(bbound - tbound > rbound - lbound) {
            major = bbound - tbound;
            minor = rbound - lbound;
        } else {
            major = rbound - lbound;
            minor = bbound - tbound;
        }

        float center_x = x_sum/((float)count);
        float center_y = y_sum/((float)count);
        if(center_x <= 430 && center_x >= 370 && center_y <= 260 && center_y >= 220) {
            cout << i << " ----------------------" << endl;
            cout << lbound << ", " << rbound << ", " << tbound << ", " << bbound << endl;
            cout << "area: " << area << endl;
            cout << center_x << ", " << center_y << endl;
            cout << "aspect: " << major/minor << endl;
        }
        //aspect ratio filter
        if(major/minor > 3) {
            //cout << i << ": aspect ratio " << major/minor << endl;
            continue;
        }
        //area filter
        if((major*minor)/area > 2) {
            //cout << i << ": area " << (major*minor)/area << endl;
            continue;
        }
        NEW_SHAPE(circle, EllipseData, new EllipseData(camShS,
                                            Point(x_sum/((float)count), y_sum/((float)count), 0),
                                            major/2, major/2));
        circle->setColor("red");
    }

    cout << "end doStart" << endl;
}

REGISTER_BEHAVIOR(CC);