Как смерджить интервалы таким вот образом?

На вход поступает вектор интервалов и цвет для заданного интервала

struct Interval 
{
    int start, end;
    std::string color;

};

std::vector<Interval> mergeIntervals(std::vector<Interval>& intervals) 

Интервалы могут быть заданы в хаотичном порядке и могут перекрывать друг друга, например:

    std::vector<Interval> arr = { { 0, 6, "r" }, { 0, 9, "g" }, { 0, 4, "b" }};

Алгоритм должен "чинить" интервалы, которые перекрывают друг друга Т.е, результатом работы алгоритма должен быть такой вектор:

{ { 0, 4, "b" }, { 4, 6, "r" }, { 6, 9, "g" }}

Если 2 интервала одинаковые, то в приоритете тот, чей индекс в массиве выше. Если цвет у интервалов одинаковый и они идут друг за другом

{0, 3, "r"} 
{2,6 "r"} 

Можно объединить в (Не обязательно, с этим справлюсь)

{0, 6, "r"}

Мой код

// A C++ program for merging overlapping intervals
#include <string>
#include <vector>
#include <iostream>
#include <stack>
#include <algorithm>
using namespace std;

// An interval has start time and end time
struct Interval {
    int start, end;
    std::string color;

};
std::ostream& operator << (std::ostream &os, const Interval &i)
{
    return os << i.start << " " << i.end << " " << i.color;
}

// Compares two intervals according to their starting time.
// This is needed for sorting the intervals using library
// function std::sort(). See http://goo.gl/iGspV
bool compareInterval(Interval i1, Interval i2)
{
    return i1.start < i2.start;
}

bool overlaps2(Interval& i1, Interval& i2) {
  return (i1.start >= i2.start && i1.start <= i2.end) ||
         (i1.end >= i2.start && i1.end <= i2.end) ||

         (i2.start >= i1.start && i2.start <= i1.end) ||
         (i2.end >= i1.start && i2.end <= i1.end);
}

bool overlaps(const Interval& a, const Interval& b) {
    return a.end >= b.start && b.end >= a.start && a.color != b.color;
}

bool contained(const Interval& a, const Interval& b) {
    return a.start >= b.start && a.end <= b.end && a.color == b.color;
}



std::vector<Interval> mergeIntervals(std::vector<Interval>& intervals) {
    std::vector<Interval> result;
    result.push_back(intervals[0]);
    std::sort(intervals.begin(), intervals.end(),compareInterval);
    for (auto& interval : intervals) {
            for(auto idx = 0; idx < result.size(); ++idx)
            {

                const Interval& last = result[idx];
                if (contained(last, interval)) {
                    // skip, this interval is already contained in the last one
                }
                else if (overlaps(last, interval))
                {
                    if (last.color == interval.color) {
                        result.back().end = std::max(result.back().end, interval.end);
                    } else {

                        int start = last.end + 1;
                        int end = interval.end;
                        std::string color = interval.color;
                        result.push_back({start, end, color});
                    }
                }
                else {
                    result.push_back(interval);
                }
            }
        }

    return result;
}




int main()
{
    std::vector<Interval> arr = { { 0, 6, "r" }, { 0, 9, "g" }, { 0, 4, "b" }};
    auto arr2 = mergeIntervals(arr);
    for(auto& f : arr2)
        std::cout << "( "  << f.start << ", " << f.end << ", " << f.color << ")" << std::endl;
    return 0;
}

Сейчас я получаю неверный результат, буду благодарен любым наводкам на решение