/*
Copyright (C) 2019-2020 JingWeiZhangHuai <jingweizhanghuai@163.com>
Licensed under the Apache License, Version 2.0; you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.
*/

#include <cblas.h>
#include "morn_tensor.h"

void _GetRowData0(float *tdata,float *data,int height,int width,int y_locate,int x_locate)
{
    if(y_locate<0)       {memset(data,0,width*sizeof(float)); return;}
    if(y_locate>=height) {memset(data,0,width*sizeof(float)); return;}

    tdata = tdata+y_locate*width;
    if(x_locate<0)
    {
        memset(data,0,(0-x_locate)*sizeof(float));
        memcpy(data-x_locate,tdata,(width+x_locate)*sizeof(float));
        return;
    }
    if(x_locate>0)
    {
        memcpy(data,tdata+x_locate,(width-x_locate)*sizeof(float));
        memset(data+width-x_locate,0,x_locate*sizeof(float));
        return;
    }
    memcpy(data,tdata,width*sizeof(float));
}
void _GetRowData(float *tdata,float *data,int height,int width,int y_locate,int x_locate,int stride)
{
    if(y_locate<0)       {memset(data,0,width/stride*sizeof(float)); return;}
    if(y_locate>=height) {memset(data,0,width/stride*sizeof(float)); return;}

    tdata = tdata+y_locate*width;

    int i=x_locate;int n=0;
    for(;i<0                        ;i+=stride) {data[n]=0;       n++;}
    for(;i<MIN(width,width+x_locate);i+=stride) {data[n]=tdata[i];n++;}
    for(;i<width+x_locate           ;i+=stride) {data[n]=0;       n++;}
}

void _SetRowData(float *tdata,float *data,int height,int width,int y_locate,int x_locate,int stride)
{
    if(y_locate<0)       return;
    if(y_locate>=height) return;

    tdata = tdata+y_locate*width;

    int i=x_locate;int n=0;
    for(;i<0                        ;i+=stride) {                  n++;}
    for(;i<MIN(width,width+x_locate);i+=stride) {tdata[i]+=data[n];n++;}
  //for(;i<width+x_locate           ;i+=stride) {                  n++;}
}

void ConvTensorToMatData0(MTensor *tns,int bc,float *mdata,int knl_height,int knl_width)
{
    int height = tns->height;
    int width  = tns->width;
    int channel= tns->channel;

    // int mheight= knl_height*knl_width*channel+1;
    int mwidth = width*height;

    int x1=0-(knl_width /2);int x2=x1+knl_width -1;
    int y1=0-(knl_height/2);int y2=y1+knl_height-1;

    float *tdata = tns->data[bc];
    for(int c=0;c<channel;c++)
    {
        int k=0;
        {
            for(int i=x1;i<=x2;i++)
            {
                float *data = mdata + k*mwidth;k++;
                for(int n=y1;n<y1+height;n++)
                {
                    _GetRowData0(tdata,data,height,width,n,i);
                    data+=width;
                }
            }
        }
        for(int j=y1+1+height-1;j<=y2+height-1;j++)
        {
            for(int i=x1;i<=x2;i++)
            {
                float *data = mdata + k*mwidth;k++;
                memcpy(data,data-knl_width*mwidth+width,(mwidth-width)*sizeof(float));
                _GetRowData0(tdata,data+(mwidth-width),height,width,j,i);
            }
        }
        tdata+=width*height;
        mdata+=mwidth*knl_height*knl_width;
    }
    for(int i=0;i<mwidth;i++) mdata[i]=1.0;
}

void ConvTensorToMatData(MTensor *tns,int bc,float *mdata,int knl_height,int knl_width,int y_stride,int x_stride)
{
    if((x_stride==1)&&(y_stride==1)) {ConvTensorToMatData0(tns,bc,mdata,knl_height,knl_width);return;}

    int height = tns->height;
    int width  = tns->width;
    int channel= tns->channel;

    int out_width = width/x_stride;
    int out_height=height/y_stride;
    // int mheight= knl_height*knl_width*channel+1;
    int mwidth = out_width*out_height;

    int x1=0-(knl_width /2);int x2=x1+knl_width -1;
    int y1=0-(knl_height/2);int y2=y1+knl_height-1;

    float *tdata = tns->data[bc];
    for(int c=0;c<channel;c++)
    {
        int k=0;
        for(int j=y1;j<=y2;j++)for(int i=x1;i<=x2;i++)
        {
            float *data = mdata + k*mwidth;k++;
            for(int n=j;n<j+height;n+=y_stride)
            {
                _GetRowData(tdata,data,height,width,n,i,x_stride);
                data+=out_width;
            }
        }
        tdata+=width*height;
        mdata+=mwidth*knl_height*knl_width;
    }
    for(int i=0;i<mwidth;i++) mdata[i]=1.0;
}

void ConvMatDataToTensor(float *mdata,MTensor *tns,int bc,int knl_height,int knl_width,int y_stride,int x_stride)
{
    int height = tns->height;
    int width  = tns->width;
    int channel= tns->channel;

    int out_width = width/x_stride;
    int out_height=height/y_stride;
    // int mheight= knl_height*knl_width*channel+1;
    int mwidth = out_height*out_width;

    // printf("height=%d,width=%d,channel=%d,out_height=%d,out_width=%d\n",height,width,channel,out_height,out_width);

    int x1=0-(knl_width /2);int x2=x1+knl_width -1;
    int y1=0-(knl_height/2);int y2=y1+knl_height-1;

    float *tdata = tns->data[bc];

    for(int c=0;c<channel;c++)
    {
        int k=0;
        for(int j=y1;j<=y2;j++)for(int i=x1;i<=x2;i++)
        {
            float *data = mdata + k*mwidth;k++;
            for(int n=j;n<j+height;n+=y_stride)
            {
                _SetRowData(tdata,data,height,width,n,i,x_stride);
                data+=out_width;
            }
        }
        tdata+=width*height;
        mdata+=mwidth*knl_height*knl_width;
    }
}

struct TensorConvPara
{
    MLayer *prev;

    int knl_num;
    int knl_height;
    int knl_width;

    int x_stride;
    int y_stride;

    int res_valid;

    float rate;
    float decay;
    float momentum;
};
void *mTensorConvPara(MFile *ini,char *name)
{
    struct TensorConvPara *para = (struct TensorConvPara *)mMalloc(sizeof(struct TensorConvPara));

    para->prev = mNetworkLayer(ini,mINIRead(ini,name,"prev"));
    mException((para->prev == NULL),EXIT,"invalid prev");
    para->res_valid = (strcmp("Input",mLayerType(para->prev))!=0);

    para->knl_num   = 1; mINIRead(ini,name,"knl_num"   ,"%d",&(para->knl_num   ));
    para->knl_height= 1; mINIRead(ini,name,"knl_height","%d",&(para->knl_height));
    para->knl_width = 1; mINIRead(ini,name,"knl_width" ,"%d",&(para->knl_width ));
    para->x_stride  = 1; mINIRead(ini,name,"x_stride"  ,"%d",&(para->x_stride  ));
    para->y_stride  = 1; mINIRead(ini,name,"y_stride"  ,"%d",&(para->y_stride  ));

    para->rate    =0.001;if(mINIRead(ini,name,"rate"    ,"%f",&(para->rate    ))==NULL) mINIRead(ini,"para","rate"    ,"%f",&(para->rate    ));
    para->momentum=0.9  ;if(mINIRead(ini,name,"momentum","%f",&(para->momentum))==NULL) mINIRead(ini,"para","momentum","%f",&(para->momentum));
    para->decay   =0.01 ;if(mINIRead(ini,name,"decay"   ,"%f",&(para->decay   ))==NULL) mINIRead(ini,"para","decay"   ,"%f",&(para->decay   ));
    mException((para->decay<0.0f)||(para->decay>=1.0f),EXIT,"invalid para decay");

    return para;
}

struct HandleTensorConv
{
    float *mat;
    float *kernel;
    float *update;
};
void endTensorConv(void *info)
{
    struct HandleTensorConv *handle = (struct HandleTensorConv *)info;
    if(handle->mat   != NULL) mFree(handle->mat);
    if(handle->kernel!= NULL) mFree(handle->kernel);
    if(handle->update!= NULL) mFree(handle->update);
}
#define HASH_TensorConv 0x9087d39c
void TensorConvSet(MLayer *layer)
{
    if(layer->state != DFLT) return;
    // mException(strcmp("Conv",mLayerType(layer)),EXIT,"invalid layer type");
    struct TensorConvPara *para = (struct TensorConvPara *)(layer->para);
    MTensor *in = para->prev->tns;
    MTensor *res= para->prev->res;
    MTensor *out= layer->tns;

    MHandle *hdl=mHandle(out,TensorConv);
    struct HandleTensorConv *handle = (struct HandleTensorConv *)(hdl->handle);

    int out_height= in->height/para->y_stride;
    int out_width = in->width /para->x_stride;
    int mwidth = (out_height*out_width);
    int mheight= para->knl_height*para->knl_width*in->channel+1;
    int data_size = para->knl_num*mheight;

    mTensorRedefine(out,in->batch,para->knl_num,out_height,out_width,NULL);
    if(morn_network_flag == MORN_TRAIN)
    {
        if(INVALID_TENSOR(res)) mTensorRedefine(res,in->batch,in->channel,in->height,in->width,in->data);
        else
        {
            // printf("llllllllllllllllllllllllll layer->name=%s\n",layer->name);
            mTensorRedefine(res,in->batch,in->channel,in->height,in->width,NULL);
        }

        if(morn_network_flag==MORN_TRAIN)
        {
            if(handle->update != NULL) mFree(handle->update);
            handle->update =(float *)mMalloc(data_size*sizeof(float));
            memset(handle->update,0,data_size*sizeof(float));
        }
    }

    if(handle->kernel !=NULL) mFree(handle->kernel);
    handle->kernel = (float *)mMalloc(data_size*sizeof(float));

    if(morn_network_parafile==NULL)
    {
        float scale = sqrt(2.0f/mheight);
        for(int i=0;i<data_size;i++)
            handle->kernel[i] = mNormalRand(0.0f,1.0f)*scale;
    }
    else
    {
        mNetworkParaRead(layer,"kernel",handle->kernel,data_size*sizeof(float));
    }

    if(handle->mat!=NULL) mFree(handle->mat);
    handle->mat = (float *)mMalloc(mheight*mwidth*sizeof(float));

    hdl->valid = 1;
}

void mTensorConvForward(MLayer *layer)
{
    mException(INVALID_POINTER(layer),EXIT,"invalid input");
    struct TensorConvPara *para = (struct TensorConvPara *)(layer->para);

    MTensor *in = para->prev->tns;
    MTensor *out=layer->tns;
    // printf("in->data[0][0]=%f\n",in->data[0][0]);

    TensorConvSet(layer);

    MHandle *hdl=mHandle(out,TensorConv);
    struct HandleTensorConv *handle = (struct HandleTensorConv *)(hdl->handle);

    int mwidth = (out->height*out->width);
    int mheight= para->knl_height*para->knl_width*in->channel+1;

    float *kernel_data= handle->kernel;
    float *in_data = handle->mat;

    for(int b=0;b<in->batch;b++)
    {
        // printf("ccccccccccccccc in->width=%d\n",in->width);
        ConvTensorToMatData(in,b,in_data,para->knl_height,para->knl_width,para->y_stride,para->x_stride);
        // printf("ccccccccccccccc in->width=%d\n",in->width);
        float *out_data = out->data[b];

        // printf("\nweight=\n");for(int ii=0;ii<100;ii++) printf("%f,",kernel_data[ii]);
        // printf("\ndata=\n");for(int ii=0;ii<100;ii++) printf("%f,",in_data[ii]*255);
        // printf("\ndata2352=\n");for(int ii=2352;ii<2352+100;ii++) printf("%f,",in_data[ii]*255);
        // printf("\ndata6272=\n");for(int ii=6272;ii<6272+100;ii++) printf("%f,",in_data[ii]*255);
        // printf("\ndata10192=\n");for(int ii=10192;ii<10192+100;ii++) printf("%f,",in_data[ii]*255);
        // printf("\ndata=\n");for(int ii=784*5;ii<784*5+100;ii++) printf("%f,",in_data[ii]*255);
        // printf("\ndata=\n");for(int ii=784*25;ii<784*25+100;ii++) printf("%f,",in_data[ii]*255);
        // printf("\ndata=\n");for(int ii=10000;ii<10000+100;ii++) printf("%f,",in_data[ii]*255);

        // float sum=0;
        // for(int ii=0;ii<75;ii++)
        // {
        //     sum+=(kernel_data[ii]*in_data[mwidth*ii]);
        //     printf("a[ii]=%f,b[n*ii]=%f,sum=%f,ii=%d,ii*mwidth=%d\n",kernel_data[ii],in_data[mwidth*ii]*255,sum,ii,ii*mwidth);
        // }
        // printf("\nsum=%f\n",sum);

        // printf("para->knl_num=%d,mwidth=%d,mheight=%d\n",para->knl_num,mwidth,mheight);
        cblas_sgemm(CblasRowMajor,CblasNoTrans,CblasNoTrans,
                    para->knl_num,mwidth,mheight,
                    1.0f,
                    kernel_data,mheight,
                        in_data,mwidth,
               0.0f,   out_data,mwidth);

        // printf("\nout=\n");for(int ii=00;ii<200;ii++) printf("%f,",out_data[ii]);
    }

    layer->state = MORN_FORWARD;
}

void mTensorConvBackward(MLayer *layer)
{
    mException(INVALID_POINTER(layer),EXIT,"invalid input");
    struct TensorConvPara *para = (struct TensorConvPara *)(layer->para);
    MTensor *in = para->prev->tns;
    MTensor *res= para->prev->res;
    MTensor *out=layer->res;

    MHandle *hdl=mHandle(layer->tns,TensorConv);
    struct HandleTensorConv *handle = (struct HandleTensorConv *)(hdl->handle);
    mException((hdl->valid == 0),EXIT,"no forward operate");

    int mwidth = (out->height*out->width);
    int mheight= para->knl_height*para->knl_width*in->channel+1;

    float *kernel_data= handle->kernel;
    float *update_data= handle->update;
    float *    in_data= handle->mat;
    float *   res_data= handle->mat;

    mNetworkParaWrite(layer,"kernel",kernel_data,para->knl_num*mheight*sizeof(float));

    float beta = para->momentum;
    for(int b=0;b<out->batch;b++)
    {
        ConvTensorToMatData(in,b,in_data,para->knl_height,para->knl_width,para->y_stride,para->x_stride);
        float *out_data = out->data[b];

        // printf("m=%d,n=%d,k=%d\n",para->knl_num,mheight,mwidth);
        // printf("\ndelta=\n");for(int ii=00;ii<200;ii++) printf("%f,",0-2*out_data[ii]);
        // printf("\nin=\n");for(int ii=00;ii<200;ii++) printf("%f,",in_data[ii]);
        // printf("\nupdate0=\n");for(int ii=00;ii<75;ii++) printf("%f,",0-update_data[ii]*2*beta);
        cblas_sgemm(CblasRowMajor,CblasNoTrans,CblasTrans,
                    para->knl_num,mheight,mwidth,
                    1.0f,//1.0f/mwidth,
                       out_data,mwidth,
                        in_data,mwidth,
                    beta,
                    update_data,mheight);

        // printf("\nupdate1=\n");for(int ii=00;ii<75;ii++) printf("%f,",0-update_data[ii]*2);
        // printf("\nupdate=\n");for(int ii=760;ii<760+75;ii++) printf("%f,",update_data[ii]);
        // printf("\nbias_updates=\n");for(int ii=0;ii<32;ii++) printf("%f,",update_data[ii*76+75]);

        beta = 1.0;
    }
    // for(int i=0;i<28;i++)printf("%f,",in_data[i]);printf("\n");
    // for(int i=0;i<8;i++) printf("%f,",kernel_data[i]);
    // printf("aaaaaaaaaaa,para->knl_num=%d,mwidth=%d\n",para->knl_num,mwidth);

    if(para->res_valid)
    {
        // if(para->prev->state == MORN_FORWARD)
        // {
        //     printf("conv res set 0,conv res set 0,conv res set 0,conv res set 0,conv res set 0res->batch=%d\n",res->batch);
        //     for(int b=0;b<res->batch;b++)

        // }

        for(int b=0;b<in->batch;b++)
        {
            float *out_data = out->data[b];

            if(para->prev->state == MORN_FORWARD)
                memset(res->data[b],0,in->height*in->width*in->channel*sizeof(float));

            // printf("\nconvdelta=\n");for(int ii=200;ii<400;ii++) printf("%f,",out_data[ii]);
            // printf("\nconvkernel=\n");for(int ii=00;ii<200;ii++) printf("%f,",kernel_data[ii]);

            cblas_sgemm(CblasRowMajor,CblasTrans,CblasNoTrans,
                        mheight,mwidth,para->knl_num,
                        1.0f,
                        kernel_data,mheight,
                           out_data,mwidth,
                    0.0,   res_data,mwidth);

            // printf("\nconvdeltaout=\n");for(int ii=00;ii<200;ii++) printf("%f,",0-res_data[ii]*2);

            ConvMatDataToTensor(res_data,res,b,para->knl_height,para->knl_width,para->y_stride,para->x_stride);

            // printf("mheight=%d,mwidth=%d\n",mheight,mwidth);
            // printf("\nconvimd=\n");for(int ii=200;ii<400;ii++) printf("%f,",res->data[0][ii]);
        }

        para->prev->state = MORN_BACKWARD;
    }

    // printf("\nkernel_data0=\n");for(int ii=0;ii<200;ii++) printf("%f,",kernel_data[ii]);
    // printf("\nupdate_data=\n");for(int ii=0;ii<200;ii++) printf("%f,",update_data[ii]);
    // printf("%s:update_data=%f,kernel_data=%f,%f\n",layer->name,update_data[10],kernel_data[10],kernel_data[10]-(para->rate/(float)(in->batch))*update_data[10]);
    cblas_saxpby(para->knl_num*mheight,
                 (0.0f-(para->rate/(float)(in->batch))),update_data,1,
                 (1.0f-(para->decay*para->rate))       ,kernel_data,1);

    // printf("\nkernel_data1=\n");for(int ii=0;ii<200;ii++) printf("%f,",kernel_data[ii]);
}

//////////////////////////////////////////////////////////////////////////////////////////////////

void GroupConvTensorToMatData(MTensor *tns,int bc,float *mdata,int knl_channel,int knl_height,int knl_width,int c_stride,int y_stride,int x_stride)
{
    int height = tns->height;
    int width  = tns->width;
    int channel= tns->channel;

    int out_width  =  width/x_stride;
    int out_height = height/y_stride;
    int out_channel=(channel-knl_channel/2+1)/c_stride;
    int mwidth = knl_height*knl_width*knl_channel+1;
    int mheight= out_height*out_width;

    int x0=(width -(out_width -1)*x_stride)/2;
    int y0=(height-(out_height-1)*y_stride)/2;

    float *tdata = tns->data[bc];
    int tsize = tns->height*tns->width;

    int k,i,j,c;
    for(k=0;k<out_channel;k++)
    {
        for(j=0;j<out_width;j++)
        {
            int n=y0           -knl_height/2;
            int m=x0+j*x_stride-knl_width /2;
            for(i=0;i<mwidth-1;i+=knl_channel)
            {
                int h= i/(knl_width*knl_channel)             +n;if(h<0)h=0;else if(h>=height)h=height-1;
                int w=(i%(knl_width*knl_channel))/knl_channel+m;if(w<0)w=0;else if(w>= width)w= width-1;
                for(c=0;c<knl_channel;c++)
                    mdata[(k*mheight+j)*mwidth+i+c]=tdata[MIN((c+k*c_stride),channel-1)*tsize+h*width+w];
            }
            mdata[(k*mheight+j)*mwidth+mwidth-1]=1.0f;
        }
        for(j=out_width;j<mheight;j++)
        {
            int num=MAX(0,(knl_height-y_stride))*knl_width*knl_channel;
            if(num>0) memcpy(mdata+(k*mheight+j)*mwidth,mdata+((k*mheight+j)-out_width)*mwidth+y_stride*knl_width*knl_channel,num*sizeof(float));

            int n=y0+j/out_width*y_stride-knl_height/2;
            int m=x0+j%out_width*x_stride-knl_width /2;
            for(i=num;i<mwidth-1;i+=knl_channel)
            {
                int h= i/(knl_width*knl_channel)             +n;if(h<0)h=0;else if(h>=height)h=height-1;
                int w=(i%(knl_width*knl_channel))/knl_channel+m;if(w<0)w=0;else if(w>= width)w= width-1;
                for(c=0;c<knl_channel;c++)
                    mdata[(k*mheight+j)*mwidth+i+c]=tdata[MIN((c+k*c_stride),channel-1)*tsize+h*width+w];
            }
            mdata[(k*mheight+j)*mwidth+mwidth-1]=1.0f;
        }
    }
}

void GroupConvMatDataToTensor(float *mdata,MTensor *tns,int bc,int knl_channel,int knl_height,int knl_width,int c_stride,int y_stride,int x_stride)
{
    int height = tns->height;
    int width  = tns->width;
    int channel= tns->channel;

    int out_width = width/x_stride;
    int out_height=height/y_stride;
    int out_channel=(channel-knl_channel/2+1)/c_stride;
    int mwidth = knl_height*knl_width*knl_channel+1;
    int mheight= out_height*out_width;

    int x0=(width -(out_width -1)*x_stride)/2;
    int y0=(height-(out_height-1)*y_stride)/2;

    float *tdata = tns->data[bc];
    int tsize = tns->height*tns->width;

    int k,i,j,c;
    for(k=0;k<out_channel;k++)for(j=0;j<mheight;j++)
    {
        int n=y0+j/out_width*y_stride+knl_height/2+1-knl_height;
        int m=x0+j%out_width*x_stride+knl_width /2+1-knl_width ;
        for(i=0;i<mwidth-1;i+=knl_channel)
        {
            int h= i/(knl_width*knl_channel)             +n;if(h<0)h=0;else if(h>=height)h=height-1;
            int w=(i%(knl_width*knl_channel))/knl_channel+m;if(w<0)w=0;else if(w>= width)w= width-1;
            for(c=0;c<knl_channel;c++)
                tdata[MIN((c+k*c_stride),channel-1)*tsize+h*width+w]+=mdata[(k*mheight+j)*mwidth+i+c];
        }
    }
}

struct TensorGroupConvPara
{
    MLayer *prev;

    int knl_num;
    int knl_height;
    int knl_width;

    int x_stride;
    int y_stride;

    int res_valid;

    float rate;
    float decay;
    float momentum;

    int knl_channel;
    int c_stride;
};
void *mTensorGroupConvPara(MFile *ini,char *name)
{
    struct TensorGroupConvPara *para = (struct TensorGroupConvPara *)mMalloc(sizeof(struct TensorConvPara));

    char *value = mINIRead(ini,name,"prev");
    para->prev = mNetworkLayer(ini,value);
    mException((para->prev == NULL),EXIT,"invalid prev");

    para->res_valid = (strcmp("Input",mLayerType(para->prev))!=0);

    value = mINIRead(ini,name,"knl_num");
    if(value != NULL) para->knl_num= atoi(value);else para->knl_num= 1;

    value = mINIRead(ini,name,"knl_channel");
    if(value != NULL) para->knl_channel= atoi(value);else para->knl_channel= DFLT;

    value = mINIRead(ini,name,"knl_height");
    if(value != NULL) para->knl_height= atoi(value);else para->knl_height= 1;

    value = mINIRead(ini,name,"knl_width");
    if(value != NULL) para->knl_width= atoi(value);else para->knl_width= 1;

    value = mINIRead(ini,name,"c_stride");
    if(value != NULL) para->c_stride= atoi(value);else para->c_stride= para->knl_channel;

    value = mINIRead(ini,name,"x_stride");
    if(value != NULL) para->x_stride= atoi(value);else para->x_stride= 1;

    value = mINIRead(ini,name,"y_stride");
    if(value != NULL) para->y_stride= atoi(value);else para->y_stride= 1;

    value = mINIRead(ini,name,"rate");
    if(value != NULL) para->rate = atof(value);
    else
    {
        value = mINIRead(ini,"para","rate");
        if(value != NULL) para->rate = atof(value);
        else              para->rate = 0.001;
    }

    value = mINIRead(ini,name,"decay");
    if(value != NULL) para->decay = atof(value);
    else
    {
        value = mINIRead(ini,"para","decay");
        if(value != NULL) para->decay = atof(value);
        else              para->decay = 0.01;
    }
    mException((para->decay<0.0f)||(para->decay>=1.0f),EXIT,"invalid para decay");

    value = mINIRead(ini,name,"momentum");
    if(value != NULL) para->momentum = atof(value);
    else
    {
        value = mINIRead(ini,"para","momentum");
        if(value != NULL) para->momentum = atof(value);
        else              para->momentum = 0.9;
    }

    return para;
}

struct HandleTensorGroupConv
{
    float *mat;
    float *kernel;
    float *update;
};
void endTensorGroupConv(void *info)
{
    struct HandleTensorGroupConv *handle = (struct HandleTensorGroupConv *)info;
    if(handle->mat   != NULL) mFree(handle->mat);
    if(handle->kernel!= NULL) mFree(handle->kernel);
    if(handle->update!= NULL) mFree(handle->update);
}
#define HASH_TensorGroupConv 0x82866393
void TensorGroupConvSet(MLayer *layer)
{
    if(layer->state != DFLT) return;

    struct TensorGroupConvPara *para = (struct TensorGroupConvPara *)(layer->para);
    if(para->knl_channel<=0)
    {
        layer->type_index=mTensorRegisterIndex("Conv");
        return;
    }

    MTensor *in = para->prev->tns;
    MTensor *res= para->prev->res;
    MTensor *out= layer->tns;

    MHandle *hdl=mHandle(out,TensorGroupConv);
    struct HandleTensorGroupConv *handle = (struct HandleTensorGroupConv *)(hdl->handle);

    int out_height= in->height/para->y_stride;
    int out_width = in->width /para->x_stride;
    int out_channel=(in->channel-para->knl_channel/2+1)/para->c_stride;

    int mheight = (out_height*out_width*out_channel);
    int mwidth = para->knl_height*para->knl_width*para->knl_channel+1;
    int data_size = para->knl_num*mwidth;

    mTensorRedefine(out,in->batch,para->knl_num*out_channel,out_height,out_width,NULL);
    if(morn_network_flag == MORN_TRAIN)
    {
        if(INVALID_TENSOR(res)) mTensorRedefine(res,in->batch,in->channel,in->height,in->width,in->data);
        else                    mTensorRedefine(res,in->batch,in->channel,in->height,in->width,NULL);

        if(handle->update != NULL) mFree(handle->update);
        handle->update =(float *)mMalloc(data_size*sizeof(float));
        memset(handle->update,0,data_size*sizeof(float));
    }

    if(handle->kernel !=NULL) mFree(handle->kernel);
    handle->kernel = (float *)mMalloc(data_size*sizeof(float));

    if(morn_network_parafile==NULL)
    {
        float scale = sqrt(2.0f/mwidth);
        for(int i=0;i<data_size;i++)
            handle->kernel[i] = scale*mNormalRand(0.0f,1.0f);
    }
    else
    {
        mNetworkParaRead(layer,"kernel",handle->kernel,data_size*sizeof(float));
    }

    if(handle->mat!=NULL) mFree(handle->mat);
    handle->mat = (float *)mMalloc(mheight*mwidth*sizeof(float));

    hdl->valid = 1;
}

void mTensorGroupConvForward(MLayer *layer)
{
    mException(INVALID_POINTER(layer),EXIT,"invalid input");
    struct TensorGroupConvPara *para = (struct TensorGroupConvPara *)(layer->para);
    if(para->knl_channel<=0) return mTensorConvForward(layer);

    mException(strcmp("GroupConv",mLayerType(layer)),EXIT,"invalid layer type");

    MTensor *in = para->prev->tns;
    MTensor *out=layer->tns;

    TensorGroupConvSet(layer);

    MHandle *hdl=mHandle(out,TensorGroupConv);
    struct HandleTensorGroupConv *handle = (struct HandleTensorGroupConv *)(hdl->handle);

    int out_channel=(in->channel-para->knl_channel/2+1)/para->c_stride;
    int mheight = (out->height*out->width*out_channel);
    int mwidth = para->knl_height*para->knl_width*para->knl_channel+1;

    float *kernel_data= handle->kernel;
    float *in_data = handle->mat;

    for(int b=0;b<in->batch;b++)
    {
        GroupConvTensorToMatData(in,b,in_data,para->knl_channel,para->knl_height,para->knl_width,para->c_stride,para->y_stride,para->x_stride);
        float *out_data = out->data[b];

        in_data[mwidth-1]=1.0f;

        cblas_sgemm(CblasRowMajor,CblasNoTrans,CblasTrans,
                    para->knl_num,mheight,mwidth,
                    1.0f,
                    kernel_data,mwidth,
                        in_data,mwidth,
               0.0f,   out_data,mheight);
    }

    layer->state = MORN_FORWARD;
}

void mTensorGroupConvBackward(MLayer *layer)
{
    mException(INVALID_POINTER(layer),EXIT,"invalid input");
    struct TensorGroupConvPara *para = (struct TensorGroupConvPara *)(layer->para);
    if(para->knl_channel<=0) return mTensorConvBackward(layer);

    mException(strcmp("GroupConv",mLayerType(layer)),EXIT,"invalid layer type");

    MTensor *in = para->prev->tns;
    MTensor *res= para->prev->res;
    MTensor *out=layer->res;

    MHandle *hdl=mHandle(layer->tns,TensorGroupConv);
    struct HandleTensorGroupConv *handle = (struct HandleTensorGroupConv *)(hdl->handle);
    mException((hdl->valid == 0),EXIT,"no forward operate");

    int out_channel=(in->channel-para->knl_channel/2+1)/para->c_stride;
    int mheight = (out->height*out->width*out_channel);
    int mwidth = para->knl_height*para->knl_width*para->knl_channel+1;

    float *kernel_data= handle->kernel;
    float *update_data= handle->update;
    float *    in_data= handle->mat;
    float *   res_data= handle->mat;

    mNetworkParaWrite(layer,"kernel",kernel_data,para->knl_num*mwidth*sizeof(float));

    for(int b=0;b<in->batch;b++)
    {
        GroupConvTensorToMatData(in,b,in_data,para->knl_channel,para->knl_height,para->knl_width,para->c_stride,para->y_stride,para->x_stride);
        float *out_data = out->data[b];

        in_data[mwidth-1]=1.0f;

        cblas_sgemm(CblasRowMajor,CblasNoTrans,CblasNoTrans,
                    para->knl_num,mwidth,mheight,
                    1.0f,
                       out_data,mheight,
                        in_data,mwidth,
                    (b==0)?para->momentum:1.0f,
                    update_data,mwidth);
    }
    cblas_saxpby(para->knl_num*mwidth,
                 (0.0f-(para->rate/(float)(in->batch))),update_data,1,
                 (1.0f-(para->rate*para->decay))       ,kernel_data,1);

    if(para->res_valid==0) return;

    if(para->prev->state == MORN_FORWARD)
    {
        for(int b=0;b<res->batch;b++)
            memset(res->data[b],0,in->height*in->width*in->channel*sizeof(float));
        para->prev->state = MORN_BACKWARD;
    }

    for(int b=0;b<in->batch;b++)
    {
        float *out_data = out->data[b];

        cblas_sgemm(CblasRowMajor,CblasTrans,CblasNoTrans,
                    mheight,mwidth,para->knl_num,
                    1.0f,
                       out_data,mheight,
                    kernel_data,mwidth,
                0.0,   res_data,mwidth);

        GroupConvMatDataToTensor(res_data,res,b,para->knl_channel,para->knl_height,para->knl_width,para->c_stride,para->y_stride,para->x_stride);
    }
}


#ifndef GBNET_RVNODE
#define GBNET_RVNODE

#include <string>
#include <vector>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_histogram.h>
#include <gsl/gsl_rstat.h>

extern gsl_rng *rng;

namespace gbn
{

    class RVNode
    {
        private:

        protected:

        public:

            unsigned int var_size, n_outcomes, n_stats;
            bool is_discrete;
            std::string name, uid, id;

            // // allow for several histograms in a single node.
            // // useful for multivariate distributions
            // gsl_histogram ** histogram;

            // online statistics
            gsl_rstat_workspace ** stats;

            RVNode ();
            virtual ~RVNode ();
            RVNode (std::string, std::string, bool, unsigned int, unsigned int);

            void print_id ();
            void burn_stats();

            virtual double get_own_likelihood ();
            double get_own_loglikelihood ();
            virtual double get_children_loglikelihood ();
            double get_blanket_loglikelihood ();

            virtual void sample (gsl_rng *, bool=false);

            double mean(unsigned int = 0);
            double variance(unsigned int = 0);
            double chain_length();
    };
}

#endif


#include <gsl/gsl_math.h>
#include <gsl/gsl_cblas.h>
#include "cblas.h"

void
cblas_srotg (float *a, float *b, float *c, float *s)
{
#define BASE float
#include "source_rotg.h"
#undef BASE
}


#ifndef SCOL_H
#define SCOL_H
#include "Common.h"
// SP_IVP.h
// Spherical Collapse IVP
//#include <omp.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_deriv.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include <cvode/cvode.h>               /* prototypes for CVODE fcts., consts.  */
#include <nvector/nvector_serial.h>    /* access to serial N_Vector            */
#include <sunmatrix/sunmatrix_dense.h> /* access to dense SUNMatrix            */
#include <sunlinsol/sunlinsol_dense.h> /* access to dense SUNLinearSolver      */
#include <cvode/cvode_direct.h>        /* access to CVDls interface            */
#include <sundials/sundials_types.h>   /* defs. of realtype, sunindextype      */
#include <sundials/sundials_math.h>    /* contains the macros ABS, SUNSQR, EXP */

#define Ith(v,i)    NV_Ith_S(v,i-1)         /* Ith numbers components 1..NEQ */
#define IJth(A,i,j) SM_ELEMENT_D(A,i-1,j-1) /* IJth numbers rows,cols 1..NEQ */

#define NEQ     2                /* number of equations  */
#define Y1      RCONST(0.0)      /* initial y components */
#define RTOL    RCONST(1.0e-5)   /* scalar relative tolerance            */
#define ATOL1   RCONST(1.0e-11)   /* vector absolute tolerance components */
#define ATOL2   RCONST(1.0e-11)
#define T00      RCONST(-10.41431317630211772495840705232694745063781738281250)      /* initial time           */
#define NOUT    1024               /* number of output times */
#define NOUT_DC    10               /* number of output times */

#define HALF    RCONST(0.5)      /* 0.5                    */
#define ONE     RCONST(1.0)      /* 1.0                    */
#define TWO     RCONST(2.0)
#define THREE   RCONST(3.0)       /* 3.0                    */
#define DELTA1  RCONST(3e-5)   /* Delta_i1 */
#define DELTA2  RCONST(0.00012)   /* Delta_i2 */
#define EPSILON RCONST(1.0e-9)
#define ZERO    RCONST(0.0)
#define NINE    RCONST(9.0)
#define TEN     RCONST(10.0)
#define Gnewton RCONST(4.302e-09)
#define coef    RCONST(8987404.41) // 1/H0^2

// structures to store spherical collapse calculations
typedef struct arrays{
  int count;
  double xx[1002];
  double yy[1002]; } *arrays_T;

typedef struct arrays3D{
  int count;
  double xx[1002];
  double yy[1002];
  double zz[1002]; } *arrays_T3;

typedef struct usdat {
  realtype IC;
  realtype OM;
  realtype Rth;
  realtype T1;
  double par1;
  double par2;
  double par3;
  int mymg;
  double maxt;
  gsl_spline *spline;
  gsl_interp_accel *acc;
} *UserData;

extern int check_flagscol(void *flagvalue, const char *funcname, int opt); //

class SCOL {
public:

// used functions in example file
   double maxP_zeta(double sig2, double dsig2dR, double OM, double Z);
   double Delta_Lambda(double OM, double Z); //
   // solves for y_enviornment
   int yenv(double OM_REAL, double XF, double delta_envi, arrays_T xxyy); // gives the environmental dependence of spherical collapse
   // solves for y_halo
   int SphericalCollapse(double *dC, arrays_T3 xxyyzz, UserData data_vec, double TMULT_REAL, double delta_g); // spherical collapse solver
   // solves for a_virial
   double myscol(double myscolparams[], double acol, double omega0, double Rthp, double sig1, double sig2, double pars[], int mymg, int yenvf); // solves for virial quantities and stores them in array myscolparams

   void PrintOutput(realtype t, realtype y1, realtype y2);
   void PrintRootInfo(int root_f1); //

   double funcscol(double xi, void *user_data); //

};

#endif


/**
 *
 * @file core_zlantr.c
 *
 *  PLASMA core_blas kernel
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.8.0
 * @author Mathieu Faverge
 * @date 2010-11-15
 * @precisions normal z -> c d s
 *
 **/
#include <lapacke.h>
#include <math.h>
#include "common.h"

#define LAPACKE_CORRECT_DLANTR

/***************************************************************************//**
 *
 * @ingroup dplasma_cores_complex64
 *
 *  CORE_zlantr returns the value
 *
 *     zlantr = ( max(abs(A(i,j))), NORM = PlasmaMaxNorm
 *              (
 *              ( norm1(A),         NORM = PlasmaOneNorm
 *              (
 *              ( normI(A),         NORM = PlasmaInfNorm
 *              (
 *              ( normF(A),         NORM = PlasmaFrobeniusNorm
 *
 *  where norm1 denotes the one norm of a matrix (maximum column sum),
 *  normI denotes the infinity norm of a matrix (maximum row sum) and
 *  normF denotes the Frobenius norm of a matrix (square root of sum
 *  of squares). Note that max(abs(A(i,j))) is not a consistent matrix
 *  norm.
 *
 *******************************************************************************
 *
 * @param[in] norm
 *          = PlasmaMaxNorm: Max norm
 *          = PlasmaOneNorm: One norm
 *          = PlasmaInfNorm: Infinity norm
 *          = PlasmaFrobeniusNorm: Frobenius norm
 *
 * @param[in] uplo
 *          Specifies whether the matrix A is upper triangular or lower triangular:
 *          = PlasmaUpper: Upper triangle of A is stored;
 *          = PlasmaLower: Lower triangle of A is stored.
 *
 * @param[in] diag
 *          Specifies whether or not A is unit triangular:
 *          = PlasmaNonUnit: A is non unit;
 *          = PlasmaUnit:    A us unit.
 *
 * @param[in] M
 *          The number of rows of the matrix A. M >= 0.
 *          If uplo == PlasmaUpper, M <= N. When M = 0, CORE_zlantr returns 0.
 *
 * @param[in] N
 *          The number of columns of the matrix A. N >= 0.
 *          If uplo == PlasmaLower, N <= M. When N = 0, CORE_zlantr returns 0.
 *
 * @param[in] A
 *          The LDA-by-N matrix A.
 *
 * @param[in] LDA
 *          The leading dimension of the array A. LDA >= max(1,M).
 *
 * @param[in,out] work
 *          Array of dimension (MAX(1,LWORK)), where LWORK >= M when norm =
 *          PlasmaInfNorm, or LWORK >= N when norm = PlasmaOneNorm; otherwise,
 *          work is not referenced.
 *
 * @param[out] normA
 *          On exit, normA is the norm of matrix A.
 *
 ******************************************************************************/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_zlantr = PCORE_zlantr
#define CORE_zlantr PCORE_zlantr
#endif
void CORE_zlantr(PLASMA_enum norm, PLASMA_enum uplo, PLASMA_enum diag,
                 int M, int N,
                 const PLASMA_Complex64_t *A, int LDA,
                 double *work, double *normA)
{
#if defined(LAPACKE_CORRECT_DLANTR)
    *normA = LAPACKE_zlantr_work(
        LAPACK_COL_MAJOR,
        lapack_const(norm),
        lapack_const(uplo),
        lapack_const(diag),
        M, N, A, LDA, work);
#else
    const PLASMA_Complex64_t *tmpA;
    double value;
    int i, j, imax;
    int idiag = (diag == PlasmaUnit) ? 1 : 0;

    if ( min(M, N) == 0 ) {
        *normA = 0;
        return;
    }

    switch ( norm ) {
    case PlasmaMaxNorm:
        if ( diag == PlasmaUnit ) {
            *normA = 1.;
        } else {
            *normA = 0.;
        }

        if ( uplo == PlasmaUpper ) {
            M = min(M, N);
            for (j = 0; j < N; j++) {
                tmpA = A+(j*LDA);
                imax = min(j+1-idiag, M);

                for (i = 0; i < imax; i++) {
                    value = cabs( *tmpA );
                    *normA = ( value > *normA ) ? value : *normA;
                    tmpA++;
                }
            }
        } else {
            N = min(M, N);
            for (j = 0; j < N; j++) {
                tmpA = A + j * (LDA+1) + idiag;

                for (i = j+idiag; i < M; i++) {
                    value = cabs( *tmpA );
                    *normA = ( value > *normA ) ? value : *normA;
                    tmpA++;
                }
            }
        }
        break;

    case PlasmaOneNorm:
        CORE_ztrasm( PlasmaColumnwise, uplo, diag, M, N,
                     A, LDA, work );
        if ( uplo == PlasmaLower )
            N = min(M,N);

        *normA = 0;
        for (i = 0; i < N; i++) {
            *normA = ( work[i] > *normA ) ? work[i] : *normA;
        }
        break;

    case PlasmaInfNorm:
        CORE_ztrasm( PlasmaRowwise, uplo, diag, M, N,
                     A, LDA, work );
        if ( uplo == PlasmaUpper )
            M = min(M,N);

        *normA = 0;
        for (i = 0; i < M; i++) {
            *normA = ( work[i] > *normA ) ? work[i] : *normA;
        }
        break;

    case PlasmaFrobeniusNorm:
    {
        double scale = 0.;
        double sumsq = 1.;
        CORE_ztrssq( uplo, diag, M, N,
                     A, LDA, &scale, &sumsq );

        *normA = scale * sqrt( sumsq );
    }
    break;
    default:
        coreblas_error(1, "Illegal value of norm");
        return;
    }
#endif
}


#pragma once

#include "bits_of_matcha/engine/memory/Buffer.h"
#include "bits_of_matcha/engine/iterations/MatrixwiseBinaryCtx.h"

#include <cblas.h>

namespace matcha::engine::cpu {

void mm(engine::Buffer* a, engine::Buffer* b, engine::Buffer* c, const MatrixwiseBinaryCtx& ctx) {
  size_t sizeA = ctx.rowsA * ctx.colsA;
  size_t sizeB = ctx.rowsB * ctx.colsB;
  size_t sizeC = ctx.rowsA * ctx.colsB;

  dynamic_cast<Buffer*>(a);
  dynamic_cast<Buffer*>(b);
  dynamic_cast<Buffer*>(c);

  auto valsA = a->as<float*>();
  auto valsB = b->as<float*>();
  auto valsC = c->as<float*>();

  int strides = (int) ctx.prefixStridesA.size();

  std::vector<unsigned> beginA(strides, 0);
  std::vector<unsigned> beginB(strides, 0);
  std::vector<unsigned> beginC(strides, 0);
  auto iterC = valsC;

  int counter = 0;
  int axis = 1;
  while (true) {
//    if (counter++ > 20) exit(69);
    if (axis < strides - 1) {
      print("==========");
      print("axis ", axis, " beginC ", beginC[axis], " stridesC ", ctx.prefixStridesC[axis - 1]);
//    print("axis: ", axis, " (strides ", strides, ")");
//      print("strides: ", ctx.prefixStridesA[axis], " ", ctx.prefixStridesB[axis], " -> ", ctx.prefixStridesC[axis]);
//      print("begins: ", beginA[axis], " ", beginB[axis], " -> ", beginC[axis]);
//      print("BC: ", beginC[axis], " ", beginC[axis - 1] + ctx.prefixStridesC[axis - 1]);
//      print("beginC ", beginC[axis]);
      if (beginC[axis] == beginC[axis - 1] + ctx.prefixStridesC[axis - 1]) {
//        print("-> dec axis");
        if (axis == 1) break;
        axis--;
      } else {
        beginA[axis + 1] = beginA[axis];
        beginB[axis + 1] = beginB[axis];
        beginC[axis + 1] = beginC[axis];
        beginA[axis] += ctx.prefixStridesA[axis];
        beginB[axis] += ctx.prefixStridesB[axis];
        beginC[axis] += ctx.prefixStridesC[axis];
//        print("-> inc axis");
        axis++;
      }
    } else {
      if (axis >= strides) {
        axis = strides - 1;
      }

//      print(ctx.prefixStridesC[axis - 1]);
//      print(beginA[axis]);
      auto matA = valsA + beginA[axis] * sizeA;
      auto matB = valsB + beginB[axis] * sizeB;
      auto loops = axis - 1 >= 0 ? ctx.prefixStridesC[axis - 1] : 1;
      for (auto itC = 0; itC != loops; itC++) {
        auto matC = valsC + (beginC[axis] + itC) * sizeC;

//        print(matA - valsA, " ", matB - valsB, " -> ", matC - valsC);
///*
        cblas_sgemm(
          CblasRowMajor,
          CblasNoTrans,
          CblasNoTrans,
          (int) ctx.rowsA,
          (int) ctx.colsB,
          (int) ctx.colsA,
          1,
          matA,
          (int) ctx.colsA,
          matB,
          (int) ctx.colsB,
          0,
          matC,
          (int) ctx.colsB
        );
//        */

        matA += sizeA * ctx.prefixStridesA[axis];
        matB += sizeB * ctx.prefixStridesB[axis];
      }
//      print("blas");

      if (axis <= 1) break;
      axis--;
    }

  }

}

}


/**
 *
 * @file core_dgeqp3_norms.c
 *
 *  PLASMA core_blas kernel
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Mark Gates
 * @date 2010-11-15
 * @generated d Tue Jan  7 11:44:49 2014
 *
 **/
#include <lapacke.h>
#include <math.h>
#include <cblas.h>
#include "common.h"

#define A(m,n) BLKADDR( A, double, m, n )

/***************************************************************************//**
 *
 * @ingroup CORE_double
 *
 *  CORE_dgeqp3_norms computes the 2-norm of each column of A[ ioff:m, joff:n ]
 *  that is marked with norms2[j] == -1 on entry. Entries that are not marked
 *  are assumed to already contain the correct 2-norm, so that the same routine
 *  can be used for computing the initial norms and for updating bad norms.
 *  The result is stored duplicated in norms1 and norms2.
 *
 *******************************************************************************
 *
 *  @param[in] A
 *          PLASMA descriptor of the matrix A.
 *          On entry, the M-by-N matrix described by the descriptor.
 *
 *  @param[in] ioff
 *          Row offset.
 *
 *  @param[in] joff
 *          Column offset.
 *
 *  @param[in,out] norms1
 *          Vector of size A.n.
 *          On exit, norms1[j] is 2-norm of column j, for j >= joff.
 *
 *  @param[in,out] norms2
 *          Vector of size A.n.
 *          On entry, if norms2[j] == -1, re-compute norm of column j.
 *          On exit, norms2[j] is 2-norm of column j, for j >= joff.
 **/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_dgeqp3_norms = PCORE_dgeqp3_norms
#define CORE_dgeqp3_norms PCORE_dgeqp3_norms
#define CORE_dgessq PCORE_dgessq
int
CORE_dgessq(int M, int N,
            const double *A, int LDA,
            double *scale, double *sumsq);
#endif
void CORE_dgeqp3_norms( PLASMA_desc A, int ioff, int joff, double *norms1, double *norms2 )
{
    const double *Ai;
    int j, ii, ioff2, len, mb, nb, lda;
    double sumsq, scale;

    if ( A.nt != 1 ) {
        coreblas_error(1, "Illegal value of A.nt");
        return;
    }

    nb = min( A.nb, A.n );
    for( j = joff; j < nb; ++j ) {
        if ( norms2[j] == -1. ) {
            scale = 0.;
            sumsq = 1.;
            ioff2 = ioff;
            for( ii = 0; ii < A.mt; ++ii ) {
                mb = min( A.mb, A.m - ii*A.mb );
                Ai = A(ii,0);
                lda = BLKLDD( A, ii );
                len = mb - ioff2;
                CORE_dgessq( len, 1, Ai + j*lda + ioff2, lda, &scale, &sumsq );
                ioff2 = 0;
            }
            norms2[j] = scale * sqrt( sumsq );
            norms1[j] = norms2[j];
        }
    }
}


/**
 *
 * @file core_sgeqp3_update.c
 *
 *  PLASMA core_blas kernel
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Mark Gates
 * @date 2010-11-15
 * @generated s Tue Jan  7 11:44:49 2014
 *
 **/
#include <math.h>
#include <cblas.h>
#include <lapacke.h>
#include "common.h"

#define A(m,n) BLKADDR( A, float, m, n )

/***************************************************************************//**
 *
 * @ingroup CORE_float
 *
 * CORE_sgeqp3_update updates row k of one tile of A
 * and subtracts that row from the column norms.
 *
 *******************************************************************************
 *
 * @param[in] Ajj
 *         Diagonal tile (jj,jj) of A.
 *
 * @param[in] lda1
 *         Leading dimension of Ajj.
 *
 * @param[in,out] Ajk
 *         Tile (jj,kk) of A, kk >= jj.
 *         On exit, updates row joff+k (i.e., as if Q was applied to trailing matrix).
 *
 * @param[in] lda2
 *         Leading dimension of Ajk.
 *
 * @param[in] Fk
 *         Tile kk of F.
 *
 * @param[in] ldf
 *         Leading dimension of Fk.
 *
 * @param[in] joff
 *         Row offset.
 *
 * @param[in] k
 *         Update row joff+k, based on having factored k columns.
 *         (That is, joff columns of this tile were factored in previous panels;
 *          k columns have been factored during this panel.)
 *
 * @param[in] koff
 *         Column to start updating.
 *         For diagonal tile, koff=joff+k+1, else koff=0.
 *
 * @param[in] nb
 *         Number of columns in kk-th block-column of A.
 *
 * @param[in,out] norms1
 *         kk-th block of partial column norms vector, dimension nb.
 *         On exit, norms1[koff:nb] -= Ajk[k, koff:nb ].
 *
 * @param[in,out] norms2
 *         kk-th block of original column norms vector, dimension nb.
 *         Unchanged on exit, except if cancellation is detected for
 *         some column j, sets norm2[j] = -1 and sets info = 1.
 *
 * @param[out] info
 *         Set to true if numerical instability (cancellation) is detected
 *         in updating column norms. sgeqp3 handles this error.
 **/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_sgeqp3_update = PCORE_sgeqp3_update
#define CORE_sgeqp3_update PCORE_sgeqp3_update
#endif
void CORE_sgeqp3_update( const float *Ajj, int lda1,
                         float       *Ajk, int lda2,
                         const float *Fk,  int ldf,
                         int joff, int k, int koff, int nb,
                         float *norms1, float *norms2,
                         int *info )
{
    float temp, temp2;
    float tol3z = sqrt( LAPACKE_slamch_work('e'));
    const float zone  =  1.0;
    const float mzone = -1.0;
    int j;

    /* update row k of A -- this is vector*matrix */
    /* Ajk[k,j:nb] -= Ajj[k,0:k+1] * Fk[j:nb,0:k+1].T */
    cblas_sgemm( CblasColMajor, CblasNoTrans, CblasTrans, 1, nb-koff, k+1,
                 (mzone), &Ajj[joff+k + joff*lda1], lda1,
                                     &Fk [koff              ], ldf,
                 (zone),  &Ajk[joff+k + koff*lda2], lda2 );

    for( j = koff; j < nb; ++j ) {
        if ( norms1[j] != 0. ) {
            /* NOTE: The following lines follow from the analysis in Lapack Working Note 176. */
            temp = fabsf( Ajk[joff+k + j*lda2] ) / norms1[j];
            temp = max( 0., (1. + temp)*(1. - temp) );
            temp2 = norms1[j] / norms2[j];
            temp2 = temp * temp2*temp2;
            norms1[j] = norms1[j]*sqrt( temp );
            if( temp2 <= tol3z ) {
                /* flag numerical problem (i.e., cancellation) in updating norm.
                *  norms1[j] will be re-computed. Above we stored the inaccurate
                *  value anyway to allow comparison with the accurate value, for
                *  easier debugging. */
                norms2[j] = -1;
                *info = 1;
            }
        }
    }
}


/* multimin/gsl_multimin.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Fabrice Rossi
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Modified by Tuomo Keskitalo to include fminimizer and
   Nelder Mead related lines */

#ifndef __GSL_MULTIMIN_H__
#define __GSL_MULTIMIN_H__

#include <stdlib.h>
#include <gsl/gsl_types.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_min.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

/* Definition of an arbitrary real-valued function with gsl_vector input and */
/* parameters */
struct gsl_multimin_function_struct
{
  double (* f) (const gsl_vector * x, void * params);
  size_t n;
  void * params;
};

typedef struct gsl_multimin_function_struct gsl_multimin_function;

#define GSL_MULTIMIN_FN_EVAL(F,x) (*((F)->f))(x,(F)->params)

/* Definition of an arbitrary differentiable real-valued function */
/* with gsl_vector input and parameters */
struct gsl_multimin_function_fdf_struct
{
  double (* f) (const gsl_vector  * x, void * params);
  void (* df) (const gsl_vector * x, void * params,gsl_vector * df);
  void (* fdf) (const gsl_vector * x, void * params,double *f,gsl_vector * df);
  size_t n;
  void * params;
};

typedef struct gsl_multimin_function_fdf_struct gsl_multimin_function_fdf;

#define GSL_MULTIMIN_FN_EVAL_F(F,x) (*((F)->f))(x,(F)->params)
#define GSL_MULTIMIN_FN_EVAL_DF(F,x,g) (*((F)->df))(x,(F)->params,(g))
#define GSL_MULTIMIN_FN_EVAL_F_DF(F,x,y,g) (*((F)->fdf))(x,(F)->params,(y),(g))

int gsl_multimin_diff (const gsl_multimin_function * f,
                       const gsl_vector * x, gsl_vector * g);

/* minimization of non-differentiable functions */

typedef struct
{
  const char *name;
  size_t size;
  int (*alloc) (void *state, size_t n);
  int (*set) (void *state, gsl_multimin_function * f,
              const gsl_vector * x,
              double * size,
              const gsl_vector * step_size);
  int (*iterate) (void *state, gsl_multimin_function * f,
                  gsl_vector * x,
                  double * size,
                  double * fval);
  void (*free) (void *state);
}
gsl_multimin_fminimizer_type;

typedef struct
{
  /* multi dimensional part */
  const gsl_multimin_fminimizer_type *type;
  gsl_multimin_function *f;

  double fval;
  gsl_vector * x;

  double size;

  void *state;
}
gsl_multimin_fminimizer;

gsl_multimin_fminimizer *
gsl_multimin_fminimizer_alloc(const gsl_multimin_fminimizer_type *T,
                              size_t n);

int
gsl_multimin_fminimizer_set (gsl_multimin_fminimizer * s,
                             gsl_multimin_function * f,
                             const gsl_vector * x,
                             const gsl_vector * step_size);

void
gsl_multimin_fminimizer_free(gsl_multimin_fminimizer *s);

const char *
gsl_multimin_fminimizer_name (const gsl_multimin_fminimizer * s);

int
gsl_multimin_fminimizer_iterate(gsl_multimin_fminimizer *s);

gsl_vector *
gsl_multimin_fminimizer_x (const gsl_multimin_fminimizer * s);

double
gsl_multimin_fminimizer_minimum (const gsl_multimin_fminimizer * s);

double
gsl_multimin_fminimizer_size (const gsl_multimin_fminimizer * s);

/* Convergence test functions */

int
gsl_multimin_test_gradient(const gsl_vector * g,double epsabs);

int
gsl_multimin_test_size(const double size ,double epsabs);

/* minimisation of differentiable functions */

typedef struct
{
  const char *name;
  size_t size;
  int (*alloc) (void *state, size_t n);
  int (*set) (void *state, gsl_multimin_function_fdf * fdf,
              const gsl_vector * x, double * f,
              gsl_vector * gradient, double step_size, double tol);
  int (*iterate) (void *state,gsl_multimin_function_fdf * fdf,
                  gsl_vector * x, double * f,
                  gsl_vector * gradient, gsl_vector * dx);
  int (*restart) (void *state);
  void (*free) (void *state);
}
gsl_multimin_fdfminimizer_type;

typedef struct
{
  /* multi dimensional part */
  const gsl_multimin_fdfminimizer_type *type;
  gsl_multimin_function_fdf *fdf;

  double f;
  gsl_vector * x;
  gsl_vector * gradient;
  gsl_vector * dx;

  void *state;
}
gsl_multimin_fdfminimizer;

gsl_multimin_fdfminimizer *
gsl_multimin_fdfminimizer_alloc(const gsl_multimin_fdfminimizer_type *T,
                                size_t n);

int
gsl_multimin_fdfminimizer_set (gsl_multimin_fdfminimizer * s,
                               gsl_multimin_function_fdf *fdf,
                               const gsl_vector * x,
                               double step_size, double tol);

void
gsl_multimin_fdfminimizer_free(gsl_multimin_fdfminimizer *s);

const char *
gsl_multimin_fdfminimizer_name (const gsl_multimin_fdfminimizer * s);

int
gsl_multimin_fdfminimizer_iterate(gsl_multimin_fdfminimizer *s);

int
gsl_multimin_fdfminimizer_restart(gsl_multimin_fdfminimizer *s);

gsl_vector *
gsl_multimin_fdfminimizer_x (gsl_multimin_fdfminimizer * s);

gsl_vector *
gsl_multimin_fdfminimizer_dx (gsl_multimin_fdfminimizer * s);

gsl_vector *
gsl_multimin_fdfminimizer_gradient (gsl_multimin_fdfminimizer * s);

double
gsl_multimin_fdfminimizer_minimum (gsl_multimin_fdfminimizer * s);

GSL_VAR const gsl_multimin_fdfminimizer_type *gsl_multimin_fdfminimizer_steepest_descent;
GSL_VAR const gsl_multimin_fdfminimizer_type *gsl_multimin_fdfminimizer_conjugate_pr;
GSL_VAR const gsl_multimin_fdfminimizer_type *gsl_multimin_fdfminimizer_conjugate_fr;
GSL_VAR const gsl_multimin_fdfminimizer_type *gsl_multimin_fdfminimizer_vector_bfgs;
GSL_VAR const gsl_multimin_fdfminimizer_type *gsl_multimin_fdfminimizer_vector_bfgs2;
GSL_VAR const gsl_multimin_fminimizer_type *gsl_multimin_fminimizer_nmsimplex;

__END_DECLS

#endif /* __GSL_MULTIMIN_H__ */


/*System includes*/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>

/*GSL includes*/
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_sf.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_roots.h>
#include <gsl/gsl_statistics_double.h>
#include <gsl/gsl_fft_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_multimin.h>
#include <pthread.h>

/*User includes*/
#include "../Lib/FileUtils.h"
#include "../Lib/MatrixUtils.h"
#include "MetroIG.h"

static char *usage[] = {"MetroIG - Fits the compound Poisson Inverse Gaussian\n",
                        "Required parameters:\n",
			"   -out  filestub      output file stub\n",
			"   -in   filename      parameter file  \n",
                        "Optional:\n",
			"   -s    integer       generate integer MCMC samples\n",
			"   -seed   long        seed random number generator\n",
			"   -sigmaA float       std. dev. of alpha prop. distn\n",
			"   -sigmaB float       ...          beta             \n",
			"   -sigmaS float       ...          S                \n",
			"   -v                  verbose\n"};

static int  nLines   = 11;

static int  verbose  = FALSE;

int main(int argc, char* argv[])
{
  int  i = 0, nNA     = 0;
  t_Params tParams;
  t_Data   tData;
  gsl_vector* ptX = gsl_vector_alloc(3); /*parameter estimates*/
  t_MetroInit atMetroInit[3];

  gsl_rng_env_setup();

  gsl_set_error_handler_off();

  /*get command line params*/
  getCommandLineParams(&tParams, argc, argv);

  /*read in abundance distribution*/
  readAbundanceData(tParams.szInputFile, &tData);

  /*set initial estimates for parameters*/
  gsl_vector_set(ptX, 0, INIT_A);
  gsl_vector_set(ptX, 1, INIT_B);
  gsl_vector_set(ptX, 2, tData.nL*2);

  printf("D = %d L = %d Chao = %f\n",tData.nL, tData.nJ, chao(&tData));

  minimiseSimplex(ptX, 3, (void*) &tData, &nLogLikelihood);

  outputResults(ptX, &tData);

  if(tParams.nIter > 0){
    mcmc(&tParams, &tData, ptX);
  }

  /*free up allocated memory*/
  gsl_vector_free(ptX);

  freeAbundanceData(&tData);

  exit(EXIT_SUCCESS);
}

void writeUsage(FILE* ofp)
{
  int i = 0;
  char *line;

  for(i = 0; i < nLines; i++){
    line = usage[i];
    fputs(line,ofp);
  }
}

char *extractParameter(int argc, char **argv, char *param,int when)
{
  int i = 0;

  while((i < argc) && (strcmp(param,argv[i]))){
    i++;
  }

  if(i < argc - 1){
    return(argv[i + 1]);
  }

  if((i == argc - 1) && (when == OPTION)){
    return "";
  }

  if(when == ALWAYS){
    fprintf(stdout,"Can't find asked option %s\n",param);
  }

  return (char *) NULL;
}

void getCommandLineParams(t_Params *ptParams,int argc,char *argv[])
{
  char *szTemp = NULL;
  char *cError = NULL;

  /*get parameter file name*/
  ptParams->szInputFile  = extractParameter(argc,argv, INPUT_FILE,ALWAYS);
  if(ptParams->szInputFile == NULL)
    goto error;

  /*get out file stub*/
  ptParams->szOutFileStub  = extractParameter(argc,argv,OUT_FILE_STUB,ALWAYS);
  if(ptParams->szOutFileStub == NULL)
    goto error;

  /*get out file stub*/
  szTemp  = extractParameter(argc,argv,SEED,OPTION);
  if(szTemp != NULL){
    ptParams->lSeed = strtol(szTemp,&cError,10);
    if(*cError != '\0'){
      goto error;
    }
  }
  else{
    ptParams->lSeed = 0;
  }

  /*verbosity*/
  szTemp = extractParameter(argc, argv, VERBOSE, OPTION);
  if(szTemp != NULL){
    verbose = TRUE;
  }

  szTemp  = extractParameter(argc,argv,SAMPLE,OPTION);
  if(szTemp != NULL){
    ptParams->nIter = strtol(szTemp,&cError,10);
    if(*cError != '\0'){
      goto error;
    }
  }
  else{
    ptParams->nIter = 0;
  }

  szTemp  = extractParameter(argc,argv,SIGMA_A,OPTION);
  if(szTemp != NULL){
    ptParams->dSigmaA = strtod(szTemp,&cError);
    if(*cError != '\0'){
      goto error;
    }
  }
  else{
    ptParams->dSigmaA = DEF_SIGMA;
  }

  szTemp  = extractParameter(argc,argv,SIGMA_B,OPTION);
  if(szTemp != NULL){
    ptParams->dSigmaB = strtod(szTemp,&cError);
    if(*cError != '\0'){
      goto error;
    }
  }
  else{
    ptParams->dSigmaB = DEF_SIGMA;
  }

  szTemp  = extractParameter(argc,argv,SIGMA_S,OPTION);
  if(szTemp != NULL){
    ptParams->dSigmaS = strtod(szTemp,&cError);
    if(*cError != '\0'){
      goto error;
    }
  }
  else{
    ptParams->dSigmaS = DEF_SIGMA_S;
  }

  return;

 error:
  writeUsage(stdout);
  exit(EXIT_FAILURE);
}

void readAbundanceData(const char *szFile, t_Data *ptData)
{
  int **aanAbund = NULL;
  int  i = 0, nNA = 0, nA = 0, nC = 0;
  int  nL = 0, nJ = 0;
  char szLine[MAX_LINE_LENGTH];
  FILE* ifp = NULL;

  ifp = fopen(szFile, "r");

  if(ifp){
    char* szTok   = NULL;
    char* pcError = NULL;

    fgets(szLine, MAX_LINE_LENGTH, ifp);

    szTok = strtok(szLine, DELIM);

    nNA = strtol(szTok,&pcError,10);
    if(*pcError != '\0'){
      goto formatError;
    }

    aanAbund = (int **) malloc(nNA*sizeof(int*));

    for(i = 0; i < nNA; i++){
      aanAbund[i] = (int *) malloc(sizeof(int)*2);

      fgets(szLine, MAX_LINE_LENGTH, ifp);

      szTok = strtok(szLine, DELIM);

      nA = strtol(szTok,&pcError,10);
      if(*pcError != '\0'){
	goto formatError;
      }

      szTok = strtok(NULL, DELIM);

      nC = strtol(szTok,&pcError,10);
      if(*pcError != '\0'){
	goto formatError;
      }

      nL += nC;
      nJ += nC*nA;

      aanAbund[i][0]  = nA;
      aanAbund[i][1]  = nC;
    }
  }
  else{
    fprintf(stderr, "Failed to open abundance data file %s aborting\n", szFile);
    fflush(stderr);
    exit(EXIT_FAILURE);
  }

  ptData->nJ          = nJ;
  ptData->nL          = nL;
  ptData->aanAbund    = aanAbund;
  ptData->nNA         = nNA;
  return;

 formatError:
  fprintf(stderr, "Incorrectly formatted abundance data file\n");
  fflush(stderr);
  exit(EXIT_FAILURE);
}

int compare_doubles(const void* a, const void* b)
{
  double* arg1 = (double *) a;
  double* arg2 = (double *) b;
  if( *arg1 < *arg2 ) return -1;
  else if( *arg1 == *arg2 ) return 0;
  else return 1;
}

double chao(t_Data *ptData)
{
  double n1 = 0.0, n2 = 0.0;
  int **aanAbund = ptData->aanAbund;

  if(aanAbund[0][0] == 1 && aanAbund[1][0] == 2){
    n1 = (double) aanAbund[0][1]; n2 = (double) aanAbund[1][1];

    return ((double) ptData->nL) + 0.5*((n1*n1)/n2);
  }
  else{
    return -1.0;
  }
}

double fX(double x, double dA, double dB, double dNDash)
{
  double dTemp1 = (dA*(x - dB)*(x - dB))/x;

  return log(x) - (1.0/dNDash)*(x + dTemp1);
}

double f2X(double x, double dA, double dB, double dNDash)
{
  double dRet = 0.0, dTemp = 2.0*dA*dB*dB;

  dRet = (1.0/(x*x))*(1.0 + (1.0/dNDash)*(dTemp/x));

  return -dRet;
}

double sd(int n, double dAlpha, double dBeta)
{
  double dGamma = -0.5;
  double dA = 0.5*(-1.0 + sqrt(1.0 + (dAlpha*dAlpha)/(dBeta*dBeta)));
  double dN = (double) n, dNDash = dN + dGamma - 1.0, dRN = 1.0/dN;
  double dTemp1 = (0.5*dN)/(1.0 + dA), dTemp2 = 4.0*dRN*dRN*(1.0 + dA)*dA*dBeta*dBeta;
  double dXStar = dTemp1*(1.0 + sqrt(1.0 + dTemp2));
  double dFX = fX(dXStar, dA, dBeta, dNDash);
  double d2FX = -dNDash*f2X(dXStar, dA, dBeta, dNDash);
  double dLogK = 0.0, dGamma1 = dGamma;

  if(dGamma1 < 0.0){
    dGamma1 *= -1.0;
  }

  dLogK = gsl_sf_bessel_lnKnu(dGamma1,2.0*dA*dBeta);

  return -2.0*dA*dBeta -log(2.0) -dLogK -dGamma*log(dBeta) + dNDash*dFX + 0.5*log(2.0*M_PI) - 0.5*log(d2FX);
}

int bessel(double* pdResult, int n, double dAlpha, double dBeta)
{
  double dGamma  = -0.5;
  double dResult = 0.0;
  double dOmega = 0.0, dGamma2 = 0.0;
  double dLogK1 = 0.0, dLogK2 = 0.0;
  double dN = (double) n, dNu = dGamma + dN;
  double dTemp1 = 0.0;

  if(dNu < 0.0){
    dNu = -dNu;
  }

  if(dGamma < 0.0){
    dGamma2 = -dGamma;
  }
  else{
    dGamma2 = dGamma;
  }

  dOmega = sqrt(dBeta*dBeta + dAlpha*dAlpha) - dBeta;

  dLogK2 = gsl_sf_bessel_lnKnu(dNu, dAlpha);

  if(!gsl_finite(dLogK2)){
    if(dAlpha < 0.1*sqrt(dNu + 1.0)){
      //printf("l ");
      dLogK2 = gsl_sf_lngamma(dNu) + (dNu - 1.0)*log(2.0) - dNu*log(dAlpha);
    }
    else{
      //printf("sd ");
      (*pdResult) = dResult;
      return FALSE;
    }
  }

  dLogK1 = dGamma*log(dOmega/dAlpha) -gsl_sf_bessel_lnKnu(dGamma2,dOmega);

  dTemp1 = log((dBeta*dOmega)/dAlpha);

  dResult = dN*dTemp1 + dLogK2 + dLogK1;
  (*pdResult) = dResult;
  return TRUE;
}

double logLikelihood(int n, double dAlpha, double dBeta)
{
  double dLogFacN = 0.0;
  int status      = 0;
  double dRet     = 0.0;

  if(n < 50){
    dLogFacN = gsl_sf_fact(n);
    dLogFacN = log(dLogFacN);
  }
  else{
    dLogFacN = gsl_sf_lngamma(((double) n) + 1.0);
  }

  status = bessel(&dRet,n, dAlpha,dBeta);
  if(status == FALSE){
    dRet = sd(n, dAlpha,dBeta);
  }

  return dRet - dLogFacN;
}

double nLogLikelihood(const gsl_vector * x, void * params)
{
  double dAlpha  = gsl_vector_get(x,0), dBeta = gsl_vector_get(x,1);
  int    nS = (int) floor(gsl_vector_get(x, 2));
  t_Data *ptData = (t_Data *) params;
  int    i       = 0;
  double dLogNot0 = 0.0, dLogL   = 0.0;
  double dLog0 = 0.0, dLog1 = 0.0, dLog2 = 0.0, dLog3 = 0.0;

  if(dAlpha <= 0.0 || dBeta <= 0.0){
    return PENALTY;
  }

  for(i = 0; i < ptData->nNA; i++){
    double dLogP = 0.0;
    int    nA    = ptData->aanAbund[i][0];

    dLogP = logLikelihood(nA, dAlpha, dBeta);

    dLogL += ((double) ptData->aanAbund[i][1])*dLogP;

    dLogL -= gsl_sf_lnfact(ptData->aanAbund[i][1]);

  }

  dLog0 = logLikelihood(0, dAlpha, dBeta);

  dLog1 = (nS - ptData->nL)*dLog0;

  dLog2 = - gsl_sf_lnfact(nS - ptData->nL);

  dLog3 = gsl_sf_lnfact(nS);

  dLogL += dLog1 + dLog2 + dLog3;

  /*return*/
  return -dLogL;
}

double negLogLikelihood(double dAlpha, double dBeta, int nS, void * params)
{
  t_Data *ptData = (t_Data *) params;
  int    i       = 0;
  double dLogNot0 = 0.0, dLogL   = 0.0;
  double dLog0 = 0.0, dLog1 = 0.0, dLog2 = 0.0, dLog3 = 0.0;

  if(dAlpha <= 0.0 || dBeta <= 0.0){
    return PENALTY;
  }

  for(i = 0; i < ptData->nNA; i++){
    double dLogP = 0.0;
    int    nA    = ptData->aanAbund[i][0];

    dLogP = logLikelihood(nA, dAlpha, dBeta);

    dLogL += ((double) ptData->aanAbund[i][1])*dLogP;

    dLogL -= gsl_sf_lnfact(ptData->aanAbund[i][1]);

  }

  dLog0 = logLikelihood(0, dAlpha, dBeta);

  dLog1 = (nS - ptData->nL)*dLog0;

  dLog2 = - gsl_sf_lnfact(nS - ptData->nL);

  dLog3 = gsl_sf_lnfact(nS);

  dLogL += dLog1 + dLog2 + dLog3;

  /*return*/
  return -dLogL;
}

int minimiseSimplex(gsl_vector* ptX, size_t nP, void* pvData, double (*f)(const gsl_vector*, void* params))
{
  const gsl_multimin_fminimizer_type *T =
    gsl_multimin_fminimizer_nmsimplex;
  gsl_multimin_fminimizer *s = NULL;
  gsl_vector *ss;
  gsl_multimin_function minex_func;
  size_t iter = 0;
  int i = 0, status;
  double size;

  /* Initial vertex size vector */
  ss = gsl_vector_alloc (nP);

  /* Set all step sizes to default constant */
  gsl_vector_set_all(ss, INIT_SIMPLEX_SIZE);

  gsl_vector_set(ss,nP - 1,INIT_S_SS*gsl_vector_get(ptX,0));

  /* Initialize method and iterate */
  minex_func.f = f;
  minex_func.n = nP;
  minex_func.params = pvData;

  s = gsl_multimin_fminimizer_alloc (T, nP);
  gsl_multimin_fminimizer_set(s, &minex_func, ptX, ss);

  do{
    iter++;
    status = gsl_multimin_fminimizer_iterate(s);

    if(status)
      break;

    size = gsl_multimin_fminimizer_size(s);
    status = gsl_multimin_test_size(size, MIN_SIMPLEX_SIZE);

    if(status == GSL_SUCCESS){
      for(i = 0; i < nP; i++){
	gsl_vector_set(ptX, i, gsl_vector_get(s->x, i));
      }

      if(verbose) printf("converged to minimum at\n");
    }

    if(verbose){
      printf ("%5d ", iter);

      for (i = 0; i < nP; i++) printf("%10.3e ", gsl_vector_get(s->x, i));

      printf("f() = %7.3f size = %.3f\n", s->fval, size);
    }
  }
  while(status == GSL_CONTINUE && iter < MAX_SIMPLEX_ITER);

  for(i = 0; i < nP; i++){
    gsl_vector_set(ptX, i, gsl_vector_get(s->x, i));
  }

  gsl_vector_free(ss);
  gsl_multimin_fminimizer_free (s);

  return status;
}

void freeAbundanceData(t_Data *ptData)
{
  int i = 0;

  for(i = 0; i < ptData->nNA; i++){
    free(ptData->aanAbund[i]);
  }
  free(ptData->aanAbund);
}

void getProposal(gsl_rng *ptGSLRNG, gsl_vector *ptXDash, gsl_vector *ptX, int* pnSDash, int nS, t_Params *ptParams)
{
  double dDeltaS =  gsl_ran_gaussian(ptGSLRNG, ptParams->dSigmaS);
  double dDeltaA =  gsl_ran_gaussian(ptGSLRNG, ptParams->dSigmaA);
  double dDeltaB =  gsl_ran_gaussian(ptGSLRNG, ptParams->dSigmaB);
  int    nSDash = 0;

  gsl_vector_set(ptXDash, 0, gsl_vector_get(ptX,0) + dDeltaA);
  gsl_vector_set(ptXDash, 1, gsl_vector_get(ptX,1) + dDeltaB);

  //printf("%e %e %e\n",dDeltaA,dDeltaB,dDeltaG);

  nSDash = nS + (int) floor(dDeltaS);
  if(nSDash < 1){
    nSDash = 1;
  }
  (*pnSDash) = nSDash;
}

void outputResults(gsl_vector *ptX, t_Data *ptData)
{
  double dAlpha = 0.0, dBeta = 0.0, dS = 0.0, dL = 0.0;

  dAlpha = gsl_vector_get(ptX, 0);

  dBeta  = gsl_vector_get(ptX, 1);

  dS = gsl_vector_get(ptX, 2);

  dL = nLogLikelihood(ptX, ptData);

  printf("\nML simplex: a = %.2f b = %.2f S = %.2f NLL = %.2f\n",dAlpha, dBeta, dS, dL);
}

void* metropolis (void * pvInitMetro)
{
  t_MetroInit *ptMetroInit  = (t_MetroInit *) pvInitMetro;
  gsl_vector  *ptX          = ptMetroInit->ptX;
  t_Data      *ptData       = ptMetroInit->ptData;
  t_Params    *ptParams     = ptMetroInit->ptParams;
  gsl_vector  *ptXDash      = gsl_vector_alloc(3); /*proposal*/
  char *szSampleFile = (char *) malloc(MAX_LINE_LENGTH*sizeof(char));
  const gsl_rng_type *T;
  gsl_rng            *ptGSLRNG;
  FILE    *sfp = NULL;
  int nS = 0, nSDash = 0, nIter = 0;
  double dRand = 0.0, dNLL = 0.0;
  void   *pvRet = NULL;

  /*set up random number generator*/
  T        = gsl_rng_default;
  ptGSLRNG = gsl_rng_alloc (T);

  nS = (int) floor(gsl_vector_get(ptX,2));

  dNLL = negLogLikelihood(gsl_vector_get(ptX,0), gsl_vector_get(ptX,1), nS,(void*) ptData);

  sprintf(szSampleFile,"%s_%d%s", ptParams->szOutFileStub, ptMetroInit->nThread, SAMPLE_FILE_SUFFIX);

  sfp = fopen(szSampleFile, "w");
  if(!sfp){
    exit(EXIT_FAILURE);
  }

  /*seed random number generator*/
  gsl_rng_set(ptGSLRNG, ptMetroInit->lSeed);

  /*now perform simple Metropolis algorithm*/
  while(nIter < ptParams->nIter){
    double dA = 0.0, dNLLDash = 0.0;

    getProposal(ptGSLRNG, ptXDash, ptX, &nSDash, nS, ptParams);

    dNLLDash = negLogLikelihood(gsl_vector_get(ptXDash,0), gsl_vector_get(ptXDash,1), nSDash, (void*) ptData);
    //printf("X' %e %e %e %d %f\n", gsl_vector_get(ptXDash,0), gsl_vector_get(ptXDash,1), gsl_vector_get(ptXDash,2), nSDash, dNLLDash);
    //printf("X %e %e %e %d %f\n", gsl_vector_get(ptX,0), gsl_vector_get(ptX,1), gsl_vector_get(ptX,2), nS, dNLL);
    dA = exp(dNLL - dNLLDash);
    if(dA > 1.0){
      dA = 1.0;
    }

    dRand = gsl_rng_uniform(ptGSLRNG);

    if(dRand < dA){
      gsl_vector_memcpy(ptX, ptXDash);
      nS = nSDash;
      dNLL = dNLLDash;
      ptMetroInit->nAccepted++;
    }

    if(nIter % SLICE == 0){
      fprintf(sfp, "%d,%e,%e,%d,%f\n",nIter,gsl_vector_get(ptX, 0), gsl_vector_get(ptX, 1), nS, dNLL);
      fflush(sfp);
    }

    nIter++;
  }

  fclose(sfp);

  /*free up allocated memory*/
  gsl_vector_free(ptXDash);
  free(szSampleFile);
  gsl_rng_free(ptGSLRNG);

  return pvRet;
}

void writeThread(t_MetroInit *ptMetroInit)
{
  gsl_vector *ptX = ptMetroInit->ptX;
    printf("%d: a = %.2f b = %.2f S = %.2f\n", ptMetroInit->nThread,
	   gsl_vector_get(ptX, 0),
	   gsl_vector_get(ptX, 1),
	   gsl_vector_get(ptX, 2));
}

void mcmc(t_Params *ptParams, t_Data *ptData, gsl_vector* ptX)
{
  pthread_t thread1, thread2, thread3;
  int       iret1  , iret2  , iret3;
  gsl_vector *ptX1 = gsl_vector_alloc(3),
             *ptX2 = gsl_vector_alloc(3),
             *ptX3 = gsl_vector_alloc(3);
  t_MetroInit atMetroInit[3];

  printf("\nMCMC iter = %d sigmaA = %.2f sigmaB = %.2f sigmaS = %.2f\n",
	   ptParams->nIter, ptParams->dSigmaA, ptParams->dSigmaB, ptParams->dSigmaS);

  gsl_vector_memcpy(ptX1, ptX);

  gsl_vector_set(ptX2, 0, gsl_vector_get(ptX,0) + 2.0*ptParams->dSigmaA);
  gsl_vector_set(ptX2, 1, gsl_vector_get(ptX,1) + 2.0*ptParams->dSigmaB);
  gsl_vector_set(ptX2, 2, gsl_vector_get(ptX,2) + 2.0*ptParams->dSigmaS);

  gsl_vector_set(ptX3, 0, gsl_vector_get(ptX,0) - 2.0*ptParams->dSigmaA);
  gsl_vector_set(ptX3, 1, gsl_vector_get(ptX,1) - 2.0*ptParams->dSigmaB);
  if(gsl_vector_get(ptX,2) - 2.0*ptParams->dSigmaS > (double) ptData->nL){
	gsl_vector_set(ptX3, 2, gsl_vector_get(ptX,2) - 2.0*ptParams->dSigmaS);
  }
  else{
	gsl_vector_set(ptX3, 2, (double) ptData->nL);
  }
  atMetroInit[0].ptParams = ptParams;
  atMetroInit[0].ptData   = ptData;
  atMetroInit[0].ptX      = ptX1;
  atMetroInit[0].nThread  = 0;
  atMetroInit[0].lSeed    = ptParams->lSeed;
  atMetroInit[0].nAccepted = 0;

  atMetroInit[1].ptParams = ptParams;
  atMetroInit[1].ptData   = ptData;
  atMetroInit[1].ptX      = ptX2;
  atMetroInit[1].nThread  = 1;
  atMetroInit[1].lSeed    = ptParams->lSeed + 1;
  atMetroInit[1].nAccepted = 0;

  atMetroInit[2].ptParams = ptParams;
  atMetroInit[2].ptData   = ptData;
  atMetroInit[2].ptX      = ptX3;
  atMetroInit[2].nThread  = 2;
  atMetroInit[2].lSeed    = ptParams->lSeed + 2;
  atMetroInit[2].nAccepted = 0;

  writeThread(&atMetroInit[0]);
  writeThread(&atMetroInit[1]);
  writeThread(&atMetroInit[2]);

  iret1 = pthread_create(&thread1, NULL, metropolis, (void*) &atMetroInit[0]);
  iret2 = pthread_create(&thread2, NULL, metropolis, (void*) &atMetroInit[1]);
  iret3 = pthread_create(&thread3, NULL, metropolis, (void*) &atMetroInit[2]);
  pthread_join(thread1, NULL);
  pthread_join(thread2, NULL);
  pthread_join(thread3, NULL);

  printf("%d: accept. ratio %d/%d = %f\n", atMetroInit[0].nThread,
	 atMetroInit[0].nAccepted, ptParams->nIter,((double) atMetroInit[0].nAccepted)/((double) ptParams->nIter));

  printf("%d: accept. ratio %d/%d = %f\n", atMetroInit[1].nThread,
	 atMetroInit[1].nAccepted, ptParams->nIter,((double) atMetroInit[1].nAccepted)/((double) ptParams->nIter));

  printf("%d: accept. ratio %d/%d = %f\n", atMetroInit[2].nThread,
	 atMetroInit[2].nAccepted, ptParams->nIter, ((double) atMetroInit[2].nAccepted)/((double) ptParams->nIter));

  gsl_vector_free(ptX1); gsl_vector_free(ptX2); gsl_vector_free(ptX3);
}


/* multifit/gsl_multifit.h
 *
 * Copyright (C) 2000, 2007 Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#ifndef __GSL_MULTIFIT_H__
#define __GSL_MULTIFIT_H__

#if !defined( GSL_FUN )
#  if !defined( GSL_DLL )
#    define GSL_FUN extern
#  elif defined( BUILD_GSL_DLL )
#    define GSL_FUN extern __declspec(dllexport)
#  else
#    define GSL_FUN extern __declspec(dllimport)
#  endif
#endif

#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

typedef struct
{
  size_t n; /* number of observations */
  size_t p; /* number of parameters */
  gsl_matrix * A;
  gsl_matrix * Q;
  gsl_matrix * QSI;
  gsl_vector * S;
  gsl_vector * t;
  gsl_vector * xt;
  gsl_vector * D;
}
gsl_multifit_linear_workspace;

GSL_FUN gsl_multifit_linear_workspace *
gsl_multifit_linear_alloc (size_t n, size_t p);

GSL_FUN void
gsl_multifit_linear_free (gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_linear (const gsl_matrix * X,
                     const gsl_vector * y,
                     gsl_vector * c,
                     gsl_matrix * cov,
                     double * chisq,
                     gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_linear_svd (const gsl_matrix * X,
                         const gsl_vector * y,
                         double tol,
                         size_t * rank,
                         gsl_vector * c,
                         gsl_matrix * cov,
                         double *chisq,
                         gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_wlinear (const gsl_matrix * X,
                      const gsl_vector * w,
                      const gsl_vector * y,
                      gsl_vector * c,
                      gsl_matrix * cov,
                      double * chisq,
                      gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_wlinear_svd (const gsl_matrix * X,
                          const gsl_vector * w,
                          const gsl_vector * y,
                          double tol,
                          size_t * rank,
                          gsl_vector * c,
                          gsl_matrix * cov,
                          double *chisq,
                          gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_linear_est (const gsl_vector * x,
                         const gsl_vector * c,
                         const gsl_matrix * cov, double *y, double *y_err);

GSL_FUN int
gsl_multifit_linear_residuals (const gsl_matrix *X, const gsl_vector *y,
                               const gsl_vector *c, gsl_vector *r);

__END_DECLS

#endif /* __GSL_MULTIFIT_H__ */


/* specfunc/exp.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_exp.h>

#include "error.h"

/* Evaluate the continued fraction for exprel.
 * [Abramowitz+Stegun, 4.2.41]
 */
static
int
exprel_n_CF(const double N, const double x, gsl_sf_result * result)
{
  const double RECUR_BIG = GSL_SQRT_DBL_MAX;
  const int maxiter = 5000;
  int n = 1;
  double Anm2 = 1.0;
  double Bnm2 = 0.0;
  double Anm1 = 0.0;
  double Bnm1 = 1.0;
  double a1 = 1.0;
  double b1 = 1.0;
  double a2 = -x;
  double b2 = N+1;
  double an, bn;

  double fn;

  double An = b1*Anm1 + a1*Anm2;   /* A1 */
  double Bn = b1*Bnm1 + a1*Bnm2;   /* B1 */

  /* One explicit step, before we get to the main pattern. */
  n++;
  Anm2 = Anm1;
  Bnm2 = Bnm1;
  Anm1 = An;
  Bnm1 = Bn;
  An = b2*Anm1 + a2*Anm2;   /* A2 */
  Bn = b2*Bnm1 + a2*Bnm2;   /* B2 */

  fn = An/Bn;

  while(n < maxiter) {
    double old_fn;
    double del;
    n++;
    Anm2 = Anm1;
    Bnm2 = Bnm1;
    Anm1 = An;
    Bnm1 = Bn;
    an = ( GSL_IS_ODD(n) ? ((n-1)/2)*x : -(N+(n/2)-1)*x );
    bn = N + n - 1;
    An = bn*Anm1 + an*Anm2;
    Bn = bn*Bnm1 + an*Bnm2;

    if(fabs(An) > RECUR_BIG || fabs(Bn) > RECUR_BIG) {
      An /= RECUR_BIG;
      Bn /= RECUR_BIG;
      Anm1 /= RECUR_BIG;
      Bnm1 /= RECUR_BIG;
      Anm2 /= RECUR_BIG;
      Bnm2 /= RECUR_BIG;
    }

    old_fn = fn;
    fn = An/Bn;
    del = old_fn/fn;

    if(fabs(del - 1.0) < 2.0*GSL_DBL_EPSILON) break;
  }

  result->val = fn;
  result->err = 4.0*(n+1.0)*GSL_DBL_EPSILON*fabs(fn);

  if(n == maxiter)
    GSL_ERROR ("error", GSL_EMAXITER);
  else
    return GSL_SUCCESS;
}

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

int gsl_sf_exp_e(const double x, gsl_sf_result * result)
{
  if(x > GSL_LOG_DBL_MAX) {
    OVERFLOW_ERROR(result);
  }
  else if(x < GSL_LOG_DBL_MIN) {
    UNDERFLOW_ERROR(result);
  }
  else {
    result->val = exp(x);
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
}

int gsl_sf_exp_e10_e(const double x, gsl_sf_result_e10 * result)
{
  if(x > INT_MAX-1) {
    OVERFLOW_ERROR_E10(result);
  }
  else if(x < INT_MIN+1) {
    UNDERFLOW_ERROR_E10(result);
  }
  else {
    const int N = (x > GSL_LOG_DBL_MAX || x < GSL_LOG_DBL_MIN) ? (int) floor(x/M_LN10) : 0;
    result->val = exp(x-N*M_LN10);
    result->err = 2.0 * (fabs(x)+1.0) * GSL_DBL_EPSILON * fabs(result->val);
    result->e10 = N;
    return GSL_SUCCESS;
  }
}

int gsl_sf_exp_mult_e(const double x, const double y, gsl_sf_result * result)
{
  const double ay  = fabs(y);

  if(y == 0.0) {
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(   ( x < 0.5*GSL_LOG_DBL_MAX   &&   x > 0.5*GSL_LOG_DBL_MIN)
          && (ay < 0.8*GSL_SQRT_DBL_MAX  &&  ay > 1.2*GSL_SQRT_DBL_MIN)
    ) {
    const double ex = exp(x);
    result->val = y * ex;
    result->err = (2.0 + fabs(x)) * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    const double ly  = log(ay);
    const double lnr = x + ly;

    if(lnr > GSL_LOG_DBL_MAX - 0.01) {
      OVERFLOW_ERROR(result);
    }
    else if(lnr < GSL_LOG_DBL_MIN + 0.01) {
      UNDERFLOW_ERROR(result);
    }
    else {
      const double sy   = GSL_SIGN(y);
      const double M    = floor(x);
      const double N    = floor(ly);
      const double a    = x  - M;
      const double b    = ly - N;
      const double berr = 2.0 * GSL_DBL_EPSILON * (fabs(ly) + fabs(N));
      result->val  = sy * exp(M+N) * exp(a+b);
      result->err  = berr * fabs(result->val);
      result->err += 2.0 * GSL_DBL_EPSILON * (M + N + 1.0) * fabs(result->val);
      return GSL_SUCCESS;
    }
  }
}

int gsl_sf_exp_mult_e10_e(const double x, const double y, gsl_sf_result_e10 * result)
{
  const double ay  = fabs(y);

  if(y == 0.0) {
    result->val = 0.0;
    result->err = 0.0;
    result->e10 = 0;
    return GSL_SUCCESS;
  }
  else if(   ( x < 0.5*GSL_LOG_DBL_MAX   &&   x > 0.5*GSL_LOG_DBL_MIN)
          && (ay < 0.8*GSL_SQRT_DBL_MAX  &&  ay > 1.2*GSL_SQRT_DBL_MIN)
    ) {
    const double ex = exp(x);
    result->val = y * ex;
    result->err = (2.0 + fabs(x)) * GSL_DBL_EPSILON * fabs(result->val);
    result->e10 = 0;
    return GSL_SUCCESS;
  }
  else {
    const double ly  = log(ay);
    const double l10_val = (x + ly)/M_LN10;

    if(l10_val > INT_MAX-1) {
      OVERFLOW_ERROR_E10(result);
    }
    else if(l10_val < INT_MIN+1) {
      UNDERFLOW_ERROR_E10(result);
    }
    else {
      const double sy  = GSL_SIGN(y);
      const int    N   = (int) floor(l10_val);
      const double arg_val = (l10_val - N) * M_LN10;
      const double arg_err = 2.0 * GSL_DBL_EPSILON * (fabs(x) + fabs(ly) + M_LN10*fabs(N));

      result->val  = sy * exp(arg_val);
      result->err  = arg_err * fabs(result->val);
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      result->e10 = N;

      return GSL_SUCCESS;
    }
  }
}

int gsl_sf_exp_mult_err_e(const double x, const double dx,
                             const double y, const double dy,
                             gsl_sf_result * result)
{
  const double ay  = fabs(y);

  if(y == 0.0) {
    result->val = 0.0;
    result->err = fabs(dy * exp(x));
    return GSL_SUCCESS;
  }
  else if(   ( x < 0.5*GSL_LOG_DBL_MAX   &&   x > 0.5*GSL_LOG_DBL_MIN)
          && (ay < 0.8*GSL_SQRT_DBL_MAX  &&  ay > 1.2*GSL_SQRT_DBL_MIN)
    ) {
    double ex = exp(x);
    result->val  = y * ex;
    result->err  = ex * (fabs(dy) + fabs(y*dx));
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    const double ly  = log(ay);
    const double lnr = x + ly;

    if(lnr > GSL_LOG_DBL_MAX - 0.01) {
      OVERFLOW_ERROR(result);
    }
    else if(lnr < GSL_LOG_DBL_MIN + 0.01) {
      UNDERFLOW_ERROR(result);
    }
    else {
      const double sy  = GSL_SIGN(y);
      const double M   = floor(x);
      const double N   = floor(ly);
      const double a   = x  - M;
      const double b   = ly - N;
      const double eMN = exp(M+N);
      const double eab = exp(a+b);
      result->val  = sy * eMN * eab;
      result->err  = eMN * eab * 2.0*GSL_DBL_EPSILON;
      result->err += eMN * eab * fabs(dy/y);
      result->err += eMN * eab * fabs(dx);
      return GSL_SUCCESS;
    }
  }
}

int gsl_sf_exp_mult_err_e10_e(const double x, const double dx,
                             const double y, const double dy,
                             gsl_sf_result_e10 * result)
{
  const double ay  = fabs(y);

  if(y == 0.0) {
    result->val = 0.0;
    result->err = fabs(dy * exp(x));
    result->e10 = 0;
    return GSL_SUCCESS;
  }
  else if(   ( x < 0.5*GSL_LOG_DBL_MAX   &&   x > 0.5*GSL_LOG_DBL_MIN)
          && (ay < 0.8*GSL_SQRT_DBL_MAX  &&  ay > 1.2*GSL_SQRT_DBL_MIN)
    ) {
    const double ex = exp(x);
    result->val  = y * ex;
    result->err  = ex * (fabs(dy) + fabs(y*dx));
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    result->e10 = 0;
    return GSL_SUCCESS;
  }
  else {
    const double ly  = log(ay);
    const double l10_val = (x + ly)/M_LN10;

    if(l10_val > INT_MAX-1) {
      OVERFLOW_ERROR_E10(result);
    }
    else if(l10_val < INT_MIN+1) {
      UNDERFLOW_ERROR_E10(result);
    }
    else {
      const double sy  = GSL_SIGN(y);
      const int    N   = (int) floor(l10_val);
      const double arg_val = (l10_val - N) * M_LN10;
      const double arg_err = dy/fabs(y) + dx + 2.0*GSL_DBL_EPSILON*fabs(arg_val);

      result->val  = sy * exp(arg_val);
      result->err  = arg_err * fabs(result->val);
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      result->e10 = N;

      return GSL_SUCCESS;
    }
  }
}

int gsl_sf_expm1_e(const double x, gsl_sf_result * result)
{
  const double cut = 0.002;

  if(x < GSL_LOG_DBL_MIN) {
    result->val = -1.0;
    result->err = GSL_DBL_EPSILON;
    return GSL_SUCCESS;
  }
  else if(x < -cut) {
    result->val = exp(x) - 1.0;
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x < cut) {
    result->val = x * (1.0 + 0.5*x*(1.0 + x/3.0*(1.0 + 0.25*x*(1.0 + 0.2*x))));
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x < GSL_LOG_DBL_MAX) {
    result->val = exp(x) - 1.0;
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    OVERFLOW_ERROR(result);
  }
}

int gsl_sf_exprel_e(const double x, gsl_sf_result * result)
{
  const double cut = 0.002;

  if(x < GSL_LOG_DBL_MIN) {
    result->val = -1.0/x;
    result->err = GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x < -cut) {
    result->val = (exp(x) - 1.0)/x;
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x < cut) {
    result->val = (1.0 + 0.5*x*(1.0 + x/3.0*(1.0 + 0.25*x*(1.0 + 0.2*x))));
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x < GSL_LOG_DBL_MAX) {
    result->val = (exp(x) - 1.0)/x;
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    OVERFLOW_ERROR(result);
  }
}

int gsl_sf_exprel_2_e(double x, gsl_sf_result * result)
{
  const double cut = 0.002;

  if(x < GSL_LOG_DBL_MIN) {
    result->val = -2.0/x*(1.0 + 1.0/x);
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x < -cut) {
    result->val = 2.0*(exp(x) - 1.0 - x)/(x*x);
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x < cut) {
    result->val = (1.0 + 1.0/3.0*x*(1.0 + 0.25*x*(1.0 + 0.2*x*(1.0 + 1.0/6.0*x))));
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x < GSL_LOG_DBL_MAX) {
    result->val = 2.0*(exp(x) - 1.0 - x)/(x*x);
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    OVERFLOW_ERROR(result);
  }
}

int
gsl_sf_exprel_n_CF_e(const double N, const double x, gsl_sf_result * result)
{
  return exprel_n_CF(N, x, result);
}

int
gsl_sf_exprel_n_e(const int N, const double x, gsl_sf_result * result)
{
  if(N < 0) {
    DOMAIN_ERROR(result);
  }
  else if(x == 0.0) {
    result->val = 1.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(fabs(x) < GSL_ROOT3_DBL_EPSILON * N) {
    result->val = 1.0 + x/(N+1) * (1.0 + x/(N+2));
    result->err = 2.0 * GSL_DBL_EPSILON;
    return GSL_SUCCESS;
  }
  else if(N == 0) {
    return gsl_sf_exp_e(x, result);
  }
  else if(N == 1) {
    return gsl_sf_exprel_e(x, result);
  }
  else if(N == 2) {
    return gsl_sf_exprel_2_e(x, result);
  }
  else {
    if(x > N && (-x + N*(1.0 + log(x/N)) < GSL_LOG_DBL_EPSILON)) {
      /* x is much larger than n.
       * Ignore polynomial part, so
       * exprel_N(x) ~= e^x N!/x^N
       */
      gsl_sf_result lnf_N;
      double lnr_val;
      double lnr_err;
      double lnterm;
      gsl_sf_lnfact_e(N, &lnf_N);
      lnterm = N*log(x);
      lnr_val  = x + lnf_N.val - lnterm;
      lnr_err  = GSL_DBL_EPSILON * (fabs(x) + fabs(lnf_N.val) + fabs(lnterm));
      lnr_err += lnf_N.err;
      return gsl_sf_exp_err_e(lnr_val, lnr_err, result);
    }
    else if(x > N) {
      /* Write the identity
       *   exprel_n(x) = e^x n! / x^n (1 - Gamma[n,x]/Gamma[n])
       * then use the asymptotic expansion
       * Gamma[n,x] ~ x^(n-1) e^(-x) (1 + (n-1)/x + (n-1)(n-2)/x^2 + ...)
       */
      double ln_x = log(x);
      gsl_sf_result lnf_N;
      double lg_N;
      double lnpre_val;
      double lnpre_err;
      gsl_sf_lnfact_e(N, &lnf_N);    /* log(N!)       */
      lg_N  = lnf_N.val - log(N);       /* log(Gamma(N)) */
      lnpre_val  = x + lnf_N.val - N*ln_x;
      lnpre_err  = GSL_DBL_EPSILON * (fabs(x) + fabs(lnf_N.val) + fabs(N*ln_x));
      lnpre_err += lnf_N.err;
      if(lnpre_val < GSL_LOG_DBL_MAX - 5.0) {
        int stat_eG;
        gsl_sf_result bigG_ratio;
        gsl_sf_result pre;
        int stat_ex = gsl_sf_exp_err_e(lnpre_val, lnpre_err, &pre);
        double ln_bigG_ratio_pre = -x + (N-1)*ln_x - lg_N;
        double bigGsum = 1.0;
        double term = 1.0;
        int k;
        for(k=1; k<N; k++) {
          term *= (N-k)/x;
          bigGsum += term;
        }
        stat_eG = gsl_sf_exp_mult_e(ln_bigG_ratio_pre, bigGsum, &bigG_ratio);
        if(stat_eG == GSL_SUCCESS) {
          result->val  = pre.val * (1.0 - bigG_ratio.val);
          result->err  = pre.val * (2.0*GSL_DBL_EPSILON + bigG_ratio.err);
          result->err += pre.err * fabs(1.0 - bigG_ratio.val);
          result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
          return stat_ex;
        }
        else {
          result->val = 0.0;
          result->err = 0.0;
          return stat_eG;
        }
      }
      else {
        OVERFLOW_ERROR(result);
      }
    }
    else if(x > -10.0*N) {
      return exprel_n_CF(N, x, result);
    }
    else {
      /* x -> -Inf asymptotic:
       * exprel_n(x) ~ e^x n!/x^n - n/x (1 + (n-1)/x + (n-1)(n-2)/x + ...)
       *             ~ - n/x (1 + (n-1)/x + (n-1)(n-2)/x + ...)
       */
      double sum  = 1.0;
      double term = 1.0;
      int k;
      for(k=1; k<N; k++) {
        term *= (N-k)/x;
        sum  += term;
      }
      result->val = -N/x * sum;
      result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      return GSL_SUCCESS;
    }
  }
}

int
gsl_sf_exp_err_e(const double x, const double dx, gsl_sf_result * result)
{
  const double adx = fabs(dx);

  /* CHECK_POINTER(result) */

  if(x + adx > GSL_LOG_DBL_MAX) {
    OVERFLOW_ERROR(result);
  }
  else if(x - adx < GSL_LOG_DBL_MIN) {
    UNDERFLOW_ERROR(result);
  }
  else {
    const double ex  = exp(x);
    const double edx = exp(adx);
    result->val  = ex;
    result->err  = ex * GSL_MAX_DBL(GSL_DBL_EPSILON, edx - 1.0/edx);
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
}

int
gsl_sf_exp_err_e10_e(const double x, const double dx, gsl_sf_result_e10 * result)
{
  const double adx = fabs(dx);

  /* CHECK_POINTER(result) */

  if(x + adx > INT_MAX - 1) {
    OVERFLOW_ERROR_E10(result);
  }
  else if(x - adx < INT_MIN + 1) {
    UNDERFLOW_ERROR_E10(result);
  }
  else {
    const int    N  = (int)floor(x/M_LN10);
    const double ex = exp(x-N*M_LN10);
    result->val = ex;
    result->err = ex * (2.0 * GSL_DBL_EPSILON * (fabs(x) + 1.0) + adx);
    result->e10 = N;
    return GSL_SUCCESS;
  }
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_exp(const double x)
{
  EVAL_RESULT(gsl_sf_exp_e(x, &result));
}

double gsl_sf_exp_mult(const double x, const double y)
{
  EVAL_RESULT(gsl_sf_exp_mult_e(x, y, &result));
}

double gsl_sf_expm1(const double x)
{
  EVAL_RESULT(gsl_sf_expm1_e(x, &result));
}

double gsl_sf_exprel(const double x)
{
  EVAL_RESULT(gsl_sf_exprel_e(x, &result));
}

double gsl_sf_exprel_2(const double x)
{
  EVAL_RESULT(gsl_sf_exprel_2_e(x, &result));
}

double gsl_sf_exprel_n(const int n, const double x)
{
  EVAL_RESULT(gsl_sf_exprel_n_e(n, x, &result));
}