// ************************************************************************** //
//                                                                            //
//    eses                   eses                                             //
//   eses                     eses                                            //
//  eses    eseses  esesese    eses   Embedded Systems Group                  //
//  ese    ese  ese ese         ese                                           //
//  ese    eseseses eseseses    ese   Department of Computer Science          //
//  eses   eses          ese   eses                                           //
//   eses   eseses  eseseses  eses    University of Kaiserslautern            //
//    eses                   eses                                             //
//                                                                            //
// ************************************************************************** //
// The algorithm below implements the Gram-Schmidt algorithm to compute an    //
// orthonormal basis of a vector space given any basis of linearly independent//
// vectors as columns of the input matrix a. The result is also given in the  //
// columns of the output matrix b. It is well-known that the algorithm is not //
// that good in terms of numeric accuracy.                                    //
// ************************************************************************** //


macro M = 4;
macro N = 3;
macro equal(a,b) = (-0.0000001 <= a-b) & (a-b<=0.0000001);
macro sqrt(x) = exp(x,0.5);


module GramSchmidt([M][N]real ?a,c,event !rdy) {
    [M][N]real b;   // to store the input matrix
    [N]real ip;     // to store the inner products

    // store input matrix a in matrix b
    for(i=0..M-1)
        for(j=0..N-1)
            b[i][j] = a[i][j];

    // start with the first vector as normalized vector of column 0 in b
    let(length = sqrt(sum(j=0..M-1) (b[j][0] * b[j][0])))
    for(i=0..M-1)
        next(c[i][0]) = b[i][0]/length;
    pause;

    for(k=1..N-1) {
        // compute inner products of b[*][k] with existing orthogonal vectors
        for(i=0..k-1)
            ip[i] = sum(j=0..M-1) (b[j][k] * c[j][i]);
        // generate new orthogonal vector c[*][k] without normalizing
        for(i=0..M-1)
            c[i][k] = b[i][k] - sum(j=0..k-1) (ip[j] * c[i][j]);
        // normalize the computed vector c[*][k] in the next step
        let(length = sqrt(sum(j=0..M-1) (c[j][k] * c[j][k])))
        for(i=0..M-1)
            next(c[i][k]) = c[i][k]/length;
        pause;
    }

    emit(rdy);
}
drivenby {
    // first vector
    a[0][0] = 2.0;
    a[1][0] = 1.0;
    a[2][0] = 0.0;
    a[3][0] = 3.0;
    // second vector
    a[0][1] = 4.0;
    a[1][1] = 2.0;
    a[2][1] = 1.0;
    a[3][1] = -1.0;
    // third vector
    a[0][2] = 1.0;
    a[1][2] = 0.0;
    a[2][2] = 2.0;
    a[3][2] = -13.0;
    await(rdy);
    // check that column vectors of c are orthogonal to each other
    for(i=0..N-1) {
        assert(equal(1.0,sum(k=0..M-1) (c[k][i] * c[k][i])));
        for(j=i+1..N-1)
            assert(equal(0.0,sum(k=0..M-1) (c[k][i] * c[k][j])));
    }
}