#ifndef FACT_HeadersFTM
#define FACT_HeadersFTM

#include <string.h>
#include <algorithm>
#include <arpa/inet.h>
#include <stdexcept>
#include <typeinfo>
#include <vector>

// For debugging
#include <iostream>

namespace Header
{
    template<typename S>
        void hton(S &s)
    {
        std::transform(reinterpret_cast<uint16_t*>(&s),
                       reinterpret_cast<uint16_t*>(&s)+sizeof(S)/2,
                       reinterpret_cast<uint16_t*>(&s),
                       htons);
    }

    template<typename S>
        void ntoh(S &s)
    {
        std::transform(reinterpret_cast<uint16_t*>(&s),
                       reinterpret_cast<uint16_t*>(&s)+sizeof(S)/2,
                       reinterpret_cast<uint16_t*>(&s),
                       ntohs);
    }

    template<typename S>
        S NtoH(const S &s)
    {
        S ret(s);
        ntoh(ret);
        return ret;
    }

    template<typename S>
        S HtoN(const S &s)
    {
        S ret(s);
        hton(ret);
        return ret;
    }

    template<typename S>
        void reset(S &s)
    {
        memset(&s, 0, sizeof(S));
    }

    template<typename S>
        void init(S &s)
    {
        if (sizeof(S)%2!=0)
            throw std::logic_error("size of "+std::string(typeid(S).name())+" not a multiple of 2.");

        reset(s);
    }

    template<typename S>
        void ntohcpy(const std::vector<uint16_t> &vec, S &s)
    {
        if (sizeof(S)!=vec.size()*2)
            throw std::logic_error("size of vector mismatch "+std::string(typeid(S).name()));

        std::transform(vec.begin(), vec.end(),
                       reinterpret_cast<uint16_t*>(&s), ntohs);
    }

    template<typename S>
        std::vector<uint16_t> htoncpy(const S &s)
    {
        if (sizeof(S)%2)
            throw std::logic_error("size of "+std::string(typeid(S).name())+" not a multiple of 2");

        std::vector<uint16_t> v(sizeof(S)/2);

        std::transform(reinterpret_cast<const uint16_t*>(&s),
                       reinterpret_cast<const uint16_t*>(&s)+sizeof(S)/2,
                       v.begin(), htons);

        return v;
    }

    template<typename T, typename S>
    void bitcpy(T *target, size_t ntarget, const S *source, size_t nsource, size_t ss=0, size_t ts=0)
    {
        const size_t targetsize = ss==0 ? sizeof(T) : std::min(ss, sizeof(T));
        const size_t sourcesize = ts==0 ? sizeof(S) : std::min(ts, sizeof(S));

        const S *const ends = source + nsource;
        const T *const endt = target + ntarget;

        const S *s = source;
              T *t = target;

        memset(t, 0, sizeof(T)*ntarget);

        size_t targetpos = 0;
        size_t sourcepos = 0;

        while (s<ends && t<endt)
        {
            // Start filling with "source size" - "position" bits
            *t |= (*s>>sourcepos)<<targetpos;

            // If not all bits fitted into targetsize the number
            // of copied bits is the number of bits which fitted
            const int n = std::min(sourcesize-sourcepos, targetsize-targetpos);

            targetpos += n;
            sourcepos += n;

            if (sourcepos>=sourcesize)
                s++;

            if (targetpos>=targetsize)
                t++;

            sourcepos %= sourcesize;
            targetpos %= targetsize;
        }
    }

}

// ====================================================================

#include <ostream>

namespace FTM
{
    using namespace Header;

    enum States
    {
        kDisconnected = 1,
        kConnected,
        kIdle,
        kTakingData,

        // FTM internal states
        kFtmIdle    = 1,
        kFtmConfig  = 2,
        kFtmRunning = 3,
        kFtmCalib   = 4,
    };

    enum Commands
    {
        kCmdRead           = 0x0001,
        kCmdWrite          = 0x0002,
        kCmdStartRun       = 0x0004,
        kCmdStopRun        = 0x0008,
        kCmdPing           = 0x0010,
        kCmdCrateReset     = 0x0020,
        kCmdDisableReports = 0x0040,
        kCmdToggleLed      = 0xc000,

        kWriteStaticData   = 0x0001,
        kWriteRegister     = 0x0004,

        kReadStaticData    = 0x0001,
        kReadDynamicData   = 0x0002,
        kReadRegister      = 0x0004,

        kStartRun          = 0x0001,
        kTakeNevents       = 0x0002,
    };

    enum Types
    {
        kHeader      = 0,   // Local extension to identify a header in fCounter
        kStaticData  = 1,
        kDynamicData = 2,
        kFtuList     = 3,
        kErrorList   = 4,
        kRegister    = 5,
    };

    // --------------------------------------------------------------------

    template<typename T>
        void Reverse(T *t)
    {
        std::reverse((uint16_t*)t, ((uint16_t*)t)+sizeof(T)/2);
    }

    enum
    {
        kDelimiterStart = 0xfb01,
        kDelimiterEnd   = 0x04fe
    };

    struct Header
    {
        uint16_t fDelimiter;        /// Start delimiter
        uint16_t fType;             /// Type of the data to be received after the header
        uint16_t fDataSize;         /// Size in words to be received after the header
        uint16_t fState;            /// State of the FTM state machine
        uint64_t fBoardId;
        uint16_t fFirmwareId;
        uint32_t fTriggerCounter;
        uint64_t fTimeStamp;

        Header() { init(*this); }

        std::vector<uint16_t> HtoN() const
        {
            Header h(*this);

            Reverse(&h.fBoardId);
            Reverse(&h.fTriggerCounter);
            Reverse(&h.fTimeStamp);

            return htoncpy(h);
        }
        void operator=(const std::vector<uint16_t> &vec)
        {
            ntohcpy(vec, *this);

            Reverse(&fBoardId);
            Reverse(&fTriggerCounter);
            Reverse(&fTimeStamp);
        }

        void clear() { reset(*this); }
        void print(std::ostream &out) const;

    } __attribute__((__packed__));

    struct DimPassport
    {
        uint64_t fBoardId;
        uint16_t fFirmwareId;

        DimPassport(const Header &h) :
            fBoardId(h.fBoardId),
            fFirmwareId(h.fFirmwareId)
        {
        }
    } __attribute__((__packed__));

    struct DimTriggerCounter
    {
        uint64_t fTimeStamp;
        uint32_t fTriggerCounter;

        DimTriggerCounter(const Header &h) :
            fTimeStamp(h.fTimeStamp),
            fTriggerCounter(h.fTriggerCounter)
        {
        }
    } __attribute__((__packed__));


    struct StaticDataBoard
    {
        uint16_t fEnable[4];   // 4x9bits
        uint16_t fDAC[5];      // 4x12bit + 1xN/40
        uint16_t fPrescaling;

        StaticDataBoard() { init(*this); }

        void print(std::ostream &out) const;

    } __attribute__((__packed__));

    struct StaticData
    {
        enum
        {
            kMaxCoincidence     = 40,
            kMaxWindow          = 0xf,
            kMaxDeadTime        = 0xffff,
            kMaxDelayTimeMarker = 0x3ff,
            kMaxDelayTrigger    = 0x3ff,
            kMaxTriggerInterval = 0x3ff,
            kMaskSequence       = 0x7ff,
            kMaskSettings       = 0xf,
            kMaskLEDs           = 0xf,
        };

        enum GeneralSettings
        {
            kTrigger    = 0x80,
            kPedestal   = 0x40,
            kLPext      = 0x20,
            kLPint      = 0x10,
            kExt2       = 0x08,
            kExt1       = 0x04,
            kVeto       = 0x02,
            kTimeMarker = 0x01,
        };

        uint16_t fGeneralSettings;         // only 8 bit used
        uint16_t fStatusLEDs;              // only 8 bit used
        uint16_t fTriggerInterval;         // only 10 bit used
        uint16_t fTriggerSequence;         // 3x5bit
        uint64_t fDummy0;
        uint16_t fCoincidencePhysics;
        uint16_t fCoincidenceCalib;
        uint16_t fDelayTrigger;
        uint16_t fDelayTimeMarker;
        uint16_t fDeadTime;
        uint32_t fClockConditioner[8];  // R0, R1, R8, R9, R11, R13, R14, R15
        uint16_t fWindowPhysics;
        uint16_t fWindowCalib;
        uint16_t fDummy1;

        StaticDataBoard fBoard[4][10];      // 4 crates * 10 boards

        uint16_t fActiveFTU[4];             // 4 crates * 10 bits

        StaticData() { init(*this); }

        std::vector<uint16_t> HtoN() const
        {
            StaticData d(*this);
            for (int i=0; i<8; i++)
                Reverse(d.fClockConditioner+i);

            return htoncpy(d);
        }

        void operator=(const std::vector<uint16_t> &vec)
        {
            ntohcpy(vec, *this);

            for (int i=0; i<8; i++)
                Reverse(fClockConditioner+i);
        }

        void clear() { reset(*this); }
        void print(std::ostream &out) const;

        StaticDataBoard &operator[](int i) { return fBoard[i/10][i%10]; }
        const StaticDataBoard &operator[](int i) const { return fBoard[i/10][i%10]; }

        void EnableFTU(int i)  { fActiveFTU[i/10] |=  (1<<(i%10)); }
        void DisableFTU(int i) { fActiveFTU[i/10] &= ~(1<<(i%10)); }

        void EnableAllFTU()    { for (int i=0; i<4; i++) fActiveFTU[i] = 0x3ff; }
        void DisableAllFTU()   { for (int i=0; i<4; i++) fActiveFTU[i] = 0;     }

        void ToggleFTU(int i)  { fActiveFTU[i/10] ^= (1<<(i%10)); }

    } __attribute__((__packed__));

    // DimStructures must be a multiple of two... I don't know why
    struct DimStaticData
    {
        uint64_t fTimeStamp;
        //8
        uint16_t fGeneralSettings;         // only 8 bit used
        uint16_t fStatusLEDs;              // only 8 bit used
        uint64_t fActiveFTU;               // 40 bits in row
        //20
        uint16_t fTriggerInterval;         // only 10 bit used
        //22
        uint16_t fTriggerSeqLPint;         // only 5bits used
        uint16_t fTriggerSeqLPext;         // only 5bits used
        uint16_t fTriggerSeqPed;           // only 5bits used
        //28
        uint16_t fCoincidencePhysics;      // 0-40
        uint16_t fCoincidenceCalib;        // 0-40
        //32
        uint16_t fWindowPhysics;
        uint16_t fWindowCalib;
        //36
        uint16_t fDelayTrigger;
        uint16_t fDelayTimeMarker;
        uint32_t fDeadTime;
        //44
        uint16_t fClockConditioner[8];
        //60
        uint16_t fEnable[80];  // 160*9bit = 180byte
        uint16_t fThreshold[160];
        uint16_t fMultiplicity[40];     // N out of 4
        uint16_t fPrescaling[40];
        // 640+60 = 700

        bool HasTrigger() const     { return fGeneralSettings & StaticData::kTrigger; }
        bool HasPedestal() const    { return fGeneralSettings & StaticData::kPedestal; }
        bool HasLPext() const       { return fGeneralSettings & StaticData::kLPext; }
        bool HasLPint() const       { return fGeneralSettings & StaticData::kLPint; }
        bool HasExt2() const        { return fGeneralSettings & StaticData::kExt2; }
        bool HasExt1() const        { return fGeneralSettings & StaticData::kExt1; }
        bool HasVeto() const        { return fGeneralSettings & StaticData::kVeto; }
        bool HasTimeMarker() const  { return fGeneralSettings & StaticData::kTimeMarker; }

        bool IsActive(int i) const { return fActiveFTU&(uint64_t(1)<<i); }
        bool IsEnabled(int i) const { return fEnable[i/16]&(1<<(i%16)); }

        DimStaticData() { memset(this, 0, sizeof(DimStaticData)); }

        DimStaticData(const Header &h, const StaticData &d) :
            fTimeStamp(h.fTimeStamp),
            fGeneralSettings(d.fGeneralSettings),
            fStatusLEDs(d.fStatusLEDs),
            fActiveFTU( uint64_t(d.fActiveFTU[0])      |
                       (uint64_t(d.fActiveFTU[1])<<10) |
                       (uint64_t(d.fActiveFTU[2])<<20) |
                       (uint64_t(d.fActiveFTU[3])<<30)),
            fTriggerInterval(d.fTriggerInterval),
            fTriggerSeqLPint((d.fTriggerSequence)&0x1f),
            fTriggerSeqLPext((d.fTriggerSequence>>5)&0x1f),
            fTriggerSeqPed((d.fTriggerSequence>>10)&0x1f),
            fCoincidencePhysics(d.fCoincidencePhysics),
            fCoincidenceCalib(d.fCoincidenceCalib),
            fWindowPhysics(d.fWindowPhysics*4+8),
            fWindowCalib(d.fWindowCalib*4+8),
            fDelayTrigger(d.fDelayTrigger*4+8),
            fDelayTimeMarker(d.fDelayTimeMarker*4+8),
            fDeadTime(uint32_t(d.fDeadTime)*4+8)
        {
            memcpy(fClockConditioner, d.fClockConditioner, sizeof(uint16_t)*8);

            uint16_t src[160];
            for (int i=0; i<40; i++)
            {
                for (int j=0; j<4; j++)
                {
                    src[i*4+j] = d[i].fEnable[j];
                    fThreshold[i*4+j] = d[i].fDAC[j];
                }

                fMultiplicity[i] = d[i].fDAC[4];
                fPrescaling[i] = d[i].fPrescaling;
            }
            bitcpy(fEnable, 80, src, 160, 9);
        }

    } __attribute__((__packed__));


    struct DynamicDataBoard
    {
        uint32_t fRatePatch[4];   // Patch 0,1,2,3
        uint32_t fRateTotal;      // Sum

        uint16_t fOverflow;       // Patches: bits 0-3, total 4
        uint16_t fCrcError;

        void print(std::ostream &out) const;

        void reverse()
        {
            for (int i=0; i<4; i++)
                Reverse(fRatePatch+i);

            Reverse(&fRateTotal);
        }

        uint32_t &operator[](int i) { return fRatePatch[i]; }

    }  __attribute__((__packed__));


    struct DynamicData
    {
        uint64_t fOnTimeCounter;
        uint16_t fTempSensor[4];  // U45, U46, U48, U49

        DynamicDataBoard fBoard[4][10];      // 4 crates * 10 boards

        DynamicData() { init(*this); }

        std::vector<uint16_t> HtoN() const
        {
            DynamicData d(*this);

            Reverse(&d.fOnTimeCounter);

            for (int c=0; c<4; c++)
                for (int b=0; b<10; b++)
                    d.fBoard[c][b].reverse();

            return htoncpy(d);
        }

        void operator=(const std::vector<uint16_t> &vec)
        {
            ntohcpy(vec, *this);

            Reverse(&fOnTimeCounter);

            for (int c=0; c<4; c++)
                for (int b=0; b<10; b++)
                    fBoard[c][b].reverse();
        }

        void clear() { reset(*this); }
        void print(std::ostream &out) const;

        DynamicDataBoard &operator[](int i) { return fBoard[i/10][i%10]; }
        const DynamicDataBoard &operator[](int i) const { return fBoard[i/10][i%10]; }

    } __attribute__((__packed__));


    struct DimDynamicData
    {
        uint64_t fTimeStamp;

        uint64_t fOnTimeCounter;
        float    fTempSensor[4];

        uint32_t fRatePatch[160];

        uint32_t fRateBoard[40];
        uint16_t fRateOverflow[40];

        uint16_t fCrcError[40];

        DimDynamicData(const Header &h, const DynamicData &d) :
            fTimeStamp(h.fTimeStamp),
            fOnTimeCounter(d.fOnTimeCounter)
        {
            for (int i=0; i<4; i++)
                fTempSensor[i] = d.fTempSensor[i];

            for (int i=0; i<40; i++)
            {
                fRateBoard[i]    = d[i].fRateTotal;
                fRateOverflow[i] = d[i].fOverflow;
                fCrcError[i]     = d[i].fCrcError;
                for (int j=0; j<4; j++)
                    fRatePatch[i*4+j] = d[i].fRatePatch[j];
            }
        }

    } __attribute__((__packed__));


    struct FtuResponse
    {
        uint16_t fPingAddr;       // Number of Pings and addr (pings= see error)
        uint64_t fDNA;
        uint16_t fErrorCounter;   //

        void reverse() { Reverse(&fDNA); }

        void print(std::ostream &out) const;

    } __attribute__((__packed__));

    struct FtuList
    {
        uint16_t fNumBoards;         /// Total number of boards responded
        uint16_t fNumBoardsCrate[4]; /// Num of board responded in crate 0-3
        uint16_t fActiveFTU[4];      /// List of active FTU boards in crate 0-3

        FtuResponse fFTU[4][10];

        FtuList() { init(*this); }

        std::vector<uint16_t> HtoN() const
        {
            FtuList d(*this);

            for (int c=0; c<4; c++)
                for (int b=0; b<10; b++)
                    d.fFTU[c][b].reverse();

            return htoncpy(d);
        }

        void operator=(const std::vector<uint16_t> &vec)
        {
            ntohcpy(vec, *this);

            for (int c=0; c<4; c++)
                for (int b=0; b<10; b++)
                    fFTU[c][b].reverse();
        }

        void clear() { reset(*this); }
        void print(std::ostream &out) const;

        FtuResponse &operator[](int i) { return fFTU[i/10][i%10]; }
        const FtuResponse &operator[](int i) const { return fFTU[i/10][i%10]; }

    } __attribute__((__packed__));

    struct DimFtuList
    {
        uint64_t fTimeStamp;
        uint64_t fActiveFTU;

        uint16_t fNumBoards;          /// Number of boards answered in total
        uint8_t  fNumBoardsCrate[4];  /// Number of boards answered per crate

        uint64_t fDNA[40];            /// DNA of FTU board
        uint8_t  fAddr[40];           /// Address of FTU board
        uint8_t  fPing[40];           /// Number of pings until response (same as in Error)

        DimFtuList(const Header &h, const FtuList &d) :
            fTimeStamp(h.fTimeStamp),
            fActiveFTU( uint64_t(d.fActiveFTU[0])      |
                       (uint64_t(d.fActiveFTU[1])<<10) |
                       (uint64_t(d.fActiveFTU[2])<<20) |
                       (uint64_t(d.fActiveFTU[3])<<30)),
            fNumBoards(d.fNumBoards)
        {
            for (int i=0; i<4; i++)
                fNumBoardsCrate[i] = d.fNumBoardsCrate[i];

            for (int i=0; i<40; i++)
            {
                fDNA[i]  =  d[i].fDNA;
                fAddr[i] =  d[i].fPingAddr&0x3f;
                fPing[i] = (d[i].fPingAddr>>8)&0x3;
            }
        }

        bool IsActive(int i) const { return fActiveFTU&(uint64_t(1)<<i); }

    } __attribute__((__packed__));


    struct Error
    {
        uint16_t fNumCalls;   // 0=error, >1 needed repetition but successfull

        uint16_t fDelimiter;
        uint16_t fDestAddress;
        uint16_t fSrcAddress;
        uint16_t fFirmwareId;
        uint16_t fCommand;
        uint16_t fData[21];
        uint16_t fCrcErrorCounter;
        uint16_t fCrcCheckSum;

        Error() { init(*this); }

        std::vector<uint16_t> HtoN() const
        {
            return htoncpy(*this);
        }

        void operator=(const std::vector<uint16_t> &vec) { ntohcpy(vec, *this); }

        void clear() { reset(*this); }

        uint16_t &operator[](int idx) { return fData[idx]; }
        const uint16_t &operator[](int idx) const { return fData[idx]; }

        void print(std::ostream &out) const;

    } __attribute__((__packed__));

    struct DimError
    {
        uint64_t fTimeStamp;
        Error    fError;

        DimError(const Header &h, const Error &e) :
            fTimeStamp(h.fTimeStamp),
            fError(e)
        {
            fError.fDestAddress = (e.fDestAddress&0x3)*10 + ((e.fDestAddress>>2)&0xf);
            fError.fSrcAddress  = (e.fSrcAddress &0x3)*10 + ((e.fSrcAddress >>2)&0xf);
        }

    }  __attribute__((__packed__));

    /*
    struct Command
    {
        uint16_t fStartDelimiter;
        uint16_t fCommand;
        uint16_t fParam[3];

        Command() { init(*this); }

        void HtoN() { hton(*this); }
        void NtoH() { ntoh(*this); }

        void operator=(const std::vector<uint16_t> &vec) { ntohcpy(vec, *this); }

        void clear() { reset(*this); }


     } __attribute__((__packed__));
    */

    // --------------------------------------------------------------------

    inline std::ostream &operator<<(std::ostream &out, const FtuResponse &h)
    {
        h.print(out);
        return out;
    }

    inline std::ostream &operator<<(std::ostream &out, const Header &h)
    {
        h.print(out);
        return out;
    }


    inline std::ostream &operator<<(std::ostream &out, const FtuList &h)
    {
        h.print(out);
        return out;
    }

    inline std::ostream &operator<<(std::ostream &out, const DynamicDataBoard &h)
    {
        h.print(out);
        return out;
    }

    inline std::ostream &operator<<(std::ostream &out, const DynamicData &h)
    {
        h.print(out);
        return out;
    }

    inline std::ostream &operator<<(std::ostream &out, const StaticDataBoard &h)
    {
        h.print(out);
        return out;
    }

    inline std::ostream &operator<<(std::ostream &out, const StaticData &h)
    {
        h.print(out);
        return out;
    }

    inline std::ostream &operator<<(std::ostream &out, const Error &h)
    {
        h.print(out);
        return out;
    }
};

#endif