diff --git a/Examples/BB2B/Program.cs b/Examples/BB2B/Program.cs
deleted file mode 100644
index 4990590..0000000
--- a/Examples/BB2B/Program.cs
+++ /dev/null
@@ -1,33 +0,0 @@
-using System.Threading;
-using RoboidControl;
-
-#if !UNITY_5_3_OR_NEWER
-class BB2B {
-    static void Main() {
-        // The robot's propulsion is a differential drive
-        DifferentialDrive bb2b = new();
-        // It has a touch sensor at the front left of the roboid
-        TouchSensor touchLeft = new(bb2b);
-        // and other one on the right
-        TouchSensor touchRight = new(bb2b);
-
-        // Do forever:
-        while (true) {
-            // The left wheel turns forward when nothing is touched on the right side
-            // and turn backward when the roboid hits something on the right
-            float leftWheelSpeed = touchRight.touchedSomething ? -600.0f : 600.0f;
-            // The right wheel does the same, but instead is controlled by
-            // touches on the left side
-            float rightWheelSpeed = touchLeft.touchedSomething ? -600.0f : 600.0f;
-            // When both sides are touching something, both wheels will turn backward
-            // and the roboid will move backwards
-            bb2b.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);
-
-            // Update the roboid state
-            bb2b.Update(true);
-            // and sleep for 100ms
-            Thread.Sleep(100);
-        }
-    }
-}
-#endif
\ No newline at end of file
diff --git a/LinearAlgebra/src/Decomposition.cs b/LinearAlgebra/src/Decomposition.cs
new file mode 100644
index 0000000..ddaf434
--- /dev/null
+++ b/LinearAlgebra/src/Decomposition.cs
@@ -0,0 +1,287 @@
+using System;
+namespace LinearAlgebra {
+    class QR {
+        // QR Decomposition of a matrix A
+        public static (Matrix2 Q, Matrix2 R) Decomposition(Matrix2 A) {
+            int nRows = A.nRows;
+            int nCols = A.nCols;
+
+            float[,] Q = new float[nRows, nCols];
+            float[,] R = new float[nCols, nCols];
+
+            // Perform Gram-Schmidt orthogonalization
+            for (uint colIx = 0; colIx < nCols; colIx++) {
+
+                // Step 1: v = column(ix) of A
+                float[] v = new float[nRows];
+                for (int rowIx = 0; rowIx < nRows; rowIx++)
+                    v[rowIx] = A.data[rowIx, colIx];
+
+                // Step 2: Subtract projections of v onto previous q's (orthogonalize)
+                for (uint colIx2 = 0; colIx2 < colIx; colIx2++) {
+                    float dotProd = 0;
+                    for (int i = 0; i < nRows; i++)
+                        dotProd += Q[i, colIx2] * v[i];
+                    for (int i = 0; i < nRows; i++)
+                        v[i] -= dotProd * Q[i, colIx2];
+                }
+
+                // Step 3: Normalize v to get column(ix) of Q
+                float norm = 0;
+                for (int rowIx = 0; rowIx < nRows; rowIx++)
+                    norm += v[rowIx] * v[rowIx];
+                norm = (float)Math.Sqrt(norm);
+
+                for (int rowIx = 0; rowIx < nRows; rowIx++)
+                    Q[rowIx, colIx] = v[rowIx] / norm;
+
+                // Store the coefficients of R
+                for (int colIx2 = 0; colIx2 <= colIx; colIx2++) {
+                    R[colIx2, colIx] = 0;
+                    for (int k = 0; k < nRows; k++)
+                        R[colIx2, colIx] += Q[k, colIx2] * A.data[k, colIx];
+                }
+            }
+            return (new Matrix2(Q), new Matrix2(R));
+        }
+
+        // Reduced QR Decomposition of a matrix A
+        public static (Matrix2 Q, Matrix2 R) ReducedDecomposition(Matrix2 A) {
+            int nRows = A.nRows;
+            int nCols = A.nCols;
+
+            float[,] Q = new float[nRows, nCols];
+            float[,] R = new float[nCols, nCols];
+
+            // Perform Gram-Schmidt orthogonalization
+            for (int colIx = 0; colIx < nCols; colIx++) {
+
+                // Step 1: v = column(colIx) of A
+                float[] columnIx = new float[nRows];
+                bool isZeroColumn = true;
+                for (int rowIx = 0; rowIx < nRows; rowIx++) {
+                    columnIx[rowIx] = A.data[rowIx, colIx];
+                    if (columnIx[rowIx] != 0)
+                        isZeroColumn = false;
+                }
+                if (isZeroColumn) {
+                    for (int rowIx = 0; rowIx < nRows; rowIx++)
+                        Q[rowIx, colIx] = 0;
+                    // Set corresponding R element to 0
+                    R[colIx, colIx] = 0;
+
+                    Console.WriteLine($"zero column {colIx}");
+
+                    continue;
+                }
+
+                // Step 2: Subtract projections of v onto previous q's (orthogonalize)
+                for (int colIx2 = 0; colIx2 < colIx; colIx2++) {
+                    // Compute the dot product of v and column(colIx2) of Q
+                    float dotProduct = 0;
+                    for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++)
+                        dotProduct += columnIx[rowIx2] * Q[rowIx2, colIx2];
+                    // Subtract the projection from v
+                    for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++)
+                        columnIx[rowIx2] -= dotProduct * Q[rowIx2, colIx2];
+                }
+
+                // Step 3: Normalize v to get column(colIx) of Q
+                float norm = 0;
+                for (int rowIx = 0; rowIx < nRows; rowIx++)
+                    norm += columnIx[rowIx] * columnIx[rowIx];
+                if (norm == 0)
+                    throw new Exception("invalid value");
+
+                norm = (float)Math.Sqrt(norm);
+
+                for (int rowIx = 0; rowIx < nRows; rowIx++)
+                    Q[rowIx, colIx] = columnIx[rowIx] / norm;
+
+                // Here is where it deviates from the Full QR Decomposition !
+
+                // Step 4: Compute the row(colIx) of R
+                for (int colIx2 = colIx; colIx2 < nCols; colIx2++) {
+                    float dotProduct = 0;
+                    for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++)
+                        dotProduct += Q[rowIx2, colIx] * A.data[rowIx2, colIx2];
+                    R[colIx, colIx2] = dotProduct;
+                }
+            }
+            if (!float.IsFinite(R[0, 0]))
+                throw new Exception("invalid value");
+
+            return (new Matrix2(Q), new Matrix2(R));
+        }
+    }
+
+    class SVD {
+        // According to ChatGPT, Mathnet uses  Golub-Reinsch SVD algorithm
+        // 1. Bidiagonalization: The input matrix AA is reduced to a bidiagonal form using Golub-Kahan bidiagonalization.
+        // This process involves applying a sequence of Householder reflections to AA to create a bidiagonal matrix.
+        // This step reduces the complexity by making the matrix simpler while retaining the essential structure needed for SVD.
+        //
+        // 2. Diagonalization: Once the matrix is in bidiagonal form,
+        // the singular values are computed using an iterative process
+        // (typically involving QR factorization or Jacobi rotations) until convergence.
+        // This process diagonalizes the bidiagonal matrix and allows extraction of the singular values.
+        //
+        // 3. Computing UU and VTVT: After obtaining the singular values,
+        // the left singular vectors UU and right singular vectors VTVT are computed
+        // using the accumulated transformations (such as Householder reflections) from the bidiagonalization step.
+
+        // Bidiagnolizations through Householder transformations
+        public static (Matrix2 U1, Matrix2 B, Matrix2 V1) Bidiagonalization(Matrix2 A) {
+            int m = A.nRows; // Rows of A
+            int n = A.nCols; // Columns of A
+            float[,] U1 = new float[m, m]; // Left orthogonal matrix
+            float[,] V1 = new float[n, n]; // Right orthogonal matrix
+            float[,] B = A.Clone().data; // Copy A to B for transformation
+
+            // Initialize U1 and V1 as identity matrices
+            for (int i = 0; i < m; i++)
+                U1[i, i] = 1;
+            for (int i = 0; i < n; i++)
+                V1[i, i] = 1;
+
+            // Perform Householder reflections to create a bidiagonal matrix B
+            for (int j = 0; j < n; j++) {
+                // Step 1: Construct the Householder vector y
+                float[] y = new float[m - j];
+                for (int i = j; i < m; i++)
+                    y[i - j] = B[i, j];
+
+                // Step 2: Compute the norm and scalar alpha
+                float norm = 0;
+                for (int i = 0; i < y.Length; i++)
+                    norm += y[i] * y[i];
+                norm = (float)Math.Sqrt(norm);
+
+                if (B[j, j] > 0)
+                    norm = -norm;
+
+                float alpha = (float)Math.Sqrt(0.5 * (norm * (norm - B[j, j])));
+                float r = (float)Math.Sqrt(0.5 * (norm * (norm + B[j, j])));
+
+                // Step 3: Apply the reflection to zero out below diagonal
+                for (int k = j; k < n; k++) {
+                    float dot = 0;
+                    for (int i = j; i < m; i++)
+                        dot += y[i - j] * B[i, k];
+                    dot /= r;
+
+                    for (int i = j; i < m; i++)
+                        B[i, k] -= 2 * dot * y[i - j];
+                }
+
+                // Step 4: Update U1 with the Householder reflection (U1 * Householder)
+                for (int i = j; i < m; i++)
+                    U1[i, j] = y[i - j] / alpha;
+
+                // Step 5: Update V1 (storing the Householder vector y)
+                // Correct indexing: we only need to store part of y in V1 from index j to n
+                for (int i = j; i < n; i++)
+                    V1[j, i] = B[j, i];
+
+                // Repeat steps for further columns if necessary
+            }
+            return (new Matrix2(U1), new Matrix2(B), new Matrix2(V1));
+        }
+
+        public static Matrix2 Bidiagonalize(Matrix2 A) {
+            int m = A.nRows; // Rows of A
+            int n = A.nCols; // Columns of A
+            float[,] B = A.Clone().data; // Copy A to B for transformation
+
+            // Perform Householder reflections to create a bidiagonal matrix B
+            for (int j = 0; j < n; j++) {
+                // Step 1: Construct the Householder vector y
+                float[] y = new float[m - j];
+                for (int i = j; i < m; i++)
+                    y[i - j] = B[i, j];
+
+                // Step 2: Compute the norm and scalar alpha
+                float norm = 0;
+                for (int i = 0; i < y.Length; i++)
+                    norm += y[i] * y[i];
+                norm = (float)Math.Sqrt(norm);
+
+                if (B[j, j] > 0)
+                    norm = -norm;
+
+                float r = (float)Math.Sqrt(0.5 * (norm * (norm + B[j, j])));
+
+                // Step 3: Apply the reflection to zero out below diagonal
+                for (int k = j; k < n; k++) {
+                    float dot = 0;
+                    for (int i = j; i < m; i++)
+                        dot += y[i - j] * B[i, k];
+                    dot /= r;
+
+                    for (int i = j; i < m; i++)
+                        B[i, k] -= 2 * dot * y[i - j];
+                }
+
+                // Repeat steps for further columns if necessary
+            }
+            return new Matrix2(B);
+        }
+
+        // QR Iteration for diagonalization of a bidiagonal matrix B
+        public static (Matrix1 singularValues, Matrix2 U, Matrix2 Vt) QRIteration(Matrix2 B) {
+            int m = B.nRows;
+            int n = B.nCols;
+
+            Matrix2 U = new(m, m); // Left singular vectors (U)
+            Matrix2 Vt = new(n, n); // Right singular vectors (V^T)
+            float[] singularValues = new float[Math.Min(m, n)]; // Singular values
+
+            // Initialize U and Vt as identity matrices
+            for (int i = 0; i < m; i++)
+                U.data[i, i] = 1;
+            for (int i = 0; i < n; i++)
+                Vt.data[i, i] = 1;
+
+            // Perform QR iterations
+            float tolerance = 1e-7f; //1e-12f; for double
+            bool converged = false;
+            while (!converged) {
+                // Perform QR decomposition on the matrix B
+                (Matrix2 Q, Matrix2 R) = QR.Decomposition(B);
+
+                // Update B to be the product Q * R //R * Q
+                B = R * Q;
+
+                // Accumulate the transformations in U and Vt
+                U *= Q;
+                Vt *= R;
+
+                // Check convergence by looking at the off-diagonal elements of B
+                converged = true;
+                for (int i = 0; i < m - 1; i++) {
+                    for (int j = i + 1; j < n; j++) {
+                        if (Math.Abs(B.data[i, j]) > tolerance) {
+                            converged = false;
+                            break;
+                        }
+                    }
+                }
+            }
+
+            // Extract singular values (diagonal elements of B)
+            for (int i = 0; i < Math.Min(m, n); i++)
+                singularValues[i] = B.data[i, i];
+
+            return (new Matrix1(singularValues), U, Vt);
+        }
+
+        public static (Matrix2 U, Matrix1 S, Matrix2 Vt) Decomposition(Matrix2 A) {
+            if (A.nRows != A.nCols)
+                throw new ArgumentException("SVD: matrix A has to be square.");
+
+            Matrix2 B = Bidiagonalize(A);
+            (Matrix1 S, Matrix2 U, Matrix2 Vt) = QRIteration(B);
+            return (U, S, Vt);
+        }
+    }
+}
\ No newline at end of file
diff --git a/LinearAlgebra/src/Matrix.cs b/LinearAlgebra/src/Matrix.cs
index 1f0542e..8ee1d0d 100644
--- a/LinearAlgebra/src/Matrix.cs
+++ b/LinearAlgebra/src/Matrix.cs
@@ -10,9 +10,9 @@ namespace LinearAlgebra
 
     public readonly struct Slice
     {
-        public uint start { get; }
-        public uint stop { get; }
-        public Slice(uint start, uint stop)
+        public int start { get; }
+        public int stop { get; }
+        public Slice(int start, int stop)
         {
             this.start = start;
             this.stop = stop;
@@ -23,10 +23,10 @@ namespace LinearAlgebra
     {
         public float[,] data { get; }
 
-        public uint nRows => (uint)data.GetLength(0);
-        public uint nCols => (uint)data.GetLength(1);
+        public int nRows => data.GetLength(0);
+        public int nCols => data.GetLength(1);
 
-        public Matrix2(uint nRows, uint nCols)
+        public Matrix2(int nRows, int nCols)
         {
             this.data = new float[nRows, nCols];
         }
@@ -46,7 +46,7 @@ namespace LinearAlgebra
             return new Matrix2(data);
         }
 
-        public static Matrix2 Zero(uint nRows, uint nCols)
+        public static Matrix2 Zero(int nRows, int nCols)
         {
             return new Matrix2(nRows, nCols);
         }
@@ -60,11 +60,11 @@ namespace LinearAlgebra
             return new Matrix2(result);
         }
 
-        public static Matrix2 Identity(uint size)
+        public static Matrix2 Identity(int size)
         {
             return Diagonal(1, size);
         }
-        public static Matrix2 Identity(uint nRows, uint nCols)
+        public static Matrix2 Identity(int nRows, int nCols)
         {
             Matrix2 m = Zero(nRows, nCols);
             m.FillDiagonal(1);
@@ -78,7 +78,7 @@ namespace LinearAlgebra
                 resultData[ix, ix] = v.data[ix];
             return new Matrix2(resultData);
         }
-        public static Matrix2 Diagonal(float f, uint size)
+        public static Matrix2 Diagonal(float f, int size)
         {
             float[,] resultData = new float[size, size];
             for (int ix = 0; ix < size; ix++)
@@ -87,13 +87,13 @@ namespace LinearAlgebra
         }
         public void FillDiagonal(Matrix1 v)
         {
-            uint n = Math.Min(Math.Min(this.nRows, this.nCols), v.size);
+            int n = (int)Math.Min(Math.Min(this.nRows, this.nCols), v.size);
             for (int ix = 0; ix < n; ix++)
                 this.data[ix, ix] = v.data[ix];
         }
         public void FillDiagonal(float f)
         {
-            uint n = Math.Min(this.nRows, this.nCols);
+            int n = Math.Min(this.nRows, this.nCols);
             for (int ix = 0; ix < n; ix++)
                 this.data[ix, ix] = f;
         }
@@ -108,9 +108,17 @@ namespace LinearAlgebra
             return new Matrix2(result);
         }
 
+        public Matrix1 GetRow(int rowIx) {
+            float[] row = new float[this.nCols];
+            for (int colIx = 0; colIx < this.nCols; colIx++) {
+                row[colIx] = this.data[rowIx, colIx];
+            }
+            return new Matrix1(row);
+        }
+
 #if UNITY_5_3_OR_NEWER
     public Vector3Float GetRow3(int rowIx) {
-        uint cols = this.nCols;
+        int cols = this.nCols;
         Vector3Float row = new() {
             x = this.data[rowIx, 0],
             y = this.data[rowIx, 1],
@@ -131,6 +139,14 @@ namespace LinearAlgebra
             this.data[rowIx, 2] = v.z;
         }
 
+        public void SwapRows(int row1, int row2) {
+            for (uint ix = 0; ix < this.nCols; ix++) {
+                float temp = this.data[row1, ix];
+                this.data[row1, ix] = this.data[row2, ix];
+                this.data[row2, ix] = temp;
+            }
+        }
+
         public Matrix1 GetColumn(int colIx)
         {
             float[] column = new float[this.nRows];
@@ -140,6 +156,15 @@ namespace LinearAlgebra
             }
             return new Matrix1(column);
         }
+        public void SetColumn(int colIx, Matrix1 v) {
+            for (uint ix = 0; ix < v.size; ix++)
+                this.data[ix, colIx] = v.data[ix];
+        }
+        public void SetColumn(int colIx, Vector3Float v) {
+            this.data[0, colIx] = v.x;
+            this.data[1, colIx] = v.y;
+            this.data[2, colIx] = v.z;
+        }
 
         public static bool AllClose(Matrix2 A, Matrix2 B, float atol = 1e-08f)
         {
@@ -302,18 +327,37 @@ namespace LinearAlgebra
             return new Matrix2(result);
         }
 
+        public Matrix2 GetRows(Slice slice) {
+            return GetRows(slice.start, slice.stop);
+        }
+        public Matrix2 GetRows(int from, int to) {
+            if (from < 0 || to >= this.nRows)
+                throw new System.ArgumentException("Slice index out of range.");
+
+            float[,] result = new float[to - from, this.nCols];
+            int resultRowIx = 0;
+            for (int rowIx = from; rowIx < to; rowIx++) {
+                for (int colIx = 0; colIx < this.nCols; colIx++)
+                    result[resultRowIx, colIx] = this.data[rowIx, colIx];
+                
+                resultRowIx++;
+            }
+
+            return new Matrix2(result);
+        }
+
         public Matrix2 Slice(Slice slice)
         {
             return Slice(slice.start, slice.stop);
         }
-        public Matrix2 Slice(uint from, uint to)
+        public Matrix2 Slice(int from, int to)
         {
             if (from < 0 || to >= this.nRows)
                 throw new System.ArgumentException("Slice index out of range.");
 
             float[,] result = new float[to - from, this.nCols];
             int resultRowIx = 0;
-            for (uint rowIx = from; rowIx < to; rowIx++)
+            for (int rowIx = from; rowIx < to; rowIx++)
             {
                 for (int colIx = 0; colIx < this.nCols; colIx++)
                 {
@@ -328,39 +372,42 @@ namespace LinearAlgebra
         {
             return Slice((rowRange.start, rowRange.stop), (colRange.start, colRange.stop));
         }
-
-        public Matrix2 Slice((uint start, uint stop) rowRange, (uint start, uint stop) colRange)
+        public Matrix2 Slice((int start, int stop) rowRange, (int start, int stop) colRange)
         {
             float[,] result = new float[rowRange.stop - rowRange.start, colRange.stop - colRange.start];
 
-            uint resultRowIx = 0;
-            uint resultColIx = 0;
-            for (uint i = rowRange.start; i < rowRange.stop; i++)
+            int resultRowIx = 0;
+            int resultColIx = 0;
+            for (int i = rowRange.start; i < rowRange.stop; i++)
             {
-                for (uint j = colRange.start; j < colRange.stop; j++)
+                for (int j = colRange.start; j < colRange.stop; j++)
                     result[resultRowIx, resultColIx] = this.data[i, j];
             }
             return new Matrix2(result);
         }
 
-        public void UpdateSlice(Slice slice, Matrix2 m)
-        {
+        public void UpdateSlice(Slice slice, Matrix2 m) {
+            UpdateSlice((slice.start, slice.stop), m);
+        }
+        public void UpdateSlice((int start, int stop) slice, Matrix2 m) {
+            // if (slice.start == slice.stop)
+            //     Console.WriteLine("WARNING: no data is updates when start equals stop in a slice!");
             int mRowIx = 0;
-            for (uint rowIx = slice.start; rowIx < slice.stop; rowIx++, mRowIx++)
-            {
+            for (int rowIx = slice.start; rowIx < slice.stop; rowIx++, mRowIx++) {
                 for (int colIx = 0; colIx < this.nCols; colIx++)
                     this.data[rowIx, colIx] = m.data[mRowIx, colIx];
             }
         }
+
         public void UpdateSlice(Slice rowRange, Slice colRange, Matrix2 m)
         {
             UpdateSlice((rowRange.start, rowRange.stop), (colRange.start, colRange.stop), m);
         }
-        public void UpdateSlice((uint start, uint stop) rowRange, (uint start, uint stop) colRange, Matrix2 m)
+        public void UpdateSlice((int start, int stop) rowRange, (int start, int stop) colRange, Matrix2 m)
         {
-            for (uint i = rowRange.start; i < rowRange.stop; i++)
+            for (int i = rowRange.start; i < rowRange.stop; i++)
             {
-                for (uint j = colRange.start; j < colRange.stop; j++)
+                for (int j = colRange.start; j < colRange.stop; j++)
                     this.data[i, j] = m.data[i - rowRange.start, j - colRange.start];
             }
         }
@@ -369,7 +416,7 @@ namespace LinearAlgebra
         {
             Matrix2 A = this;
             // unchecked
-            uint n = A.nRows;
+            int n = A.nRows;
 
             // Create an identity matrix of the same size as the original matrix
             float[,] augmentedMatrix = new float[n, 2 * n];
@@ -404,10 +451,10 @@ namespace LinearAlgebra
             }
 
             // Back substitution
-            for (uint i = n - 1; i >= 0; i--)
+            for (int i = n - 1; i >= 0; i--)
             {
                 // Eliminate the column above the pivot
-                for (uint j = i - 1; j >= 0; j--)
+                for (int j = i - 1; j >= 0; j--)
                 {
                     float factor = augmentedMatrix[j, i];
                     for (int k = 0; k < 2 * n; k++)
@@ -428,7 +475,7 @@ namespace LinearAlgebra
 
         public float Determinant()
         {
-            uint n = this.nRows;
+            int n = this.nRows;
             if (n != this.nCols)
                 throw new System.ArgumentException("Matrix must be square.");
 
@@ -448,7 +495,7 @@ namespace LinearAlgebra
         // Helper function to compute the minor of a matrix
         private Matrix2 Minor(int rowToRemove, int colToRemove)
         {
-            uint n = this.nRows;
+            int n = this.nRows;
             float[,] minor = new float[n - 1, n - 1];
 
             int r = 0, c = 0;
@@ -469,15 +516,46 @@ namespace LinearAlgebra
 
             return new Matrix2(minor);
         }
+
+        public static Matrix2 DeleteRows(Matrix2 A, Slice rowRange) {
+            float[,] result = new float[A.nRows - (rowRange.stop - rowRange.start), A.nCols];
+
+            int resultRowIx = 0;
+            for (int i = 0; i < A.nRows; i++) {
+                if (i >= rowRange.start && i < rowRange.stop)
+                    continue;
+
+                for (int j = 0; j < A.nCols; j++)
+                    result[resultRowIx, j] = A.data[i, j];
+                    
+                resultRowIx++;
+            }
+            return new Matrix2(result);
+        }
+
+        internal static Matrix2 DeleteColumns(Matrix2 A, Slice colRange) {
+            float[,] result = new float[A.nRows, A.nCols - (colRange.stop - colRange.start)];
+
+            for (int i = 0; i < A.nRows; i++) {
+                int resultColIx = 0;
+                for (int j = 0; j < A.nCols; j++) {
+                    if (j >= colRange.start && j < colRange.stop)
+                        continue;
+
+                    result[i, resultColIx++] = A.data[i, j];
+                }
+            }
+            return new Matrix2(result);
+        }    
     }
 
     public class Matrix1
     {
         public float[] data { get; }
 
-        public uint size => (uint)data.GetLength(0);
+        public int size => data.GetLength(0);
 
-        public Matrix1(uint size)
+        public Matrix1(int size)
         {
             this.data = new float[size];
         }
@@ -487,7 +565,7 @@ namespace LinearAlgebra
             this.data = data;
         }
 
-        public static Matrix1 Zero(uint size)
+        public static Matrix1 Zero(int size)
         {
             return new Matrix1(size);
         }
@@ -604,6 +682,18 @@ namespace LinearAlgebra
             return result;
         }
 
+        public static Matrix1 operator -(Matrix1 A, Matrix1 B) {
+            if (A.size != B.size)
+                throw new System.ArgumentException("Size of A must match size of B.");
+
+            float[] result = new float[A.size];
+
+            for (int i = 0; i < A.size; i++) {
+                result[i] = A.data[i] - B.data[i];
+            }
+            return new Matrix1(result);
+        }
+
         public static Matrix1 operator *(Matrix1 A, float f)
         {
             float[] result = new float[A.size];
@@ -618,18 +708,35 @@ namespace LinearAlgebra
             return A * f;
         }
 
+        public static Matrix1 operator /(Matrix1 A, float f) {
+            float[] result = new float[A.size];
+
+            for (int i = 0; i < A.size; i++)
+                result[i] = A.data[i] / f;
+
+            return new Matrix1(result);
+        }
+        public static Matrix1 operator /(float f, Matrix1 A) {
+            float[] result = new float[A.size];
+
+            for (int i = 0; i < A.size; i++)
+                result[i] = f / A.data[i];
+
+            return new Matrix1(result);
+        }
+
         public Matrix1 Slice(Slice range)
         {
             return Slice(range.start, range.stop);
         }
-        public Matrix1 Slice(uint from, uint to)
+        public Matrix1 Slice(int from, int to)
         {
             if (from < 0 || to >= this.size)
                 throw new System.ArgumentException("Slice index out of range.");
 
             float[] result = new float[to - from];
             int resultIx = 0;
-            for (uint ix = from; ix < to; ix++)
+            for (int ix = from; ix < to; ix++)
                 result[resultIx++] = this.data[ix];
 
             return new Matrix1(result);
@@ -637,7 +744,7 @@ namespace LinearAlgebra
         public void UpdateSlice(Slice slice, Matrix1 v)
         {
             int vIx = 0;
-            for (uint ix = slice.start; ix < slice.stop; ix++, vIx++)
+            for (int ix = slice.start; ix < slice.stop; ix++, vIx++)
                 this.data[ix] = v.data[vIx];
         }
     }
diff --git a/LinearAlgebra/src/Quaternion.cs b/LinearAlgebra/src/Quaternion.cs
index f481ef4..cd7e9f4 100644
--- a/LinearAlgebra/src/Quaternion.cs
+++ b/LinearAlgebra/src/Quaternion.cs
@@ -3,24 +3,26 @@ using System;
 using Quaternion = UnityEngine.Quaternion;
 #endif
 
-namespace LinearAlgebra
-{
+namespace LinearAlgebra {
 
-    public class QuaternionOf<T>
-    {
+    public class QuaternionOf<T> {
         public T x;
         public T y;
         public T z;
         public T w;
 
-        public QuaternionOf(T x, T y, T z, T w)
-        {
+        public QuaternionOf(T x, T y, T z, T w) {
             this.x = x;
             this.y = y;
             this.z = z;
             this.w = w;
         }
 
+        public static Quaternion Reflect(Quaternion q) {
+            return new(-q.x, -q.y, -q.z, q.w);
+        }
+
+
 #if UNITY_5_3_OR_NEWER
         public static Matrix2 ToRotationMatrix(Quaternion q) {
             float w = q.x, x = q.y, y = q.z, z = q.w;
diff --git a/Unity/SiteServer.cs b/Unity/SiteServer.cs
index d1f4236..4dde64b 100644
--- a/Unity/SiteServer.cs
+++ b/Unity/SiteServer.cs
@@ -18,7 +18,8 @@ namespace RoboidControl.Unity {
         }
 
         void OnApplicationQuit() {
-            site.Close();
+            if (site != null)
+                site.Close();
         }
 
         public void HandleNewThing(RoboidControl.Thing thing) {
diff --git a/Unity/Thing.cs b/Unity/Thing.cs
index 98551fe..129f34b 100644
--- a/Unity/Thing.cs
+++ b/Unity/Thing.cs
@@ -31,7 +31,7 @@ namespace RoboidControl.Unity {
             core.component = this;
 
             SiteServer siteServer = FindAnyObjectByType<SiteServer>();
-            if (siteServer == null) {
+            if (siteServer == null || siteServer.site == null) {
                 Debug.LogWarning("No site server found");
                 return;
             }
@@ -93,7 +93,7 @@ namespace RoboidControl.Unity {
         }
 
         private void PoseChanged() {
-            Debug.Log($"{this} pose changed");
+            // Debug.Log($"{this} pose changed");
             if (core.positionUpdated)
                 this.transform.localPosition = core.position.ToVector3();
             if (core.orientationUpdated)
diff --git a/Unity/TouchSensor.cs b/Unity/TouchSensor.cs
index 2859fc4..bcce86e 100644
--- a/Unity/TouchSensor.cs
+++ b/Unity/TouchSensor.cs
@@ -1,5 +1,4 @@
 #if UNITY_5_3_OR_NEWER
-using Unity.VisualScripting;
 using UnityEngine;
 
 namespace RoboidControl.Unity {
diff --git a/src/Messages/BinaryMsg.cs b/src/Messages/BinaryMsg.cs
index 6953188..d9ab9f2 100644
--- a/src/Messages/BinaryMsg.cs
+++ b/src/Messages/BinaryMsg.cs
@@ -12,7 +12,7 @@ namespace RoboidControl {
         /// The length of the message, excluding the binary data
         /// </summary>
         /// For the total size of the message this.bytes.Length should be added to this value.
-        public const byte length = 2;
+        public const byte length = 4;
         /// <summary>
         /// The network ID identifying the thing
         /// </summary>
@@ -23,23 +23,15 @@ namespace RoboidControl {
         public byte thingId;
         
         public Thing thing;        
-
+        /// <summary>
+        /// The length of the data
+        /// </summary>
+        public byte dataLength;
         /// <summary>
         /// The binary data
         /// </summary>
-        public byte[] bytes;
+        public byte[] data;
 
-        /// <summary>
-        /// Create a new message for sending
-        /// </summary>
-        /// <param name="networkId">The netowork ID of the thing</param>
-        /// <param name="thingId">The ID of the thing</param>
-        /// <param name="bytes">The binary data for the thing</param>
-        public BinaryMsg(byte networkId, byte thingId, byte[] bytes) : base() {
-            this.networkId = networkId;
-            this.thingId = thingId;
-            this.bytes = bytes;
-        }
         /// <summary>
         /// Create an empty message for sending
         /// </summary>
@@ -49,31 +41,34 @@ namespace RoboidControl {
             this.networkId = networkId;
             this.thingId = thing.id;
             this.thing = thing;
-            this.bytes = this.thing.GenerateBinary();
-            // if (this.bytes.Length > 0)
-            //     System.Console.Write($"Binary message for [{networkId}/{thing.id}]");
+            this.data = this.thing.GenerateBinary();
+            this.dataLength = (byte)this.data.Length;
         }
         /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
         public BinaryMsg(byte[] buffer) {
             byte ix = 1;
             this.networkId = buffer[ix++];
             this.thingId = buffer[ix++];
-            byte length = (byte)(buffer.Length - ix);
-            this.bytes = new byte[length];
-            for (uint bytesIx = 0; bytesIx < length; bytesIx++)
-                this.bytes[bytesIx] = buffer[ix++];
+            this.dataLength = buffer[ix++];
+            this.data = new byte[this.dataLength];
+            for (uint dataIx = 0; dataIx < this.dataLength; dataIx++)
+                this.data[dataIx] = buffer[ix++];
         }
 
         /// @copydoc Passer::RoboidControl::IMessage::Serialize
         public override byte Serialize(ref byte[] buffer) {            
-            if (buffer.Length < BinaryMsg.length + this.bytes.Length || this.bytes.Length == 0)
+            if (buffer.Length < BinaryMsg.length + this.data.Length || this.data.Length == 0)
                 return 0;
-                
+            
+#if DEBUG
+            System.Console.WriteLine($"Send BinaryMsg [{this.networkId}/{this.thingId}] {this.dataLength}");
+#endif
             byte ix = 0;
             buffer[ix++] = BinaryMsg.Id;
             buffer[ix++] = this.networkId;
             buffer[ix++] = this.thingId;
-            foreach (byte b in bytes)
+            buffer[ix++] = this.dataLength;
+            foreach (byte b in data)
                 buffer[ix++] = b;
 
             return ix;
diff --git a/src/Messages/ModelUrlMsg.cs b/src/Messages/ModelUrlMsg.cs
index be15087..64efac5 100644
--- a/src/Messages/ModelUrlMsg.cs
+++ b/src/Messages/ModelUrlMsg.cs
@@ -21,6 +21,10 @@ namespace RoboidControl {
         /// </summary>
         public byte thingId;
         /// <summary>
+        /// The length of the url string, excluding the null terminator
+        /// </summary>
+        public byte urlLength;
+        /// <summary>
         /// The URL of the model
         /// </summary>
         public string url = null;
@@ -33,6 +37,7 @@ namespace RoboidControl {
         public ModelUrlMsg(byte networkId, Thing thing) {
             this.networkId = networkId;
             this.thingId = thing.id;
+            this.urlLength = (byte)thing.modelUrl.Length;
             this.url = thing.modelUrl;
         }
         /// <summary>
@@ -41,26 +46,30 @@ namespace RoboidControl {
         /// <param name="networkId">The network ID of the thing</param>
         /// <param name="thingId">The ID of the thing</param>
         /// <param name="url">The URL to send</param>
-        public ModelUrlMsg(byte networkId, byte thingId, string url) {
-            this.networkId = networkId;
-            this.thingId = thingId;
-            this.url = url;
-        }
+        // public ModelUrlMsg(byte networkId, byte thingId, string url) {
+        //     this.networkId = networkId;
+        //     this.thingId = thingId;
+        //     this.urlLength = (byte)url.Length;
+        //     this.url = url;
+        // }
         /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
         public ModelUrlMsg(byte[] buffer) {
             byte ix = 1;
             this.networkId = buffer[ix++];
             this.thingId = buffer[ix++];
 
-            int strlen = buffer[ix++];
-            url = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen);
+            this.urlLength = buffer[ix++];
+            this.url = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, this.urlLength);
         }
 
         /// @copydoc Passer::RoboidControl::IMessage::Serialize
         public override byte Serialize(ref byte[] buffer) {
-            if (this.url == null)
+            if (string.IsNullOrEmpty(this.url))
                 return 0;
 
+#if DEBUG
+            System.Console.WriteLine($"Send ModelUrlMsg [{this.networkId}/{this.thingId}] {this.urlLength} {this.url}");
+#endif
             byte ix = 0;
             buffer[ix++] = ModelUrlMsg.Id;
             buffer[ix++] = this.networkId;
diff --git a/src/Messages/NameMsg.cs b/src/Messages/NameMsg.cs
index 632540f..04bd745 100644
--- a/src/Messages/NameMsg.cs
+++ b/src/Messages/NameMsg.cs
@@ -20,10 +20,11 @@ namespace RoboidControl {
         /// The ID of the thing
         /// </summary>
         public byte thingId;
+
         /// <summary>
         /// The length of the name, excluding null terminator
         /// </summary>
-        public byte len;
+        public byte nameLength;
         /// <summary>
         /// The name of the thing, not terminated with a null character
         /// </summary>
@@ -38,6 +39,7 @@ namespace RoboidControl {
             this.networkId = networkId;
             this.thingId = thing.id;
             this.name = thing.name;
+            this.nameLength = (byte)this.name.Length;
         }
         /// <summary>
         /// Create a new message for sending
@@ -45,34 +47,34 @@ namespace RoboidControl {
         /// <param name="networkId">The network ID of the thing</param>
         /// <param name="thingId">The ID of the thing</param>
         /// <param name="name">The name of the thing</param>
-        public NameMsg(byte networkId, byte thingId, string name) {
-            this.networkId = networkId;
-            this.thingId = thingId;
-            this.name = name;
-        }
+        // public NameMsg(byte networkId, byte thingId, string name) {
+        //     this.networkId = networkId;
+        //     this.thingId = thingId;
+        //     this.name = name;
+        // }
         /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
         public NameMsg(byte[] buffer) {
             byte ix = 1;
             this.networkId = buffer[ix++];
             this.thingId = buffer[ix++];
-            int strlen = buffer[ix++];
-            this.name = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen);
+            this.nameLength = buffer[ix++];
+            this.name = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, this.nameLength);
         }
 
         /// @copydoc Passer::RoboidControl::IMessage::Serialize
         public override byte Serialize(ref byte[] buffer) {
-            if (buffer.Length < NameMsg.length + this.name.Length || this.name.Length == 0)
+            if (buffer.Length < NameMsg.length + this.name.Length || string.IsNullOrEmpty(this.name))
                 return 0;
 
+#if DEBUG
+            System.Console.WriteLine($"Send NameMsg [{this.networkId}/{this.thingId}] {this.nameLength} {this.name}");
+#endif
             byte ix = 0;
             buffer[ix++] = NameMsg.Id;
             buffer[ix++] = this.networkId;
             buffer[ix++] = this.thingId;
-
-            int nameLength = this.name.Length;
-
-            buffer[ix++] = (byte)nameLength;
-            for (int nameIx = 0; nameIx < nameLength; nameIx++)
+            buffer[ix++] = this.nameLength;
+            for (int nameIx = 0; nameIx < this.nameLength; nameIx++)
                 buffer[ix++] = (byte)this.name[nameIx];
 
             return ix;
diff --git a/src/Messages/NetworkIdMsg.cs b/src/Messages/NetworkIdMsg.cs
index 8d81e27..ed51018 100644
--- a/src/Messages/NetworkIdMsg.cs
+++ b/src/Messages/NetworkIdMsg.cs
@@ -33,7 +33,10 @@ namespace RoboidControl {
         public override byte Serialize(ref byte[] buffer) {
             if (buffer.Length < NetworkIdMsg.length)
                 return 0;
-                
+
+#if DEBUG
+            System.Console.WriteLine($"Send NetworkIdMsg {this.networkId}");
+#endif
             buffer[0] = NetworkIdMsg.Id;
             buffer[1] = this.networkId;
             return NetworkIdMsg.length;
diff --git a/src/Messages/PoseMsg.cs b/src/Messages/PoseMsg.cs
index f29588f..1a1e5f2 100644
--- a/src/Messages/PoseMsg.cs
+++ b/src/Messages/PoseMsg.cs
@@ -88,6 +88,34 @@ namespace RoboidControl {
             this.linearVelocity = linearVelocity;
             this.angularVelocity = angularVelocity;
         }
+        public PoseMsg(byte networkId, Thing thing, bool force = false) {
+            this.networkId = networkId;
+            this.thingId = thing.id;
+
+            this.poseType = 0;
+            if (thing.positionUpdated || force) {
+                this.position = thing.position;
+                this.poseType |= Pose_Position;
+                //thing.positionUpdated = false; // this is also reset in Thing.update, leave it out here?
+            }
+            if (thing.orientationUpdated || force) {
+                this.orientation = thing.orientation;
+                this.poseType |= Pose_Orientation;
+                //thing.orientationUpdated = false; // this is also reset in Thing.update, leave it out here?
+            }
+            if (thing.linearVelocityUpdated) {
+                this.linearVelocity = thing.linearVelocity;
+                this.poseType |= Pose_LinearVelocity;
+                thing.linearVelocityUpdated = false;
+            }
+            if (thing.angularVelocityUpdated) {
+                this.angularVelocity = thing.angularVelocity;
+                this.poseType |= Pose_AngularVelocity;
+                thing.angularVelocityUpdated = false;
+            }
+
+        }
+
         /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
         public PoseMsg(byte[] buffer) : base(buffer) {
             byte ix = 1;
diff --git a/src/Messages/TextMsg.cs b/src/Messages/TextMsg.cs
index a1de990..6fdfc0c 100644
--- a/src/Messages/TextMsg.cs
+++ b/src/Messages/TextMsg.cs
@@ -13,6 +13,10 @@ namespace RoboidControl {
         /// </summary>
         public const byte length = 2;
         /// <summary>
+        /// The length of the text without the null terminator
+        /// </summary>
+        public byte textLength;
+        /// <summary>
         /// The text
         /// </summary>
         public string text = "";
@@ -22,20 +26,27 @@ namespace RoboidControl {
         /// </summary>
         /// <param name="text">The text to send</param>
         public TextMsg(string text) {
+            this.textLength = (byte)text.Length;
             this.text = text;
         }
 
         /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
-        public TextMsg(byte[] buffer) : base(buffer) { }
+        public TextMsg(byte[] buffer) : base(buffer) {
+            this.textLength = buffer[0];
+            this.text = System.Text.Encoding.UTF8.GetString(buffer, 1, this.textLength);
+         }
 
         /// @copydoc Passer::RoboidControl::IMessage::Serialize
         public override byte Serialize(ref byte[] buffer) {
             if (buffer.Length < TextMsg.length + this.text.Length || this.text.Length == 0)
                 return 0;
 
+#if DEBUG
+            System.Console.WriteLine($"Send TextMsg {this.textLength} {this.text}");
+#endif
             byte ix = 0;
             buffer[ix++] = TextMsg.Id;
-            buffer[ix++] = (byte)this.text.Length;
+            buffer[ix++] = this.textLength;
             for (int textIx = 0; textIx < this.text.Length; textIx++)
                 buffer[ix++] = (byte)this.text[textIx];
             return ix;
diff --git a/src/Messages/ThingMsg.cs b/src/Messages/ThingMsg.cs
index 9725e23..bf93a75 100644
--- a/src/Messages/ThingMsg.cs
+++ b/src/Messages/ThingMsg.cs
@@ -77,6 +77,9 @@ namespace RoboidControl {
             if (buffer.Length < ThingMsg.length)
                 return 0;
                 
+#if DEBUG
+            System.Console.WriteLine($"Send ThingMsg [{this.networkId}/{this.thingId}] {this.thingType} {this.parentId}");
+#endif
             byte ix = 0;
             buffer[ix++] = ThingMsg.id;
             buffer[ix++] = this.networkId;
diff --git a/src/Participant.cs b/src/Participant.cs
index 172aa35..c85d437 100644
--- a/src/Participant.cs
+++ b/src/Participant.cs
@@ -44,6 +44,16 @@ namespace RoboidControl {
         /// </summary>
         protected readonly List<Thing> things = new List<Thing>();
 
+        public virtual void Update(ulong currentTimeMS = 0) {
+            int n = this.things.Count;
+            for (int ix = 0; ix < n; ix++) {
+                Thing thing = this.things[ix];
+                if (thing != null)
+                    thing.Update(currentTimeMS, true);                
+            }
+
+        }
+
         /// <summary>
         /// Get a thing with the given ids
         /// </summary>
diff --git a/src/LocalParticipant.cs b/src/ParticipantUDP.cs
similarity index 72%
rename from src/LocalParticipant.cs
rename to src/ParticipantUDP.cs
index 7dcdde7..01071b5 100644
--- a/src/LocalParticipant.cs
+++ b/src/ParticipantUDP.cs
@@ -9,7 +9,7 @@ namespace RoboidControl {
     /// <summary>
     /// A participant is used for communcation between things
     /// </summary>
-    public class LocalParticipant : Participant {
+    public class ParticipantUDP : Participant {
         public byte[] buffer = new byte[1024];
         public ulong publishInterval = 3000; // = 3 seconds
 
@@ -26,7 +26,7 @@ namespace RoboidControl {
         /// <summary>
         /// Create a porticiapnt
         /// </summary>
-        public LocalParticipant() {
+        public ParticipantUDP() {
             //senders.Add(this);
         }
 
@@ -34,7 +34,7 @@ namespace RoboidControl {
         /// Create a participant with the give UDP port
         /// </summary>
         /// <param name="port">The port number on which to communicate</param>
-        public LocalParticipant(int port) : this() {
+        public ParticipantUDP(int port) : this() {
             this.port = port;
         }
 
@@ -43,7 +43,7 @@ namespace RoboidControl {
         /// </summary>
         /// <param name="ipAddress">The ip address of the site server</param>
         /// <param name="port">The port number of the site server</param>
-        public LocalParticipant(string ipAddress = "0.0.0.0", int port = 7681) : this() {
+        public ParticipantUDP(string ipAddress = "0.0.0.0", int port = 7681) : this() {
             this.ipAddress = ipAddress;
             this.port = port;
 
@@ -61,15 +61,15 @@ namespace RoboidControl {
         /// </summary>
         /// <param name="udpClient">UDP client to use for communication</param>
         /// <param name="port">The port number on which to communicate</param>
-        public LocalParticipant(UdpClient udpClient, int port) : this() {
+        public ParticipantUDP(UdpClient udpClient, int port) : this() {
             this.udpClient = udpClient;
             this.port = port;
         }
 
-        private static LocalParticipant isolatedParticipant = null;
-        public static LocalParticipant Isolated() {
+        private static ParticipantUDP isolatedParticipant = null;
+        public static ParticipantUDP Isolated() {
             if (isolatedParticipant == null)
-                isolatedParticipant = new LocalParticipant(0);
+                isolatedParticipant = new ParticipantUDP(0);
             return isolatedParticipant;
         }
 
@@ -94,20 +94,20 @@ namespace RoboidControl {
 
         protected readonly Dictionary<byte, Func<Participant, byte, byte, Thing>> thingMsgProcessors = new();
 
-        public delegate Thing ThingConstructor(Participant sender, byte networkId, byte thingId);
-        public void Register(byte thingType, ThingConstructor constr) {
-            thingMsgProcessors[thingType] = new Func<Participant, byte, byte, Thing>(constr);
-        }
+        // public delegate Thing ThingConstructor(Participant sender, byte networkId, byte thingId);
+        // public void Register(byte thingType, ThingConstructor constr) {
+        //     thingMsgProcessors[thingType] = new Func<Participant, byte, byte, Thing>(constr);
+        // }
 
-        public void Register<ThingClass>(Thing.Type thingType) where ThingClass : Thing {
-            Register<ThingClass>((byte)thingType);
-        }
+        // public void Register<ThingClass>(Thing.Type thingType) where ThingClass : Thing {
+        //     Register<ThingClass>((byte)thingType);
+        // }
 
-        public void Register<ThingClass>(byte thingType) where ThingClass : Thing {
-            thingMsgProcessors[thingType] = (participant, networkId, thingId) =>
-                Activator.CreateInstance(typeof(ThingClass), participant, networkId, thingId) as ThingClass;
-            Console.WriteLine($"Registering {typeof(ThingClass)} for thing type {thingType}");
-        }
+        // public void Register<ThingClass>(byte thingType) where ThingClass : Thing {
+        //     thingMsgProcessors[thingType] = (participant, networkId, thingId) =>
+        //         Activator.CreateInstance(typeof(ThingClass), participant, networkId, thingId) as ThingClass;
+        //     Console.WriteLine($"Registering {typeof(ThingClass)} for thing type {thingType}");
+        // }
 
         #endregion Init
 
@@ -134,7 +134,7 @@ namespace RoboidControl {
         }
 
         protected ulong nextPublishMe = 0;
-        public virtual void Update(ulong currentTimeMS = 0) {
+        public override void Update(ulong currentTimeMS = 0) {
             if (currentTimeMS == 0) {
 #if UNITY_5_3_OR_NEWER
                 currentTimeMS = (ulong)(UnityEngine.Time.time * 1000);
@@ -152,12 +152,19 @@ namespace RoboidControl {
                 Thing thing = this.things[ix];
                 if (thing != null && thing.parent == null) {
                     thing.Update(currentTimeMS, true);
-                    // if (thing.owner != this) {
-                    //     BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
-                    //     this.Send(thing.owner, binaryMsg);
-                    // }
+                    // if (this.isIsolated == false) {
+                    if (thing.owner != this) { // should not happen....
+                        PoseMsg poseMsg = new(thing.owner.networkId, thing);
+                        this.Send(thing.owner, poseMsg);
+                        BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
+                        this.Send(thing.owner, binaryMsg);
+                    }
                 }
             }
+            for (int ownerIx = 0; ownerIx < this.owners.Count; ownerIx++) {
+                Participant owner = this.owners[ownerIx];
+                owner.Update(currentTimeMS);
+            }
         }
 
         public virtual void Publish() {
@@ -169,7 +176,7 @@ namespace RoboidControl {
         #region Send
 
         public void SendThingInfo(Participant owner, Thing thing) {
-            Console.WriteLine("Send thing info");
+            // Console.WriteLine("Send thing info");
             this.Send(owner, new ThingMsg(this.networkId, thing));
             this.Send(owner, new NameMsg(this.networkId, thing));
             this.Send(owner, new ModelUrlMsg(this.networkId, thing));
@@ -188,7 +195,7 @@ namespace RoboidControl {
         }
 
         public void PublishThingInfo(Thing thing) {
-            //Console.WriteLine("Publish thing info");
+            // Console.WriteLine("Publish thing info");
             this.Publish(new ThingMsg(this.networkId, thing));
             this.Publish(new NameMsg(this.networkId, thing));
             this.Publish(new ModelUrlMsg(this.networkId, thing));
@@ -200,7 +207,7 @@ namespace RoboidControl {
             if (bufferSize <= 0)
                 return true;
 
-            // Console.WriteLine($"publish to {broadcastIpAddress.ToString()} {this.port}");
+            Console.WriteLine($"publish to {broadcastIpAddress.ToString()} {this.port}");
             this.udpClient?.Send(this.buffer, bufferSize, this.broadcastIpAddress, this.port);
             return true;
         }
@@ -227,7 +234,7 @@ namespace RoboidControl {
 
         #region Receive
 
-        public void ReceiveData(byte[] data, Participant remoteParticipant) {
+        public void ReceiveData(byte[] data, Participant sender) {
             byte msgId = data[0];
             if (msgId == 0xFF) {
                 // Timeout
@@ -236,29 +243,29 @@ namespace RoboidControl {
 
             switch (msgId) {
                 case ParticipantMsg.Id: // 0xA0 / 160
-                    this.Process(remoteParticipant, new ParticipantMsg(data));
+                    this.Process(sender, new ParticipantMsg(data));
                     break;
                 case NetworkIdMsg.Id: // 0xA1 / 161
-                    this.Process(remoteParticipant, new NetworkIdMsg(data));
+                    this.Process(sender, new NetworkIdMsg(data));
                     break;
                 case InvestigateMsg.Id: // 0x81
                     // result = await InvestigateMsg.Receive(dataStream, client, packetSize);
                     break;
                 case ThingMsg.id: // 0x80 / 128
-                    this.Process(remoteParticipant, new ThingMsg(data));
+                    this.Process(sender, new ThingMsg(data));
                     break;
                 case NameMsg.Id: // 0x91 / 145
-                    this.Process(remoteParticipant, new NameMsg(data));
+                    this.Process(sender, new NameMsg(data));
                     break;
                 case ModelUrlMsg.Id: // 0x90 / 144
-                    this.Process(remoteParticipant, new ModelUrlMsg(data));
+                    this.Process(sender, new ModelUrlMsg(data));
                     break;
                 case PoseMsg.Id: // 0x10 / 16
-                    this.Process(remoteParticipant, new PoseMsg(data));
+                    this.Process(sender, new PoseMsg(data));
                     // result = await PoseMsg.Receive(dataStream, client, packetSize);
                     break;
                 case BinaryMsg.Id: // 0xB1 / 177
-                    this.Process(remoteParticipant, new BinaryMsg(data));
+                    this.Process(sender, new BinaryMsg(data));
                     break;
                 case TextMsg.Id: // 0xB0 / 176
                     // result = await TextMsg.Receive(dataStream, client, packetSize);
@@ -275,10 +282,17 @@ namespace RoboidControl {
 
         #region Process
 
-        protected virtual void Process(Participant sender, ParticipantMsg msg) { }
+        protected virtual void Process(Participant sender, ParticipantMsg msg) {
+#if DEBUG
+            Console.WriteLine($"{this.name} Process participantMsg {msg.networkId}");
+#endif
+        }
 
         protected virtual void Process(Participant sender, NetworkIdMsg msg) {
-            Console.WriteLine($"{this.name} receive network id {this.networkId} {msg.networkId}");
+#if DEBUG            
+            Console.WriteLine($"{this.name} Process SiteMsg {this.networkId} -> {msg.networkId}");
+#endif
+
             if (this.networkId != msg.networkId) {
                 this.networkId = msg.networkId;
                 foreach (Thing thing in this.things) //Thing.GetAllThings())
@@ -286,53 +300,76 @@ namespace RoboidControl {
             }
         }
 
-        protected virtual void Process(Participant sender, InvestigateMsg msg) { }
+        protected virtual void Process(Participant sender, InvestigateMsg msg) {
+#if DEBUG
+            Console.WriteLine($"Participant: InvestigateMsg [{msg.networkId}/{msg.thingId}]");
+#endif
+        }
 
         protected virtual void Process(Participant sender, ThingMsg msg) {
-            //Console.WriteLine($"Participant: Process thing [{msg.networkId}/{msg.thingId}]");
+#if DEBUG            
+            Console.WriteLine($"Participant: Process ThingMsg [{msg.networkId}/{msg.thingId}] {msg.thingType} {msg.parentId}");
+#endif            
         }
 
         protected virtual void Process(Participant sender, NameMsg msg) {
-            Console.WriteLine($"Participant: Process name [{msg.networkId}/{msg.thingId}] {msg.name}");
+#if DEBUG            
+            Console.WriteLine($"Participant: Process NameMsg [{msg.networkId}/{msg.thingId}] {msg.nameLength} {msg.name}");
+#endif
+
             Thing thing = sender.Get(msg.networkId, msg.thingId);
             if (thing != null)
                 thing.name = msg.name;
         }
 
         protected virtual void Process(Participant sender, ModelUrlMsg msg) {
-            Console.WriteLine($"Participant: Process model [{msg.networkId}/{msg.thingId}] {msg.url}");
+#if DEBUG            
+            Console.WriteLine($"Participant: Process ModelUrlMsg [{msg.networkId}/{msg.thingId}] {msg.urlLength} {msg.url}");
+#endif
+
             Thing thing = sender.Get(msg.networkId, msg.thingId);
             if (thing != null)
                 thing.modelUrl = msg.url;
         }
 
         protected virtual void Process(Participant sender, PoseMsg msg) {
-            //Console.WriteLine($"Participant: Process pose [{msg.networkId}/{msg.thingId}] {msg.poseType}");
+#if DEBUG            
+            Console.WriteLine($"Participant: Process PoseMsg [{msg.networkId}/{msg.thingId}] {msg.poseType}");
+#endif            
             Thing thing = sender.Get(msg.networkId, msg.thingId);
             if (thing != null) {
                 if ((msg.poseType & PoseMsg.Pose_Position) != 0)
                     thing.position = msg.position;
-                if ((msg.poseType & PoseMsg.Pose_Orientation) != 0) {
+                if ((msg.poseType & PoseMsg.Pose_Orientation) != 0)
                     thing.orientation = msg.orientation;
-                    Console.Write($"{thing.id} orientation changed");
-                }
                 if ((msg.poseType & PoseMsg.Pose_LinearVelocity) != 0)
                     thing.linearVelocity = msg.linearVelocity;
                 if ((msg.poseType & PoseMsg.Pose_AngularVelocity) != 0)
                     thing.angularVelocity = msg.angularVelocity;
-
             }
         }
 
         protected virtual void Process(Participant sender, BinaryMsg msg) {
-            // Console.WriteLine($"Participant: Process binary [{msg.networkId}/{msg.thingId}]");
+#if DEBUG            
+            Console.WriteLine($"Participant: Process BinaryMsg [{msg.networkId}/{msg.thingId}] {msg.dataLength}");
+#endif            
             Thing thing = sender.Get(msg.networkId, msg.thingId);
-            thing?.ProcessBinary(msg.bytes);
+            thing?.ProcessBinary(msg.data);
         }
 
-        protected virtual void Process(Participant sender, TextMsg temsgxt) { }
+        protected virtual void Process(Participant sender, TextMsg msg) {
+#if DEBUG
+            Console.WriteLine($"Participant: Process TextMsg {msg.textLength} {msg.text}");
+#endif            
 
-        protected virtual void Process(Participant sender, DestroyMsg msg) { }
+        }
+
+        protected virtual void Process(Participant sender, DestroyMsg msg) {
+#if DEBUG
+            Console.WriteLine($"Participant: Process ThingMsg [{msg.networkId}/{msg.thingId}]");
+#endif            
+
+        }
 
         private void ForwardMessage(IMessage msg) {
             // foreach (Participant client in senders) {
diff --git a/src/SiteServer.cs b/src/SiteServer.cs
index 616cada..1032c7f 100644
--- a/src/SiteServer.cs
+++ b/src/SiteServer.cs
@@ -7,7 +7,7 @@ namespace RoboidControl {
     /// <summary>
     /// A site server is a participant which provides a shared simulated environment 
     /// </summary>
-    public class SiteServer : LocalParticipant {
+    public class SiteServer : ParticipantUDP {
 
         public SiteServer(int port = 7681) : this("0.0.0.0", port) { }
 
@@ -29,7 +29,7 @@ namespace RoboidControl {
                 new AsyncCallback(result => ReceiveUDP(result)),
                 new Tuple<UdpClient, IPEndPoint>(this.udpClient, new(IPAddress.Any, port)));
 
-            Register<TouchSensor>(Thing.Type.TouchSensor);
+            // Register<TouchSensor>(Thing.Type.TouchSensor);
         }
 
         /// <summary>
@@ -42,11 +42,12 @@ namespace RoboidControl {
         public override void Publish() {
         }
 
-        protected override void Process(Participant remoteParticipant, ParticipantMsg msg) {
-            if (msg.networkId == 0) {
-                Console.WriteLine($"{this.name} received New Participant -> {remoteParticipant.networkId}");
-                this.Send(remoteParticipant, new NetworkIdMsg(remoteParticipant.networkId));
-            }
+        protected override void Process(Participant sender, ParticipantMsg msg) {
+            base.Process(sender, msg);
+            //if (msg.networkId == 0) {
+                //Console.WriteLine($"{this.name} received New Participant -> {sender.networkId}");
+                this.Send(sender, new NetworkIdMsg(sender.networkId));
+            //}
         }
 
         protected override void Process(Participant sender, NetworkIdMsg msg) { }
@@ -56,15 +57,15 @@ namespace RoboidControl {
             Thing thing = sender.Get(msg.networkId, msg.thingId);
             if (thing == null) {
                 Thing newThing = null;
-                if (thingMsgProcessors.TryGetValue(msg.thingType, out Func<Participant, byte, byte, Thing> msgProcessor)) {
-                    //Console.WriteLine("Found thing message processor");
-                    if (msgProcessor != null)
-                        newThing = msgProcessor(sender, msg.networkId, msg.thingId);
-                }
-                if (newThing == null) {
+                // if (thingMsgProcessors.TryGetValue(msg.thingType, out Func<Participant, byte, byte, Thing> msgProcessor)) {
+                //     //Console.WriteLine("Found thing message processor");
+                //     if (msgProcessor != null)
+                //         newThing = msgProcessor(sender, msg.networkId, msg.thingId);
+                // }
+                // if (newThing == null) {
                     newThing = new Thing(sender, msg.networkId, msg.thingId, msg.thingType);
                     // Console.WriteLine("Created generic new core thing");
-                }
+                // }
                 if (msg.parentId != 0) {
                     Thing parentThing = Get(msg.networkId, msg.parentId);
                     if (parentThing == null)
diff --git a/src/Thing.cs b/src/Thing.cs
index 607beec..00a2020 100644
--- a/src/Thing.cs
+++ b/src/Thing.cs
@@ -49,11 +49,12 @@ namespace RoboidControl {
         public Thing(Participant owner, byte thingType = (byte)Type.Undetermined, bool invokeEvent = true) {
             this.owner = owner;
             this.type = thingType;
-            this.owner.Add(this);
+            if (this.owner != null)
+                this.owner.Add(this);
             if (invokeEvent)
                 InvokeNewThing(this);
         }
-        public Thing(Participant owner) : this(owner, Type.Undetermined) {}
+        public Thing(Participant owner) : this(owner, Type.Undetermined) { }
         /// <summary>
         /// Create a new thing for a participant
         /// </summary>
@@ -65,7 +66,7 @@ namespace RoboidControl {
         /// Create a new thing without communication abilities
         /// </summary>
         /// <param name="thingType">The type of thing</param>
-        public Thing(byte thingType = (byte)Type.Undetermined, bool invokeEvent = true) : this(LocalParticipant.Isolated(), thingType, invokeEvent) {
+        public Thing(byte thingType = (byte)Type.Undetermined, bool invokeEvent = true) : this(ParticipantUDP.Isolated(), thingType, invokeEvent) {
         }
 
         /// <summary>
@@ -286,10 +287,6 @@ namespace RoboidControl {
             }
         }
         /// <summary>
-        /// Event triggered when the orientation has changed
-        /// </summary>
-        //public event ChangeHandler OnOrientationChanged = delegate { };
-        /// <summary>
         /// Boolean indicating the thing has an updated orientation
         /// </summary>
         public bool orientationUpdated = false;
@@ -303,7 +300,7 @@ namespace RoboidControl {
             set {
                 if (_linearVelocity != value) {
                     _linearVelocity = value;
-                    hasLinearVelocity = _linearVelocity.distance != 0;
+                    linearVelocityUpdated = true;
                     OnLinearVelocityChanged?.Invoke(_linearVelocity);
                 }
             }
@@ -315,7 +312,7 @@ namespace RoboidControl {
         /// <summary>
         /// Boolean indicating the thing has an updated linear velocity
         /// </summary>
-        public bool hasLinearVelocity = false;
+        public bool linearVelocityUpdated = false;
 
         private Spherical _angularVelocity = Spherical.zero;
         /// <summary>
@@ -326,7 +323,7 @@ namespace RoboidControl {
             set {
                 if (_angularVelocity != value) {
                     _angularVelocity = value;
-                    hasAngularVelocity = _angularVelocity.distance != 0;
+                    angularVelocityUpdated = true;
                     OnAngularVelocityChanged?.Invoke(_angularVelocity);
                 }
             }
@@ -338,7 +335,7 @@ namespace RoboidControl {
         /// <summary>
         /// Boolean indicating the thing has an updated angular velocity
         /// </summary>
-        public bool hasAngularVelocity = false;
+        public bool angularVelocityUpdated = false;
 
 #if UNITY_5_3_OR_NEWER
         /// <summary>
diff --git a/src/Things/DifferentialDrive.cs b/src/Things/DifferentialDrive.cs
index 768ad01..f47ea3a 100644
--- a/src/Things/DifferentialDrive.cs
+++ b/src/Things/DifferentialDrive.cs
@@ -8,7 +8,7 @@ namespace RoboidControl {
         public DifferentialDrive() { }
         /// @brief Create a differential drive with networking support
         /// @param participant The local participant
-        public DifferentialDrive(LocalParticipant participant) : base(participant, Type.Undetermined) { }
+        public DifferentialDrive(ParticipantUDP participant) : base(participant, Type.Undetermined) { }
 
         /// @brief Configures the dimensions of the drive
         /// @param wheelDiameter The diameter of the wheels in meters
diff --git a/src/Things/TemperatureSensor.cs b/src/Things/TemperatureSensor.cs
index 6f7e009..34a7233 100644
--- a/src/Things/TemperatureSensor.cs
+++ b/src/Things/TemperatureSensor.cs
@@ -17,7 +17,7 @@ namespace RoboidControl {
         /// <param name="participant">The participant for with the sensor is needed</param>
         /// <param name="networkId">The network ID of the sensor</param>
         /// <param name="thingId">The ID of the thing</param>
-        public TemperatureSensor(LocalParticipant participant, byte networkId, byte thingId) : base(participant, networkId, thingId, (byte)Type.TemperatureSensor) { }
+        public TemperatureSensor(Participant participant, byte networkId, byte thingId) : base(participant, networkId, thingId, (byte)Type.TemperatureSensor) { }
 
         /// <summary>
         /// Function to extract the temperature received in the binary message
diff --git a/src/Things/TouchSensor.cs b/src/Things/TouchSensor.cs
index 630adea..f85cfd9 100644
--- a/src/Things/TouchSensor.cs
+++ b/src/Things/TouchSensor.cs
@@ -26,7 +26,7 @@ namespace RoboidControl {
 
         public TouchSensor(Thing parent) : base(parent) { }
 
-        public LocalParticipant thisParticipant;
+        public ParticipantUDP thisParticipant;
 
         /// <summary>
         /// Value which is true when the sensor is touching something, false otherwise