import sys
import math
sys.path.append('../matrix')
from matrix import vector, matrix

def qr (M:matrix) :
	for q in range(0,M.size2) :
		for p in range(q+1,M.size1) :
			theta=math.atan2(M[p,q],M[q,q]);
			c=math.cos(theta); s=math.sin(theta);
			for k in range(q,M.size2) :
				xq=M[q,k]; xp=M[p,k]
				M[q,k] =  xq*c+xp*s;
				M[p,k] = -xq*s+xp*c;
			M[p,q]=theta;

def apply_QT_inplace(M:matrix,b:vector) :
	for q in range(0,M.size2):
		for p in range(q+1,M.size1):
			xq=b[q]; xp=b[p]; theta = M[p,q];
			b[q] =  xq*math.cos(theta)+xp*math.sin(theta);
			b[p] = -xq*math.sin(theta)+xp*math.cos(theta);

def unpack_Q (M:matrix) :
	Q = matrix(M.size1,M.size2)
	ei = vector(M.size1)
	for i in range(0,M.size1) :
		ei.set_basis(i);
		apply_QT_inplace(M,ei);
		for j in range(0,M.size2) : Q[i,j] = ei[j];
	return Q;

def unpack_R(M:matrix) :
	m = M.size2;
	R = matrix(m,m);
	for c in range(0,m) :
		for r in range(0,c+1) :
			R[r,c] = M[r,c]
	return R;

def solve(M:matrix,b:vector):
	x=b.copy()
	solve_inplace(M,x)
	return x

def solve_inplace(M:matrix,x:vector) :
	apply_QT_inplace(M,x)
	for i in reversed(range(0,M.size2)):
		s=0
		for k in range(i+1,M.size2) : s+=M[i,k]*x[k];
		x[i] = (x[i]-s)/M[i,i]

def inverse(M:matrix) :
	B = matrix(M.size2,M.size2)
	B.set_identity();
	for i in range(0,B.size2) :
		solve_inplace(M,B[i]);
	return B;