//It is not necessary to have CS compiler to build this self-test in WIN10.
//Find MSBuild.exe in C:\Windows\Microsoft.NET\Framework64\v4.0.30319 or other version of your framework
//Make ASCII file with extension *.csproj and copy/paste following fragment:

//<Project xmlns="http://schemas.microsoft.com/developer/msbuild/2003" DefaultTargets="Build" ToolsVersion="12">
//    <ItemGroup>
//        <FilesToCompile Include="Program.cs" />
//    </ItemGroup>
//    <Target Name="Build">
//        <Csc Sources="@(FilesToCompile)" OutputAssembly="$(OutputAssembly)" Optimize="$(Optimize)" TargetType="exe" Resources="@(CompiledResources)"/>
//    </Target>
// </Project>

//File name "Program.cs" is this code

//Using DOS prompt navigate to folder with MSBuild.exe and execute command:
//MSBuild C:\YourFolder\YourProjectFile.csproj

//model is found in https://www.math.colostate.edu/~gerhard/MATH331/ch6.pdf

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;

namespace NL
{
    class Cholesky
    {
        //main method for Cholesky decomposition.
        //input         n  size of matrix
        //input/output  a  Symmetric positive def. matrix
        //output        p  vector of resulting diag of a
        //author:       <Vadum Kutsyy, kutsyy@hotmail.com>
        //Converted to c# by Andrew Polar
        private bool choldc1(int n, double[,] a, double[] p)
        {
            int i, j, k;
            double sum;

            for (i = 0; i < n; i++)
            {
                for (j = i; j < n; j++)
                {
                    sum = a[i, j];
                    for (k = i - 1; k >= 0; k--)
                    {
                        sum -= a[i, k] * a[j, k];
                    }
                    if (i == j)
                    {
                        if (sum <= 0)
                        {
                            Console.WriteLine("Matrix is not positive definite!");
                            return false;
                        }
                        p[i] = Math.Sqrt(sum);
                    }
                    else
                    {
                        a[j, i] = sum / p[i];
                    }
                }
            }
            return true;
        }

        private bool choldc(int n, double[,] A, double[,] a)
        {
            int i, j;
            double[] p = new double[n];

            for (i = 0; i < n; i++)
                for (j = 0; j < n; j++)
                    a[i, j] = A[i, j];

            if (!choldc1(n, a, p))
            {
                return false;
            }

            for (i = 0; i < n; i++)
            {
                a[i, i] = p[i];
                for (j = i + 1; j < n; j++)
                {
                    a[i, j] = 0;
                }
            }

            p = null;
            return true;
        }

        ///////////////////////////////////////////////////////////////////
        //solution for M * x = y
        //M must be positive definite
        public bool CholeskySolution(double[,] M, double[] x, double[] y, int size)
        {
            //regularization
            //double trace = 0.0;
            //for (int i = 0; i < size; ++i)
            //{
            //    trace += M[i, i];
            //}
            //trace /= size;
            //trace *= 0.001;
            //for (int i = 0; i < size; ++i)
            //{
            //    M[i, i] += trace;
            //}
            //end regularization

            double[,] T = new double[size, size];

            //find lower triangular
            if (!choldc(size, M, T))
            {
                return false;
            }
            //make is symmetric
            for (int i = 0; i < size; ++i)
                for (int j = i + 1; j < size; ++j)
                    T[i, j] = T[j, i];

            double[] v = new double[size];

            //solve lower triangular
            for (int i = 0; i < size; ++i)
            {
                double sum = 0.0;
                for (int j = 0; j < i; ++j)
                {
                    sum += T[i, j] * v[j];
                }
                v[i] = (y[i] - sum) / T[i, i];
            }

            //solve upper triangular
            for (int i = size - 1; i >= 0; --i)
            {
                double sum = 0.0;
                for (int j = size - 1; j > i; --j)
                {
                    sum += T[i, j] * x[j];
                }
                x[i] = (v[i] - sum) / T[i, i];
            }

            v = null;
            T = null;

            return true;
        }

        public void SelfTest()
        {
            const int size = 10;
            double[,] H = new double[size, size];

            for (int i = 0; i < size; ++i)
            {
                for (int j = 0; j < size; ++j)
                {
                    H[i, j] = 1.0 / (double)(i + j + 1);
                }
            }

            double[] x = new double[size];
            double[] y = new double[size];

            for (int i = 0; i < size; ++i)
            {
                x[i] = (double)(i + 1);
                y[i] = 0.0;
            }

            for (int i = 0; i < size; ++i)
            {
                for (int j = 0; j < size; ++j)
                {
                    y[i] += H[i, j] * x[j];
                }
            }
            for (int i = 0; i < size; ++i)
            {
                x[i] = 0.0;
            }

            Cholesky ch = new Cholesky();
            if (ch.CholeskySolution(H, x, y, size))
            {
                for (int i = 0; i < size; ++i)
                {
                    Console.WriteLine(x[i]);
                }
            }
            else
            {
                Console.WriteLine("fatal: failed to obtain cholesky solution");
                Environment.Exit(1);
            }
        }
    }

    class Program
    {
        static int N = 100000;
        static double[] x = new double[N];
        static double[] y = new double[N];
        static int[] X = new int[N];
        static int[] Y = new int[N];

        static int Size = 100;
        static int nt = 6;
        static double[,] U = new double[Size, nt];
        static double[] z = new double[nt];
        private static int[,] map = new int[Size, nt];

        static double G1 = 0.7;
        static double C1 = 0.5;
        static double D1 = 0.5;
        static double G2 = 0.2;
        static double C2 = 2.0;
        static double D2 = 0.1;

        static double noise_suppression = 0.1;
        static int map_size = 10;
        static int number_of_steps = 10;

        static void GenerateX()
        {
            Random rnd = new Random();
            for (int i = 0; i < N; ++i)
            {
                x[i] = rnd.Next() % 30;
            }
            for (int cnt = 0; cnt < 2; ++cnt)
            {
                for (int i = 0; i < N - 1; ++i)
                {
                    x[i] = (x[i] + x[i + 1]) / 2.0;
                }
            }
        }

        static void GenerateY()
        {
            y[0] = 0.5;
            y[1] = 0.5;

            double fprev = 0.5;
            double rprev = 0.5;
            for (int i = 2; i < N; ++i)
            {
                double f = (1.0 - G1) * fprev + C1 * fprev * rprev - D1 * fprev * fprev + 0.5 * x[i - 1] / (40 - x[i - 2]);
                double r = (1.0 + G2) * rprev - C2 * fprev * rprev - D2 * rprev * rprev + 0.1 * Math.Exp(-x[i - 2] / 7.0);
                fprev = f;
                rprev = r;

                y[i] = f;
            }
        }

        static void NormalizeSignals()
        {
            double maxx = x.Max();
            double minx = x.Min();
            double maxy = y.Max();
            double miny = y.Min();

            for (int i = 0; i < N; ++i)
            {
                X[i] = (int)((x[i] - minx) / (maxx - minx) * 100.0);
                Y[i] = (int)((y[i] - miny) / (maxy - miny) * 100.0);
                if (X[i] < 0) X[i] = 0;
                if (X[i] > 99) X[i] = 99;
                if (Y[i] < 0) Y[i] = 0;
                if (Y[i] > 99) Y[i] = 99;
            }
        }

        static void AddErrors(int percent)
        {
            Thread.Sleep(200);
            Random rnd = new Random();
            for (int i = 0; i < N; ++i)
            {
                int err = 0;
                int random = rnd.Next() % 100;
                if (random < 33) err = -percent;
                if (random > 66) err = percent;

                X[i] += err;
                if (X[i] < 0) X[i] = 0;
                if (X[i] > 99) X[i] = 99;

                err = 0;
                random = rnd.Next() % 100;
                if (random < 33) err = -percent;
                if (random > 66) err = percent;

                Y[i] += err;
                if (Y[i] < 0) Y[i] = 0;
                if (Y[i] > 99) Y[i] = 99;
            }
        }

        static void ShowSignals()
        {
            for (int i = 0; i < N; ++i)
            {
                Console.WriteLine("{0}, {1}", X[i], Y[i]);
            }
        }

        static void ShowOperator()
        {
            Console.WriteLine();
            for (int i = 0; i < Size; ++i)
            {
                for (int j = 0; j < nt-1; ++j)
                {
                    Console.Write("{0:0.000}, ", U[i, j]);
                }
                Console.Write("{0:0.000} ", U[i, nt-1]);
                Console.WriteLine();
            }
        }

        static void MakeGridIdentification()
        {
            Console.WriteLine("Regular grid identification, size {0}", Size * nt);
            //Initialization of operator
            for (int i = 0; i < Size; ++i)
            {
                for (int j = 0; j < nt; ++j)
                {
                    U[i, j] = 0.0;
                }
            }

            //Identification loop
            double diff = 0.0;
            double value = 0.0;
            double processDiff = 0.0;
            for (int step = 0; step < number_of_steps; ++step)
            {
                diff = 0.0;
                value = 0.0;
                processDiff = 0.0;
                for (int i = nt; i < N - N / 4; ++i)
                {
                    double delta = Y[i] - (U[X[i - 1], 0] + U[X[i - 2], 1] + U[X[i - 3], 2] + U[Y[i - 1], 3] + U[Y[i - 2], 4] + U[Y[i - 3], 5]);
                    diff = delta;
                    delta /= nt;
                    delta *= noise_suppression;
                    U[X[i - 1], 0] += delta;
                    U[X[i - 2], 1] += delta;
                    U[X[i - 3], 2] += delta;
                    U[Y[i - 1], 3] += delta;
                    U[Y[i - 2], 4] += delta;
                    U[Y[i - 3], 5] += delta;

                    processDiff += Math.Abs(diff);
                    value += Math.Abs(Y[i]);
                }
                Console.WriteLine("Accuracy (self) {0}", processDiff / value);
            }

            //Accuracy estimation on unseen data
            diff = 0.0;
            value = 0.0;
            processDiff = 0.0;
            for (int i = N - N / 4; i < N; ++i)
            {
                double delta = Y[i] - (U[X[i - 1], 0] + U[X[i - 2], 1] + U[X[i - 3], 2] + U[Y[i - 1], 3] + U[Y[i - 2], 4] + U[Y[i - 3], 5]);
                diff = delta;
                processDiff += Math.Abs(diff);
                value += Math.Abs(Y[i]);
            }
            Console.WriteLine("Accuracy (unseen) {0}\n", processDiff / value);
        }

        static void MakeLinearIdentification()
        {
            Console.WriteLine("Linear model, size {0}", nt);
            double[,] ATA = new double[nt, nt];
            double[] ATV = new double[nt];

            ATA[0, 0] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATA[0, 0] += X[i - 1] * X[i - 1];
            }

            ATA[0, 1] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATA[0, 1] += X[i - 1] * X[i - 2];
            }

            ATA[0, 2] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATA[0, 2] += X[i - 1] * X[i - 3];
            }

            ATA[0, 3] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATA[0, 3] += X[i - 1] * Y[i - 1];
            }

            ATA[0, 4] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATA[0, 4] += X[i - 1] * Y[i - 2];
            }

            ATA[0, 5] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATA[0, 5] += X[i - 1] * Y[i - 3];
            }

            ATA[1, 1] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATA[1, 1] += X[i - 2] * X[i - 2];
            }

            ATA[1, 2] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATA[1, 2] += X[i - 2] * X[i - 3];
            }

            ATA[1, 3] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATA[1, 3] += X[i - 2] * Y[i - 1];
            }

            ATA[1, 4] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATA[1, 4] += X[i - 2] * Y[i - 2];
            }

            ATA[1, 5] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATA[1, 5] += X[i - 2] * Y[i - 3];
            }

            ATA[2, 2] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATA[2, 2] += X[i - 3] * X[i - 3];
            }

            ATA[2, 3] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATA[2, 3] += X[i - 3] * Y[i - 1];
            }

            ATA[2, 4] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATA[2, 4] += X[i - 3] * Y[i - 2];
            }

            ATA[2, 5] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATA[2, 5] += X[i - 3] * Y[i - 3];
            }

            ATA[3, 3] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATA[3, 3] += Y[i - 1] * Y[i - 1];
            }

            ATA[3, 4] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATA[3, 4] += Y[i - 1] * Y[i - 2];
            }

            ATA[3, 5] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATA[3, 5] += Y[i - 1] * Y[i - 3];
            }

            ATA[4, 4] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATA[4, 4] += Y[i - 2] * Y[i - 2];
            }

            ATA[4, 5] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATA[4, 5] += Y[i - 2] * Y[i - 3];
            }

            ATA[5, 5] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATA[5, 5] += Y[i - 3] * Y[i - 3];
            }

            ATA[1, 0] = ATA[0, 1];
            ATA[2, 0] = ATA[0, 2];
            ATA[3, 0] = ATA[0, 3];
            ATA[4, 0] = ATA[0, 4];
            ATA[5, 0] = ATA[0, 5];

            ATA[2, 1] = ATA[1, 2];
            ATA[3, 1] = ATA[1, 3];
            ATA[4, 1] = ATA[1, 4];
            ATA[5, 1] = ATA[1, 5];

            ATA[3, 2] = ATA[2, 3];
            ATA[4, 2] = ATA[2, 4];
            ATA[5, 2] = ATA[2, 5];

            ATA[4, 3] = ATA[3, 4];
            ATA[5, 3] = ATA[3, 5];

            ATA[5, 4] = ATA[4, 5];

            ATV[0] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATV[0] += Y[i - 0] * X[i - 1];
            }

            ATV[1] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATV[1] += Y[i - 0] * X[i - 2];
            }

            ATV[2] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATV[2] += Y[i - 0] * X[i - 3];
            }

            ATV[3] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATV[3] += Y[i - 0] * Y[i - 1];
            }

            ATV[4] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATV[4] += Y[i - 0] * Y[i - 2];
            }

            ATV[5] = 0.0;
            for (int i = nt; i < N - N / 4; ++i)
            {
                ATV[5] += Y[i - 0] * Y[i - 3];
            }

            Cholesky ch = new Cholesky();
            ch.CholeskySolution(ATA, z, ATV, nt);

            //Accuracy estimation on unseen data
            double diff = 0.0;
            double value = 0.0;
            double processDiff = 0.0;
            for (int i = N - N / 4; i < N; ++i)
            {
                double delta = Y[i] - (z[0] * X[i - 1] + z[1] * X[i - 2] + z[2] * X[i - 3] +
                    z[3] * Y[i - 1] + z[4] * Y[i - 2] + z[5] * Y[i - 3]);
                diff = delta;
                processDiff += Math.Abs(diff);
                value += Math.Abs(Y[i]);
            }
            Console.WriteLine("Accuracy for linear system (unseen) {0}\n", processDiff / value);
        }

        static void CompareSignals()
        {
            for (int i = N - N / 4; i < N; ++i)
            {
                int linear = (int)Math.Round(z[0] * X[i - 1] + z[1] * X[i - 2] + z[2] * X[i - 3] + z[3] * Y[i - 1] + z[4] * Y[i - 2] + z[5] * Y[i - 3]);
                int u = (int)Math.Round(U[X[i - 1], 0] + U[X[i - 2], 1] + U[X[i - 3], 2] + U[Y[i - 1], 3] + U[Y[i - 2], 4] + U[Y[i - 3], 5]);
                Console.WriteLine("{0}, {1}, {2}", Y[i], linear, u);
            }
        }

        //--------------------------------------------------------------------
        //Next block is identification of grid model with linear interpolation
        //--------------------------------------------------------------------
        private static void MakeMap(int steps)
        {
            for (int i = 0; i < Size; ++i)
            {
                for (int j = 0; j < nt; ++j)
                {
                    map[i, j] = 0;
                }
            }
            int step = Size / steps;
            if (step < 1) step = 1;
            for (int j = 0; j < nt; ++j)
            {
                for (int i = 0; i < Size; ++i)
                {
                    if (i % step == 0) map[i, j] = 1;
                }
                if (map[Size - 1, j] == 0) map[Size - 1, j] = 1;
            }
        }

        private static int GetTop(int x, int j)
        {
            if (map[x, j] == 1) return x;
            while (true)
            {
                if (map[++x, j] == 1) return x;
            }
        }

        private static int GetBottom(int x, int j)
        {
            if (map[x, j] == 1) return x;
            while (true)
            {
                if (map[--x, j] == 1) return x;
            }
        }

        private static double GetLinear(int pos1, int pos2, double U1, double U2, int pos)
        {
            if (pos == pos1) return U1;
            if (pos == pos2) return U2;
            return (U2 - U1) / (double)(pos2 - pos1) * (double)(pos - pos1) + U1;
        }

        private static void MakeReducedGridInterpolation()
        {
            Console.WriteLine("Grid with linear interpolation, size {0}", map_size * nt);
            MakeMap(map_size);
            //set all zeros
            for (int i = 0; i < Size; ++i)
            {
                for (int j = 0; j < nt; ++j)
                {
                    U[i, j] = 0.0;
                }
            }
            //U[X[i - 1], 0] + U[X[i - 2], 1] + U[X[i - 3], 2] + U[Y[i - 1], 3] + U[Y[i - 2], 4] + U[Y[i - 3], 5]
            for (int counter = 0; counter < number_of_steps; ++counter)
            {
                double avdelta = 0.0;
                double avoutput = 0.0;
                for (int i = nt - 1; i < N - N/4; ++i)
                {
                    //get difference
                    double delta = 0.0;
                    for (int j = 0; j < 3; ++j)
                    {
                        int pos = (int)(X[i - j - 1]);
                        int top = GetTop(X[i - j - 1], j);
                        int bottom = GetBottom(X[i - j - 1], j);
                        double u = GetLinear(top, bottom, U[top, j], U[bottom, j], pos);
                        delta += u;
                    }
                    for (int j = 3; j < nt; ++j)
                    {
                        int pos = (int)(Y[i - j + 2]);
                        int top = GetTop(Y[i - j + 2], j);
                        int bottom = GetBottom(Y[i - j + 2], j);
                        double u = GetLinear(top, bottom, U[top, j], U[bottom, j], pos);
                        delta += u;
                    }
                    delta = (Y[i] - delta);
                    avdelta += Math.Abs(delta);
                    delta /= nt;
                    delta *= noise_suppression;
                    avoutput += Math.Abs(Y[i]);
                    //update
                    for (int j = 0; j < 3; ++j)
                    {
                        int pos = (int)(X[i - j - 1]);
                        int top = GetTop(X[i - j - 1], j);
                        int bottom = GetBottom(X[i - j - 1], j);
                        double deltaTop = 0.0;
                        double deltaBottom = 0.0;
                        if (pos == top) deltaTop = delta;
                        else if (pos == bottom) deltaBottom = delta;
                        else
                        {
                            deltaTop = delta / (double)(top - bottom) * (double)(pos - bottom);
                            deltaBottom = delta / (double)(top - bottom) * (double)(top - pos);
                        }
                        U[top, j] += deltaTop;
                        U[bottom, j] += deltaBottom;
                    }
                    for (int j = 3; j < nt; ++j)
                    {
                        int pos = (int)(Y[i - j + 2]);
                        int top = GetTop(Y[i - j + 2], j);
                        int bottom = GetBottom(Y[i - j + 2], j);
                        double deltaTop = 0.0;
                        double deltaBottom = 0.0;
                        if (pos == top) deltaTop = delta;
                        else if (pos == bottom) deltaBottom = delta;
                        else
                        {
                            deltaTop = delta / (double)(top - bottom) * (double)(pos - bottom);
                            deltaBottom = delta / (double)(top - bottom) * (double)(top - pos);
                        }
                        U[top, j] += deltaTop;
                        U[bottom, j] += deltaBottom;
                    }
                }
                Console.WriteLine("Accuracy (self) {0}", avdelta / avoutput);
            }

            double avdelta2 = 0.0;
            double avoutput2 = 0.0;
            for (int i = N - N / 4; i < N; ++i)
            {
                //get difference
                double delta = 0.0;
                for (int j = 0; j < 3; ++j)
                {
                    int pos = (int)(X[i - j - 1]);
                    int top = GetTop(X[i - j - 1], j);
                    int bottom = GetBottom(X[i - j - 1], j);
                    double u = GetLinear(top, bottom, U[top, j], U[bottom, j], pos);
                    delta += u;
                }
                for (int j = 3; j < nt; ++j)
                {
                    int pos = (int)(Y[i - j + 2]);
                    int top = GetTop(Y[i - j + 2], j);
                    int bottom = GetBottom(Y[i - j + 2], j);
                    double u = GetLinear(top, bottom, U[top, j], U[bottom, j], pos);
                    delta += u;
                }
                delta = (Y[i] - delta);
                avdelta2 += Math.Abs(delta);
                avoutput2 += Math.Abs(Y[i]);
            }
            Console.WriteLine("Accuracy (unseen) {0}", avdelta2 / avoutput2);

            //Filling operator gap
            for (int j = 0; j < nt; ++j)
            {
                int pos1 = 0;
                for (int i = 1; i < Size; ++i)
                {
                    if (map[i, j] > 0)
                    {
                        int pos2 = i;
                        if (pos2 - pos1 > 1)
                        {
                            double u1 = U[pos1, j];
                            double u2 = U[pos2, j];
                            for (int k = pos1 + 1; k < pos2; ++k)
                            {
                                U[k, j] = (u2 - u1) / (double)(pos2 - pos1) * (double)(k - pos1) + u1;
                            }
                        }
                        pos1 = pos2;
                    }
                }
            }
        }
        //----------------------------------------------------
        //end identification with linear interpolation in grid
        //----------------------------------------------------

        static void Main(string[] args)
        {
            GenerateX();
            GenerateY();
            NormalizeSignals();
            AddErrors(1);
            MakeGridIdentification();
            MakeLinearIdentification();
            //ShowSignals();
            //ShowOperator();
            //CompareSignals();
            MakeReducedGridInterpolation();
            //ShowOperator();
        }
    }
}