#include <mex.h>
double Chebps(double x,double *f)
{
    double b[6]={0,0,0,0,0,0};
    int k;
    b[5]=1;
    for (k=4;k>=1;k--)
    {
        b[k]=2*x*b[k+1]-b[k+2]+f[5-k];
    }
    return (x*b[1]-b[2]+f[5]/2);
}

void Az_lsp(double *lsp,double *a,double *old_lsp,double *grid)
{
    double f1[6],f2[6],coef[6];
    int nf;          /* number of found frequencies */
    int ip;          /* indicator for f1 or f2      */
    double xlow,ylow,xhigh,yhigh,xmid,ymid,temp,sign,xint;
    int i,j;
    for (i=0;i<6;i++)
    {
        f1[i]=0;
        f2[i]=0;
    }
    f1[0]=1;
    f2[0]=1;
    for (i=0;i<5;i++)
    {
        f1[i+1]=a[i+1]+a[10-i]-f1[i];
        f2[i+1]=a[i+1]-a[10-i]+f2[i];
    }
    nf=0;
    ip=0;
    for (i=0;i<6;i++)
    {
        coef[i]=f1[i];
    }
    xlow=grid[0];
    ylow=Chebps(xlow,coef);
    j=0;
    while((nf < 10)&&(j < 50))
    {
           j =j+1;
           xhigh = xlow;
           yhigh = ylow;
           xlow  = grid[j];
           ylow=Chebps(xlow,coef);
           temp=ylow*yhigh;
           if (temp<=0)
           {
                for (i=0;i<2;i++)
                {
                    xmid = (xlow + xhigh)/2;
                    ymid =Chebps(xmid,coef);
                    temp=ylow*ymid;
                    if (temp<=0)
                    {
                        yhigh = ymid;
                        xhigh = xmid; 
                    }
                    else
                    {
                        ylow = ymid;
                        xlow = xmid;
                    }
                }
                sign=yhigh-ylow;
                if (sign==0)
                {
                    xint=xlow;
                }
                else
                {
                    xint = xlow - ylow*(xhigh-xlow)/(yhigh-ylow);
                }
                lsp[nf] = xint;
                xlow = xint;
                nf=nf+1;
                if (ip==0)
                {
                    ip = 1;
                    for (i=0;i<6;i++)
                    {
                        coef[i]=f2[i];
                    }
                }
                else
                {
                    ip = 0;
                    for (i=0;i<6;i++)
                    {
                        coef[i]=f1[i];
                    }
                }
                ylow=Chebps(xlow,coef);
           }
    }
    if ((nf-10)<0)
    {
        //lsp=old_lsp;
        for (i=0;i<10;i++)
        {
            lsp[i]=old_lsp[i];
        }
    }
}

void mexFunction(int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[])
{
    //輸入
    double *a,*old_lsp,*grid;          //a-11,old_lsp-10
    //輸出
    double *lsp;                   //lsp-10
    a=mxGetPr(prhs[0]);
    old_lsp=mxGetPr(prhs[1]);
    grid=mxGetPr(prhs[2]);
    plhs[0]=mxCreateDoubleMatrix(1,10,mxREAL);
    lsp=mxGetPr(plhs[0]);
    Az_lsp(lsp,a,old_lsp,grid);   
}