#ifndef MARS_DrsCalib
#define MARS_DrsCalib

#include <math.h>   // fabs
#include <errno.h>  // errno

#include "fits.h"

class DrsCalibrate
{
protected:
    uint64_t fNumEntries;

    size_t fNumSamples;
    size_t fNumChannels;

    std::vector<int64_t> fSum;
    std::vector<int64_t> fSum2;

public:
    DrsCalibrate() : fNumEntries(0), fNumSamples(0), fNumChannels(0) { }
    void Reset()
    {
        fNumEntries  = 0;
        fNumSamples  = 0;
        fNumChannels = 0;

        fSum.clear();
        fSum2.clear();
    }

    void InitSize(uint16_t channels, uint16_t samples)
    {
        fNumChannels = channels;
        fNumSamples  = samples;

        fSum.resize(samples*channels);
        fSum2.resize(samples*channels);
    }

    void AddRel(const int16_t *val, const int16_t *start)
    {
        for (size_t ch=0; ch<fNumChannels; ch++)
        {
            const int16_t spos = start[ch];
            if (spos<0)
                continue;

            const size_t pos = ch*1024;

            for (size_t i=0; i<1024; i++)
            {
                // Value is relative to trigger
                // Abs is corresponding index relative to DRS pipeline
                const size_t rel = pos +  i;
                const size_t abs = pos + (spos+i)%1024;

                const int64_t v = val[rel];

                fSum[abs]  += v;
                fSum2[abs] += v*v;
            }
        }

        fNumEntries++;
    }

    void AddRel(const int16_t *val,    const int16_t *start,
                const int32_t *offset, const uint32_t scale)
    {
        for (size_t ch=0; ch<fNumChannels; ch++)
        {
            const int16_t spos = start[ch];
            if (spos<0)
                continue;

            const size_t pos = ch*fNumSamples;

            for (size_t i=0; i<fNumSamples; i++)
            {
                // Value is relative to trigger
                // Offset is relative to DRS pipeline
                // Abs is corresponding index relative to DRS pipeline
                const size_t rel = pos +  i;
                const size_t abs = pos + (spos+i)%fNumSamples;

                const int64_t v = int64_t(val[rel])*scale-offset[abs];

                fSum[abs]  += v;
                fSum2[abs] += v*v;
            }
        }

        fNumEntries++;
    }
    /*
    void AddAbs(const int16_t *val,    const  int16_t *start,
                const int32_t *offset, const uint32_t scale)
    {
        for (size_t ch=0; ch<fNumChannels; ch++)
        {
            const int16_t spos = start[ch];
            if (spos<0)
                continue;

            const size_t pos = ch*fNumSamples;

            for (size_t i=0; i<fNumSamples; i++)
            {
                // Value is relative to trigger
                // Offset is relative to DRS pipeline
                // Abs is corresponding index relative to DRS pipeline
                const size_t rel = pos +  i;
                const size_t abs = pos + (spos+i)%fNumSamples;

                const int64_t v = int64_t(val[rel])*scale-offset[abs];

                fSum[rel]  += v;
                fSum2[rel] += v*v;
            }
        }

        fNumEntries++;
    }*/

    void AddAbs(const int16_t *val,    const  int16_t *start,
                const int32_t *offset, const uint32_t scale)
    {
        // 1440 without tm, 1600 with tm
        for (size_t ch=0; ch<fNumChannels; ch++)
        {
            const int16_t spos = start[ch];
            if (spos<0)
                continue;

            const size_t pos = ch*fNumSamples;
            const size_t drs = ch>1439 ? ((ch-1440)*9+8)*1024 : ch*1024;

            for (size_t i=0; i<fNumSamples; i++)
            {
                // Value is relative to trigger
                // Offset is relative to DRS pipeline
                // Abs is corresponding index relative to DRS pipeline
                const size_t rel = pos +  i;
                const size_t abs = drs + (spos+i)%1024;

                const int64_t v = int64_t(val[rel])*scale-offset[abs];

                fSum[rel]  += v;
                fSum2[rel] += v*v;
            }
        }

        fNumEntries++;
    }

    void AddT(const int16_t *val, const int16_t *start)
    {
        // 1440 without tm, 1600 with tm
        for (size_t ch=0; ch<fNumChannels; ch++)
        {
            const int16_t spos = start[ch];
            if (spos<0)
                continue;

            const size_t pos = ch*fNumSamples;

            uint32_t nperiods = 0;

            for (size_t i=0; i<fNumSamples; i++)
            {
                const size_t abs0 = pos + (spos+i  )%1024;
                const size_t abs1 = pos + (spos+i+1)%1024;

                const float &v0 = val[abs0];
                const float &v1 = val[abs1];

                // Has sign changed?
                if (v0*v1>0)
                {
                    // Sign has not changed
                    fSum[abs0]  += nperiods;
                    fSum2[abs0] += nperiods*nperiods;
                    continue;
                }

                const double p = v0==v1 ? 1 : v0/(v0-v1);

                const double value = nperiods*p + (nperiods+1)*(1-p);

                fSum[abs0]  += value;
                fSum2[abs0] += value*value;

                nperiods++;
            }
        }

        fNumEntries++;
    }

    static void ApplyCh(float *vec, const int16_t *val, int16_t start, uint32_t roi,
                        const int32_t *offset, const uint32_t scaleabs,
                        const int64_t *gain,   const uint64_t scalegain)
    {
        if (start<0)
        {
            memset(vec, 0, roi);
            return;
        }

        for (size_t i=0; i<roi; i++)
        {
            // Value is relative to trigger
            // Offset is relative to DRS pipeline
            // Abs is corresponding index relative to DRS pipeline
            const size_t abs = (start+i)%1024;

            const int64_t v =
                + int64_t(val[i])*scaleabs-offset[abs]
                ;

            const int64_t div = gain[abs];
            vec[i] = double(v)*scalegain/div;
        }
    }

    static void ApplyCh(float *vec, const int16_t *val, int16_t start, uint32_t roi,
                        const int32_t *offset, const uint32_t scaleabs,
                        const int64_t *gain,   const uint64_t scalegain,
                        const int64_t *trgoff, const uint64_t scalerel)
    {
        if (start<0)
        {
            memset(vec, 0, roi);
            return;
        }

        for (size_t i=0; i<roi; i++)
        {
            // Value is relative to trigger
            // Offset is relative to DRS pipeline
            // Abs is corresponding index relative to DRS pipeline
            const size_t abs = (start+i)%1024;

            const int64_t v =
                + (int64_t(val[i])*scaleabs-offset[abs])*scalerel
                - trgoff[i]
                ;

            const int64_t div = gain[abs]*scalerel;
            vec[i] = double(v)*scalegain/div;
        }
    }

    static void RemoveSpikes(float *vec, uint32_t roi)
    {
        if (roi<4)
            return;

        for (size_t ch=0; ch<1440; ch++)
        {
            float *p = vec + ch*roi;

            for (size_t i=1; i<roi-2; i++)
            {
                if (p[i]-p[i-1]>25 && p[i]-p[i+1]>25)
                {
                    p[i] = (p[i-1]+p[i+1])/2;
                }

                if (p[i]-p[i-1]>22 && fabs(p[i]-p[i+1])<4 && p[i+1]-p[i+2]>22)
                {
                    p[i] = (p[i-1]+p[i+2])/2;
                    p[i+1] = p[i];
                }
            }
        }
    }

    std::pair<std::vector<double>,std::vector<double> > GetSampleStats() const
    {
        if (fNumEntries==0)
            return make_pair(std::vector<double>(),std::vector<double>());

        std::vector<double> mean(fSum.size());
        std::vector<double> error(fSum.size());

        std::vector<int64_t>::const_iterator it = fSum.begin();
        std::vector<int64_t>::const_iterator i2 = fSum2.begin();
        std::vector<double>::iterator        im = mean.begin();
        std::vector<double>::iterator        ie = error.begin();

        while (it!=fSum.end())
        {
            *im = /*cnt<fResult.size() ? fResult[cnt] :*/ double(*it)/fNumEntries;
            *ie = sqrt(double(*i2*int64_t(fNumEntries) - *it * *it))/fNumEntries;

            im++;
            ie++;
            it++;
            i2++;
        }


        /*
         valarray<double> ...

         mean /= fNumEntries;
         error = sqrt(error/fNumEntries - mean*mean);
         */

        return make_pair(mean, error);
    }

    void GetSampleStats(float *ptr, float scale) const
    {
        const size_t sz = fNumSamples*fNumChannels;

        if (fNumEntries==0)
        {
            memset(ptr, 0, sizeof(float)*sz*2);
            return;
        }

        std::vector<int64_t>::const_iterator it = fSum.begin();
        std::vector<int64_t>::const_iterator i2 = fSum2.begin();

        while (it!=fSum.end())
        {
            *ptr      = scale*double(*it)/fNumEntries;
            *(ptr+sz) = scale*sqrt(double(*i2*int64_t(fNumEntries) - *it * *it))/fNumEntries;

            ptr++;
            it++;
            i2++;
        }
    }

    static void GetPixelStats(float *ptr, const float *data, uint16_t roi)
    {
        if (roi==0)
            return;

        for (int i=0; i<1440; i++)
        {
            const float *vec = data+i*roi;

            int    pos  = 0;
            double sum  = vec[0];
            double sum2 = vec[0]*vec[0];
            for (int j=1; j<roi; j++)
            {
                sum  += vec[j];
                sum2 += vec[j]*vec[j];

                if (vec[j]>vec[pos])
                    pos = j;
            }
            sum  /= roi;
            sum2 /= roi;

            *(ptr+0*1440+i) = sum;
            *(ptr+1*1440+i) = sqrt(sum2 - sum * sum);
            *(ptr+2*1440+i) = vec[pos];
            *(ptr+3*1440+i) = pos;
        }
    }

    static void GetPixelMax(float *max, const float *data, uint16_t roi, int32_t first, int32_t last)
    {
        if (roi==0 || first<0 || last<0 || first>=roi || last>=roi || last<first)
            return;

        for (int i=0; i<1440; i++)
        {
            const float *beg = data+i*roi+first;
            const float *end = data+i*roi+last;

            const float *pmax = beg;

            for (const float *ptr=beg+1; ptr<=end; ptr++)
                if (*ptr>*pmax)
                    pmax = ptr;

            max[i] = *pmax;
        }

    }

    const std::vector<int64_t> &GetSum() const { return fSum; }

    uint64_t GetNumEntries() const { return fNumEntries; }
};

struct DrsCalibration
{
    std::vector<int32_t> fOffset;
    std::vector<int64_t> fGain;
    std::vector<int64_t> fTrgOff;

    uint64_t fNumOffset;
    uint64_t fNumGain;
    uint64_t fNumTrgOff;

    uint32_t fStep;
    uint16_t fRoi;   // Region of interest for trgoff
    uint16_t fNumTm; // Number of time marker channels in trgoff

//    uint16_t fDAC[8];

    DrsCalibration() :
        fOffset  (1440*1024, 0),
        fGain    (1440*1024, 4096),
        fTrgOff  (1600*1024, 0),
        fNumOffset(1),
        fNumGain(2000),
        fNumTrgOff(1),
        fStep(0)
    {
    }

    void Clear()
    {
        // Default gain:
        // 0.575*[45590]*2.5V / 2^16 = 0.99999 V
        fOffset.assign(1440*1024, 0);
        fGain.assign  (1440*1024, 4096);
        fTrgOff.assign(1600*1024, 0);

        fNumOffset = 1;
        fNumGain   = 2000;
        fNumTrgOff = 1;

        fStep = 0;
    }

    std::string ReadFitsImp(const std::string &str, std::vector<float> &vec)
    {
        std::fits file(str);
        if (!file)
        {
            std::ostringstream msg;
            msg << "Could not open file " << str << ": " << strerror(errno);
            return msg.str();
        }

        if (file.GetStr("TELESCOP")!="FACT")
        {
            std::ostringstream msg;
            msg << "Reading " << str << " failed: Not a valid FACT file (TELESCOP not FACT in header)";
            return msg.str();
        }

        if (!file.HasKey("STEP"))
        {
            std::ostringstream msg;
            msg << "Reading " << str << " failed: Is not a DRS calib file (STEP not found in header)";
            return msg.str();
        }

        if (file.GetNumRows()!=1)
        {
            std::ostringstream msg;
            msg << "Reading " << str << " failed: Number of rows in table is not 1.";
            return msg.str();
        }

        fStep      = file.GetUInt("STEP");
        fNumOffset = file.GetUInt("NBOFFSET");
        fNumGain   = file.GetUInt("NBGAIN");
        fNumTrgOff = file.GetUInt("NBTRGOFF");
        fRoi       = file.GetUInt("NROI");
        fNumTm     = file.HasKey("NTM") ? file.GetUInt("NTM") : 0;
/*
        fDAC[0]    = file.GetUInt("DAC_A");
        fDAC[1]    = file.GetUInt("DAC_B");
        fDAC[4]    = file.GetUInt("DAC_C");
*/
        vec.resize(1440*1024*4 + (1440+fNumTm)*fRoi*2 + 4);

        float *base = vec.data();

        reinterpret_cast<uint32_t*>(base)[0] = fRoi;

        file.SetPtrAddress("RunNumberBaseline",      base+1, 1);
        file.SetPtrAddress("RunNumberGain",          base+2, 1);
        file.SetPtrAddress("RunNumberTriggerOffset", base+3, 1);
        file.SetPtrAddress("BaselineMean",           base+4+0*1024*1440,           1024*1440);
        file.SetPtrAddress("BaselineRms",            base+4+1*1024*1440,           1024*1440);
        file.SetPtrAddress("GainMean",               base+4+2*1024*1440,           1024*1440);
        file.SetPtrAddress("GainRms",                base+4+3*1024*1440,           1024*1440);
        file.SetPtrAddress("TriggerOffsetMean",      base+4+4*1024*1440,           fRoi*1440);
        file.SetPtrAddress("TriggerOffsetRms",       base+4+4*1024*1440+fRoi*1440, fRoi*1440);
        if (fNumTm>0)
        {
            file.SetPtrAddress("TriggerOffsetTMMean", base+4+4*1024*1440+ 2*fRoi*1440,              fRoi*fNumTm);
            file.SetPtrAddress("TriggerOffsetTMRms",  base+4+4*1024*1440+ 2*fRoi*1440+ fRoi*fNumTm, fRoi*fNumTm);
        }

        if (!file.GetNextRow())
        {
            std::ostringstream msg;
            msg << "Reading data from " << str << " failed.";
            return msg.str();
        }
/*
        fDAC[2] = fDAC[1];
        fDAC[4] = fDAC[1];

        fDAC[5] = fDAC[4];
        fDAC[6] = fDAC[4];
        fDAC[7] = fDAC[4];
*/
        fOffset.resize(1024*1440);
        fGain.resize(1024*1440);

        fTrgOff.resize(fRoi*(1440+fNumTm));

        // Convert back to ADC counts: 256/125 = 4096/2000
        // Convert back to sum (mean * num_entries)
        for (int i=0; i<1024*1440; i++)
        {
            fOffset[i] = fNumOffset         *256*base[i+1024*1440*0+4]/125;
            fGain[i]   = fNumOffset*fNumGain*256*base[i+1024*1440*2+4]/125;
        }

        for (int i=0; i<fRoi*1440; i++)
            fTrgOff[i] = fNumOffset*fNumTrgOff*256*base[i+1024*1440*4+4]/125;

        for (int i=0; i<fRoi*fNumTm; i++)
            fTrgOff[i+1440*fRoi] = fNumOffset*fNumTrgOff*256*base[i+1024*1440*4+2*fRoi*1440+4]/125;


        // DAC:  0..2.5V == 0..65535
        // V-mV: 1000
        //fNumGain *= 2500*50000;
        //for (int i=0; i<1024*1440; i++)
        //    fGain[i] *= 65536;
        if (fStep==0)
        {
            for (int i=0; i<1024*1440; i++)
                fGain[i] = fNumOffset*4096;
        }
        else
        {
            fNumGain *= 1953125;
            for (int i=0; i<1024*1440; i++)
                fGain[i] *= 1024;
        }

        // Now mark the stored DRS data as "officially valid"
        // However, this is not thread safe. It only ensures that
        // this data is not used before it is completely and correctly
        // read.
        fStep++;

        return std::string();
    }

    std::string ReadFitsImp(const std::string &str)
    {
        std::vector<float> vec;
        return ReadFitsImp(str, vec);
    }

    bool IsValid() { return fStep>2; }

    bool Apply(float *vec, const int16_t *val, const int16_t *start, uint32_t roi)
    {
        if (roi!=fRoi)
        {
            for (size_t ch=0; ch<1440; ch++)
            {
                const size_t pos = ch*roi;
                const size_t drs = ch*1024;

                DrsCalibrate::ApplyCh(vec+pos, val+pos, start[ch], roi,
                                      fOffset.data()+drs, fNumOffset,
                                      fGain.data()  +drs, fNumGain);
            }

            return false;
        }

        for (size_t ch=0; ch<1440; ch++)
        {
            const size_t pos = ch*fRoi;
            const size_t drs = ch*1024;

            DrsCalibrate::ApplyCh(vec+pos, val+pos, start[ch], roi,
                                  fOffset.data()+drs, fNumOffset,
                                  fGain.data()  +drs, fNumGain,
                                  fTrgOff.data()+pos, fNumTrgOff);
        }

        for (size_t ch=0; ch<fNumTm; ch++)
        {
            const size_t pos = (ch+1440)*fRoi;
            const size_t drs = (ch*9+8)*1024;

            DrsCalibrate::ApplyCh(vec+pos, val+pos, start[ch], roi,
                                  fOffset.data()+drs, fNumOffset,
                                  fGain.data()  +drs, fNumGain,
                                  fTrgOff.data()+pos, fNumTrgOff);
        }

        return true;
    }
};

#endif