Problem:
Score: 500 points

Problem Statement

Given is a permutation P of \{1, 2, \ldots, N\}.

For a pair (L, R) (1 \le L \lt R \le N), let X_{L, R} be the second largest value among P_L, P_{L+1}, \ldots, P_R.

Find \displaystyle \sum_{L=1}^{N-1} \sum_{R=L+1}^{N} X_{L,R}.

Constraints

2 \le N \le 10^5

1 \le P_i \le N

P_i \neq P_j (i \neq j)

All values in input are integers.

Input

Input is given from Standard Input in the following format:

N
P_1 P_2 \ldots P_N

Output

Print \displaystyle \sum_{L=1}^{N-1} \sum_{R=L+1}^{N} X_{L,R}.

Sample Input 1

3
2 3 1

Sample Output 1

5

X_{1, 2} = 2, X_{1, 3} = 2, and X_{2, 3} = 1, so the sum is 2 + 2 + 1 = 5.

Sample Input 2

5
1 2 3 4 5

Sample Output 2

30

Sample Input 3

8
8 2 7 3 4 5 6 1

Sample Output 3

136

Solution in C# (Mono 4.6.2.0) (judge verdict: Accepted)

#region いつもの
using System;
using System.Collections.Generic;
using System.Linq;
using AtCoderProject;
using AtCoderProject.Reader;
using IEnumerable = System.Collections.IEnumerable;
using IEnumerator = System.Collections.IEnumerator;
using BigInteger = System.Numerics.BigInteger;
using StringBuilder = System.Text.StringBuilder;
using static AtCoderProject.Global;
using static AtCoderProject.NumGlobal;

namespace AtCoderProject { public static class Global { public static void Swap<T>(ref T a, ref T b) { T tmp = a; a = b; b = tmp; } public static T[] NewArray<T>(int len0, T value) => new T[len0].Fill(value); public static T[] NewArray<T>(int len0, Func<T> factory) { var arr = new T[len0]; for (int i = 0; i < arr.Length; i++) arr[i] = factory(); return arr; } public static T[][] NewArray<T>(int len0, int len1, T value) where T : struct { var arr = new T[len0][]; for (int i = 0; i < arr.Length; i++) arr[i] = NewArray(len1, value); return arr; } public static T[][] NewArray<T>(int len0, int len1, Func<T> factory) { var arr = new T[len0][]; for (int i = 0; i < arr.Length; i++) arr[i] = NewArray(len1, factory); return arr; } public static T[][][] NewArray<T>(int len0, int len1, int len2, T value) where T : struct { var arr = new T[len0][][]; for (int i = 0; i < arr.Length; i++) arr[i] = NewArray(len1, len2, value); return arr; } public static T[][][] NewArray<T>(int len0, int len1, int len2, Func<T> factory) { var arr = new T[len0][][]; for (int i = 0; i < arr.Length; i++) arr[i] = NewArray(len1, len2, factory); return arr; } public static T[][][][] NewArray<T>(int len0, int len1, int len2, int len3, T value) where T : struct { var arr = new T[len0][][][]; for (int i = 0; i < arr.Length; i++) arr[i] = NewArray(len1, len2, len3, value); return arr; } public static T[][][][] NewArray<T>(int len0, int len1, int len2, int len3, Func<T> factory) { var arr = new T[len0][][][]; for (int i = 0; i < arr.Length; i++) arr[i] = NewArray(len1, len2, len3, factory); return arr; } public static string AllLines<T>(IEnumerable<T> source) => string.Join("\n", source); public static string AllJoin<T>(IEnumerable<T> source) => string.Join(" ", source); public static string AllGrid<T>(IEnumerable<IEnumerable<T>> source) => AllLines(source.Select(AllJoin)); public static bool UpdateMax(ref int r, int val) { if (r < val) { r = val; return true; } return false; } public static bool UpdateMax(ref long r, long val) { if (r < val) { r = val; return true; } return false; } public static bool UpdateMin(ref int r, int val) { if (r > val) { r = val; return true; } return false; } public static bool UpdateMin(ref long r, long val) { if (r > val) { r = val; return true; } return false; } } public static class NumGlobal { public static int Pow(int x, int y) { int res = 1; for (; y > 0; y >>= 1) { if ((y & 1) == 1) res *= x; x *= x; } return res; } public static BigInteger ParseBigInteger(string s) { /* 自前実装の方が速い */ var res = BigInteger.Zero; var splited = new string[(s.Length + 7) / 8]; for (var i = 0; i < splited.Length - 1; i++) { splited[i] = s.Substring(8 * i, 8); } splited[splited.Length - 1] = s.Substring(8 * (splited.Length - 1)); foreach (var sp in splited) { res *= (int)Math.Pow(10, sp.Length); res += int.Parse(sp); } return res; } public static int BitCount(int x) { x -= (x >> 1) & 0x55555555; x = (x & 0x33333333) + ((x >> 2) & 0x33333333); x = (x + (x >> 4)) & 0x0f0f0f0f; x += x >> 8; x += x >> 16; return x & 0x3f; } public static int BitCount(long x) { x -= (x >> 1) & 0x5555555555555555; x = (x & 0x3333333333333333) + ((x >> 2) & 0x3333333333333333); x = (x + (x >> 4)) & 0x0f0f0f0f0f0f0f0f; x += x >> 8; x += x >> 16; x += x >> 32; return (int)(x & 0x0000007f); } public static int MSB(int x) { x |= x >> 1; x |= x >> 2; x |= x >> 4; x |= x >> 8; x |= x >> 16; return BitCount(x) - 1; } public static int MSB(long x) { x |= x >> 1; x |= x >> 2; x |= x >> 4; x |= x >> 8; x |= x >> 16; x |= x >> 32; return BitCount(x) - 1; } public static int LSB(int x) { x |= x << 1; x |= x << 2; x |= x << 4; x |= x << 8; x |= x << 16; return 32 - BitCount(x); } public static int LSB(long x) { x |= x << 1; x |= x << 2; x |= x << 4; x |= x << 8; x |= x << 16; x |= x << 32; return 64 - BitCount(x); } } public static class Ext { public static long ToLong(this int i) => i; public static T[] Fill<T>(this T[] arr, T value) { for (var i = 0; i < arr.Length; i++) arr[i] = value; return arr; } public static T[] Sort<T>(this T[] arr) { Array.Sort(arr); return arr; } public static string[] Sort(this string[] arr) => Sort(arr, StringComparer.OrdinalIgnoreCase); public static T[] Sort<T, U>(this T[] arr, Func<T, U> selector) where U : IComparable<U> => Sort(arr, (a, b) => selector(a).CompareTo(selector(b))); public static T[] Sort<T>(this T[] arr, Comparison<T> comparison) { Array.Sort(arr, comparison); return arr; } public static T[] Sort<T>(this T[] arr, IComparer<T> comparer) { Array.Sort(arr, comparer); return arr; } public static T[] Reverse<T>(this T[] arr) { Array.Reverse(arr); return arr; } public static Tuple<int, T> MaxBy<T>(this T[] arr) where T : IComparable<T> { T max = arr[0]; int maxIndex = 0; for (int i = 0; i < arr.Length; i++) { if (max.CompareTo(arr[i]) < 0) { max = arr[i]; maxIndex = i; } } return Tuple.Create(maxIndex, max); } public static Tuple<TSource, TMax> MaxBy<TSource, TMax>(this IEnumerable<TSource> source, Func<TSource, TMax> maxBySelector) where TMax : IComparable<TMax> { TMax max; TSource maxByItem; var e = source.GetEnumerator(); e.MoveNext(); maxByItem = e.Current; max = maxBySelector(maxByItem); while (e.MoveNext()) { var item = e.Current; var next = maxBySelector(item); if (max.CompareTo(next) < 0) { max = next; maxByItem = item; } } return Tuple.Create(maxByItem, max); } public static Tuple<int, T> MinBy<T>(this T[] arr) where T : IComparable<T> { T min = arr[0]; int minIndex = 0; for (int i = 0; i < arr.Length; i++) { if (min.CompareTo(arr[i]) > 0) { min = arr[i]; minIndex = i; } } return Tuple.Create(minIndex, min); } public static Tuple<TSource, TMin> MinBy<TSource, TMin>(this IEnumerable<TSource> source, Func<TSource, TMin> minBySelector) where TMin : IComparable<TMin> { TMin min; TSource minByItem; var e = source.GetEnumerator(); e.MoveNext(); minByItem = e.Current; min = minBySelector(minByItem); while (e.MoveNext()) { var item = e.Current; var next = minBySelector(item); if (min.CompareTo(next) > 0) { min = next; minByItem = item; } } return Tuple.Create(minByItem, min); } public static IEnumerable<TSource> Append<TSource>(this IEnumerable<TSource> source, TSource element) { foreach (var item in source) yield return item; yield return element; } public static IEnumerable<TSource> Prepend<TSource>(this IEnumerable<TSource> source, TSource element) { yield return element; foreach (var item in source) yield return item; } public static IComparer<T> Reverse<T>(this IComparer<T> comparer) => Comparer<T>.Create((x, y) => comparer.Compare(y, x)); public static Dictionary<TKey, int> GroupCount<TSource, TKey>(this IEnumerable<TSource> source, Func<TSource, TKey> keySelector) => source.GroupBy(keySelector).ToDictionary(g => g.Key, g => g.Count()); public static Dictionary<TKey, int> GroupCount<TKey>(this IEnumerable<TKey> source) => source.GroupCount(i => i); public static TValue Get<TKey, TValue>(this Dictionary<TKey, TValue> dic, TKey key) { TValue v; dic.TryGetValue(key, out v); return v; } public static TValue GetOrInit<TKey, TValue>(this Dictionary<TKey, TValue> dic, TKey key, TValue value) { TValue v; if (dic.TryGetValue(key, out v)) return v; return dic[key] = value; } } public class ReverseComparer<T> : Comparer<T> where T : IComparable<T> { private static ReverseComparer<T> defaultComparer; public static new IComparer<T> Default => defaultComparer ?? (defaultComparer = new ReverseComparer<T>()); public override int Compare(T y, T x) => x.CompareTo(y); public override bool Equals(object obj) => obj != null && GetType() == obj.GetType(); public override int GetHashCode() => GetType().GetHashCode(); } public class ΔDebugView<T> { private IEnumerable<T> collection; public ΔDebugView(IEnumerable<T> collection) { if (collection == null) throw new ArgumentNullException(nameof(collection)); this.collection = collection; }[System.Diagnostics.DebuggerBrowsable(System.Diagnostics.DebuggerBrowsableState.RootHidden)] public T[] Items => collection.ToArray(); } }
namespace AtCoderProject.Reader { using System.IO; using System.Text; using System.Diagnostics; using System.Runtime.CompilerServices;[DebuggerStepThrough] public class ConsoleReader { const int BufSize = 1 << 12; private readonly byte[] buffer = new byte[BufSize]; private readonly Stream input; private readonly Encoding encoding; private int pos = 0; private int len = 0; public ConsoleReader(Stream input, Encoding encoding) { this.input = input; this.encoding = encoding; } public ConsoleReader(Stream input) : this(input, Encoding.UTF8) { } public ConsoleReader(string text) : this(new MemoryStream(Encoding.UTF8.GetBytes(text))) { } private void MoveNext() { if (++pos >= len) { len = input.Read(buffer, 0, buffer.Length); if (len == 0) { buffer[0] = 10; } pos = 0; } } public int Int { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { int res = 0; bool neg = false; while (buffer[pos] < 48) { neg = buffer[pos] == 45; MoveNext(); } do { res = checked(res * 10 + (buffer[pos] ^ 48)); MoveNext(); } while (48 <= buffer[pos]); return neg ? -res : res; } } public int Int0 => this.Int - 1; public long Long { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { long res = 0; bool neg = false; while (buffer[pos] < 48) { neg = buffer[pos] == 45; MoveNext(); } do { res = res * 10 + (buffer[pos] ^ 48); MoveNext(); } while (48 <= buffer[pos]); return neg ? -res : res; } } public long Long0 => this.Long - 1; public string String { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { var sb = new List<byte>(); while (buffer[pos] <= 32) MoveNext(); do { sb.Add(buffer[pos]); MoveNext(); } while (32 < buffer[pos]); return this.encoding.GetString(sb.ToArray()); } } public string Ascii { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { var sb = new StringBuilder(); while (buffer[pos] <= 32) MoveNext(); do { sb.Append((char)buffer[pos]); MoveNext(); } while (32 < buffer[pos]); return sb.ToString(); } } public string Line { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { var sb = new List<byte>(); while (buffer[pos] < 32) MoveNext(); do { sb.Add(buffer[pos]); MoveNext(); } while (buffer[pos] != 10 && buffer[pos] != 13); return this.encoding.GetString(sb.ToArray()); } } public double Double => double.Parse(this.Ascii);[DebuggerStepThrough] public struct RepeatReader { ConsoleReader cr; int count; public RepeatReader(ConsoleReader cr, int count) { this.cr = cr; this.count = count; } public T[] Select<T>(Func<ConsoleReader, T> factory) { var arr = new T[count]; for (var i = 0; i < count; i++) arr[i] = factory(cr); return arr; } public string[] Line { get { var arr = new string[count]; for (var i = 0; i < count; i++) arr[i] = cr.Line; return arr; } } public string[] String { get { var arr = new string[count]; for (var i = 0; i < count; i++) arr[i] = cr.String; return arr; } } public string[] Ascii { get { var arr = new string[count]; for (var i = 0; i < count; i++) arr[i] = cr.Ascii; return arr; } } public int[] Int { get { var arr = new int[count]; for (var i = 0; i < count; i++) arr[i] = cr.Int; return arr; } } public int[] Int0 { get { var arr = new int[count]; for (var i = 0; i < count; i++) arr[i] = cr.Int0; return arr; } } public long[] Long { get { var arr = new long[count]; for (var i = 0; i < count; i++) arr[i] = cr.Long; return arr; } } public long[] Long0 { get { var arr = new long[count]; for (var i = 0; i < count; i++) arr[i] = cr.Long0; return arr; } } public double[] Double { get { var arr = new double[count]; for (var i = 0; i < count; i++) arr[i] = cr.Double; return arr; } } public static implicit operator int[](RepeatReader rr) => rr.Int; public static implicit operator long[](RepeatReader rr) => rr.Long; public static implicit operator double[](RepeatReader rr) => rr.Double; public static implicit operator string[](RepeatReader rr) => rr.Ascii; } public RepeatReader Repeat(int count) => new RepeatReader(this, count);[DebuggerStepThrough] public struct SplitReader { ConsoleReader cr; public SplitReader(ConsoleReader cr) { this.cr = cr; } public string[] String { get { while (cr.buffer[cr.pos] <= 32) cr.MoveNext(); var list = new List<string>(); do { if (cr.buffer[cr.pos] < 32) cr.MoveNext(); else list.Add(cr.String); } while (cr.buffer[cr.pos] != 10 && cr.buffer[cr.pos] != 13); return list.ToArray(); } } public string[] Ascii { get { while (cr.buffer[cr.pos] <= 32) cr.MoveNext(); var list = new List<string>(); do { if (cr.buffer[cr.pos] < 32) cr.MoveNext(); else list.Add(cr.Ascii); } while (cr.buffer[cr.pos] != 10 && cr.buffer[cr.pos] != 13); return list.ToArray(); } } public int[] Int { get { while (cr.buffer[cr.pos] <= 32) cr.MoveNext(); var list = new List<int>(); do { if (cr.buffer[cr.pos] < 32) cr.MoveNext(); else list.Add(cr.Int); } while (cr.buffer[cr.pos] != 10 && cr.buffer[cr.pos] != 13); return list.ToArray(); } } public int[] Int0 { get { while (cr.buffer[cr.pos] <= 32) cr.MoveNext(); var list = new List<int>(); do { if (cr.buffer[cr.pos] < 32) cr.MoveNext(); else list.Add(cr.Int0); } while (cr.buffer[cr.pos] != 10 && cr.buffer[cr.pos] != 13); return list.ToArray(); } } public long[] Long { get { while (cr.buffer[cr.pos] <= 32) cr.MoveNext(); var list = new List<long>(); do { if (cr.buffer[cr.pos] < 32) cr.MoveNext(); else list.Add(cr.Long); } while (cr.buffer[cr.pos] != 10 && cr.buffer[cr.pos] != 13); return list.ToArray(); } } public long[] Long0 { get { while (cr.buffer[cr.pos] <= 32) cr.MoveNext(); var list = new List<long>(); do { if (cr.buffer[cr.pos] < 32) cr.MoveNext(); else list.Add(cr.Long0); } while (cr.buffer[cr.pos] != 10 && cr.buffer[cr.pos] != 13); return list.ToArray(); } } public double[] Double { get { while (cr.buffer[cr.pos] <= 32) cr.MoveNext(); var list = new List<double>(); do { if (cr.buffer[cr.pos] < 32) cr.MoveNext(); else list.Add(cr.Double); } while (cr.buffer[cr.pos] != 10 && cr.buffer[cr.pos] != 13); return list.ToArray(); } } public static implicit operator int[](SplitReader sr) => sr.Int; public static implicit operator long[](SplitReader sr) => sr.Long; public static implicit operator double[](SplitReader sr) => sr.Double; public static implicit operator string[](SplitReader sr) => sr.Ascii; } public SplitReader Split => new SplitReader(this); public static implicit operator int(ConsoleReader cr) => cr.Int; public static implicit operator long(ConsoleReader cr) => cr.Long; public static implicit operator double(ConsoleReader cr) => cr.Double; public static implicit operator string(ConsoleReader cr) => cr.Ascii; } }
public class Program
{
    [System.Diagnostics.DebuggerBrowsable(System.Diagnostics.DebuggerBrowsableState.Never)] private ConsoleReader cr; public Program(ConsoleReader consoleReader) { this.cr = consoleReader; }
    static void Main() => Console.WriteLine(new Program(new ConsoleReader(Console.OpenStandardInput())).Result());
    #endregion
    public string Result() { var obj = Calc(); if (obj is bool) return (bool)obj ? "Yes" : "No"; if (obj is double) return ((double)obj).ToString("0.####################"); return obj.ToString(); }
    private object Calc()
    {
        N = cr;
        P = cr.Repeat(N);
        var pq = new PriorityQueue<int, int>(N);
        for (int i = 0; i < P.Length; i++)
            pq.Add(-P[i], i);
        var ss = new Set<int>
        {
            pq.Dequeue().Value
        };
        long res = 0;
        while (pq.Count > 0)
        {
            var ix = pq.Dequeue().Value;
            var node = ss.FindNodeLowerBound(ix);
            int right1 = N;
            int right2 = N;
            int left1 = -1;
            int left2 = -1;
            if (node == null)
            {
                left1 = ss.Max;
                var l = ss.Reversed().Take(2).ToList();
                if (l.Count > 0) left1 = l[0];
                if (l.Count > 1) left2 = l[1];
            }
            else
            {
                right1 = node.Item;
                var r = ss.Enumerate(node).Skip(1).Take(1).ToList();
                if (r.Count > 0) right2 = r[0];
                var l = ss.Enumerate(node, true).Skip(1).Take(2).ToList();
                if (l.Count > 0) left1 = l[0];
                if (l.Count > 1) left2 = l[1];
            }
            res += (long)(right1 - ix) * (left1 - left2) * P[ix];
            res += (long)(ix - left1) * (right2 - right1) * P[ix];

            ss.Add(ix);
        }
        return res;
    }
    int N;
    int[] P;
}

[System.Diagnostics.DebuggerTypeProxy(typeof(ΔDebugView<>))] [System.Diagnostics.DebuggerDisplay("Count = {" + nameof(Count) + "}")] class Set<T> : ICollection<T>, IReadOnlyCollection<T> { /* * Original * * Copyright (c) .NET Foundation and Contributors * Released under the MIT license * https: */ public virtual bool IsMulti => false; public T Min { get { if (root == null) return default(T); var cur = root; while (cur.Left != null) { cur = cur.Left; } return cur.Item; } } public T Max { get { if (root == null) return default(T); var cur = root; while (cur.Right != null) { cur = cur.Right; } return cur.Item; } } public Node FindNode(T item) { Node current = root; while (current != null) { int order = comparer.Compare(item, current.Item); if (order == 0) return current; current = order < 0 ? current.Left : current.Right; } return null; } public int Index(Node node) { var ret = NodeSize(node.Left); Node prev = node; node = node.Parent; while (prev != root) { if (node.Left != prev) { ret += NodeSize(node.Left) + 1; } prev = node; node = node.Parent; } return ret; } public Node FindByIndex(int index) { var current = root; var currentIndex = current.Size - NodeSize(current.Right) - 1; while (currentIndex != index) { if (currentIndex > index) { current = current.Left; if (current == null) break; currentIndex -= NodeSize(current.Right) + 1; } else { current = current.Right; if (current == null) break; currentIndex += NodeSize(current.Left) + 1; } } return current; } public KeyValuePair<Node, int> BinarySearch(T item, bool isLowerBound) { Node right = null; Node current = root; if (current == null) return new KeyValuePair<Node, int>(null, -1); int ri = -1; int ci = NodeSize(current.Left); while (true) { var order = comparer.Compare(item, current.Item); if (order < 0 || (isLowerBound && order == 0)) { right = current; ri = ci; current = current.Left; if (current != null) ci -= NodeSize(current.Right) + 1; else break; } else { current = current.Right; if (current != null) ci += NodeSize(current.Left) + 1; else break; } } return new KeyValuePair<Node, int>(right, ri); } public Node FindNodeLowerBound(T item) => BinarySearch(item, true).Key; public Node FindNodeUpperBound(T item) => BinarySearch(item, false).Key; public T LowerBoundItem(T item) => BinarySearch(item, true).Key.Item; public T UpperBoundItem(T item) => BinarySearch(item, false).Key.Item; public int LowerBoundIndex(T item) => BinarySearch(item, true).Value; public int UpperBoundIndex(T item) => BinarySearch(item, false).Value; public IEnumerable<T> Reversed() { var e = new Enumerator(this, true, null); while (e.MoveNext()) yield return e.Current; } public IEnumerable<T> Enumerate(Node from) => Enumerate(from, false); public IEnumerable<T> Enumerate(Node from, bool reverse) { if (from == null) yield break; var e = new Enumerator(this, reverse, from); while (e.MoveNext()) yield return e.Current; } public Set() : this(Comparer<T>.Default) { } public Set(IEnumerable<T> collection) : this(collection, Comparer<T>.Default) { } public Set(IComparer<T> comparer) { this.comparer = comparer; } public Set(IEnumerable<T> collection, IComparer<T> comparer) { this.comparer = comparer; var arr = InitArray(collection); this.root = ConstructRootFromSortedArray(arr, 0, arr.Length - 1, null); } protected T[] InitArray(IEnumerable<T> collection) { T[] arr; if (this.IsMulti) { arr = collection.ToArray(); Array.Sort(arr, this.comparer); } else { var list = new List<T>(collection); list.Sort(this.comparer); for (int i = list.Count - 1; i > 0; i--) if (this.comparer.Compare(list[i - 1], list[i]) == 0) list.RemoveAt(i); arr = list.ToArray(); } return arr; } public int Count => NodeSize(root); protected static int NodeSize(Node node) => node == null ? 0 : node.Size; protected readonly IComparer<T> comparer; bool ICollection<T>.IsReadOnly => false; private Node root; private static Node ConstructRootFromSortedArray(T[] arr, int startIndex, int endIndex, Node redNode) { int size = endIndex - startIndex + 1; Node root; switch (size) { case 0: return null; case 1: root = new Node(arr[startIndex], false); if (redNode != null) { root.Left = redNode; } break; case 2: root = new Node(arr[startIndex], false) { Right = new Node(arr[endIndex], true) }; if (redNode != null) { root.Left = redNode; } break; case 3: root = new Node(arr[startIndex + 1], false) { Left = new Node(arr[startIndex], false), Right = new Node(arr[endIndex], false) }; if (redNode != null) { root.Left.Left = redNode; } break; default: int midpt = ((startIndex + endIndex) / 2); root = new Node(arr[midpt], false) { Left = ConstructRootFromSortedArray(arr, startIndex, midpt - 1, redNode), Right = size % 2 == 0 ? ConstructRootFromSortedArray(arr, midpt + 2, endIndex, new Node(arr[midpt + 1], true)) : ConstructRootFromSortedArray(arr, midpt + 1, endIndex, null) }; break; } return root; } public void Add(T item) { if (root == null) { root = new Node(item, false); return; } Node current = root; Node parent = null; Node grandParent = null; Node greatGrandParent = null; int order = 0; while (current != null) { order = comparer.Compare(item, current.Item); if (order == 0 && !this.IsMulti) { root.IsRed = false; return; } if (Is4Node(current)) { Split4Node(current); if (IsNonNullRed(parent) == true) { InsertionBalance(current, ref parent, grandParent, greatGrandParent); } } greatGrandParent = grandParent; grandParent = parent; parent = current; current = (order < 0) ? current.Left : current.Right; } Node node = new Node(item, true); if (order >= 0) parent.Right = node; else parent.Left = node; if (parent.IsRed) InsertionBalance(node, ref parent, grandParent, greatGrandParent); root.IsRed = false; } public bool Remove(T item) { if (root == null) return false; Node current = root; Node parent = null; Node grandParent = null; Node match = null; Node parentOfMatch = null; bool foundMatch = false; while (current != null) { if (Is2Node(current)) { if (parent == null) { current.IsRed = true; } else { Node sibling = GetSibling(current, parent); if (sibling.IsRed) { if (parent.Right == sibling) RotateLeft(parent); else RotateRight(parent); parent.IsRed = true; sibling.IsRed = false; ReplaceChildOrRoot(grandParent, parent, sibling); grandParent = sibling; if (parent == match) parentOfMatch = sibling; sibling = (parent.Left == current) ? parent.Right : parent.Left; } if (Is2Node(sibling)) { Merge2Nodes(parent); } else { TreeRotation rotation = GetRotation(parent, current, sibling); Node newGrandParent = Rotate(parent, rotation); newGrandParent.IsRed = parent.IsRed; parent.IsRed = false; current.IsRed = true; ReplaceChildOrRoot(grandParent, parent, newGrandParent); if (parent == match) { parentOfMatch = newGrandParent; } } } } int order = foundMatch ? -1 : comparer.Compare(item, current.Item); if (order == 0) { foundMatch = true; match = current; parentOfMatch = parent; } grandParent = parent; parent = current; current = order < 0 ? current.Left : current.Right; } if (match != null) { ReplaceNode(match, parentOfMatch, parent, grandParent); } if (root != null) { root.IsRed = false; } return foundMatch; } public virtual void Clear() { root = null; } public virtual bool Contains(T item) => FindNode(item) != null; public void CopyTo(T[] array, int arrayIndex) { foreach (var item in this) array[arrayIndex++] = item; } public Enumerator GetEnumerator() => new Enumerator(this); IEnumerator<T> IEnumerable<T>.GetEnumerator() => new Enumerator(this); IEnumerator IEnumerable.GetEnumerator() => new Enumerator(this); private static bool Is2Node(Node node) => IsNonNullBlack(node) && IsNullOrBlack(node.Left) && IsNullOrBlack(node.Right); private static bool Is4Node(Node node) => IsNonNullRed(node.Left) && IsNonNullRed(node.Right); private static bool IsNonNullRed(Node node) => node != null && node.IsRed; private static bool IsNonNullBlack(Node node) => node != null && !node.IsRed; private static bool IsNullOrBlack(Node node) => node == null || !node.IsRed; private void ReplaceNode(Node match, Node parentOfMatch, Node succesor, Node parentOfSuccesor) { if (succesor == match) { succesor = match.Left; } else { if (succesor.Right != null) { succesor.Right.IsRed = false; } if (parentOfSuccesor != match) { parentOfSuccesor.Left = succesor.Right; succesor.Right = match.Right; } succesor.Left = match.Left; } if (succesor != null) { succesor.IsRed = match.IsRed; } ReplaceChildOrRoot(parentOfMatch, match, succesor); } private static void Merge2Nodes(Node parent) { parent.IsRed = false; parent.Left.IsRed = true; parent.Right.IsRed = true; } private static void Split4Node(Node node) { node.IsRed = true; node.Left.IsRed = false; node.Right.IsRed = false; } private static Node GetSibling(Node node, Node parent) { return parent.Left == node ? parent.Right : parent.Left; } private void InsertionBalance(Node current, ref Node parent, Node grandParent, Node greatGrandParent) { bool parentIsOnRight = grandParent.Right == parent; bool currentIsOnRight = parent.Right == current; Node newChildOfGreatGrandParent; if (parentIsOnRight == currentIsOnRight) { newChildOfGreatGrandParent = currentIsOnRight ? RotateLeft(grandParent) : RotateRight(grandParent); } else { newChildOfGreatGrandParent = currentIsOnRight ? RotateLeftRight(grandParent) : RotateRightLeft(grandParent); parent = greatGrandParent; } grandParent.IsRed = true; newChildOfGreatGrandParent.IsRed = false; ReplaceChildOrRoot(greatGrandParent, grandParent, newChildOfGreatGrandParent); } private static Node Rotate(Node node, TreeRotation rotation) { switch (rotation) { case TreeRotation.Right: node.Left.Left.IsRed = false; return RotateRight(node); case TreeRotation.Left: node.Right.Right.IsRed = false; return RotateLeft(node); case TreeRotation.RightLeft: return RotateRightLeft(node); case TreeRotation.LeftRight: return RotateLeftRight(node); default: throw new InvalidOperationException(); } } private static Node RotateLeft(Node node) { Node child = node.Right; node.Right = child.Left; child.Left = node; return child; } private static Node RotateLeftRight(Node node) { Node child = node.Left; Node grandChild = child.Right; node.Left = grandChild.Right; grandChild.Right = node; child.Right = grandChild.Left; grandChild.Left = child; return grandChild; } private static Node RotateRight(Node node) { Node child = node.Left; node.Left = child.Right; child.Right = node; return child; } private static Node RotateRightLeft(Node node) { Node child = node.Right; Node grandChild = child.Left; node.Right = grandChild.Left; grandChild.Left = node; child.Left = grandChild.Right; grandChild.Right = child; return grandChild; } private void ReplaceChildOrRoot(Node parent, Node child, Node newChild) { if (parent != null) { if (parent.Left == child) { parent.Left = newChild; } else { parent.Right = newChild; } } else { root = newChild; } } private static TreeRotation GetRotation(Node parent, Node current, Node sibling) { if (IsNonNullRed(sibling.Left)) { if (parent.Left == current) { return TreeRotation.RightLeft; } return TreeRotation.Right; } else { if (parent.Left == current) { return TreeRotation.Left; } return TreeRotation.LeftRight; } } public struct Enumerator : IEnumerator<T> { private Set<T> tree; private Stack<Node> stack; private Node current; private bool reverse; internal Enumerator(Set<T> set) : this(set, false, null) { } internal Enumerator(Set<T> set, bool reverse, Node startNode) { tree = set; stack = new Stack<Node>(2 * Log2(tree.Count + 1)); current = null; this.reverse = reverse; if (startNode == null) IntializeAll(); else Intialize(startNode); } private void IntializeAll() { var node = tree.root; while (node != null) { var next = reverse ? node.Right : node.Left; stack.Push(node); node = next; } } private void Intialize(Node startNode) { if (startNode == null) throw new InvalidOperationException(nameof(startNode) + "is null"); current = null; var node = startNode; var list = new List<Node>(Log2(tree.Count + 1)); var comparer = tree.comparer; if (reverse) { while (node != null) { list.Add(node); var parent = node.Parent; if (parent == null || parent.Left == node) { node = parent; break; } node = parent; } while (node != null) { var parent = node.Parent; if (parent == null || parent.Right == node) { node = parent; break; } node = parent; } while (node != null) { if (comparer.Compare(startNode.Item, node.Item) >= 0) list.Add(node); node = node.Parent; } } else { while (node != null) { list.Add(node); var parent = node.Parent; if (parent == null || parent.Right == node) { node = parent; break; } node = parent; } while (node != null) { var parent = node.Parent; if (parent == null || parent.Left == node) { node = parent; break; } node = parent; } while (node != null) { if (comparer.Compare(startNode.Item, node.Item) <= 0) list.Add(node); node = node.Parent; } } list.Reverse(); foreach (var n in list) stack.Push(n); } private static int Log2(int num) => num == 0 ? 0 : NumGlobal.MSB(num) + 1; public T Current => current == null ? default(T) : current.Item; public bool MoveNext() { if (stack.Count == 0) { current = null; return false; } current = stack.Pop(); var node = reverse ? current.Left : current.Right; while (node != null) { var next = reverse ? node.Right : node.Left; stack.Push(node); node = next; } return true; } object IEnumerator.Current => this.Current; public void Dispose() { } public void Reset() { throw new NotSupportedException(); } } public class Node { public bool IsRed; public T Item; public Node Parent { get; private set; } private Node _left; public Node Left { get { return _left; } set { _left = value; if (value != null) value.Parent = this; for (var cur = this; cur != null; cur = cur.Parent) { if (!cur.UpdateSize()) break; if (cur.Parent != null && cur.Parent.Left != cur && cur.Parent.Right != cur) { cur.Parent = null; break; } } } } private Node _right; public Node Right { get { return _right; } set { _right = value; if (value != null) value.Parent = this; for (var cur = this; cur != null; cur = cur.Parent) { if (!cur.UpdateSize()) break; if (cur.Parent != null && cur.Parent.Left != cur && cur.Parent.Right != cur) { cur.Parent = null; break; } } } } public int Size { get; private set; } = 1; public Node(T item, bool isRed) { this.Item = item; this.IsRed = isRed; } public bool UpdateSize() { var oldsize = this.Size; var size = 1; if (Left != null) size += Left.Size; if (Right != null) size += Right.Size; this.Size = size; return oldsize != size; } public override string ToString() => $"Size = {Size}, Item = {Item}"; } private enum TreeRotation : byte { Left = 1, Right = 2, RightLeft = 3, LeftRight = 4, } }

[System.Diagnostics.DebuggerDisplay("Count = {" + nameof(Count) + "}")] class PriorityQueue<T> { protected readonly List<T> data; protected readonly IComparer<T> comparer; public PriorityQueue() : this(Comparer<T>.Default) { } public PriorityQueue(int capacity) : this(capacity, Comparer<T>.Default) { } public PriorityQueue(IComparer<T> comparer) { this.data = new List<T>(); this.comparer = comparer; } public PriorityQueue(int capacity, IComparer<T> comparer) { this.data = new List<T>(capacity); this.comparer = comparer; }[System.Diagnostics.DebuggerBrowsable(System.Diagnostics.DebuggerBrowsableState.Never)] public int Count => data.Count; public T Peek => data[0]; public void Add(T value) { data.Add(value); UpdateUp(data.Count - 1); } public T Dequeue() { var res = data[0]; data[0] = data[data.Count - 1]; data.RemoveAt(data.Count - 1); UpdateDown(0); return res; } private void UpdateUp(int i) { if (i > 0) { var p = (i - 1) >> 1; if (comparer.Compare(data[i], data[p]) < 0) { var tmp = data[p]; data[p] = data[i]; data[i] = tmp; UpdateUp(p); } } } private void UpdateDown(int i) { var n = data.Count; var child = 2 * i + 1; if (child < n) { if (child != n - 1 && comparer.Compare(data[child], data[child + 1]) > 0) child++; if (comparer.Compare(data[i], data[child]) > 0) { var tmp = data[child]; data[child] = data[i]; data[i] = tmp; UpdateDown(child); } } }[System.Diagnostics.DebuggerBrowsable(System.Diagnostics.DebuggerBrowsableState.RootHidden)] private T[] Items => data.ToArray().Sort((a, b) => comparer.Compare(a, b)); }
class PriorityQueue<TKey, TValue> : PriorityQueue<KeyValuePair<TKey, TValue>> { private class KeyComparer : IComparer<KeyValuePair<TKey, TValue>> { public readonly IComparer<TKey> comparer; public KeyComparer(IComparer<TKey> comparer) { this.comparer = comparer; } public int Compare(KeyValuePair<TKey, TValue> x, KeyValuePair<TKey, TValue> y) => comparer.Compare(x.Key, y.Key); } public PriorityQueue() : this(Comparer<TKey>.Default) { } public PriorityQueue(int capacity) : this(capacity, Comparer<TKey>.Default) { } public PriorityQueue(IComparer<TKey> comparer) : base(new KeyComparer(comparer)) { } public PriorityQueue(int capacity, IComparer<TKey> comparer) : base(capacity, new KeyComparer(comparer)) { } public void Add(TKey key, TValue value) => Add(new KeyValuePair<TKey, TValue>(key, value)); }