#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <tgmath.h>
#include <time.h>
#include <iterative_point_recovery.clh>
#include <ptrc_test_settings.h>
flt32_t fSolves=0.0f; /* counter of number of Chi solves */

#include <iterative_point_recovery.clc>

#define __PTRC_TS_FRND \
                      (((flt32_t)(rand()-(RAND_MAX/2)))/((flt32_t)(RAND_MAX/2)))

typedef struct _PTRC_TS_STATISTICAL_DATA_SET {
    flt32_t* pf32Vals;
    flt32_t  f32ExpVal;
    flt32_t  f32SigmaVal;
    flt32_t  f32MinVal;
    flt32_t  f32MaxVal;
} _PTRC_STAT_DSET;

typedef struct _PTRC_TS_CASES {
    flt32_t  f32SensorUnc;
    _PTRC_STAT_DSET Errors;
    _PTRC_STAT_DSET Solves;
    uint64_t u64NofCases;
} _PTRC_TS_CASES;

int32_t __ptrc_ts_oneCase(const uint64_t u64D,
                          const uint64_t u64NofS,
                          const flt32_t f32RngMult,
                          _PTRC_TS_CASES* const pCases);
flt32_t __ptrc_ts_expVal(const flt32_t* const pf32Vals,
                               uint64_t       u64NofVals,
                         const flt32_t        f32DeltaX);
int32_t __ptrc_ts_minMaxVals(      flt32_t* const pf32Min,
                                   flt32_t* const pf32Max,
                             const flt32_t* const pf32Vals,
                             const uint64_t       u64NofVals);

int32_t main(void) {
    _PTRC_TS_CASES* pCases=NULL;
    uint64_t u64NofCasesBlocks=
        (uint64_t)((_PTRC_TS_MAX_SENSOR_UNCERTAINTY-
                    _PTRC_TS_MIN_SENSOR_UNCERTAINTY)/
                    _PTRC_TS_SENSOR_UNCERTAINTY_INCREMENT)+1ul;
    int32_t err=0;
    srand((uint32_t)time(NULL));
    pCases=malloc(u64NofCasesBlocks*sizeof(_PTRC_TS_CASES));
    if(!pCases) { fprintf(stderr,"Error!!!!!\n"); return(1); }
    { uint64_t i;
        for(i=0ul;i<u64NofCasesBlocks;i++) {
            pCases[i].Errors.pf32Vals=malloc(
                       _PTRC_TS_NUM_OF_CASES_FOR_ONE_INCREMENT*sizeof(flt32_t));
            pCases[i].Solves.pf32Vals=malloc(
                       _PTRC_TS_NUM_OF_CASES_FOR_ONE_INCREMENT*sizeof(flt32_t));
            if(!pCases[i].Errors.pf32Vals || !pCases[i].Solves.pf32Vals) {
                uint64_t j;
                for(j=0ul;j<i;j++) {
                    if(pCases[j].Errors.pf32Vals)
                        free(pCases[j].Errors.pf32Vals);
                    if(pCases[j].Solves.pf32Vals)
                        free(pCases[j].Solves.pf32Vals);
                }
                if(pCases) free(pCases);
                fprintf(stderr,"Error!!!!!\n");
                return(1);
            }
        }
    }
    { uint64_t i;
        for(i=0ul;i<u64NofCasesBlocks;i++) {
            uint64_t j;
            _PTRC_TS_CASES* pCurCaseBlocks=&pCases[i];
            pCurCaseBlocks->f32SensorUnc=_PTRC_TS_MIN_SENSOR_UNCERTAINTY+
                           (((flt32_t)i)*_PTRC_TS_SENSOR_UNCERTAINTY_INCREMENT);
            pCurCaseBlocks->u64NofCases=0ul;
            pCurCaseBlocks->Errors.f32ExpVal=pCurCaseBlocks->Errors.f32SigmaVal=
            pCurCaseBlocks->Solves.f32ExpVal=pCurCaseBlocks->Solves.f32SigmaVal=
                0.0f;
            for(j=0ul;j<_PTRC_TS_NUM_OF_CASES_FOR_ONE_INCREMENT;j++) {
                err=__ptrc_ts_oneCase(_PTRC_TS_DIMENSIONALITY,
                                      _PTRC_TS_NUM_OF_SPHERES,
                                      _PTRC_TS_RANGE_MULTIPLIER,
                                      pCurCaseBlocks);
                if(err && err!=2) {
                    uint64_t k;
                    for(k=0ul;k<u64NofCasesBlocks;k++) {
                        if(pCases[k].Errors.pf32Vals)
                            free(pCases[k].Errors.pf32Vals);
                        if(pCases[k].Solves.pf32Vals)
                            free(pCases[k].Solves.pf32Vals);
                    }
                    if(pCases) free(pCases);
                    fprintf(stderr,"Error!!!!!\n");
                    return(1);
                }
            }
        }
    }
    {
        FILE* pStatFile=NULL;
        uint64_t i;
        flt32_t f32DeltaX=_PTRC_TS_STATISTICS_DISCRETISATION_STEP;
        printf("Calculating and saving statistics - ");
        pStatFile=fopen(_PTRC_TS_STATISTICS_FILENAME,"w");
        if(!pStatFile) {
            uint64_t k;
            for(k=0ul;k<u64NofCasesBlocks;k++) {
                if(pCases[k].Errors.pf32Vals)
                    free(pCases[k].Errors.pf32Vals);
                if(pCases[k].Solves.pf32Vals)
                    free(pCases[k].Solves.pf32Vals);
            }
            if(pCases) free(pCases);
            fprintf(stderr,"Error!!!!!\n");
            return(1);
        }
        fprintf(pStatFile,"Dimensionality %lu\n", _PTRC_TS_DIMENSIONALITY);
        fprintf(pStatFile,"Number_of_known_spheres %lu\n",
                _PTRC_TS_NUM_OF_SPHERES);
        fprintf(pStatFile,"Number_of_runs %lu\n",
                _PTRC_TS_NUM_OF_RUNS);
        fprintf(pStatFile,"Multiplier "_PTRC_TS_FLOAT_OUTFORM"\n",
                (flt32_t)_PTRC_TS_RANGE_MULTIPLIER);
        fprintf(pStatFile,"Start_uncertainty "_PTRC_TS_FLOAT_OUTFORM"\n",
                (flt32_t)_PTRC_TS_MIN_SENSOR_UNCERTAINTY);
        fprintf(pStatFile,"End_uncertainty "_PTRC_TS_FLOAT_OUTFORM"\n",
                (flt32_t)_PTRC_TS_MAX_SENSOR_UNCERTAINTY);
        fprintf(pStatFile,"Uncertainty_incr "_PTRC_TS_FLOAT_OUTFORM"\n",
                (flt32_t)_PTRC_TS_SENSOR_UNCERTAINTY_INCREMENT);
        fprintf(pStatFile,"Samples_for_one_uncertainty_value %lu\n",
                _PTRC_TS_NUM_OF_CASES_FOR_ONE_INCREMENT);
        fprintf(pStatFile,"Statistical_discretisation "
                          _PTRC_TS_FLOAT_OUTFORM"\n",
                (flt32_t)_PTRC_TS_STATISTICS_DISCRETISATION_STEP);
        fprintf(pStatFile,"Sensor_uncertainty");
        for(i=0ul;i<u64NofCasesBlocks;i++)
            fprintf(pStatFile," "_PTRC_TS_FLOAT_OUTFORM,pCases[i].f32SensorUnc);
        fprintf(pStatFile,"\nExpected_err");
        for(i=0ul;i<u64NofCasesBlocks;i++)
            fprintf(pStatFile," "_PTRC_TS_FLOAT_OUTFORM,
                    pCases[i].Errors.f32ExpVal=__ptrc_ts_expVal(
                        pCases[i].Errors.pf32Vals,pCases[i].u64NofCases,
                        f32DeltaX));
        fprintf(pStatFile,"\nSigma_err");
        for(i=0ul;i<u64NofCasesBlocks;i++) {
            uint64_t j;
            pCases[i].Errors.f32SigmaVal=0.0f;
            for(j=0ul;j<pCases[i].u64NofCases;j++) {
                flt32_t x=(floor(pCases[i].Errors.pf32Vals[j]/f32DeltaX)*f32DeltaX)-
                          pCases[i].Errors.f32ExpVal;
                pCases[i].Errors.f32SigmaVal+=(x*x);
            }
            pCases[i].Errors.f32SigmaVal/=((flt32_t)pCases[i].u64NofCases);
            pCases[i].Errors.f32SigmaVal=sqrt(pCases[i].Errors.f32SigmaVal);
            fprintf(pStatFile," "_PTRC_TS_FLOAT_OUTFORM,
                    pCases[i].Errors.f32SigmaVal);
        }
        for(i=0ul;i<u64NofCasesBlocks;i++)
            __ptrc_ts_minMaxVals(&pCases[i].Errors.f32MinVal,
                                 &pCases[i].Errors.f32MaxVal,
                                 pCases[i].Errors.pf32Vals,
                                 pCases[i].u64NofCases);
        fprintf(pStatFile,"\nMin_err");
        for(i=0ul;i<u64NofCasesBlocks;i++) fprintf(pStatFile,
                                                   " "_PTRC_TS_FLOAT_OUTFORM,
                                                   pCases[i].Errors.f32MinVal);
        fprintf(pStatFile,"\nMax_err");
        for(i=0ul;i<u64NofCasesBlocks;i++) fprintf(pStatFile,
                                                   " "_PTRC_TS_FLOAT_OUTFORM,
                                                   pCases[i].Errors.f32MaxVal);
        fprintf(pStatFile,"\nExpected_solves");
        for(i=0ul;i<u64NofCasesBlocks;i++)
            fprintf(pStatFile," "_PTRC_TS_FLOAT_OUTFORM,
                    pCases[i].Solves.f32ExpVal=__ptrc_ts_expVal(
                         pCases[i].Solves.pf32Vals,pCases[i].u64NofCases,1.0f));
        fprintf(pStatFile,"\nSigma_solves");
        for(i=0ul;i<u64NofCasesBlocks;i++) {
            uint64_t j;
            pCases[i].Solves.f32SigmaVal=0.0f;
            for(j=0ul;j<pCases[i].u64NofCases;j++) {
                flt32_t x=pCases[i].Solves.pf32Vals[j]-
                          pCases[i].Solves.f32ExpVal;
                pCases[i].Solves.f32SigmaVal+=(x*x);
            }
            pCases[i].Solves.f32SigmaVal/=((flt32_t)pCases[i].u64NofCases);
            pCases[i].Solves.f32SigmaVal=sqrt(pCases[i].Solves.f32SigmaVal);
            fprintf(pStatFile," "_PTRC_TS_FLOAT_OUTFORM,
                    pCases[i].Solves.f32SigmaVal);
        }
        for(i=0ul;i<u64NofCasesBlocks;i++)
           __ptrc_ts_minMaxVals(&pCases[i].Solves.f32MinVal,
                                &pCases[i].Solves.f32MaxVal,
                                pCases[i].Solves.pf32Vals,
                                pCases[i].u64NofCases);
        fprintf(pStatFile,"\nMin_solves");
        for(i=0ul;i<u64NofCasesBlocks;i++)
            fprintf(pStatFile," %lu",(uint64_t)pCases[i].Solves.f32MinVal);
        fprintf(pStatFile,"\nMax_solves");
        for(i=0ul;i<u64NofCasesBlocks;i++)
            fprintf(pStatFile," %lu",(uint64_t)pCases[i].Solves.f32MaxVal);
        fprintf(pStatFile,"\nSensor_uncertainty Raw_Deltas\n");
        for(i=0ul;i<u64NofCasesBlocks;i++) {
            uint64_t j;
            fprintf(pStatFile,_PTRC_TS_FLOAT_OUTFORM,pCases[i].f32SensorUnc);
            for(j=0ul;j<pCases[i].u64NofCases;j++)
                 fprintf(pStatFile," "_PTRC_TS_FLOAT_OUTFORM,
                         pCases[i].Errors.pf32Vals[j]);
            fprintf(pStatFile,"\n");
        }
        fprintf(pStatFile,"\nSensor_uncertainty Raw_Solves\n");
        for(i=0ul;i<u64NofCasesBlocks;i++) {
            uint64_t j;
            fprintf(pStatFile,_PTRC_TS_FLOAT_OUTFORM,pCases[i].f32SensorUnc);
            for(j=0ul;j<pCases[i].u64NofCases;j++)
                 fprintf(pStatFile," %lu",
                         (uint64_t)pCases[i].Solves.pf32Vals[j]);
            fprintf(pStatFile,"\n");
        }
        fclose(pStatFile);
        printf("ok\n");
    }
    { uint64_t k;
        for(k=0ul;k<u64NofCasesBlocks;k++) {
            if(pCases[k].Errors.pf32Vals) free(pCases[k].Errors.pf32Vals);
            if(pCases[k].Solves.pf32Vals) free(pCases[k].Solves.pf32Vals);
        }
        if(pCases) free(pCases);
    }
    return(0);
}

int32_t __ptrc_ts_oneCase(const uint64_t u64D,
                          const uint64_t u64NofS,
                          const flt32_t f32RngMult,
                          _PTRC_TS_CASES* const pCases) {
    _PTRC_INDAT in={ .u64D=u64D, .u64NofS=u64NofS, .pfCnt=NULL, .pfRds=NULL };
    flt32_t* pf32RealPnt=NULL,
           * pf32Res=NULL,
             f32Qlt=0.0f;
    int32_t err=0;
    pf32RealPnt=malloc(u64D*sizeof(flt32_t));
    in.pfCnt=malloc(u64NofS*u64D*sizeof(flt32_t));
    in.pfRds=malloc(u64NofS*sizeof(flt32_t));
    pf32Res=malloc(u64D*sizeof(flt32_t));
    if(!(pf32RealPnt && in.pfCnt && in.pfRds && pf32Res)) {
        if(pf32RealPnt) free(pf32RealPnt);
        if(in.pfCnt) free(in.pfCnt);
        if(in.pfRds) free(in.pfRds);
        if(pf32Res) free(pf32Res);
        return(1);
    }
    {   uint64_t i;
        for(i=0;i<u64D;i++) pf32RealPnt[i]=__PTRC_TS_FRND*f32RngMult;
        for(i=0;i<(u64D*u64NofS);i++) in.pfCnt[i]=__PTRC_TS_FRND*f32RngMult;
        for(i=0;i<u64NofS;i++) {
            flt32_t fault;
            in.pfRds[i]=__ptrc_EuclDist(pf32RealPnt,&in.pfCnt[i*u64D],u64D);
            fault=__PTRC_TS_FRND*pCases->f32SensorUnc*in.pfRds[i];
            in.pfRds[i]+=fault;
        }
    }
#if defined(_PTRC_TS_DATA_OUTPUT_MODE)
    printf(_PTRC_TS_FLOAT_OUTFORM" %lu\n",
           pCases->f32SensorUnc,_PTRC_MAX_ITERATIONS_ON_SPHERE);
#endif /* _PTRC_TS_DATA_OUTPUT_MODE */
    fSolves=0.0f;
    err=_ptrc_recoverPoint(pf32Res,&f32Qlt,in,_PTRC_TS_NUM_OF_RUNS);
    if(err) {
        if(pf32RealPnt) free(pf32RealPnt);
        if(in.pfCnt) free(in.pfCnt);
        if(in.pfRds) free(in.pfRds);
        if(pf32Res)  free(pf32Res);
        return(2);
    }
    pCases->Errors.pf32Vals[pCases->u64NofCases]=
                                      __ptrc_EuclDist(pf32RealPnt,pf32Res,u64D);
    pCases->Solves.pf32Vals[pCases->u64NofCases]=fSolves;
    pCases->u64NofCases++;
#if defined(_PTRC_TS_DATA_OUTPUT_MODE)
    {   uint64_t j;
        printf("%d ",(-2));
        for(j=0ul;j<u64D;j++) printf(_PTRC_TS_FLOAT_OUTFORM" ",pf32RealPnt[j]);
        printf(_PTRC_TS_FLOAT_OUTFORM" \n",
               pCases->Errors.pf32Vals[(pCases->u64NofCases)-1ul]); }
#endif /* _PTRC_TS_DATA_OUTPUT_MODE */
    if(pf32RealPnt) free(pf32RealPnt);
    if(in.pfCnt)    free(in.pfCnt);
    if(in.pfRds)    free(in.pfRds);
    if(pf32Res)     free(pf32Res);
    return(0);
}

flt32_t __ptrc_ts_expVal(const flt32_t* const pf32Vals,
                               uint64_t       u64NofVals,
                         const flt32_t        f32DeltaX) {
    flt32_t f32Res=0.0f;
    uint64_t i;
    for(i=0ul;i<u64NofVals;i++)
        f32Res+=floor(pf32Vals[i]/f32DeltaX)*f32DeltaX;
    f32Res/=((flt32_t)u64NofVals);
    return(f32Res);
}

int32_t __ptrc_ts_minMaxVals(      flt32_t* const pf32Min,
                                   flt32_t* const pf32Max,
                             const flt32_t* const pf32Vals,
                             const uint64_t       u64NofVals) {
    *pf32Min=*pf32Max=pf32Vals[0];
    uint64_t i;
    for(i=1ul;i<u64NofVals;i++) {
        if(pf32Vals[i]<(*pf32Min)) *pf32Min=pf32Vals[i];
        if(pf32Vals[i]>(*pf32Max)) *pf32Max=pf32Vals[i];
    }
    return(0);
}