//LinearEquation.inl		線性方程(組)求解函數(方法)定義
// Ver 1.0.0.0
// 版權所有(C) 何渝, 2002
// 最后修改: 2002.5.31

#ifndef _LINEAREQUATION_INL
#define _LINEAREQUATION_INL

//全選主元高斯消去法
template <class _Ty>
int LE_TotalChoiceGauss(matrix<_Ty>& a, valarray<_Ty>& b)  
{ 
	long double MaxValue, tmp;		//記錄主元絕對值
	int l(1), i, j, is;
    bool yn;
    
	int n = a.GetColNum();			//方程組階數
	
	valarray<int> js(n);			//保存換列位置
    
	for(int k = 0; k < n - 1; k++)	//全選主元
	{	
		MaxValue = 0.0;				//給保存主元絕對值變量賦初值
		        
		for(i = k; i < n; i++)
			for(j = k; j < n; j++)
            {		
				tmp = Abs(a(i, j));	//求m(i,j)絕對值
				if(tmp > MaxValue)	//發現一個更大的主元
				{ 
					MaxValue = tmp;	//保存新主元絕對值
					js[k] = j;		//新主元所在列
					is = i;			//新主元所在行
				}
            }
			
		yn = FloatEqual(MaxValue, 0);
        if(yn) l = 0;				//主元為0
		else
		{
			if(js[k] != k)			//換列
				for(i = 0; i < n; i++) swap(a(i, k), a(i, js[k]));
								
			if(is != k)				//換行
			{ 
				for (j = k; j < n; j++)	swap(a(k, j), a(is, j));

				swap(b[k], b[is]);	//方程組右邊第k元素與第is元素交換
			}
		}
        
		if(l == 0)					//矩陣奇異(主元為0)
		{
			printf("fail 1\n");
            return 0;				// 求解失敗，返回0值
		}
        
		MaxValue =  Abs(a(k, k));

        for(j = k + 1; j < n; j++)	a(k, j) /= a(k, k); //MaxValue;
        
		b[k] /= a(k, k); //MaxValue;
        for(i = k + 1; i < n; i++)
		{
			for(j = k + 1; j < n; j++)
			{
                a(i, j) = a(i, j) - a(i, k) * a(k, j);
			}
            
			b[i] = b[i] - a(i, k) * b[k];
		}
	}
    
	MaxValue = Abs(a((n - 1), (n - 1)));	//主元

	yn = FloatEqual(MaxValue, 0);
    if(yn)							//主元為0
	{
		cout<<"fail 2"<<endl;
        return(0);					//求解失敗，返回0值
	}

	b[n - 1] /= a((n - 1), (n - 1));//求解方程組右邊X的解

    for(i = n - 2; i >= 0; i--)		//回代過程
	{
		_Ty t = 0.0;
        
		for(j = i + 1; j < n; j++)	t = t + a(i, j) * b[j];
        
		b[i] = b[i] - t;
	}
    
	js[n - 1] = n - 1;				//X最后一個元素不用換
    for(k = n - 2; k >= 0; k --)	//k可以從n-2開始
		if(js[k] != k)				//交換X的元素位置(由全選換列產生的)
			swap(b[k], b[js[k]]);
    
	return(1);						//方程組求解成功！
}

//全選主元高斯-約當消去法
template <class _Ty>
int LE_TotalChoiceGaussJordan(matrix<_Ty> & a, matrix<_Ty> & b)
{ 
	long double MaxValue, tmp;	//主元絕對值
	int l(1), k, i, j, is;
	bool yn;
	
    int n = a.GetColNum();		//方程組階數
	int m = b.GetColNum();		//方程組右端常數向量的個數

	valarray<int> js(n);  
	for(k = 0; k < n; k++)
	{
		MaxValue = long double(0.0);
        
		for(i = k; i < n; i++)
			for(j = k; j < n; j++)
            {
				tmp = Abs( a(i, j) );
				if(tmp > MaxValue)
				{ 
					MaxValue = tmp;
					js[k] = j;
					is = i;
				}
            }
		
		yn = FloatEqual(MaxValue, 0);
		if(yn) l = 0; 		
		else
		{
			if(js[k] != k)
				for(i = 0; i < n; i++)
					swap(a(i, k), a(i, js[k]));
				
			if(is != k)
			{
				for(j = k; j < n; j++)
					swap(a(k, j), a(is, j));
				
				for(j = 0; j < m; j++)
					swap(b(k, j), b(is, j));

			}
		}

		if(l == 0)
		{
			cout<<"fail"<<endl;
			return 0;
		}
		
		for(j = k + 1; j < n; j++)
			a(k, j) /= a(k, k);

		for(j = 0; j < m; j++)
			b(k, j) /= a(k, k);				

		for(j = k + 1; j < n; j++)
			for(i = 0; i < n; i++)
				if(i != k)
					a(i, j) -= a(i, k) * a(k, j);

		for(j = 0; j < m; j++)
			for(i = 0; i< n; i++)
				if(i != k)
					b(i, j) -= a(i, k) * b(k, j);					
	}

    for(k = n - 1; k >= 0; k --)
	{
		if(js[k] != k)
			for(j = 0 ; j < m;  j++)
				swap(b(k, j), b(js[k], j));

	}
	return 1;				//成功返回
}

//求解三對角線方程組的追趕法
template <class _Ty>
int LE_TridiagonalEquationGauss(valarray<_Ty>& b, valarray<_Ty>& d)
{
	int j, k;
    _Ty s;
	bool yn;

    int n = d.size();	//方程組階數
	//m為n階三對角矩陣三條對角線上的元素個數，也是一維數組b的長度。
	//它的值應為m=3n-2, 在本函數中要對此值作檢查
	int m = b.size();	

    if(m != (3 * n - 2))	//驗證m是否等于3*n-2
	{
		cout << "Error!" << endl;
		return(-2);
	}
    
	for(k = 0; k < n - 1; k++)
	{
		j = 3 * k;
		s = b[j];

		yn = FloatEqual(s, 0.0);
		if(yn)						//主元為0
		{
			cout << "Fail!" << endl;
			return(0);
		}

        b[j + 1] = b[j + 1] / s;
        d[k] = d[k] / s;
        b[j + 3] = b[j + 3] - b[j + 2] * b[j + 1];
        d[k + 1] = d[k + 1] - b[j + 2] * d[k];
	}
    
	s = b[3 * n - 3];
    
	yn = FloatEqual(s, 0.0);
	if(yn)						//主元為0
	{
		cout << "Fail!" << endl;
		return(0);
	}
    
	d[n - 1] = d[n - 1] / s;
    
	for(k = n - 2; k >= 0; k--)
		d[k] = d[k] - b[3 * k + 1] * d[k + 1];
    
	return (2);	//運算成功，正常返回
}

//一般帶型方程組的求解
template <class _Ty>
int LE_StrapEquationGauss(matrix<_Ty>& b, matrix<_Ty>& d, int l, int il)
{
	int ls, k, i, j, is, u, v, x, y, hh, xx, yy, xxx, yyy;
    _Ty p, t, tt;
	bool yn;
    
	int n = b.GetRowNum();		//矩陣b的行數
    int nn = b.GetColNum();
	int mm = d.GetColNum();		//方程組右邊常數向量的個數
	
    if(il != (2 * l + 1))		//參數中半帶寬l與帶寬il的關系不對
	{
		cout<<"fail 1"<<endl;
		return(-2);
	}
    
	ls = l;
    
	for(k = 0; k < n - 1; k++)
	{
		p = 0.0;

        for(i = k ; i <= ls; i++)
		{
			u = i * il;
			y = u % nn;  x = (u - y) / nn;
			t = Abs(b(x, y));
            if(t > p) 
			{ 
				p = t;
				is = i;
			}
		}
        
		if( FloatEqual(p, 0.0) )		
		{
			cout << "fail 2" << endl;
			return(0);
		}
        
		for(j = 0; j < mm; j++)
		{
			u = k * mm + j;
			v = is * mm + j;
			y = u % mm;
			x = (u - y) / mm;
			yy = v % mm;
			xx = (v - yy) / mm;
    		tt = d(x, y);
			d(x, y) = d(xx, yy);
			d(xx, yy) = tt;
        }
        
		for(j = 0; j < il; j++)
		{
			u = k * il + j; 
			v = is * il + j;
            y = u % nn;  
			x = (u - y) / nn;
			yy = v % nn; 
			xx = (v - yy) / nn;
    		tt = b(x, y);
			b(x, y) = b(xx, yy); 
			b(xx, yy) = tt;
		}

        for(j = 0; j < mm; j++)
		{
			u = k * mm + j; v = k * il;
			y = u % mm;  x = (u - y) / mm;
            yy = v % nn;  xx = (v - yy) / nn;
	
			d(x, y) = d(x, y) / b(xx, yy);
		}

        for(j = 1; j < il; j++)
		{
			u = k * il + j;
			v = k * il;
			y = u % nn;  x = (u - y) / nn;
			yy = v % nn;  xx = (v - yy) / nn;
			b(x, y) = b(x, y) / b(xx, yy);
		}
        
		for(i = k + 1; i <= ls; i++)
		{
            hh = i * il;
		    y = hh % nn;
			x = (hh - y) / nn;
			t = b(x, y);
            for(j = 0; j < mm; j++)
			{
				u = i * mm + j;
				v = k * mm + j;
				y = u % mm;
				x = (u - y) / mm;
				yy = v % mm;
				xx = (v - yy) / mm;    
                d(x, y) = d(x, y) - t * d(xx, yy);
			}
            
			for(j = 1; j < il; j++)
			{
				u=i * il  +j;
				v = k * il + j;
				y = u % nn;  
				x = (u - y) / nn;
				yy = v % nn; 
				xx = (v - yy) / nn;
                yyy = (u - 1) % nn;  xxx = (u - yyy - 1) / nn;
                b(xxx, yyy) = b(x, y) - t * b(xx, yy);
			}
            
			u = i * il + il - 1; 
			y = u % nn;  x = (u - y) / nn;
			
			b(x, y) = 0.0;
		}
        
		if(ls != (n - 1))	ls++;
	}
    
	u = (n - 1) * il; 
	y = u % nn;  
	x = (u - y) / nn;
	p = b(x, y);
    
	yn = FloatEqual(p, 0.0);
	if(yn)
	{
		cout<<"fail 3"<<endl;
		return(0);
	}
    
	for(j = 0; j < mm; j++)
	{
		u = (n - 1) * mm + j; 
		y = u % mm;  
		x = (u - y) / mm;
		d(x, y) = d(x, y) / p;
	}

    ls = 1;
    for(i = n  - 2; i >= 0; i--)
	{
		for(k = 0; k < mm; k++)
		{ 
			u = i * mm + k;
            for(j = 1; j <= ls; j++)
			{
				v = i * il +j;
				is = (i + j) * mm + k;
                y = u % mm;  
				x = (u - y) / mm;
				yy = v % nn;  
				xx = (v - yy) / nn;
                yyy = is % mm;  
				xxx = (is - yyy) / mm;
                d(x, y) = d(x, y) - b(xx, yy) * d(xxx, yyy);
			}
		}
        if(ls != (il - 1)) ls++;
	}
    
	return 2;	//運算成功，正常返回
}

//求解對稱方程組的分解法
template <class _Ty>
int LE_SymmetryEquation(matrix<_Ty>& a, matrix<_Ty>& c)
{
	int i, j, k, k1, k2, k3;
    _Ty p;
	bool yn;

	if(MatrixSymmetry(a)!=true)
		return (-1);			//方程組不對稱
    
	int n = a.GetColNum();		//方程組階數
	int m = c.GetColNum();		//方程組右邊常數向量的個數
    
	yn = FloatEqual(a(0,0), 0.0);
	if(yn)						//主元為0
	{
		cout << "fail" << endl;
		return (-2);
	}

    for(i = 1; i < n; i++)
		a(i, 0) /= a(0, 0);

    for(i = 1; i < n - 1; i++)
	{
        for(j = 1; j <= i; j++)
            a(i, i) -= a(i, (j - 1)) * a(i, (j - 1)) * a((j - 1), (j - 1));
        
		p = a(i,i);
        yn = FloatEqual(p, 0.0);
		if(yn)						//主元為0
		{
			cout<<"fail"<<endl;
			return (-2);
		}
        
		for(k = i + 1; k < n; k++)
		{
            for(j = 1; j <= i; j++)
				a(k, i) -= a(k, (j - 1)) * a(i,(j - 1)) * a((j - 1), (j - 1));
            a(k, i) /= p;
		}
	}
    
	
    for(j = 1; j < n; j++)
	    a((n-1), (n-1)) -= a((n - 1), (j - 1)) * a((n - 1), (j - 1)) * a((j - 1), (j - 1));
    
	p=a((n - 1), (n - 1));
    yn = FloatEqual(p, 0.0);
	if(yn)						//主元為0
	{
		cout<<"fail"<<endl;
		return (-2);
	}
    
	for(j = 0; j < m; j++)
		for(i = 1; i < n; i++)
			for(k = 1; k <= i; k++)
				c(i, j) -= a(i, (k - 1)) * c((k - 1),j);
		
	for(i = 1; i < n; i++)
		for(j = i; j < n; j++)
            a((i - 1), j) = a((i - 1), (i - 1)) * a(j, (i - 1));
    
	for(j = 0; j < m; j++)  
    {
		c((n - 1), j) /= p;
		for(k = 1; k < n; k++)
		{
			k1 = n - k; 
			k3 = k1 - 1; 
            for(k2 = k1; k2 < n; k2++)
                c(k3, j) -= a(k3, k2) * c(k2, j);
			c(k3, j) /= a(k3, k3);
		}
	}
    
	return (1);	//運算成功，正常返回
}

//求解對稱正定方程組的平方根法
template <class _Ty>
int LE_SymmetryRegularEuationSquareRoot(matrix<_Ty> & a, matrix<_Ty> & d)
{
	int i, j, k;

	if(MatrixSymmetryRegular(a,1) != 2)	//判別矩陣a是否對稱正定
		return(-1);						//矩陣a不對稱正定
    
	int n = a.GetColNum();				//方程組階數
	int m = d.GetColNum();				//方程組右邊常數向量的個數
	
	a(0,0) = sqrt( a(0, 0) );