From 68c0417368195ff23cebbbc5092c3b96633fdd7a Mon Sep 17 00:00:00 2001
From: Christoph Ruegg <git@cdrnet.ch>
Date: Tue, 11 Oct 2011 21:31:57 +0200
Subject: [PATCH] Sparse Linear Algebra: fix addition bug gh-20 gh-18.

---
 .../LinearAlgebra/Complex/SparseVector.cs     | 69 +++++++++++++++++--
 .../LinearAlgebra/Complex32/SparseVector.cs   | 69 +++++++++++++++++--
 .../LinearAlgebra/Double/SparseVector.cs      | 69 +++++++++++++++++--
 .../LinearAlgebra/Single/SparseVector.cs      | 69 +++++++++++++++++--
 4 files changed, 248 insertions(+), 28 deletions(-)

diff --git a/src/Numerics/LinearAlgebra/Complex/SparseVector.cs b/src/Numerics/LinearAlgebra/Complex/SparseVector.cs
index a087a64a..38edfa32 100644
--- a/src/Numerics/LinearAlgebra/Complex/SparseVector.cs
+++ b/src/Numerics/LinearAlgebra/Complex/SparseVector.cs
@@ -403,18 +403,73 @@ namespace MathNet.Numerics.LinearAlgebra.Complex
         /// </param>
         protected override void DoAdd(Vector<Complex> other, Vector<Complex> result)
         {
-            if (ReferenceEquals(this, result))
+            var otherSparse = other as SparseVector;
+            if (otherSparse == null)
             {
-                CommonParallel.For(
-                    0,
-                    NonZerosCount,
-                    index => _nonZeroValues[index] += _nonZeroValues[index]);
+                base.DoAdd(other, result);
+                return;
+            }
+
+            var resultSparse = result as SparseVector;
+            if (resultSparse == null)
+            {
+                base.DoAdd(other, result);
+                return;
+            }
+
+            // TODO (ruegg, 2011-10-11): Options to optimize?
+
+            if (ReferenceEquals(this, resultSparse))
+            {
+                int i = 0, j = 0;
+                while (i < NonZerosCount || j < otherSparse.NonZerosCount)
+                {
+                    if (i < NonZerosCount && j < otherSparse.NonZerosCount && _nonZeroIndices[i] == otherSparse._nonZeroIndices[j])
+                    {
+                        _nonZeroValues[i++] += otherSparse._nonZeroValues[j++];
+                    }
+                    else if (j >= otherSparse.NonZerosCount || i < NonZerosCount && _nonZeroIndices[i] < otherSparse._nonZeroIndices[j])
+                    {
+                        _nonZeroValues[i] += otherSparse.At(_nonZeroIndices[i]);
+                        i++;
+                    }
+                    else
+                    {
+                        var otherValue = otherSparse._nonZeroValues[j];
+                        if (otherValue != Complex.Zero)
+                        {
+                            InsertAtUnchecked(i++, otherSparse._nonZeroIndices[j], otherValue);
+                        }
+                        j++;
+                    }
+                }
             }
             else
             {
-                for (var index = 0; index < Count; index++)
+                result.Clear();
+                int i = 0, j = 0, last = -1;
+                while (i < NonZerosCount || j < otherSparse.NonZerosCount)
                 {
-                    result.At(index, At(index) + other.At(index));
+                    if (j >= otherSparse.NonZerosCount || i < NonZerosCount && _nonZeroIndices[i] <= otherSparse._nonZeroIndices[j])
+                    {
+                        var next = _nonZeroIndices[i];
+                        if (next != last)
+                        {
+                            last = next;
+                            result.At(next, _nonZeroValues[i] + otherSparse.At(next));
+                        }
+                        i++;
+                    }
+                    else
+                    {
+                        var next = otherSparse._nonZeroIndices[j];
+                        if (next != last)
+                        {
+                            last = next;
+                            result.At(next, At(next) + otherSparse._nonZeroValues[j]);
+                        }
+                        j++;
+                    }
                 }
             }
         }
diff --git a/src/Numerics/LinearAlgebra/Complex32/SparseVector.cs b/src/Numerics/LinearAlgebra/Complex32/SparseVector.cs
index ef79608b..3b82a5a1 100644
--- a/src/Numerics/LinearAlgebra/Complex32/SparseVector.cs
+++ b/src/Numerics/LinearAlgebra/Complex32/SparseVector.cs
@@ -433,18 +433,73 @@ namespace MathNet.Numerics.LinearAlgebra.Complex32
         /// </param>
         protected override void DoAdd(Vector<Complex32> other, Vector<Complex32> result)
         {
-            if (ReferenceEquals(this, result))
+            var otherSparse = other as SparseVector;
+            if (otherSparse == null)
             {
-                CommonParallel.For(
-                    0,
-                    NonZerosCount,
-                    index => _nonZeroValues[index] += _nonZeroValues[index]);
+                base.DoAdd(other, result);
+                return;
+            }
+
+            var resultSparse = result as SparseVector;
+            if (resultSparse == null)
+            {
+                base.DoAdd(other, result);
+                return;
+            }
+
+            // TODO (ruegg, 2011-10-11): Options to optimize?
+
+            if (ReferenceEquals(this, resultSparse))
+            {
+                int i = 0, j = 0;
+                while (i < NonZerosCount || j < otherSparse.NonZerosCount)
+                {
+                    if (i < NonZerosCount && j < otherSparse.NonZerosCount && _nonZeroIndices[i] == otherSparse._nonZeroIndices[j])
+                    {
+                        _nonZeroValues[i++] += otherSparse._nonZeroValues[j++];
+                    }
+                    else if (j >= otherSparse.NonZerosCount || i < NonZerosCount && _nonZeroIndices[i] < otherSparse._nonZeroIndices[j])
+                    {
+                        _nonZeroValues[i] += otherSparse.At(_nonZeroIndices[i]);
+                        i++;
+                    }
+                    else
+                    {
+                        var otherValue = otherSparse._nonZeroValues[j];
+                        if (otherValue != Complex32.Zero)
+                        {
+                            InsertAtUnchecked(i++, otherSparse._nonZeroIndices[j], otherValue);
+                        }
+                        j++;
+                    }
+                }
             }
             else
             {
-                for (var index = 0; index < Count; index++)
+                result.Clear();
+                int i = 0, j = 0, last = -1;
+                while (i < NonZerosCount || j < otherSparse.NonZerosCount)
                 {
-                    result.At(index, At(index) + other.At(index));
+                    if (j >= otherSparse.NonZerosCount || i < NonZerosCount && _nonZeroIndices[i] <= otherSparse._nonZeroIndices[j])
+                    {
+                        var next = _nonZeroIndices[i];
+                        if (next != last)
+                        {
+                            last = next;
+                            result.At(next, _nonZeroValues[i] + otherSparse.At(next));
+                        }
+                        i++;
+                    }
+                    else
+                    {
+                        var next = otherSparse._nonZeroIndices[j];
+                        if (next != last)
+                        {
+                            last = next;
+                            result.At(next, At(next) + otherSparse._nonZeroValues[j]);
+                        }
+                        j++;
+                    }
                 }
             }
         }
diff --git a/src/Numerics/LinearAlgebra/Double/SparseVector.cs b/src/Numerics/LinearAlgebra/Double/SparseVector.cs
index 60eb51bf..4d516bfe 100644
--- a/src/Numerics/LinearAlgebra/Double/SparseVector.cs
+++ b/src/Numerics/LinearAlgebra/Double/SparseVector.cs
@@ -351,18 +351,73 @@ namespace MathNet.Numerics.LinearAlgebra.Double
         /// </param>
         protected override void DoAdd(Vector<double> other, Vector<double> result)
         {
-            if (ReferenceEquals(this, result))
+            var otherSparse = other as SparseVector;
+            if (otherSparse == null)
             {
-                CommonParallel.For(
-                    0,
-                    NonZerosCount,
-                    index => _nonZeroValues[index] += _nonZeroValues[index]);
+                base.DoAdd(other, result);
+                return;
+            }
+
+            var resultSparse = result as SparseVector;
+            if (resultSparse == null)
+            {
+                base.DoAdd(other, result);
+                return;
+            }
+
+            // TODO (ruegg, 2011-10-11): Options to optimize?
+
+            if (ReferenceEquals(this, resultSparse))
+            {
+                int i = 0, j = 0;
+                while (i < NonZerosCount || j < otherSparse.NonZerosCount)
+                {
+                    if (i < NonZerosCount && j < otherSparse.NonZerosCount && _nonZeroIndices[i] == otherSparse._nonZeroIndices[j])
+                    {
+                        _nonZeroValues[i++] += otherSparse._nonZeroValues[j++];
+                    }
+                    else if (j >= otherSparse.NonZerosCount || i < NonZerosCount && _nonZeroIndices[i] < otherSparse._nonZeroIndices[j])
+                    {
+                        _nonZeroValues[i] += otherSparse.At(_nonZeroIndices[i]);
+                        i++;
+                    }
+                    else
+                    {
+                        var otherValue = otherSparse._nonZeroValues[j];
+                        if (otherValue != 0.0)
+                        {
+                            InsertAtUnchecked(i++, otherSparse._nonZeroIndices[j], otherValue);
+                        }
+                        j++;
+                    }
+                }
             }
             else
             {
-                for (var index = 0; index < Count; index++)
+                result.Clear();
+                int i = 0, j = 0, last = -1;
+                while (i < NonZerosCount || j < otherSparse.NonZerosCount)
                 {
-                    result.At(index, At(index) + other.At(index));
+                    if (j >= otherSparse.NonZerosCount || i < NonZerosCount && _nonZeroIndices[i] <= otherSparse._nonZeroIndices[j])
+                    {
+                        var next = _nonZeroIndices[i];
+                        if (next != last)
+                        {
+                            last = next;
+                            result.At(next, _nonZeroValues[i] + otherSparse.At(next));
+                        }
+                        i++;
+                    }
+                    else
+                    {
+                        var next = otherSparse._nonZeroIndices[j];
+                        if (next != last)
+                        {
+                            last = next;
+                            result.At(next, At(next) + otherSparse._nonZeroValues[j]);
+                        }
+                        j++;
+                    }
                 }
             }
         }
diff --git a/src/Numerics/LinearAlgebra/Single/SparseVector.cs b/src/Numerics/LinearAlgebra/Single/SparseVector.cs
index 7e9e6c7d..1c8ce6f8 100644
--- a/src/Numerics/LinearAlgebra/Single/SparseVector.cs
+++ b/src/Numerics/LinearAlgebra/Single/SparseVector.cs
@@ -381,18 +381,73 @@ namespace MathNet.Numerics.LinearAlgebra.Single
         /// </param>
         protected override void DoAdd(Vector<float> other, Vector<float> result)
         {
-            if (ReferenceEquals(this, result))
+            var otherSparse = other as SparseVector;
+            if (otherSparse == null)
             {
-                CommonParallel.For(
-                    0,
-                    NonZerosCount,
-                    index => _nonZeroValues[index] += _nonZeroValues[index]);
+                base.DoAdd(other, result);
+                return;
+            }
+
+            var resultSparse = result as SparseVector;
+            if (resultSparse == null)
+            {
+                base.DoAdd(other, result);
+                return;
+            }
+
+            // TODO (ruegg, 2011-10-11): Options to optimize?
+
+            if (ReferenceEquals(this, resultSparse))
+            {
+                int i = 0, j = 0;
+                while (i < NonZerosCount || j < otherSparse.NonZerosCount)
+                {
+                    if (i < NonZerosCount && j < otherSparse.NonZerosCount && _nonZeroIndices[i] == otherSparse._nonZeroIndices[j])
+                    {
+                        _nonZeroValues[i++] += otherSparse._nonZeroValues[j++];
+                    }
+                    else if (j >= otherSparse.NonZerosCount || i < NonZerosCount && _nonZeroIndices[i] < otherSparse._nonZeroIndices[j])
+                    {
+                        _nonZeroValues[i] += otherSparse.At(_nonZeroIndices[i]);
+                        i++;
+                    }
+                    else
+                    {
+                        var otherValue = otherSparse._nonZeroValues[j];
+                        if (otherValue != 0.0)
+                        {
+                            InsertAtUnchecked(i++, otherSparse._nonZeroIndices[j], otherValue);
+                        }
+                        j++;
+                    }
+                }
             }
             else
             {
-                for (var index = 0; index < Count; index++)
+                result.Clear();
+                int i = 0, j = 0, last = -1;
+                while (i < NonZerosCount || j < otherSparse.NonZerosCount)
                 {
-                    result.At(index, At(index) + other.At(index));
+                    if (j >= otherSparse.NonZerosCount || i < NonZerosCount && _nonZeroIndices[i] <= otherSparse._nonZeroIndices[j])
+                    {
+                        var next = _nonZeroIndices[i];
+                        if (next != last)
+                        {
+                            last = next;
+                            result.At(next, _nonZeroValues[i] + otherSparse.At(next));
+                        }
+                        i++;
+                    }
+                    else
+                    {
+                        var next = otherSparse._nonZeroIndices[j];
+                        if (next != last)
+                        {
+                            last = next;
+                            result.At(next, At(next) + otherSparse._nonZeroValues[j]);
+                        }
+                        j++;
+                    }
                 }
             }
         }