/*  Programming template for Final Part 2 Question 2

    Refer to Exercise 4.7 from the text. Consider the underdetermined
    system Ax=b where A is a m x n matrix with m<n.  Write a subroutine
    called undsolve to solve the optimization problem

        minimize ||x|| such that Ax=b

    by taking x=A^T(AA^T)^(-1)b.

    Feel free to use the matrix library developed in class or any other
    code that you find useful as part of your program.

    Note that the correct answer is given by xgold below.
*/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "matrixlib.h"

#define M 4
#define N 16
double A[M][N] = {
    {-3, -2,  7,  2, -3,  3,  5, -5, -1,  2, -6,  7, -5,  8,  5, -4},
    {-2, -7, -0, -5, -4, -1,  2,  1, -1, -3,  6,  3,  1, -1, -3,  5},
    { 4, -4, -1, -6,  7, -4, -3, -2,  6, -1, -3, -6, -1, -5, -2,  5},
    {-4,  5, -8, -0, -4,  2, -8,  3,  7,  7, -2, -0, -2,  3, -3, -6}};
double b[M] = {7, 4, -4, 6};
double xgold[N] = {
    -0.27614700835926598, -0.22110834746877939, -0.04157058765128363,
    -0.18063390490155223, -0.34581044195606775,  0.08855790447178240,
    -0.02641799063545121, -0.02437364725820414,  0.17141471409301212,
     0.14204694827411254, -0.00104446795047347,  0.30504086362707772,
    -0.17234209664819994,  0.26272280322091501, -0.07944203453328721,
    -0.06705152726498441 };
double x[N];

//  Find the solution to the underdetermined system Ax=b of least norm.
void undsolve(int m,int n,double A[m][n],double x[n],double b[m]){
    double AAT[m][m];
    bzero(AAT,sizeof(double)*m*m);
    for(int i=0;i<m;i++){
        for(int k=0;k<n;k++){
            for(int j=0;j<m;j++){
                AAT[i][j]+=A[i][k]*A[j][k];
            }
        }
    }
    double *P[m];
    PLUfact(m,AAT,P);
    double y[m];
    PLUsolve(m,AAT,P,y,b);
    bzero(x,sizeof(double)*n);
    for(int i=0;i<m;i++){
        for(int j=0;j<n;j++){
            x[j]+=A[i][j]*y[i];
        }
    }
}

int main(){
    printf("Final Part 2 Question 2.\n");
    undsolve(M,N,A,x,b);
    // At this point x should equal xgold.
    printf("x=\n");
    for(int i=0;i<N;i++) printf("%g\n",x[i]);
    double r=0;
    for(int i=0;i<N;i++){
        double t=x[i]-xgold[i];
        r+=t*t;
    }
    r=sqrt(r);
    printf("|r-rgold|=%g\n",r);
    if(r<1e-7){
        printf("Correct!  x and xgold agree!\n");
    } else {
        printf("Incorrect!  Computation of x is wrong!\n");
    }
    return 0;
}