// Created with MonoDevelop
//
//    Copyright (C) 2009 Hiperscan GmbH Dresden, Germany
//
//    This program is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with this program.  If not, see <http://www.gnu.org/licenses/>.

using System;
using System.Collections.Generic;

using Hiperscan.Unix;

using MathNet.Numerics.LinearAlgebra.Double;
using MathNet.Numerics.LinearAlgebra.Factorization;


namespace Hiperscan.Spectroscopy.Math
{

    public static class SavitzkyGolay
    {
        private enum MatrixType
        {
            FIR,
            SG
        }

        private static Dictionary<string, Matrix> cache = new Dictionary<string, Matrix>();


        public static Matrix Vandermonde(Vector v)
        {
            Matrix M = new DenseMatrix(v.Count, v.Count);

            for (int ix = 0; ix < v.Count; ++ix)
            {
                for (int jx = 0; jx < v.Count; ++jx)
                {
                    M[ix, jx] = System.Math.Pow(v[ix], (double)(v.Count - jx - 1));
                }
            }
            return M;
        }

        private static string AssembleKey(int order, int window_size, MatrixType type)
        {
            return $"{order}:{window_size}:{Enum.GetName(typeof(MatrixType), type)}";
        }

        public static void ComputeMatrices(int order, int window_size)
        {
            if (window_size % 2 == 0)
                throw new ArgumentException(Catalog.GetString("Cannot create Savitzky-Golay filter: " +
                                                              "Window size must be odd."));

            if (order >= window_size)
                throw new ArgumentException(Catalog.GetString("Cannot create Savitzky-Golay filter: " +
                                                              "Window size must be greater than polynomial order."));

            Vector v = new DenseVector(window_size);

            int val = -(window_size - 1) / 2;
            for (int ix = 0; ix < window_size; ++ix)
            {
                v[ix] = (double)(val++);
            }

            Matrix V = SavitzkyGolay.Vandermonde(v);
            Matrix S = new DenseMatrix(window_size, order + 1);

            for (int ix = 0; ix < window_size; ++ix)
            {
                for (int jx = 0; jx < order + 1; ++jx)
                {
                    S[ix, jx] = V[ix, window_size - jx - 1];
                }
            }

            QR<double> decomposition = S.QR();

            Matrix Ri  = (Matrix)decomposition.R.Inverse();
            Matrix Rit = (Matrix)Ri.Transpose();
            Matrix G   = (Matrix)S.Multiply(Ri).Multiply(Rit);

            Matrix St = (Matrix)S.Transpose();
            Matrix B  = (Matrix)G.Multiply(St);

            string key = SavitzkyGolay.AssembleKey(order, window_size, MatrixType.FIR);
            lock (cache)
            {
                if (cache.ContainsKey(key))
                    cache[key] = B;
                else
                    cache.Add(key, B);

                key = SavitzkyGolay.AssembleKey(order, window_size, MatrixType.SG);
                if (cache.ContainsKey(key))
                    cache[key] = G;
                else
                    cache.Add(key, G);
            }
        }

        public static Matrix FIR(int order, int window_size)
        {
            string key = SavitzkyGolay.AssembleKey(order, window_size, MatrixType.FIR);

            lock (cache)
            {
                if (cache.ContainsKey(key) == false)
                    SavitzkyGolay.ComputeMatrices(order, window_size);

                return cache[key];
            }
        }

        public static Matrix SG(int order, int window_size)
        {
            string key = SavitzkyGolay.AssembleKey(order, window_size, MatrixType.SG);

            lock (cache)
            {
                if (!cache.ContainsKey(key))
                    SavitzkyGolay.ComputeMatrices(order, window_size);

                return cache[key];
            }
        }
    }
}
