// This file is part of LeanSDR Copyright (C) 2016-2018 <pabr@pabr.org>.
// See the toplevel README for more information.
//
// 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, see <http://www.gnu.org/licenses/>.

#ifndef LEANSDR_HDLC_H
#define LEANSDR_HDLC_H

#include "leansdr/framework.h"

namespace leansdr
{

// HDLC deframer

struct hdlc_dec
{

    hdlc_dec(
        int _minframesize, // Including CRC, excluding HDLC flags.
        int _maxframesize,
        bool _invert
    ) :
        minframesize(_minframesize),
        maxframesize(_maxframesize),
        invertmask(_invert ? 0xff : 0),
        framebuf(new u8[maxframesize]),
        debug(false)
    {
        reset();
    }

    ~hdlc_dec()
    {
        delete[] framebuf;
    }

    void reset()
    {
        shiftreg = 0;
        inframe = false;
    }

    void begin_frame()
    {
        framesize = 0;
        crc16 = crc16_init;
    }

    // Decode (*ppin)[count] as MSB-packed HDLC bitstream.
    // Return pointer to buffer[*pdatasize], or nullptr if no valid frame.
    // Return number of discarded bytes in *discarded.
    // Return number of checksum errors in *fcs_errors.
    // *ppin will have increased by at least 1 (unless count==0).

    u8 *decode(u8 **ppin, int count, int *pdatasize, int *hdlc_errors, int *fcs_errors)
    {
        *hdlc_errors = 0;
        *fcs_errors = 0;
        *pdatasize = -1;
        u8 *pin = *ppin, *pend = pin + count;

        for (; pin < pend; ++pin)
        {
            u8 byte_in = (*pin) ^ invertmask;

            for (int bits = 8; bits--; byte_in <<= 1)
            {
                u8 bit_in = byte_in & 128;
                shiftreg = (shiftreg >> 1) | bit_in;

                if (!inframe)
                {
                    if (shiftreg == 0x7e)
                    { // HDLC flag 01111110
                        inframe = true;
                        nbits_out = 0;
                        begin_frame();
                    }
                }
                else
                {
                    if ((shiftreg & 0xfe) == 0x7c)
                    { // 0111110x HDLC stuffing
                        // Unstuff this 0
                    }
                    else if (shiftreg == 0x7e)
                    { // 01111110 HDLC flag
                        if (nbits_out != 7)
                        {
                            // Not at byte boundary
                            if (debug) {
                                fprintf(stderr, "^");
                            }

                            ++*hdlc_errors;
                        }
                        else
                        {
                            // Checksum
                            crc16 ^= 0xffff;

                            if (framesize < 2 || framesize < minframesize || crc16 != crc16_check)
                            {
                                if (debug) {
                                    fprintf(stderr, "!");
                                }

                                ++*hdlc_errors;

                                // Do not report random noise as FCS errors
                                if (framesize >= minframesize) {
                                    ++*fcs_errors;
                                }
                            }
                            else
                            {
                                if (debug) {
                                    fprintf(stderr, "_");
                                }

                                // This will trigger output, but we finish the byte first.
                                *pdatasize = framesize - 2;
                            }
                        }

                        nbits_out = 0;
                        begin_frame();
                        // Keep processing up to 7 remaining bits from byte_in.
                        // Special cases 0111111 and 1111111 cannot affect *pdatasize.
                    }
                    else if (shiftreg == 0xfe)
                    { // 11111110 HDLC invalid
                        if (framesize)
                        {
                            if (debug) {
                                fprintf(stderr, "^");
                            }

                            ++*hdlc_errors;
                        }

                        inframe = false;
                    }
                    else
                    {                                        // Data bit
                        byte_out = (byte_out >> 1) | bit_in; // HDLC is LSB first
                        ++nbits_out;

                        if (nbits_out == 8)
                        {
                            if (framesize < maxframesize)
                            {
                                framebuf[framesize++] = byte_out;
                                crc16_byte(byte_out);
                            }

                            nbits_out = 0;
                        }
                    }
                } // inframe
            }     // bits

            if (*pdatasize != -1)
            {
                // Found a complete frame
                *ppin = pin + 1;
                return framebuf;
            }
        }

        *ppin = pin;
        return nullptr;
    }

  private:
    // Config
    int minframesize, maxframesize;
    u8 invertmask;
    u8 *framebuf; // [maxframesize]
    // State
    u8 shiftreg;   // Input bitstream
    bool inframe;  // Currently receiving a frame ?
    u8 byte_out;   // Accumulator for data bits
    int nbits_out; // Number of data bits in byte_out
    int framesize; // Number of bytes in framebuf, if inframe
    u16 crc16;     // CRC of framebuf[framesize]
    // CRC
    static const u16 crc16_init = 0xffff;
    static const u16 crc16_poly = 0x8408; // 0x1021 MSB-first
    static const u16 crc16_check = 0x0f47;

    void crc16_byte(u8 data)
    {
        crc16 ^= data;

        for (int bit = 8; bit--;) {
            crc16 = (crc16 & 1) ? (crc16 >> 1) ^ crc16_poly : (crc16 >> 1);
        }
    }

  public:
    bool debug;
};
// hdlc_dec

// HDLC synchronizer with polarity detection

struct hdlc_sync : runnable
{
    hdlc_sync(
        scheduler *sch, pipebuf<u8> &_in, // Packed bits
        pipebuf<u8> &_out,                // Bytes
        int _minframesize,                // Including CRC, excluding HDLC flags.
        int _maxframesize,
        // Status
        pipebuf<int> *_lock_out = nullptr,
        pipebuf<int> *_framecount_out = nullptr,
        pipebuf<int> *_fcserrcount_out = nullptr,
        pipebuf<int> *_hdlcbytecount_out = nullptr,
        pipebuf<int> *_databytecount_out = nullptr
    ) :
        runnable(sch, "hdlc_sync"),
        minframesize(_minframesize),
        maxframesize(_maxframesize),
        chunk_size(maxframesize + 2),
        in(_in),
        out(_out, _maxframesize + chunk_size),
        lock_out(opt_writer(_lock_out)),
        framecount_out(opt_writer(_framecount_out)),
        fcserrcount_out(opt_writer(_fcserrcount_out)),
        hdlcbytecount_out(opt_writer(_hdlcbytecount_out)),
        databytecount_out(opt_writer(_databytecount_out)),
        cur_sync(0),
        resync_phase(0),
        lock_state(false),
        resync_period(32),
        header16(false)
    {
        for (int s = 0; s < NSYNCS; ++s)
        {
            syncs[s].dec = new hdlc_dec(minframesize, maxframesize, s != 0);

            for (int h = 0; h < NERRHIST; ++h) {
                syncs[s].errhist[h] = 0;
            }
        }

        syncs[cur_sync].dec->debug = sch->debug;
        errslot = 0;
    }

    void run()
    {
        if (!opt_writable(lock_out) ||
            !opt_writable(framecount_out) ||
            !opt_writable(fcserrcount_out) ||
            !opt_writable(hdlcbytecount_out) ||
            !opt_writable(databytecount_out))
        {
            return;
        }

        bool previous_lock_state = lock_state;
        int fcserrcount = 0, framecount = 0;
        int hdlcbytecount = 0, databytecount = 0;

        // Note: hdlc_dec may already hold one frame ready for output.
        while ((long)in.readable() >= chunk_size && (long)out.writable() >= maxframesize + chunk_size)
        {
            if (!resync_phase)
            {
                // Once every resync_phase, try all decoders
                for (int s = 0; s < NSYNCS; ++s)
                {
                    if (s != cur_sync) {
                        syncs[s].dec->reset();
                    }

                    syncs[s].errhist[errslot] = 0;

                    for (u8 *pin = in.rd(), *pend = pin + chunk_size; pin < pend;)
                    {
                        int datasize, hdlc_errors, fcs_errors;
                        u8 *f = syncs[s].dec->decode(&pin, pend - pin, &datasize, &hdlc_errors, &fcs_errors);
                        syncs[s].errhist[errslot] += hdlc_errors;

                        if (s == cur_sync)
                        {
                            if (f)
                            {
                                lock_state = true;
                                output_frame(f, datasize);
                                databytecount += datasize;
                                ++framecount;
                            }

                            fcserrcount += fcs_errors;
                            framecount += fcs_errors;
                        }
                    }
                }

                errslot = (errslot + 1) % NERRHIST;
                // Switch to another sync option ?
                // Compare total error counts over about NERRHIST frames.
                int total_errors[NSYNCS];

                for (int s = 0; s < NSYNCS; ++s)
                {
                    total_errors[s] = 0;

                    for (int h = 0; h < NERRHIST; ++h) {
                        total_errors[s] += syncs[s].errhist[h];
                    }
                }

                int best = cur_sync;

                for (int s = 0; s < NSYNCS; ++s)
                {
                    if (total_errors[s] < total_errors[best]) {
                        best = s;
                    }
                }

                if (best != cur_sync)
                {
                    lock_state = false;

                    if (sch->debug) {
                        fprintf(stderr, "[%d:%d->%d:%d]", cur_sync, total_errors[cur_sync], best, total_errors[best]);
                    }

                    // No verbose messages on candidate syncs
                    syncs[cur_sync].dec->debug = false;
                    cur_sync = best;
                    syncs[cur_sync].dec->debug = sch->debug;
                }
            }
            else
            {
                // Use only the currently selected decoder
                for (u8 *pin = in.rd(), *pend = pin + chunk_size; pin < pend;)
                {
                    int datasize, hdlc_errors, fcs_errors;
                    u8 *f = syncs[cur_sync].dec->decode(&pin, pend - pin, &datasize, &hdlc_errors, &fcs_errors);

                    if (f)
                    {
                        lock_state = true;
                        output_frame(f, datasize);
                        databytecount += datasize;
                        ++framecount;
                    }

                    fcserrcount += fcs_errors;
                    framecount += fcs_errors;
                }
            } // resync_phase

            in.read(chunk_size);
            hdlcbytecount += chunk_size;

            if (++resync_phase >= resync_period) {
                resync_phase = 0;
            }
        } // Work to do

        if (lock_state != previous_lock_state) {
            opt_write(lock_out, lock_state ? 1 : 0);
        }

        opt_write(framecount_out, framecount);
        opt_write(fcserrcount_out, fcserrcount);
        opt_write(hdlcbytecount_out, hdlcbytecount);
        opt_write(databytecount_out, databytecount);
    }

  private:
    void output_frame(u8 *f, int size)
    {
        if (header16)
        {
            // Removed 16-bit CRC, add 16-bit prefix -> Still <= maxframesize.
            out.write(size >> 8);
            out.write(size & 255);
        }

        memcpy(out.wr(), f, size);
        out.written(size);
        opt_write(framecount_out, 1);
    }

    int minframesize, maxframesize;
    int chunk_size;
    pipereader<u8> in;
    pipewriter<u8> out;
    pipewriter<int> *lock_out;
    pipewriter<int> *framecount_out, *fcserrcount_out;
    pipewriter<int> *hdlcbytecount_out, *databytecount_out;
    static const int NSYNCS = 2;   // Two possible polarities
    static const int NERRHIST = 2; // Compare error counts over two frames

    struct
    {
        hdlc_dec *dec;
        int errhist[NERRHIST];
    } syncs[NSYNCS];

    int errslot;
    int cur_sync;
    int resync_phase;
    bool lock_state;

  public:
    int resync_period;
    bool header16; // Output length prefix
}; // hdlc_sync

} // namespace leansdr

#endif // LEANSDR_HDLC_H