int main()
{
    //int arr[] = {16, 5, 3, 2, 1, 4, 7};
    //int arr[] = {16, 14, 13, 4, 6, 8, 4, 6};
    int arr[] = {6, 5, 4, 3, 2, 1, 6, 5, 4, 3, 2, 1, 5};
    //int arr[] = {0,1,2,3,4,5,6,1,2,3,6,1,2,3,6,1,2,3};
    
    std::vector<int> bar_heights (arr,
                                  arr + (sizeof(arr)/sizeof(arr[0])));

    std::cout << get_holding_capacity(bar_heights) << std::endl;

    return 0;
}
int DetectorBarcode::Detect() {
    std::cout << "DetectorBarcode::Detect()" << std::endl;

    assert(this->image_.data != NULL);

    bool debugstripecode = true; //@TODO MAKE SURE TO SET ME TO FALSE IN PRODUCTION
    bool useAdaptiveThersholding = true;

    int dpi = 400; //this works well for all scales and sizes..

    Mat matImageK;
    cvtColor(this->image_, matImageK, cv::COLOR_BGR2GRAY);
    cv::Mat matThres;

    // VARIABLES //
    double bar_height_mm_min = 3.7; //[7.5mm=our NMNH c39] [10.7mm=NMNH cover c39]
    double bar_height_mm_max = 20;

    double bar_ar_min = 4;
    double bar_ar_max = 110;

    int min_characters = 5; //minimum characters in barcode string

    double bar_dist_group_mm_max = 9.0; //Maximum distance between any grouped bar to be part of the bar group

    // COMPUTE //
    double bar_height_px_min = bar_height_mm_min/25.4*dpi;
    double bar_height_px_max = bar_height_mm_max/25.4*dpi;

    double bar_area_px_min = bar_height_px_min*(bar_height_px_min*1.0/bar_ar_max);

    //Dont allow the area to be less than 1px row
    bar_area_px_min = bar_area_px_min < bar_height_px_min ? bar_height_px_min : bar_area_px_min;

    double bar_area_px_max = bar_height_px_max*(bar_height_px_max*1.0/bar_ar_min);

    double bar_dist_group_px_max = bar_dist_group_mm_max/25.4*dpi;

    if (useAdaptiveThersholding) {
        //int AT_blocksize = dpi*0.05; 
        int AT_blocksize = bar_height_px_min*0.5;
        int AT_iseven=AT_blocksize%2;
        AT_blocksize += 1+AT_iseven; //Makes sure the blocksize is an even number
        //cout << "AT_blocksize=" << AT_blocksize << endl;
        adaptiveThreshold(matImageK, matThres, 255, ADAPTIVE_THRESH_MEAN_C, THRESH_BINARY, AT_blocksize, 20);
    } else {
        threshold(matImageK, matThres, 127, 255, THRESH_BINARY_INV);
    }

    if (debugstripecode) {
        //cout << "dpi=" << dpi << endl;
        imwrite("/Users/tzaman/Desktop/bc/matImage.tif", this->image_);
        imwrite("/Users/tzaman/Desktop/bc/matThres.tif", matThres);
    }

    vector< vector<Point> > contours;
    vector<Vec4i> hierarchy;
    findContours( matThres, contours, hierarchy, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE );
    
    //cout << "contours.size()=" << contours.size() << endl;
    
    if (contours.size() == 0) {
        string strErr = "No contours found.";
        cout << strErr << endl;
        return RET_NONE_FOUND;
    }

    //RANSAC vars
    int min_inliers = (min_characters+2)*5*0.75; //+2 (start&stop), *5 (stripes per char), *0.x (margin)
    double max_px_dist = (bar_height_px_min+bar_height_px_max)*0.5*0.05; //Maximum distance from RANSAC line to a point

    vector<RotatedRect> stripeCandidates;
    for(int i = 0; i >= 0; i = hierarchy[i][0] ) {
        double cArea = contourArea( contours[i],false); 
        if (cArea < bar_area_px_min*0.5){
            continue;
        }
        if (cArea > bar_area_px_max){
            continue;
        }
        //cout << "[" << i << "]" << " cArea=" << cArea << endl;

        RotatedRect rotRect= minAreaRect(contours[i]);
        double ar = max(double(rotRect.size.width),double(rotRect.size.height)) / min(double(rotRect.size.width),double(rotRect.size.height));

        if (ar < bar_ar_min){
            continue;
        }
        if (ar > bar_ar_max){
            continue;
        }

        double width = std::min(rotRect.size.width, rotRect.size.height);
        double height = std::max(rotRect.size.width, rotRect.size.height);

        //Check the length
        if (height < bar_height_px_min){
            //Stripe too small
            continue;
        }

        if (height > bar_height_px_max ){
            //Stripe too long
            continue;
        }


        //cout << i << " rotRect: sz=" << rotRect.size << " cp=" << rotRect.center << " a=" << rotRect.angle << " ar=" << ar << endl;


        Rect rCrop = boundingRect(contours[i]);

        //Below parameter is dynamic, plz note
        double min_area_fill=0.15;// = 0.25 ;// 0.4 means 40% of the bounding rectangle of the contour needs to be filled
        //The min_area_fill threshold should be dependent on the width in pixels, because there's more noise in thinner ones
        if (width<3){
            min_area_fill = 0.05;
        } else if (width <5){
            min_area_fill = 0.10;
        }


        //Check if the rectangle is actually filled well
        int fullarea = rCrop.area();

        
        if ( (double(cArea)/double(fullarea)) < min_area_fill){
            continue;
        }

        //cout << i << " fullarea=" << fullarea << " carea=" << cArea << endl;

        if (debugstripecode){
            imwrite("/Users/tzaman/Desktop/seg/" + std::to_string(i) +  ".tif", matImageK(rCrop));
        }
        stripeCandidates.push_back(rotRect);
    }


    if (debugstripecode){
        Mat matBarcodeFull = this->image_.clone();
        for (int j=0; j<stripeCandidates.size(); j++){
            util::rectangle(matBarcodeFull, stripeCandidates[j], cv::Scalar(255,0,0), 2);
        }
        imwrite("/Users/tzaman/Desktop/bc/_candidates.tif", matBarcodeFull);
    }


    //cout << "stripeCandidates.size()=" << stripeCandidates.size() << endl;

    if (stripeCandidates.size() < min_inliers){
        string strErr = "Code 39 did not find enough bars to accurately make a code.";
        cout << strErr << endl;
        return RET_NONE_FOUND;
    }
    
    std::vector<Point> vecPtRectCenter = util::vecrotrect2vecpt(stripeCandidates);
    std::vector<std::vector<int> > vecGroupIdxs = util::groupPoints(vecPtRectCenter, bar_dist_group_px_max, min_inliers);
    //std::vector<std::vector<cv::Point> > vecGroupPts(vecGroupIdxs.size());
    std::vector<std::vector<cv::RotatedRect> > vecGroupRects(vecGroupIdxs.size());

    //Relate indexes to points and add to group vector
    for (int i=0; i<vecGroupIdxs.size(); i++){
        //vecGroupPts[i].resize(vecGroupIdxs[i].size());
        vecGroupRects[i].resize(vecGroupIdxs[i].size());
        for (int j=0; j<vecGroupIdxs[i].size(); j++){
            //cout << i << "," << j << endl;
            //vecGroupPts[i][j] = vecPtRectCenter[vecGroupIdxs[i][j]];
            vecGroupRects[i][j] = stripeCandidates[vecGroupIdxs[i][j]];
        }
    }

    //Draw all groups
    //if(debugstripecode){
    //  for (int i=0; i<vecGroupPts.size(); i++){
    //      Mat matGroup = matImage.clone();
    //      for (int j=0; j<vecGroupPts[i].size(); j++){
    //          circle(matGroup, vecGroupPts[i][j], 5, Scalar(255,0,255), 1, CV_AA,0);              
    //      }
    //      imwrite("/Users/tzaman/Desktop/bc/_group_" + std::to_string(i) + ".tif", matGroup);
    //  }
    //}
    //exit(-1);

    //cout << "vecGroupPts.size()=" << vecGroupPts.size() << endl;
    //Erase small groups
    //for (int i=vecGroupPts.size()-1; i>=0; i--){
    //  if (vecGroupPts[i].size() < min_inliers){
    //      //Skipping group, too small.
    //      vecGroupIdxs.erase(vecGroupIdxs.begin()+i);
    //      vecGroupPts.erase(vecGroupPts.begin()+i);
    //  }
    //}
    //cout << "vecGroupPts.size()=" << vecGroupPts.size() << endl;

    if (vecGroupIdxs.size() == 0) {
        string strErr = "Code 39 failed to ransac bars in a line.";
        cout << strErr << endl;
        return RET_NONE_FOUND;
    }

    //Now cycle over the groups
    vector<vector<int> > vecVecInlierIdx;
    vector<Vec4f> vecLines;
    vector<int> vecFromGroup; //Keeps track of which group the vecvecInlierIdx belongs to
    for (int i = 0; i < vecGroupRects.size(); i++) {
        Ransac(vecGroupRects[i], min_inliers, max_px_dist, vecVecInlierIdx, vecLines, this->image_);
        vecFromGroup.resize(vecVecInlierIdx.size(), i);
    }

    if (vecLines.size() == 0) {
        string strErr = "Code 39 failed to ransac bars in a line.";
        cout << strErr << endl;
        return RET_NONE_FOUND;
    } else {
        //cout << "Code39 ransac succesfull" << endl;
    }

    //for (int i=0; i<vecGroupIdxs.size(); i++){
    //  cout << "Group " << i << " (" << vecGroupIdxs[i].size() << ") : ";
    //  for (int j=0; j<vecGroupIdxs[i].size(); j++){
    //      cout << vecGroupIdxs[i][j] << " ";
    //  }
    //  cout << endl;
    //}


    //Convert back vecVecInlierIdx to original indices
    for (int i=0; i<vecVecInlierIdx.size(); i++){
        //cout << "vecVecInlierIdx[" << i << "] is from group " << vecFromGroup[i] << endl;
        for (int j=0; j<vecVecInlierIdx[i].size(); j++){
            //cout << " " << vecVecInlierIdx[i][j] << " -> " << vecGroupIdxs[vecFromGroup[i]][vecVecInlierIdx[i][j]] << endl;
            vecVecInlierIdx[i][j] = vecGroupIdxs[vecFromGroup[i]][vecVecInlierIdx[i][j]];
        }
    }


    for (int i=0; i < vecLines.size(); i++){
        int numpts = vecVecInlierIdx[i].size();
        cout << "Potential barcode #" << i << " with " << numpts << " points." << endl;

        
        //double angle=atan2(vecLines[i][1],vecLines[i][0])*180/M_PI; //For some reason it clips from [-90,90]
        double angle_rad = atan2(vecLines[i][1],vecLines[i][0]); //For some reason it clips from [-90,90]
        double angle_deg = angle_rad*180.0/M_PI;
        //cout << " angle_deg=" << angle_deg << endl;

        vector<double> bar_heights(numpts);
        vector<double> bar_widths(numpts);
        vector<double> coords_x(numpts);
        //Loop over all found and ransac-verified stripes in this barcode
        vector<cv::RotatedRect> stripesVerified(numpts);
        for (int j=0; j < numpts; j++){
            //cout << vecVecInlierIdx[i][j] << endl;
            //cout << "checking out stripecandidate[" << vecVecInlierIdx[i][j] << "] #" << vecVecInlierIdx[i][j] << endl;
            stripesVerified[j] = stripeCandidates[vecVecInlierIdx[i][j]];
            double dim_smallest = min(stripesVerified[j].size.width, stripesVerified[j].size.height); //For rotation invariance
            double dim_tallest  = max(stripesVerified[j].size.width, stripesVerified[j].size.height); //For rotation invariance
            bar_heights[j] = dim_tallest;
            bar_widths[j]  = dim_smallest;

            //Rotate the points straight
            Point2f ptRot = util::rotatePoint(stripesVerified[j].center, Point(matImageK.cols, matImageK.rows), angle_rad);
            //cout << ptRot << endl;
            coords_x[j]    = ptRot.x;
        }
        
        double height_median = util::calcMedian(bar_heights);
        double width_mean = util::calcMean(bar_widths);
        //cout << "height_median=" << height_median <<" width_mean=" << width_mean << endl;

        //Find the start and end position for reading
        vector<size_t> coords_sorted_index;
        vector<double> coords_x_sorted;
        sort(coords_x, coords_x_sorted, coords_sorted_index);
        //cout << coords_x_sorted[0] << " -> " << coords_x_sorted[coords_x_sorted.size()-1] << endl;

        //Get extrema-stripes
        Point2f pt_stripe_left = stripeCandidates[vecVecInlierIdx[i][coords_sorted_index[0]]].center;
        Point2f pt_stripe_right = stripeCandidates[vecVecInlierIdx[i][coords_sorted_index[coords_sorted_index.size() - 1]]].center;
        //cout << "pt_stripe_left=" << pt_stripe_left << endl;
        //cout << "pt_stripe_right=" << pt_stripe_right << endl;

        Point2f pt_barcode_center = (pt_stripe_left + pt_stripe_right) * 0.5;
        //cout << "pt_barcode_center=" << pt_barcode_center << endl;

        //Calculate width of the barcode
        double barcode_width = util::pointDist(pt_stripe_left, pt_stripe_right);
        //cout << "barcode_width=" << barcode_width << endl;

        //Make the rotated rectangle around the barcode
        cv::RotatedRect rotrect_candidate(pt_barcode_center, Size2f(barcode_width, height_median), angle_deg);

        const double add_width_on_sides_before_decoding = 7.0; // this number will be multiplied by average bar width

        //Add margin (of a few median widths)
        rotrect_candidate.size += Size2f(width_mean * add_width_on_sides_before_decoding, 0);

        const double height_retrainer_for_collapse = 0.25;

        //Extract the barcode itself
        cv::RotatedRect rotrect_candidate_thin = rotrect_candidate;

        //Crop off some margin in thickness because we dont want to collapse the entire barcode.
        if (rotrect_candidate_thin.size.width < rotrect_candidate_thin.size.height) {
            rotrect_candidate_thin.size.width *= height_retrainer_for_collapse;
        } else {
            rotrect_candidate_thin.size.height *= height_retrainer_for_collapse;
        }

        openbarcode::code code_candidate;
        code_candidate.rotrect = rotrect_candidate_thin;

        this->code_candidates_.push_back(code_candidate);
    }

    return RET_SUCCESS;
}