From 90c873421c2fccc0db1eac70bf4d0f45accec896 Mon Sep 17 00:00:00 2001
From: Bartosz Taudul <wolf.pld@gmail.com>
Date: Sun, 7 Feb 2021 15:50:27 +0100
Subject: [PATCH] Implement main sorting functionality in SortedVector.

---
 server/TracySortedVector.hpp | 57 +++++++++++++++++++++++++++++++++---
 1 file changed, 53 insertions(+), 4 deletions(-)

diff --git a/server/TracySortedVector.hpp b/server/TracySortedVector.hpp
index 21f65e2e..fa94db93 100644
--- a/server/TracySortedVector.hpp
+++ b/server/TracySortedVector.hpp
@@ -1,29 +1,34 @@
 #ifndef __TRACYSORTEDVECTOR_HPP__
 #define __TRACYSORTEDVECTOR_HPP__
 
+#include "TracySort.hpp"
 #include "TracyVector.hpp"
 
 namespace tracy
 {
 
 #pragma pack( 1 )
-template<typename T>
+template<typename T, class CompareDefault = std::less<T>>
 class SortedVector
 {
 public:
     using iterator = T*;
     using const_iterator = const T*;
 
-    tracy_force_inline SortedVector() {}
+    tracy_force_inline SortedVector()
+        : sortedEnd( 0 )
+    {}
 
     SortedVector( const SortedVector& ) = delete;
     tracy_force_inline SortedVector( SortedVector&& src ) noexcept
         : v( std::move( src.v ) )
+        , sortedEnd( src.sortedEnd )
     {
     }
 
     tracy_force_inline SortedVector( const T& value )
         : v( value )
+        , sortedEnd( 0 )
     {
     }
 
@@ -31,13 +36,19 @@ public:
     tracy_force_inline SortedVector& operator=( SortedVector&& src ) noexcept
     {
         v = std::move( src.v );
+        sortedEnd = src.sortedEnd;
         return *this;
     }
 
-    tracy_force_inline void swap( SortedVector& other ) { v.swap( other.v ); }
+    tracy_force_inline void swap( SortedVector& other )
+    {
+        v.swap( other.v );
+        std::swap( sortedEnd, other.sortedEnd );
+    }
 
     tracy_force_inline bool empty() const { return v.empty(); }
     tracy_force_inline size_t size() const { return v.size(); }
+    tracy_force_inline bool is_sorted() const { return sortedEnd == 0; }
 
     /*
     tracy_force_inline void set_size( size_t sz ) { v.set_size( sz ); }
@@ -60,6 +71,22 @@ public:
     tracy_force_inline T& operator[]( size_t idx ) { return v[idx]; }
     tracy_force_inline const T& operator[]( size_t idx ) const { return v[idx]; }
 
+    tracy_force_inline void push_back( const T& val ) { push_back( val, CompareDefault() ); }
+
+    template<class Compare>
+    tracy_force_inline void push_back( const T& val, Compare comp )
+    {
+        if( sortedEnd != 0 || v.empty() || comp( v.back(), val ) )
+        {
+            v.push_back( val );
+        }
+        else
+        {
+            sortedEnd = (uint32_t)v.size();
+            v.push_back( val );
+        }
+    }
+
     /*
     tracy_force_inline void push_back( const T& val ) { v.push_back( val ); }
     tracy_force_inline void push_back_non_empty( const T& val ) { v.push_back_non_empty( val ); }
@@ -88,10 +115,32 @@ public:
     template<size_t U>
     tracy_force_inline void reserve_exact( uint32_t sz, Slab<U>& slab ) { v.reserve_exact( sz, slab ); }
 
-    tracy_force_inline void clear() { v.clear(); }
+    tracy_force_inline void clear() { v.clear(); sortedEnd = 0; }
+
+    tracy_force_inline void sort() { sort( CompareDefault() ); }
+
+    template<class Compare>
+    void sort( Compare comp )
+    {
+        assert( !is_sorted() );
+        const auto sb = v.begin();
+        const auto se = sb + sortedEnd;
+        const auto sl = se - 1;
+        const auto ue = v.end();
+#ifdef NO_PARALLEL_SORT
+        pdqsort_branchless( se, ue, comp );
+#else
+        std::sort( std::execution::par_unseq, se, ue, comp );
+#endif
+        const auto ss = std::lower_bound( sb, se, *se, comp );
+        const auto uu = std::lower_bound( se, ue, *sl, comp );
+        std::inplace_merge( ss, se, uu, comp );
+        sortedEnd = 0;
+    }
 
 private:
     Vector<T> v;
+    uint32_t sortedEnd;
 };
 
 #pragma pack()