#include <stdio.h>
#include <math.h>
#define  PI             3.141592653589793
#define  MAX_AR_ORDER   2
#define  MAX_POINT      512

double matrix_inv();

main()
{   int    i,j,k;
    int    ar_order,channel;
    double d[MAX_POINT][MAX_AR_ORDER+1],op[MAX_AR_ORDER][MAX_POINT];
    double d2[MAX_AR_ORDER][MAX_AR_ORDER],d2_inv[MAX_AR_ORDER][MAX_AR_ORDER];
    double p[MAX_AR_ORDER];
    double b,c,rt[MAX_AR_ORDER],rt_abs,rt_arg;
    static char   dataf[13]="         ";
    FILE *fp;
    
    printf("Input Parameters !!\n");
    printf("Data File             = ");
    scanf("%s",&dataf);

    printf("AR Order         [1-%4d] = ",MAX_AR_ORDER);
    scanf("%d",&ar_order);
    if((ar_order < 1) || (ar_order > MAX_AR_ORDER)) 
    {       printf("Error !!");
        exit();
    }
    printf("Number of data point [%1d-%4d] = ",MAX_AR_ORDER,MAX_POINT);
    scanf("%d",&channel);

/** read data  d[i][0] **/
    fp=fopen(dataf,"rt");
    for(i=0;i<channel;i++) fscanf(fp,"%lf\n",&d[i][0]);
    fclose(fp);

/** set matrix D **/
    for(j=1;j<=ar_order;j++)
    {   for(i=0;i<j;i++) d[i][j]=0;
        for(i=j;i<channel;i++) d[i][j]=d[i-j][0];
    }

/** initialize matrix d2 **/
    for(i=0;i<ar_order;i++)
    {   for(j=0;j<ar_order;j++) d2[i][j] = 0;
    }

/** calculate matrix d2 = D^t*D **/
    for(i=0;i<ar_order;i++)
    {       for(j=0;j<ar_order;j++)
        {
        for(k=ar_order;k<channel;k++) d2[i][j] = d2[i][j]+d[k][i+1]*d[
k][j+1];
        }
    }

/** calculate matrix d2_inv = inverse matrix of D^t*D **/
    matrix_inv(d2,d2_inv,ar_order);

/** initialize matrix op **/
    for(i=0;i<ar_order;i++)
    {   for(j=0;j<channel;j++) op[i][j] = 0;
    }

/** calculate matrix op = ((D^t*D)^-1)*D^t **/
    for(i=0;i<ar_order;i++)
    {   for(j=ar_order;j<channel;j++)
        {       
        for(k=0;k<ar_order;k++) op[i][j] = op[i][j] + d2_inv[i][k]*d[j][k+1];
        }
    }

/** initialize vector p **/
    for(i=0;i<ar_order;i++) p[i]=0;
    
/** calcurate vector p = op*x **/
    for(i=0;i<ar_order;i++)
    {       for(k=ar_order;k<channel;k++) p[i] = p[i] + op[i][k]*d[k][0];
    }

/** print parameter p[i] **/
    for(i=0;i<ar_order;i++) printf("p[%1d] = %+5.15lf\n",i,p[i]);
    
/** calculate roots **/
    printf("\n*** Results ***\n");
    switch(ar_order)
    {   case 1 : rt[0] = p[0];
            printf("z1 : %13lE\n\n",rt[0]); 
            break;
        case 2 : b = -p[0]; c = -p[1];
            if(b*b - 4*c >= 0)
            {   if(b>0) rt[0] = (-b - sqrt(b*b - 4*c))/2;
                else rt[0] = (-b + sqrt(b*b - 4*c))/2;
                rt[1] = c / rt[0];
                printf("z1 : %13lE\n",rt[0]);
                printf("z2 : %13lE\n\n",rt[1]);
            }
            else
            {   rt[0] = -b/2; rt[1] = sqrt(4*c - b*b)/2;
                rt_abs = sqrt(c);
                rt_arg = atan2(rt[1],rt[0]);
                printf("z1 : %13lE     %13lE i\n",rt[0],rt[1]);
                printf(" abs %13lE arg %13lE\n",rt_abs, rt_arg);
                printf("z2 : %13lE     %13lE i\n",rt[0],-rt[1]);
                printf(" abs %13lE arg %13lE\n\n",rt_abs,-rt_arg);
            }
            break;
        default : printf("Error !!\n");
    }
            
}

double matrix_inv(a,b,n)
double a[MAX_AR_ORDER][MAX_AR_ORDER],b[MAX_AR_ORDER][MAX_AR_ORDER];
int n;
{   int i,j,k;
    double e[MAX_AR_ORDER][MAX_AR_ORDER];
    double temp,det;
    for(i=0;i<n;i++)
    {   for(j=0;j<n;j++) e[i][j] = 0;
        e[i][i]=1;
    }

    for(i=0;i<n-1;i++)
    {   for(k=i+1;k<n;k++)
        {   temp = a[k][i];
            for(j=n-1;j>=0;j--)
            {   e[k][j]=e[k][j] - e[i][j]*temp/a[i][i];
                a[k][j]=a[k][j] - a[i][j]*temp/a[i][i];
            }
        }
    }

    for(k=n-1;k>=1;k--)
    {   for(i=k-1;i>=0;i--)
        {   for(j=0;j<n;j++) e[i][j] = e[i][j] - e[k][j]*a[i][k]/a[k][k]; 
        }
    }

    det=1;
    for(i=0;i<n;i++)
    {   for(j=0;j<n;j++) b[i][j] = e[i][j]/a[i][i];
        det = det*a[i][i];
    }
    return det;
}

