C: LU Complete Pivoting Decomposition and Matrix Solver; Something isn't right

I've been working on a DIY linalg solver for a few days, and its coming together (no small things to you guys at stackexchange) But I'm currently experiencing a Brain Fart and can't see what's wrong with the current code. Any insights would be appreciated; you guys rock!

The below code should be copy-pastable; the results should be -15,8,2, but its currently pumping out 2,inf,-inf, which is, needless to say, incorrect.

EDIT: I think the last thing left to fix is the Back Substitution / Ux=x stage, but as far as I can tell this is 'correct'. I'm following through this example to check my intermediate working

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

#define MAT1 3
#define TINY 1e-20
#define a(i,j) a[(i)*MAT1+(j)]

void h_pivot_decomp(float *a, int *p, int *q){
    int i,j,k;
    int n=MAT1;
    int pi,pj,tmp;
    float max;
    float ftmp;
    for (k=0;k<n;k++){
        pi=-1,pj=-1,max=0.0;
        //find pivot in submatrix a(k:n,k:n)
        for (i=k;i<n;i++) {
            for (j=k;j<n;j++) {
                if (fabs(a(i,j))>max){
                    max = fabs(a(i,j));
                    pi=i;
                    pj=j;
                }
            }
        }
        //Swap Row
        tmp=p[k];
        p[k]=p[pi];
        p[pi]=tmp;
        for (j=0;j<n;j++){
            ftmp=a(k,j);
            a(k,j)=a(pi,j);
            a(pi,j)=ftmp;
        }
        //Swap Col
        tmp=q[k];
        q[k]=q[pj];
        q[pj]=tmp;
        for (i=0;i<n;i++){
            ftmp=a(i,k);
            a(i,k)=a(i,pj);
            a(i,pj)=ftmp;
        }
        //END PIVOT

        //check pivot size and decompose
        if ((fabs(a(k,k))>TINY)){
            for (i=k+1;i<n;i++){
                //Column normalisation
                ftmp=a(i,k)/=a(k,k);
                for (j=k+1;j<n开发者_如何学运维;j++){
                    //a(ik)*a(kj) subtracted from lower right submatrix elements
                    a(i,j)-=(ftmp*a(k,j));
                }
            }
        }
        //END DECOMPOSE
    }
}


void h_solve(float *a, float *x, int *p, int *q){
    //forward substitution; see  Golub, Van Loan 96
    //And see http://www.cs.rutgers.edu/~richter/cs510/completePivoting.pdf
    int i,ii=0,ip,j,tmp;
    float ftmp;
    float xtmp[MAT1];
    //Swap rows (x=Px)
    puts("x=Px Stage");
    for (i=0; i<MAT1; i++){
        xtmp[i]=x[p[i]]; //value that should be here
        printf("x:%.1lf,q:%d\n",xtmp[i],q[i]);
    }
    //Lx=x
    puts("Lx=x Stage");
    //I suspect this is where this is falling down, as my implementation
    //uses the combined LU matrix, and this is using the non-unit diagonal
    for (i=0;i<MAT1;i++){
        ftmp=xtmp[i];
        if (ii != 0)
            for (j=ii-1;j<i;j++)
                ftmp-=a(i,j)*xtmp[j];
        else
            if (ftmp!=0.0)
                ii=i+1;
        xtmp[i]=ftmp;
        printf("x:%.1lf,q:%d\n",xtmp[i],q[i]);
    }
    puts("Ux=x");
    //backward substitution
    //partially taken from Sourcebook on Parallel Computing p577
    //solves Uy=z
    for (j=0;j<MAT1;j++){
        xtmp[j]=xtmp[j]/a(j,j);
        for (i=j+1;i<MAT1;i++){
            xtmp[i]-=a(i,j)*xtmp[j];
        }
        printf("x:%.1lf,q:%d\n",xtmp[i],q[i]);
    }
    //Last bit
    //solves x=Qy
    puts("x=Qx Stage");
    for (i=0;i<MAT1;i++){
        x[i]=xtmp[p[i]];
        printf("x:%.1lf,q:%d\n",x[i],q[i]);
    }
}


void main(){
    //3x3 Matrix
    //float a[]={1,-2,3,2,-5,12,0,2,-10};
    //float a[]={1,3,-2,3,5,6,2,4,3};
    //float b[]={5,7,8};
    //float a[]={1,2,3,2,-1,1,3,4,-1};
    //float b[]={14,3,8};
    float a[]={1,-2,1,0,2,2,-2,4,2};
    float b[]={1,4,2};
    int sig;
    puts("Declared Stuff");

    //pivot array (not used currently)
    int* p_pivot = (int *)malloc(sizeof(int)*MAT1);
    int* q_pivot = (int *)malloc(sizeof(int)*MAT1);
    puts("Starting Stuff");
    for (unsigned int i=0; i<MAT1; i++){
        p_pivot[i]=i;
        q_pivot[i]=i;
        printf("%.1lf|",b[i]);
        for (unsigned int j=0;j<MAT1; j++){
            printf("%.1lf,",a(i,j));
        }
        printf("|%d,%d",p_pivot[i],q_pivot[i]);
        puts("");
    }

    h_pivot_decomp(&a[0],p_pivot,q_pivot);
    puts("After Pivot");
    for (unsigned int i=0; i<MAT1; i++){
        printf("%.1lf|",b[i]);
        for (unsigned int j=0;j<MAT1; j++){
            printf("%.1lf,",a(i,j));
        }
        printf("|%d,%d",p_pivot[i],q_pivot[i]);
        puts("");
    }

    h_solve(&a[0],&b[0],p_pivot,q_pivot);
    puts("Finished Solve");

    for (unsigned int i=0; i<MAT1; i++){
        printf("%.1lf|",b[i]);
        for (unsigned int j=0;j<MAT1; j++){
            printf("%.1lf,",a(i,j));
        }
        puts("");
    }
}

---

Sorted; see here for full answers and code; there were too many small problems to itemise.

EDIT updated link (People still need this after 7 years?! :D )