//------------------------------------------------------------------------------
//	Remez のアルゴリズム
//
//		作成者：三上直樹，2022/12/07 作成，(C)三上直樹 2022
//------------------------------------------------------------------------------

using System;
using System.Linq;

using static System.Math;

namespace Mikami.Remez
{
	public class Remez
	{
		public enum FType {passStop, diff, hilbert};

		private const double M_2PI_	  = 2*PI;
		private const int GRIDDNS_	  = 32;
		private const int MAX_ITERAT_ = 50;
		private readonly double[] hk_;
		private readonly int nTaps_;
		
		// 奇数のとき true を返す
		private bool IsOdd(int x) => (x %2) != 0;

		public Remez(int order, out double[] hk)
		{
			nTaps_ = order+1;
			hk_ = new double[nTaps_];
			hk = hk_;
		}
		
		// Remez のアルゴリズム
		public bool Do(int nBands, double[] bands, double[] dg,
					   double[] weight, double[] deviat, FType type, double fs)
		{
			bool symmetry = (type == FType.passStop) ? true : false;
			int r = (IsOdd(nTaps_) && symmetry) ? (nTaps_/2 + 1) : nTaps_/2;	// 極値の数
			int gSize = 0;
			double[] nmBands = new double[2*nBands];
			for (int n=0; n<2*nBands; n++) nmBands[n] = bands[n]/fs;

			for (int n=0; n<nBands; n++)
				gSize = gSize + (int)(2*r*GRIDDNS_*(nmBands[2*n+1] - nmBands[2*n]) + 0.5);
			if (!symmetry) gSize--;

			double[] grid = new double[gSize+1];
			double[] wght = new double[gSize+1];
			double[] desired = new double[gSize+1];
			double[] w_er = new double[gSize+1];
			double[] taps = new double[r+1];
			double[] cwk = new double[r+1];
			double[] ck = new double[r+1];
			double[] bk = new double[r+1];
			int[] ext = new int[r+1];

			SetGrid(r, nBands, nmBands, dg, weight, ref gSize, grid,
					desired, wght, symmetry);

			// 極値の初期値
			for (int n=0; n<=r; n++) ext[n] = n*(gSize-1)/r;

			if (type == FType.diff)	// 微分器の場合
				for (int n=0; n<gSize; n++)
				{
					desired[n] = desired[n]*grid[n];
					if (desired[n] > 0.0001) wght[n] = wght[n]/grid[n];
				}

			if (symmetry)
			{
				if (!IsOdd(nTaps_)) Set(Cos, gSize, PI, grid, desired, wght);
			}
			else
			{
				if (IsOdd(nTaps_)) Set(Sin, gSize, M_2PI_, grid, desired, wght);
				else			  Set(Sin, gSize, PI, grid, desired, wght);
			}

			// Remez のアルゴリズムの実行
			int iter;
			double deviation = 0;
			for (iter=0; iter<MAX_ITERAT_; iter++)
			{
				if (!CalcParms(r, ext, grid, desired, wght, bk, cwk, ck))
					return false;
				for (int i=0; i<gSize; i++)
					w_er[i] = wght[i]*(desired[i] - CalcResp(grid[i], r, bk, cwk, ck));
				if (!SearchExtrms(r, ext, gSize, w_er)) return false;
				if (IsConverged(r, ext, w_er, out deviation)) break;
			}
			if (iter == MAX_ITERAT_)
				return false;	// 収束しない

			for (int n=0; n<nBands; n++) deviat[n] = deviation/weight[n];

			if (!CalcParms(r, ext, grid, desired, wght, bk, cwk, ck))
				return false;	// 収束しない

			double csx;
			for (int n=0; n<=nTaps_/2; n++)
			{
				if (symmetry)
					csx = IsOdd(nTaps_) ? 1.0 : Cos(PI*n/nTaps_);
				else
					csx = IsOdd(nTaps_) ? Sin(M_2PI_*n/nTaps_)
									   : Sin(PI*n/nTaps_);
				taps[n] = CalcResp((double)n/nTaps_, r, bk, cwk, ck)*csx;
			}

			ToCoefs(taps, symmetry);
			return true;	// 収束した
		}

		// 誤差極値の初期値の設定
		private void SetGrid(int r, int nBands, double[] bands, double[] dg,
							 double[] weight, ref int gSize, double[] grid,
							 double[] desired, double[] wght, bool symmetry)
		{
			var bandsX = bands.Select(x => x).ToArray();

			double dF = 0.5/(GRIDDNS_*r);
			if (!symmetry && (dF > bandsX[0])) bandsX[0] = dF;

			int j=0;
			for (int nb=0; nb<nBands; nb++)
			{
				grid[j] = bandsX[2*nb];
				double fL = bandsX[2*nb];
				double fH = bandsX[2*nb+1];
				int k = (int)((fH - fL)/dF + 0.5);

				for (int i=0; i<k; i++)
				{
					desired[j] = dg[nb];
					wght[j] = weight[nb];
					grid[j] = fL;
					fL = fL + dF;
					j++;
				}
				grid[j-1] = fH;
			}

			if (!symmetry && (grid[gSize-1] > (0.5 - dF)) && IsOdd(nTaps_) )
				grid[gSize-1] = 0.5 - dF;
		}

		// desired[], wght[] の設定
		private void Set(Func<double, double> fx, int gSize, double pi,
						 double[] grid, double[] desired, double[] wght)
		{
			for (int n=0; n<gSize; n++)
			{
				double csx = fx(pi*grid[n]);
				desired[n] = desired[n]/csx;
				wght[n] = wght[n]*csx;
			}
		}

		// パラメータの計算
		private bool CalcParms(int r, int[] ext, double[] grid, double[] desired,
							   double[] wght, double[] bk, double[] cwk, double[] ck)
		{
			for (int n=0; n<=r; n++) cwk[n] = Cos(M_2PI_*grid[ext[n]]);

			int jmax = (r - 1)/15 + 1;
			double denom;
			for (int n=0; n<=r; n++)
			{
				denom = 1.0;
				double xi = cwk[n];
				for (int j=0; j<jmax; j++)
					for (int k=j; k<=r; k+=jmax)
						if (k != n) denom =  2.0*denom*(xi - cwk[k]);
				if (Abs(denom)<0.00001) denom = 0.00001;
				bk[n] = 1.0/denom;
			}

			double numer = 0.0;
			denom = 0.0;
			double sign = 1;
			for (int n=0; n<=r; n++)
			{
				numer = numer + bk[n]*desired[ext[n]];
				denom = denom + sign*bk[n]/wght[ext[n]];
				sign = -sign;
			}
			if (denom == 0.0) return false;
	
			double delta = numer/denom;
			sign = 1;

			for (int n=0; n<=r; n++)
			{
				ck[n] = desired[ext[n]] - sign*delta/wght[ext[n]];
				sign = -sign;
			}
		
			return true;
		}

		// フィルタの応答の計算
		private double CalcResp(double freq, int r, double[] bk, double[] cwk, double[] ck)
		{
			double denom = 0.0;
			double numer = 0.0;
			double cos_f = Cos(M_2PI_*freq);

			for (int i=0; i<=r; i++)
			{
				double cfx = cos_f - cwk[i];
				if (Abs(cfx) < 1.0e-10)
				{
					numer = ck[i];
					denom = 1.0;
					break;
				}
				cfx = bk[i]/cfx;
				denom = denom + cfx;
				numer = numer + cfx*ck[i];
			}

			return (numer/denom);
		}

		// 誤差の極大，極小の探索
		private bool SearchExtrms(int r, int[] ext, int gSize, double[] w_er)
		{
			int[] f_ext = new int[2*r+1];

			int k = 0;
			int i;

			if (((w_er[0]>0.0) && (w_er[0]>w_er[1])) ||
				((w_er[0]<0.0) && (w_er[0]<w_er[1]))) f_ext[k++] = 0;
			try
			{
				for (i=1; i<gSize-1; i++)
					if (((w_er[i]>=w_er[i-1]) && (w_er[i]>w_er[i+1]) && (w_er[i]>0.0)) ||
						((w_er[i]<=w_er[i-1]) && (w_er[i]<w_er[i+1]) && (w_er[i]<0.0)))
						f_ext[k++] = i;

				i = gSize - 1;
				if (((w_er[i]>0.0) && (w_er[i]>w_er[i-1])) ||
					((w_er[i]<0.0) && (w_er[i]<w_er[i-1]))) f_ext[k++] = i;
			}
			catch
			{
				return false;
			}

			int extra = k - (r + 1);

			int m;
			bool alt;
			while (extra > 0)
			{
				bool up;
				if (w_er[f_ext[0]] > 0.0) up = true;	// 最初が極大の場合
				else					  up = false;	// 最初が極小の場合

				m=0;
				alt = true;
				for (int n=1; n<k; n++)
				{
					if (Abs(w_er[f_ext[n]]) < Abs(w_er[f_ext[m]]))
						m = n;
					if (up && (w_er[f_ext[n]] < 0.0))
						up = false;
					else
					{
						if ((!up) && (w_er[f_ext[n]] > 0.0))
							up = true;
						else
						{
							alt = false;
							break;
						}
					}
				}

				if (alt && (extra == 1))
				{
					if (Abs(w_er[f_ext[k-1]]) < Abs(w_er[f_ext[0]]))
						m = f_ext[k-1];		// 最後の極小を除去
					else
						m = f_ext[0];		// 最初の極小を除去
				}

				for (int n=m; n<k; n++)
				{
					if ((n+1) > (f_ext.Length-1)) return false;
					f_ext[n] = f_ext[n+1];
				}
				k--;
				extra--;
			}

			for (int n=0; n<=r; n++) ext[n] = f_ext[n];

			return true;
		}

		// 最適近似関数からフィルタ係数を計算
		private void ToCoefs(double[] taps, bool symmetry)
		{
			double nh = (nTaps_ - 1.0)/2.0;

			if (symmetry)
				for (int n=0; n<nTaps_; n++)
				{
					double sum = taps[0];
					for (int k=1; k<=(nTaps_-1)/2; k++)
						sum = sum + 2.0*taps[k]*Cos(k*M_2PI_*(n - nh)/nTaps_);
					hk_[n] = sum/nTaps_;
				}
			else
				for (int n=0; n<nTaps_; n++)
				{
					double sum = IsOdd(nTaps_) ? 0.0 : taps[nTaps_/2]*Sin(PI*(n - nh));
					for (int k=1; k<=(nTaps_-1)/2; k++)
						sum = sum + 2.0*taps[k]*Sin(k*M_2PI_*(n - nh)/nTaps_);
					hk_[n] = sum/nTaps_;
				}
		}

		// 収束の判定
		private bool IsConverged(int r, int[] ext, double[] w_er, out double deviation)
		{
			double min = ext.Take(r+1).Select(n => Abs(w_er[n])).Min();
			double max = ext.Take(r+1).Select(n => Abs(w_er[n])).Max();

			deviation = max;

			if (((max-min)/max) < 1.0e-10) return true;	// 収束
			return false; 								// 未収束
		}
	}
}
