/*
Algorithm taken from Matrix Algorithms Vol. II by G.W. Stewart.
His original pseudocode is in the comments.
This function takes a vector a and produces a u that generates the
Householder transformation H = I - uuH such that Ha = ve1.
n is the length of the vectors.
*/
void housegen(Complex *a, Complex *u, Complex *v, int n)
{
int i;
Real tmp;
Complex rho;
//u = a
copyVect(a,u,n);
//v = ||a||2
v->real = norm(a,n);
v->imag = 0.0;//must be done after norm is taken above!
//if(v = 0)u[1] = sqrt(2); return; fi <-no clue what 'fi' is
if(v->real == 0.0)//v can only be real right now should i be using epsilon?
{
u[0].real = sqrt(2);
u[0].imag = 0;
return;
}
//if(u[1] != 0)
if(u[0].real != 0.0 || u[0].imag != 0.0)// not sure if I should use an epsilon here?
{
//rho = u_bar[1]/|u[1]|
tmp = ABS(u[0]);
rho.real = u[0].real/tmp;
rho.imag = -u[0].imag/tmp;
}
else//else rho = 1;
{
rho.real = 1.0;
rho.imag = 0.0;
}
//u = (rho/v)*u
for(i = 0; i < n; i++)
{
//v can still only be real at this point
u[i] = complexMult(rho, u[i]);
u[i].real /= v->real;
u[i].imag /= v->real;
}
//u[1] = 1 + u[1]
u[0].real = 1.0 + u[0].real;
//u = u/sqrt(u[1])
tmp = u[0].real;
for(i = 0; i < n; i++)
{
u[i].real = u[i].real/sqrt(tmp);//I think u[0] has tpo be real here
u[i].imag = u[i].imag/sqrt(tmp);
}
//v = -rho_bar*v
rho.real = -rho.real;
*v = complexMult(rho, *v);
}
/*! take measurements of strong signals for IQ balancing
*/
void Spectrum::measureIQError(double bg, double spec[])
{
// if (settings.value(s_sdr_iqdata[nrig],s_sdr_iqdata_def[nrig]).toBool()==false) return;
double a1[2];
double a2[2];
double z[2];
double p;
// cut level is 30 dB over background
double cut_calib = bg * 1000.0;
for (int i = 0; i < sizes.sample_length; i++) {
if (spec[i] > cut_calib) {
int j = i / 8;
a1[0] = out[i][0];
a1[1] = out[i][1];
a2[0] = out[sizes.sample_length - i - 1][0];
a2[1] = out[sizes.sample_length - i - 1][1];
complexMult(z, a1, a2);
p = a1[0] * a1[0] + a1[1] * a1[1] + a2[0] * a2[0] + a2[1] * a2[1];
z[0] /= p;
z[1] /= p;
calibSigList[j].n++;
calibSigList[j].zsum[0] += z[0];
calibSigList[j].zsum[1] += z[1];
}
}
}
/*
Multiply matrices mat1 and mat2 and put the result into dest. mat1 is an n1*m1 matrix.
mat2 is an m1*m2 matrix and dest is an n1*m2 matrix.
*/
void matrixMult(Complex *mat1, int n1, int m1, Complex *mat2, int m2, Complex *dest)
{
int i, j, k;
Complex tmp;
//first fill dest with zeros
for(i = 0; i < n1; i++)
{
for(j = 0; j < m2; j++)
{
INDEX(dest, n1, i, j).real = 0.0;
INDEX(dest, n1, i, j).imag = 0.0;
}
}
//now do the multiply
for(i = 0; i < n1; i++)
{
for(j = 0; j < m1; j++)
{
for(k = 0; k < m2; k++)
{
tmp = complexMult(INDEX(mat1, n1, i, j), INDEX(mat2, m1, j, k));
INDEX(dest, n1, i, k).real += tmp.real;
INDEX(dest, n1, i, k).imag += tmp.imag;
}
}
}
}
//This function takes the matrix A and a vector l containing its eigenvalues
//as well as a matrix V containing the eigenvectors. For each vector v in V
//it finds A*v-l*v and records the norm. Ideally it should be
//zero. The lagest norm will be returned.
//This function uses malloc, so it can't be used on the GPU.
Real eigenTest(Complex *A, Complex *V, Complex *l, int n)
{
int i,j,k;
Complex tmp_c;
Real max = -1.0;
Complex *tmp = malloc(n*sizeof(Complex));
for(i = 0; i < n; i++)//this will loop through eigenvectors
{
for(j = 0; j < n; j++)
{
tmp[j].real = 0.0;
tmp[j].imag = 0.0;
for(k = 0; k < n; k++)
{
tmp_c = complexMult(INDEX(A,n,j,k), INDEX(V,n,k,i));
tmp[j].real += tmp_c.real;
tmp[j].imag += tmp_c.imag;
}
}
for(j = 0; j < n; j++)
{
tmp_c = complexMult(INDEX(V,n,j,i), l[i]);
tmp[j].real -= tmp_c.real;
tmp[j].imag -= tmp_c.imag;
}
for(j = 0; j < n; j++)
{
if(norm(tmp,n) > max)
max = norm(tmp,n);
}
}
return max;
}
complex_t* fft(complex_t* x, int N)
{
complex_t* X = (complex_t*) malloc(sizeof(struct Complex) * N);
complex_t * d, * e, * D, * E;
int k;
if (N == 1)
{
X[0] = x[0];
return X;
}
e = (complex_t*) malloc(sizeof(struct Complex) * N / 2);
d = (complex_t*) malloc(sizeof(struct Complex) * N / 2);
for (k = 0; k < N / 2; k++)
{
e[k] = x[2 * k];
d[k] = x[2 * k + 1];
}
E = fft(e, N / 2);
D = fft(d, N / 2);
free(e);
free(d);
for (k = 0; k < N / 2; k++)
{
/* Multiply entries of D by the twiddle factors e^(-2*pi*i/N * k) */
D[k] = complexMult(complexFromPolar(1, -2.0 * M_PI *k / N), D[k]);
}
for (k = 0; k < N / 2; k++)
{
X[k] = complexAdd(E[k], D[k]);
X[k + N/2] = complexSub(E[k], D[k]);
}
free(D);
free(E);
return X;
}
/*
Perform an eigendecomposition of the complex n*n matrix A.
eigenValues is an array of length n where the eigenvalues will be stored.
V is an n*n matrix where the right eigenvalues will be stored.
rv1,cv1,cv2, and cv3 are arrays of length n used for scratch work.
Any values below espilon will be considered zero. A good value would probably
be the largest value that could be added to the largest value in the matrix
without changing it at the given Real precision (float or double).
maxiter is the maximum number of times we will attempt to deflate the matrix at
one eigenvalue (10-50 is probably good).
*/
void eigenDecomp(Complex *A, Complex *V, Complex *Q, Complex *eigenValues, Real *rv1, Complex *cv1, Complex *cv2, Complex *cv3, int n, Real epsilon, int maxiter)
{
int i, j, k;
Complex tmp_c;
//Put the upper hessenberg form of A into V
hessreduce(A, V, Q, cv1, cv2, n);
//Convert V into Schur form
hqr(V, Q, rv1, cv1, cv2, cv3, eigenValues, n, 50, epsilon);
//read the eigenvalues off the diagonal of the Schur matrix
for(i = 0; i < n; i++)
{
eigenValues[i] = INDEX(V,n,i,i);
}
//Now if I want to compute the eigenvectors, I can either use the method in
//the G.W. Stewart book or I can use gaussian elimination on the original
//matrix. Both methods would require a third matrix, so I went with his
//method.
righteigvec(V, A, n);
//now multiply the eigenvectors in X by Q and store in V (V = QX) (X = A)
for(i = 0; i < n; i++)
{
for(j = 0; j < n; j++)
{
INDEX(V,n,i,j).real = 0.0;
INDEX(V,n,i,j).imag = 0.0;
for(k = 0; k < n; k++)
{
tmp_c = complexMult(INDEX(Q,n,i,k), INDEX(A,n,k,j));
INDEX(V,n,i,j).real -= tmp_c.real;
INDEX(V,n,i,j).imag -= tmp_c.imag;
}
}
}
}
/*
Algorithm taken from Matrix Algorithms Vol. II by G.W. Stewart.
REturns the right eigenvectors of the upper tiangular matrix T in the matrix X.
T and X and n*n Complex matrices.
*/
void righteigvec(Complex *T, Complex *X, int n)
{
int k, i, j;
Real dmin, tmp, s;
Complex d, tmp_c;
//fill X with zeros just in case
for(i = 0; i < n; i++)
{
for(j = 0; j < n; j++)
{
INDEX(X,n,i,j).real = 0.0;
INDEX(X,n,i,j).imag = 0.0;
}
}
//4. for k = n to 1 by -1
for(k = n-1; k >= 0; k--)
{
//5. X[1:k-1,k] = -T[1:k-1,k]
for(i = 0; i <= k-1; i++)
{
INDEX(X,n,i,k).real = -INDEX(T,n,i,k).real;
INDEX(X,n,i,k).imag = -INDEX(T,n,i,k).imag;
}
//6. X[k,k] = 1
INDEX(X,n,k,k).real = 1.0;
INDEX(X,n,k,k).imag = 0.0;
//7. X[k+1:n,k] = 0
for(i = k+1; i < n; i++)
{
INDEX(X,n,i,k).real = 0.0;
INDEX(X,n,i,k).imag = 0.0;
}
//8. dmin = max{eps_M*|T[k,k]|,smallnum}
dmin = 0.0;
tmp = INDEX(T,n,k,k).real * INDEX(T,n,k,k).real + INDEX(T,n,k,k).imag * INDEX(T,n,k,k).imag;
tmp = EPS_M * sqrt(tmp);
if(tmp > SMALLNUM)
dmin = tmp;
else
dmin = SMALLNUM;
//9. for j = k-1 to 1 by -1
for(j = k-1; j >= 0; j--)
{
//10. d = T[j,j] - T[k,k]
d.real = INDEX(T,n,j,j).real - INDEX(T,n,k,k).real;
d.imag = INDEX(T,n,j,j).imag - INDEX(T,n,k,k).imag;
//11. if(|d| <= dmin) d = dmin fi
if(norm(&d,1) <= dmin)
{
d.real = dmin;
d.imag = 0.0;
}
//12. if(|X[j,k]|/bignum >= |d|)
if(norm(&(INDEX(X,n,j,k)),1)/BIGNUM >= norm(&d,1))
{
//13. s = |d|/|X[j,k]|
s = norm(&d, 1)/norm(&INDEX(X,n,j,k),1);
//14. X[1:k,k] = s*X[1:k,k]
for(i = 0; i <= k; i++)
{
INDEX(X,n,i,k).real = s*INDEX(X,n,i,k).real;
INDEX(X,n,i,k).imag = s*INDEX(X,n,i,k).imag;
}
}//15. endif
//16. X[j,k] = X[j,k]/d
tmp = INDEX(X,n,j,k).real;
INDEX(X,n,j,k).real = INDEX(X,n,j,k).real*d.real + INDEX(X,n,j,k).imag*d.imag;
INDEX(X,n,j,k).imag = INDEX(X,n,j,k).imag*d.real - tmp*d.imag;
tmp = d.real*d.real + d.imag*d.imag;
INDEX(X,n,j,k).real /= tmp;
INDEX(X,n,j,k).imag /= tmp;
//17. X[1:j-1,k] = X[1:j-1,k] - X[j,k]*T[1:j-1,j]
for(i = 0; i <= j-1; i++)
{
tmp_c = complexMult(INDEX(X,n,j,k), INDEX(T,n,i,j));
INDEX(X,n,i,k).real = INDEX(X,n,i,k).real - tmp_c.real;
INDEX(X,n,i,k).imag = INDEX(X,n,i,k).imag - tmp_c.imag;
}
}//18. end for j
//19. X[1:k,k] = X[1:k,k]/||X[1:k,k]||_2
tmp = norm(&INDEX(X,n,0,k),k+1);
for(i = 0; i <= k; i++)
{
INDEX(X,n,i,k).real /= tmp;
INDEX(X,n,i,k).imag /= tmp;
}
}//20. end for k
}//21. end righteigvec
/*
Algorithm taken from Matrix Algorithms Vol. II by G.W. Stewart.
Given the elements of the matrix:
B = | a b |
| c d |
wilkshift returns the eigenvalue of B that is nearest to d
*/
void wilkshift(Complex *a, Complex *b, Complex *c, Complex *d, Complex *k)
{
Real s, tmp;
Complex q, p, r, c_tmp;
//2. k = d
k->real = d->real;
k->imag = d->imag;
//3. s = |a| + |b| + |c| + |d|
s = sqrt(a->real*a->real + a->imag*a->imag) +
sqrt(b->real*b->real + b->imag*b->imag) +
sqrt(c->real*c->real + c->imag*c->imag) +
sqrt(d->real*d->real + d->imag*d->imag);
//4. if(s=0) return fi
if(s == 0.0)//TODO - This should probably be an epsilon
return;
//5. q = (b/s)*(c/s)
q = complexMult(*b,*c);
q.real = q.real/s;
q.imag = q.imag/s;
q.real = q.real/s;
q.imag = q.imag/s;
//6. if(q != 0)
if(q.real != 0.0 || q.imag != 0.0)//TODO - again, epsilon?
{
//7. p = 0.5*((a/s) - (d/s))
p.real = 0.5*(a->real/s - d->real/s);
p.imag = 0.5*(a->imag/s - d->imag/s);
//8. r = sqrt(p^2 + q)
//According to wikipedia, the principal square root of a complex number
//in cartesian coordinates is:
//sqrt(x + iy) = sqrt((r+x)/2) +/- i*sqrt((r-x)/2)
//where r = |x+iy| = sqrt(x^2 + y^2)
//The other root is -1 times the principal root (naturally)
c_tmp = complexMult(p,p);
c_tmp.real += q.real;
c_tmp.imag += q.imag;//c_tmp is now p^2 + q
tmp = sqrt(c_tmp.real*c_tmp.real + c_tmp.imag*c_tmp.imag);
r.real = sqrt((tmp + c_tmp.real)/2.0);
r.imag = sqrt((tmp - c_tmp.real)/2.0);
//9. if(Re(p)*Re(r) + Im(p)*Im(r) < 0) r = -r fi
if(p.real*r.real + p.imag*r.imag < 0.0)
r.real = -r.real;
r.imag = -r.imag;
//10. k = k - s*(q/(p+r))
c_tmp.real = p.real + r.real;
c_tmp.imag = p.imag + r.imag;
tmp = c_tmp.real*c_tmp.real + c_tmp.imag*c_tmp.imag;
c_tmp.real = c_tmp.real/tmp;
c_tmp.imag = -c_tmp.imag/tmp;
//now c_tmp = 1/(p+r)
c_tmp = complexMult(q,c_tmp);
k->real -= s*c_tmp.real;
k->imag -= s*c_tmp.imag;
}//11. end if
}//12. end wilkshift
/*
Algorithm taken from Matrix Algorithms Vol. II by G.W. Stewart.
This function creates a plane rotation from a and b. The cosine of the rotation
is real. a is overwritten with its final value and b is overwritten with zero.
TODO:
Right now a and b are being compared with 0.0. Depending on how they are
computed before being passed in, I may want to cmpare with an epsilon instead.
*/
void rotgen(Complex *a, Complex *b, Real *c, Complex *s)
{
Complex u;
Real v, norm_a, norm_b_sq;
//2. if(b = 0)
if(b->real == 0.0 && b->imag == 0.0)
{
//3. c = 1;s=0;return
*c = 1.0;
s->real = 0.0;
s->imag = 0.0;
return;
}//4. endif
//5. If (a = 0)
if(a->real == 0.0 && a->imag == 0.0)
{
//6. c = 0; s = 1; a = b; b = 0;return
*c = 0.0;
s->real = 0.0;
s->imag = 0.0;
a->real = b->real;
a->imag = b->imag;
b->real = 0.0;
b->imag = 0.0;
return;
}//7. endif
//8. mu = a/|a|
norm_a = sqrt(a->real * a->real + a->imag * a->imag);
u.real = a->real/norm_a;
u.imag = a->imag/norm_a;
//9. tau = |a| + |b|
//They instroduced this becuase they said it would save us a sqrt
//operation, claiming that t = |Re(a)| + |Im(a)| + |Re(b)| + |Im(b)|,
//which I don't see.
//10. v = t*sqrt(|a/t|^2 + |b/t|^2)
//Am I mistaken or does this not equal sqrt(|a|^2 + |b|^2),since t is real?
//It is
norm_b_sq = b->real * b->real + b->imag * b->imag;
v = sqrt(norm_a*norm_a + norm_b_sq);
//11. c = |a|/v
*c = norm_a/v;
//12. s = u*b_bar/v
b->imag = -b->imag;//It's okay that I'm overwriting this - not used anymore
*s = complexMult(u,*b);
s->real = s->real/v;
s->imag = s->imag/v;
//13. a = v*u
a->real = v*u.real;
a->imag = v*u.imag;
//14. b = 0
b->real = 0.0;
b->imag = 0.0;
}//15. end rotgen
/*
Algorithm taken from Matrix Algorithms Vol. II by G.W. Stewart.
This function takes a vector A of order n and reduces it to Hessenberg form
by Householder transformations.
In the future it may be possible to remove H entirely and do the transformation
in place.
A is an n*n matrix that I am transforming
H is an n*n matrix where I will put the result
Q is an n*n matrix where I will accumulate the transformations
u is a vector of length n for scratch work.
vH is another vector of length n for scratch work
*/
void hessreduce(Complex *A, Complex *H, Complex *Q, Complex *u, Complex *vH, int n)
{
int k, i, j, l;
Complex tmp;
if(n < 2)
return;
//Reduce A
//H = A
copyMat(A,H,n,n);
//3. for k = 1 to n-2
for(k = 0; k < n-2; k++)
{
//Generate the Transformation
//housegen(H[k+1:n,k],u,H[k+1,k])
housegen(&INDEX(H,n,(k+1),k),u+k+1,&INDEX(H,n,(k+1),k),n-k-1);
//5. Q[k+1:n,k] = u
copyVect(u+k+1, &INDEX(Q, n, (k+1), k), n-k-1);
//Premultiply the transformation
//6. vH = uH*H[k+1:n,k+1:n]
for(i = k+1; i < n; i++)
{
vH[i].real = 0.0;
vH[i].imag = 0.0;
for(j = k+1; j < n; j++)
{
tmp = INDEX(H,n,j,i);
vH[i].real += u[j].real * tmp.real;
vH[i].real += u[j].imag * tmp.imag;//minus minus for hermitian
vH[i].imag += u[j].real * tmp.imag;
vH[i].imag -= u[j].imag * tmp.real;//minus sign is for hermitian
}
}
//7. H[k+1:n,k+1:n] = H[k+1:n, k+1:n] - u*vH
for(i = k+1; i < n; i++)
{
for(j = k+1; j < n; j++)
{
INDEX(H,n,i,j).real -= u[i].real *vH[j].real;
INDEX(H,n,i,j).real += u[i].imag *vH[j].imag;
INDEX(H,n,i,j).imag -= u[i].real *vH[j].imag;
INDEX(H,n,i,j).imag -= u[i].imag *vH[j].real;
}
}
//H[k+2:n,k] = 0
for(i = k+2; i < n; i++)
{
INDEX(H,n,i,k).real = 0.0;
INDEX(H,n,i,k).imag = 0.0;
}
//Postmultiply the transformation
//9. v = H[1:n, k+1:n]*u
//I will use the variable vH for v (for space).
for(i = 0; i < n; i++)
{
vH[i].real = 0.0;
vH[i].imag = 0.0;
for(j = k+1; j < n; j++)
{
tmp = INDEX(H,n,i,j);
vH[i].real += tmp.real * u[j].real;
vH[i].real -= tmp.imag * u[j].imag;
vH[i].imag += tmp.real * u[j].imag;
vH[i].imag += tmp.imag * u[j].real;
}
}
//10. H[1:n, k+1:n] = H[1:n,k+1:n] - v*uH
for(i = 0; i < n; i++)
{
for(j = k+1; j < n; j++)
{
INDEX(H,n,i,j).real -= vH[i].real * u[j].real;
INDEX(H,n,i,j).real -= vH[i].imag * u[j].imag;
INDEX(H,n,i,j).imag += vH[i].real * u[j].imag;
INDEX(H,n,i,j).imag -= vH[i].imag * u[j].real;
}
}
}//end k
//Accumulate the Transformations
//12. Q[:,n] = e_n; Q[:,n-1] = e_(n-1)
for(i = 0; i < n; i++)
{
INDEX(Q,n,i,(n-1)).real = 0.0;
INDEX(Q,n,i,(n-1)).imag = 0.0;
INDEX(Q,n,i,(n-2)).real = 0.0;
INDEX(Q,n,i,(n-2)).imag = 0.0;
}
INDEX(Q,n,(n-1),(n-1)).real = 1.0;
INDEX(Q,n,(n-2),(n-2)).real = 1.0;
//13. for k = n-2 to 1 by -1
for(k = n-3; k >= 0; k--)
{
//14. u = Q[k+1:n,k]
copyVect(&INDEX(Q,n,(k+1),k), u+k+1, n-(k+1));
//15. vH = uH*Q[k+1:n,k+1:n]//Q[k+1:n,k] = u
for(i = k+1; i < n; i++)
{
vH[i].real = 0.0;
vH[i].imag = 0.0;
for(j = k+1; j < n; j++)
{
tmp.real = u[j].real;
tmp.imag = -u[j].imag;
tmp = complexMult(tmp, INDEX(Q, n, j, i));
vH[i].real += tmp.real;
vH[i].imag += tmp.imag;
}
}
//16. Q[k+1:n, k+1:n] = Q[k+1:n, k+1:n] -u*vH
for(i = k+1; i < n; i++)
{
for(j = k+1; j < n; j++)
{
tmp = complexMult(u[i],vH[j]);
INDEX(Q,n,i,j).real -= tmp.real;
INDEX(Q,n,i,j).imag -= tmp.imag;
}
}
//17. Q[:,k] = e_k
for(i = 0; i < n; i++)
{
INDEX(Q,n,i,k).real = 0.0;
INDEX(Q,n,i,k).imag = 0.0;
}
INDEX(Q,n,k,k).real = 1.0;
}// 18. end for k
}//end hessreduce
