#include <ctime>

#include <QCanBus>
#include <QCommandLineParser>
#include <QCoreApplication>
#include <QDateTime>
#include <QDebug>
#include <QElapsedTimer>
#include <QFile>
#include <QRegularExpression>
#include <QSharedPointer>
#include <QString>
#include <QStringList>
#include <QTextStream>

constexpr quint8 syncByte = 0xA5;
constexpr int headerMsgIdOffset = 3;

int main(int argc, char *argv[])
{
    QCoreApplication app(argc, argv);
    app.setApplicationName("CANDumpPlayer");
    app.setApplicationVersion("1.0");

    QCommandLineParser parser;
    parser.setApplicationDescription("Replays a candump log file onto a CAN interface.");
    parser.addHelpOption();
    parser.addVersionOption();
    parser.addPositionalArgument("can_interface", "CAN device (e.g. can0). Not required with --test.");
    parser.addPositionalArgument("candump_file", "Input CAN dump file.");

    QCommandLineOption speedOption({"s", "speed"},
                                   "Replay speed multiplier (float). 0 = immediate (default), 1 = real-time, "
                                   "<N> = <N>x faster than real-time.",
                                   "speed", "0");
    parser.addOption(speedOption);

    QCommandLineOption testOption({"t", "test"},
                                  "Test mode: skip CAN interface, print each frame with calculated and actual "
                                  "time deltas. Pass the candump file as the only positional argument.");
    parser.addOption(testOption);

    parser.process(app);

    const bool testMode = parser.isSet(testOption);

    bool speedOk = false;
    const double speed = parser.value(speedOption).toDouble(&speedOk);
    if (!speedOk || speed < 0.0) {
        qCritical().noquote() << "ERROR: --speed must be a non-negative floating point number.";
        return 1;
    }

    const QStringList args = parser.positionalArguments();
    const int expectedArgs = testMode ? 1 : 2;
    if (args.length() != expectedArgs) {
        qCritical().noquote() << Qt::endl
                              << QString("ERROR: incorrect number of arguments (expected %1, but received %2).")
                                     .arg(expectedArgs)
                                     .arg(args.length())
                              << Qt::endl;
        parser.showHelp(1);
        return 1;
    }

    QSharedPointer<QCanBusDevice> canDevice;
    if (!testMode) {
        QString error;
        canDevice.reset(QCanBus::instance()->createDevice(QStringLiteral("socketcan"), args.at(0), &error));
        if (!canDevice) {
            qCritical().noquote()
                << QString("ERROR: could not open CAN device %1 (error=%2)").arg(args.at(0)).arg(error);
            return 1;
        }
        canDevice->setConfigurationParameter(QCanBusDevice::CanFdKey, false);
        canDevice->setConfigurationParameter(QCanBusDevice::BitRateKey, 250000);
        canDevice->connectDevice();
    }

    const QString canFile = testMode ? args.at(0) : args.at(1);
    QFile file(canFile);
    if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
        qCritical().noquote() << QString("ERROR: could not open input CAN dump file %1").arg(canFile);
        return 1;
    }

    QTextStream stream(&file);
    QString line;
    const QRegularExpression regexEntry(
        "^\\s*\\((?<timestamp>.+)\\)\\s+(?<device>\\S+)\\s+(?<can_id>\\S+)\\s+\\[(?<size>\\S+)\\]\\s+(?<payload>.*)$"
    );
    const QRegularExpression regexPayload("(?:\\s*)(\\S+)");

    // Candump files may use either Unix epoch ("1234567890.123456") or
    // wall-clock ("YYYY-MM-DD HH:MM:SS.ffffff") timestamp format.
    // Parse both and normalise to microseconds since epoch as qint64.
    const QString datetimeFormat = QStringLiteral("yyyy-MM-dd HH:mm:ss.zzz");

    auto parseTimestampUs = [&](const QString &raw) -> qint64 {
        // Try wall-clock format first (contains a space).
        if (raw.contains(' ')) {
            // QDateTime::fromString only handles millisecond precision with 'zzz'.
            // Truncate the fractional part to 3 digits for parsing, then recover
            // the full microseconds from the original string.
            const int dotPos = raw.lastIndexOf('.');
            const QString msRaw = raw.left(dotPos + 4);  // "YYYY-MM-DD HH:MM:SS.mmm"
            const QDateTime dt = QDateTime::fromString(msRaw, datetimeFormat);
            if (!dt.isValid()) {
                return -1;
            }
            const qint64 subSecUs = raw.mid(dotPos + 1).leftJustified(6, '0').left(6).toLongLong();
            return dt.toMSecsSinceEpoch() / 1000 * 1'000'000 + subSecUs;
        }
        // Unix epoch format: parse as double then convert.
        bool ok = false;
        const double epochSec = raw.toDouble(&ok);
        return ok ? static_cast<qint64>(epochSec * 1'000'000) : -1;
    };

    // Record the monotonic clock origin at the start of replay.
    // Use origin + (logOffset / speed) for frame replay time to prevent accumulating drift.
    struct timespec replayOrigin;
    clock_gettime(CLOCK_MONOTONIC, &replayOrigin);

    qint64 firstTimestampUs = -1;
    qint64 lastTimestampUs = -1;
    qint64 prevTimestampUs = -1;
    QElapsedTimer wallTimer;
    unsigned int lineCount = 1;

    quint64 playedCount = 0;
    qint64 totalDeltaNs = 0;
    qint64 avgDeltaNs = 0;
    QElapsedTimer replayTimer;
    replayTimer.start();

    quint64 messageCount = 0;
    QElapsedTimer msgTimer;
    qint64 msgTotalDeltaNs = 0;
    qint64 msgAvgDeltaNs = 0;
    qint64 msgDeltaNs = 0;
    qint64 msgCalcDeltaUs = 0;
    qint64 msgPrevTimestampUs = -1;
    unsigned int msgId = 0;

    while (stream.readLineInto(&line)) {
        auto match = regexEntry.match(line);
        if (match.hasMatch()) {
            QByteArray payload;
            auto it = regexPayload.globalMatch(match.captured(QStringLiteral("payload")));
            while (it.hasNext()) {
                auto payloadMatch = it.next();
                payload.append(static_cast<char>(payloadMatch.captured(1).toUInt(nullptr, 16)));
            }

            // Grossly count sent messages based off frames that start with the sync byte,
            // this may not be completely accurate, but close enough.
            const bool isMessageStart = !payload.isEmpty() && static_cast<quint8>(payload.at(0)) == syncByte;

            const qint64 timestampUs = parseTimestampUs(match.captured(QStringLiteral("timestamp")));
            const qint64 calcDeltaUs = (prevTimestampUs >= 0 && timestampUs >= 0) ? timestampUs - prevTimestampUs : 0;

            // Save the first and last time stamps for reporting later
            if (timestampUs >= 0) {
                if (firstTimestampUs < 0) {
                    firstTimestampUs = timestampUs;
                }
                lastTimestampUs = timestampUs;
            }

            if (speed > 0.0 && timestampUs >= 0) {
                const qint64 offsetUs = static_cast<qint64>((timestampUs - firstTimestampUs) / speed);
                struct timespec wakeTime;
                wakeTime.tv_sec = replayOrigin.tv_sec + offsetUs / 1'000'000;
                wakeTime.tv_nsec = replayOrigin.tv_nsec + (offsetUs % 1'000'000) * 1000;
                wakeTime.tv_sec += wakeTime.tv_nsec / 1'000'000'000;
                wakeTime.tv_nsec = wakeTime.tv_nsec % 1'000'000'000;
                clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &wakeTime, nullptr);
            }

            // Wall-clock interval since the previous frame.
            // start() on an already-running timer restarts it.
            const qint64 actualDeltaNs = wallTimer.isValid() ? wallTimer.nsecsElapsed() : 0;
            wallTimer.start();
            playedCount++;
            if (playedCount > 1) {
                totalDeltaNs += actualDeltaNs;
                avgDeltaNs = totalDeltaNs / qint64(playedCount - 1);
            }

            // Same interval measurement, but only across message starts, so it spans a
            // whole message rather than one frame of it.
            if (isMessageStart) {
                msgDeltaNs = msgTimer.isValid() ? msgTimer.nsecsElapsed() : 0;
                msgTimer.start();
                messageCount++;
                if (messageCount > 1) {
                    msgTotalDeltaNs += msgDeltaNs;
                    msgAvgDeltaNs = msgTotalDeltaNs / qint64(messageCount - 1);
                }
                // Logged interval between sync byte frames
                msgCalcDeltaUs = (msgPrevTimestampUs >= 0 && timestampUs >= 0) ? timestampUs - msgPrevTimestampUs : 0;
                msgPrevTimestampUs = timestampUs;

                msgId = (payload.size() >= headerMsgIdOffset + 2)
                    ? (static_cast<quint8>(payload.at(headerMsgIdOffset)) << 8)
                        | static_cast<quint8>(payload.at(headerMsgIdOffset + 1))
                    : 0;
            }

            const unsigned int canId = match.captured(QStringLiteral("can_id")).toUInt(nullptr, 16);
            if (!testMode) {
                QCanBusFrame frame(canId, payload);
                canDevice->writeFrame(frame);
            }

            qInfo().noquote() << QString("Frame %1: canId=0x%2, time since last frame [actual=%3ms, calc=%4ms], "
                                         "avg time between frames=%5ms, avg time between messages=%6ms")
                .arg(playedCount)
                .arg(QString("%1").arg(canId, 4, 16, QChar('0')).toUpper())
                .arg((playedCount > 1) ? actualDeltaNs / 1'000'000.0 : 0.0, 6, 'f', 3, QChar(' '))
                .arg((playedCount > 1) ? calcDeltaUs / 1000.0 : 0.0, 6, 'f', 3, QChar(' '))
                .arg(avgDeltaNs / 1'000'000.0, 6, 'f', 3, QChar(' '))
                .arg(msgAvgDeltaNs / 1'000'000.0, 6, 'f', 3, QChar(' '));

            if (isMessageStart) {
                qInfo().noquote() << QString("Message %1: msgId=0x%2, time since last [actual=%3ms, calc=%4ms], "
                                             "avg time between=%5ms.")
                    .arg(messageCount)
                    .arg(QString("%1").arg(QString("%1").arg(msgId, 4, 16, QChar('0')).toUpper()).toUpper())
                    .arg((messageCount > 1) ? msgDeltaNs / 1'000'000.0 : 0.0, 6, 'f', 3, QChar(' '))
                    .arg((messageCount > 1) ? msgCalcDeltaUs / 1000.0 : 0.0, 6, 'f', 3, QChar(' '))
                    .arg(msgAvgDeltaNs / 1'000'000.0, 6, 'f', 3, QChar(' '));
            }

            prevTimestampUs = timestampUs;
        }
        else {
            qWarning().noquote()
                << QString("WARNING: \"%1\" (line %2) did not match expected format").arg(line).arg(lineCount);
        }
        lineCount++;
    }

    // Span the dump covers, i.e. the last frame timestamp minus the first. Compare
    // against the replay time to see how faithfully the replay tracked the log.
    const qint64 logDurationUs = (firstTimestampUs >= 0) ? (lastTimestampUs - firstTimestampUs) : 0;

    qInfo().noquote() << QString("replay complete: messages(approx)=%1, frames=%2, avg time between frames=%3ms, "
                                 "avg time between messages=%4ms, total replay time=%5ms, total log time=%6m")
        .arg(messageCount)
        .arg(playedCount)
        .arg(avgDeltaNs / 1'000'000.0, 0, 'f', 3, QChar(' '))
        .arg(msgAvgDeltaNs / 1'000'000.0, 0, 'f', 3, QChar(' '))
        .arg(replayTimer.nsecsElapsed() / 1'000'000.0, 0, 'f', 3, QChar(' '))
        .arg(logDurationUs / 60'000'000.0, 0, 'f', 2, QChar(' '));

    file.close();
    if (canDevice) {
        canDevice->disconnectDevice();
    }

    return 0;
}