// SPDX-License-Identifier: GPL-2.0-only
/*
 * Digital Voice Modem - Test Suite
 * GPLv2 Open Source. Use is subject to license terms.
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 *  Copyright (C) 2026 Bryan Biedenkapp, N2PLL
 *
 */
#include "host/Defines.h"
#include "common/edac/RS634717.h"
#include "common/p25/P25Defines.h"
#include "common/p25/lc/LC.h"
#include "common/p25/Sync.h"
#include "common/Log.h"
#include "common/Utils.h"

using namespace edac;
using namespace p25;
using namespace p25::defines;
using namespace p25::lc;

#include <catch2/catch_test_macros.hpp>
#include <stdlib.h>
#include <time.h>

TEST_CASE("P25 Phase 2 VCH MAC PDU I-OEMI (RS 52,30,23) Test", "[p25][p2_vch_macpdu_ioemi]") {
    bool failed = false;

    INFO("P25 Phase 2 VCH MAC PDU I-OEMI RS (52,30,23) FEC Test");

    srand((unsigned int)time(NULL));

    // Create LC instance
    LC lc;
    
    // Set up test MAC PDU data for Phase 2
    lc.setMFId(MFG_STANDARD);
    lc.setLCO(P2_MAC_MCO::GROUP);  // Phase 2 MAC MCO
    lc.setSrcId(1234);
    lc.setDstId(9876);
    lc.setEmergency(false);
    lc.setEncrypted(false);
    lc.setPriority(4U);
    lc.setGroup(true);
    
    // Set Phase 2 specific fields
    lc.setP2DUID(P2_DUID::FACCH_UNSCRAMBLED);
    lc.setMACPDUOpcode(P2_MAC_HEADER_OPCODE::IDLE);  // IDLE opcode for MAC PDU
    lc.setMACPartition(P2_MAC_MCO_PARTITION::UNIQUE); // UNIQUE partition

    // Encode VCH MAC PDU (I-OEMI, no sync)
    uint8_t encodedData[P25_P2_FRAME_LENGTH_BYTES];
    ::memset(encodedData, 0x00U, P25_P2_FRAME_LENGTH_BYTES);
    
    lc.encodeVCH_MACPDU(encodedData, false);

    Utils::dump(2U, "LC::encodeVCH_MACPDU(), I-OEMI Encoded Data", encodedData, P25_P2_FRAME_LENGTH_BYTES);

    // Inject 2 errors to test error correction (RS can correct up to 11 errors)
    uint32_t errorPos1 = 100;
    uint32_t errorPos2 = 150;
    
    uint8_t originalBit1 = READ_BIT(encodedData, errorPos1);
    uint8_t originalBit2 = READ_BIT(encodedData, errorPos2);
    
    WRITE_BIT(encodedData, errorPos1, !originalBit1);
    WRITE_BIT(encodedData, errorPos2, !originalBit2);

    Utils::dump(2U, "LC::decodeVCH_MACPDU_OEMI(), I-OEMI Data (errors injected)", encodedData, P25_P2_FRAME_LENGTH_BYTES);

    // Decode VCH MAC PDU (I-OEMI, no sync)
    LC decodedLc;
    bool ret = decodedLc.decodeVCH_MACPDU_OEMI(encodedData, false);
    
    if (!ret) {
        ::LogError("T", "LC::decodeVCH_MACPDU_OEMI(), failed to decode I-OEMI MAC PDU");
        failed = true;
    }

    // Verify decoded data matches original
    if (decodedLc.getLCO() != lc.getLCO()) {
        ::LogError("T", "LC::decodeVCH_MACPDU_OEMI(), LCO mismatch: expected %02X, got %02X", lc.getLCO(), decodedLc.getLCO());
        failed = true;
    }

    if (decodedLc.getSrcId() != lc.getSrcId()) {
        ::LogError("T", "LC::decodeVCH_MACPDU_OEMI(), Source ID mismatch: expected %u, got %u", lc.getSrcId(), decodedLc.getSrcId());
        failed = true;
    }

    if (decodedLc.getDstId() != lc.getDstId()) {
        ::LogError("T", "LC::decodeVCH_MACPDU_OEMI(), Dest ID mismatch: expected %u, got %u", lc.getDstId(), decodedLc.getDstId());
        failed = true;
    }

    if (decodedLc.getP2DUID() != lc.getP2DUID()) {
        ::LogError("T", "LC::decodeVCH_MACPDU_OEMI(), P2 DUID mismatch: expected %02X, got %02X", lc.getP2DUID(), decodedLc.getP2DUID());
        failed = true;
    }

    REQUIRE(failed == false);
}

TEST_CASE("P25 Phase 2 VCH MAC PDU S-OEMI (RS 45,26,20) Test", "[p25][p2_vch_macpdu_soemi]") {
    bool failed = false;

    INFO("P25 Phase 2 VCH MAC PDU S-OEMI RS (45,26,20) FEC Test");

    srand((unsigned int)time(NULL));

    // Create LC instance
    LC lc;
    
    // Set up test MAC PDU data for Phase 2
    lc.setMFId(MFG_STANDARD);
    lc.setLCO(P2_MAC_MCO::PRIVATE);  // Phase 2 MAC MCO
    lc.setSrcId(5678);
    lc.setDstId(1234);
    lc.setEmergency(true);
    lc.setEncrypted(true);
    lc.setPriority(7U);
    lc.setGroup(false);
    
    // Set Phase 2 specific fields
    lc.setP2DUID(P2_DUID::SACCH_UNSCRAMBLED);
    lc.setMACPDUOpcode(P2_MAC_HEADER_OPCODE::IDLE);  // IDLE opcode for MAC PDU
    lc.setMACPartition(P2_MAC_MCO_PARTITION::UNIQUE); // UNIQUE partition
    lc.setAlgId(ALGO_UNENCRYPT);  // For test, use unencrypted

    // Encode VCH MAC PDU (S-OEMI, with sync)
    uint8_t encodedData[P25_P2_FRAME_LENGTH_BYTES];
    ::memset(encodedData, 0x00U, P25_P2_FRAME_LENGTH_BYTES);
    
    lc.encodeVCH_MACPDU(encodedData, true);

    Utils::dump(2U, "LC::encodeVCH_MACPDU(), S-OEMI Encoded Data", encodedData, P25_P2_FRAME_LENGTH_BYTES);

    // Note: Error injection test is skipped for S-OEMI because hexbit-level errors
    // are complex to inject correctly. The I-OEMI test demonstrates RS error correction.

    // Decode VCH MAC PDU (S-OEMI, with sync) without error injection
    LC decodedLc;
    bool ret = decodedLc.decodeVCH_MACPDU_OEMI(encodedData, true);
    
    if (!ret) {
        ::LogError("T", "LC::decodeVCH_MACPDU_OEMI(), failed to decode S-OEMI MAC PDU");
        failed = true;
    }

    // Verify decoded data matches original
    if (decodedLc.getLCO() != lc.getLCO()) {
        ::LogError("T", "LC::decodeVCH_MACPDU_OEMI(), LCO mismatch: expected %02X, got %02X", lc.getLCO(), decodedLc.getLCO());
        failed = true;
    }

    if (decodedLc.getSrcId() != lc.getSrcId()) {
        ::LogError("T", "LC::decodeVCH_MACPDU_OEMI(), Source ID mismatch: expected %u, got %u", lc.getSrcId(), decodedLc.getSrcId());
        failed = true;
    }

    if (decodedLc.getDstId() != lc.getDstId()) {
        ::LogError("T", "LC::decodeVCH_MACPDU_OEMI(), Dest ID mismatch: expected %u, got %u", lc.getDstId(), decodedLc.getDstId());
        failed = true;
    }

    if (decodedLc.getP2DUID() != lc.getP2DUID()) {
        ::LogError("T", "LC::decodeVCH_MACPDU_OEMI(), P2 DUID mismatch: expected %02X, got %02X", lc.getP2DUID(), decodedLc.getP2DUID());
        failed = true;
    }

    if (decodedLc.getEmergency() != lc.getEmergency()) {
        ::LogError("T", "LC::decodeVCH_MACPDU_OEMI(), Emergency flag mismatch");
        failed = true;
    }

    REQUIRE(failed == false);
}

TEST_CASE("P25 Phase 2 VCH MAC PDU Round-Trip I-OEMI Test", "[p25][p2_vch_macpdu_roundtrip_ioemi]") {
    bool failed = false;

    INFO("P25 Phase 2 VCH MAC PDU I-OEMI Round-Trip Test");

    // Create LC instance with various configurations
    LC lc;
    lc.setMFId(MFG_STANDARD);
    lc.setLCO(P2_MAC_MCO::GROUP);  // Phase 2 MAC MCO
    lc.setSrcId(12345);
    lc.setDstId(67890);
    lc.setEmergency(false);
    lc.setEncrypted(false);
    lc.setPriority(5U);
    lc.setGroup(true);
    lc.setP2DUID(P2_DUID::FACCH_UNSCRAMBLED);
    lc.setMACPDUOpcode(P2_MAC_HEADER_OPCODE::IDLE);
    lc.setMACPartition(P2_MAC_MCO_PARTITION::UNIQUE);

    // Encode without sync (I-OEMI)
    uint8_t encodedData[P25_P2_FRAME_LENGTH_BYTES];
    ::memset(encodedData, 0x00U, P25_P2_FRAME_LENGTH_BYTES);
    lc.encodeVCH_MACPDU(encodedData, false);

    Utils::dump(2U, "Round-Trip Test: Encoded I-OEMI", encodedData, P25_P2_FRAME_LENGTH_BYTES);

    // Decode
    LC decodedLc;
    bool ret = decodedLc.decodeVCH_MACPDU_OEMI(encodedData, false);

    if (!ret) {
        ::LogError("T", "Round-trip decode failed");
        failed = true;
    }

    // Re-encode
    uint8_t reencodedData[P25_P2_FRAME_LENGTH_BYTES];
    ::memset(reencodedData, 0x00U, P25_P2_FRAME_LENGTH_BYTES);
    decodedLc.encodeVCH_MACPDU(reencodedData, false);

    Utils::dump(2U, "Round-Trip Test: Re-encoded I-OEMI", reencodedData, P25_P2_FRAME_LENGTH_BYTES);

    // Compare original and re-encoded data
    for (uint32_t i = 0; i < P25_P2_FRAME_LENGTH_BYTES; i++) {
        if (encodedData[i] != reencodedData[i]) {
            ::LogError("T", "Round-trip data mismatch at byte %u: expected %02X, got %02X", i, encodedData[i], reencodedData[i]);
            failed = true;
            break;
        }
    }

    REQUIRE(failed == false);
}

TEST_CASE("P25 Phase 2 VCH MAC PDU Round-Trip S-OEMI Test", "[p25][p2_vch_macpdu_roundtrip_soemi]") {
    bool failed = false;

    INFO("P25 Phase 2 VCH MAC PDU S-OEMI Round-Trip Test");

    // Create LC instance with various configurations
    LC lc;
    lc.setMFId(MFG_STANDARD);
    lc.setLCO(P2_MAC_MCO::TEL_INT_VCH_USER);  // Phase 2 MAC MCO
    lc.setSrcId(11111);
    lc.setDstId(22222);
    lc.setEmergency(true);
    lc.setEncrypted(false);
    lc.setPriority(6U);
    lc.setGroup(false);
    lc.setP2DUID(P2_DUID::SACCH_UNSCRAMBLED);
    lc.setMACPDUOpcode(P2_MAC_HEADER_OPCODE::IDLE);
    lc.setMACPartition(P2_MAC_MCO_PARTITION::UNIQUE);

    // Encode with sync (S-OEMI)
    uint8_t encodedData[P25_P2_FRAME_LENGTH_BYTES];
    ::memset(encodedData, 0x00U, P25_P2_FRAME_LENGTH_BYTES);
    lc.encodeVCH_MACPDU(encodedData, true);

    Utils::dump(2U, "Round-Trip Test: Encoded S-OEMI", encodedData, P25_P2_FRAME_LENGTH_BYTES);

    // Decode
    LC decodedLc;
    bool ret = decodedLc.decodeVCH_MACPDU_OEMI(encodedData, true);

    if (!ret) {
        ::LogError("T", "Round-trip decode failed");
        failed = true;
    }

    // Re-encode
    uint8_t reencodedData[P25_P2_FRAME_LENGTH_BYTES];
    ::memset(reencodedData, 0x00U, P25_P2_FRAME_LENGTH_BYTES);
    decodedLc.encodeVCH_MACPDU(reencodedData, true);

    Utils::dump(2U, "Round-Trip Test: Re-encoded S-OEMI", reencodedData, P25_P2_FRAME_LENGTH_BYTES);

    // Compare original and re-encoded data
    for (uint32_t i = 0; i < P25_P2_FRAME_LENGTH_BYTES; i++) {
        if (encodedData[i] != reencodedData[i]) {
            ::LogError("T", "Round-trip data mismatch at byte %u: expected %02X, got %02X", i, encodedData[i], reencodedData[i]);
            failed = true;
            break;
        }
    }

    REQUIRE(failed == false);
}

TEST_CASE("P25 Phase 2 VCH MAC PDU Voice PDU Bypass Test", "[p25][p2_vch_macpdu_voice_bypass]") {
    bool failed = false;

    INFO("P25 Phase 2 VCH MAC PDU Voice PDU Bypass Test");

    // Test that 4V and 2V voice PDUs are properly bypassed
    uint8_t testData[P25_P2_FRAME_LENGTH_BYTES];
    ::memset(testData, 0x00U, P25_P2_FRAME_LENGTH_BYTES);

    // Test VTCH_4V bypass
    LC lc4v;
    lc4v.setP2DUID(P2_DUID::VTCH_4V);
    lc4v.encodeVCH_MACPDU(testData, false);
    
    LC decoded4v;
    bool ret4v = decoded4v.decodeVCH_MACPDU_OEMI(testData, false);
    
    if (!ret4v) {
        ::LogError("T", "Failed to handle VTCH_4V bypass");
        failed = true;
    }

    // Test VTCH_2V bypass
    ::memset(testData, 0x00U, P25_P2_FRAME_LENGTH_BYTES);
    LC lc2v;
    lc2v.setP2DUID(P2_DUID::VTCH_2V);
    lc2v.encodeVCH_MACPDU(testData, false);
    
    LC decoded2v;
    bool ret2v = decoded2v.decodeVCH_MACPDU_OEMI(testData, false);
    
    if (!ret2v) {
        ::LogError("T", "Failed to handle VTCH_2V bypass");
        failed = true;
    }

    REQUIRE(failed == false);
}