#include <iostream>
#include <string>
#include <vector>
#include <algorithm>


std::vector<double> broken_insert_sort(std::vector<double> centers, double new_num) {
    centers.push_back(new_num);
    
    size_t k = centers.size() - 1;
    while( k > 0 && centers[k] < centers[k-1]) // Wrong comparison here
    {
        centers[k] = centers[k-1];
        k--;
    }
    centers[k] = new_num;
    return centers;
}

std::vector<double> proper_insert_sort(std::vector<double> centers, double new_num) {
    centers.push_back(new_num);
    
    size_t k = centers.size() - 1;
    while( k > 0 && new_num < centers[k-1]) // Proper comparison here
    {
        centers[k] = centers[k-1];
        k--;
    }
    centers[k] = new_num;
    return centers;
}

void print_vec(const std::vector<double>& vec) {
    std::cout << "[";
    std::for_each(vec.begin(), vec.end(), [](double v) {
        std::cout << v << ", ";
    });
    std::cout << "]" << std::endl;
}

int main()
{
    std::vector<double> centers{3.0, 4.0, 5.0, 6.0};
    
    auto bugged = broken_insert_sort(centers, 2.0); // This is the version of insertion sort found in blobdetector.cpp
    auto fixed = proper_insert_sort(centers, 2.0); // This includes the fix (regular insertion sort)
    
    print_vec(bugged);
    print_vec(fixed);
}