"""Experimental shortened CCSDS 131.0-B-5 rate-1/6 Turbo code.

102 payload bits -> 636 mother code bits -> 640 channel bits.
NOT a standards-compliant CCSDS interleaver/block size.

Dependencies: numpy, numba.  Channel bits are 0/1. LLR convention:
positive = 0 more likely, negative = 1 more likely.

CCSDS 16-state constituent: backward 10011, forward
G1=11011, G2=10101, G3=11111. Output ordering per six-tuple:
(systematic_A, parity1_A, parity2_A, parity3_A, parity1_B, parity3_B).
Each component terminates independently in four steps.
"""
from __future__ import annotations
import numpy as np
from numba import njit

K = 102
MOTHER_N = 6 * (K + 4)  # 636
N = 640
REPEAT_POS = np.array([0, 6, 12, 18], dtype=np.int64)  # repeat 4 systematic bits
INTERLEAVER_SEED = 13100640
PI = np.random.default_rng(INTERLEAVER_SEED).permutation(K).astype(np.int64)
INV_PI = np.argsort(PI)

@njit(cache=True)
def _bitpar(x):
    x ^= x >> 2
    x ^= x >> 1
    return x & 1

@njit(cache=True)
def _transition(s, u):
    fb = ((s >> 2) ^ (s >> 3)) & 1
    v = u ^ fb
    ns = ((s << 1) & 15) | v
    # parity masks [v,s0,s1,s2,s3] G1 11011, G2 10101, G3 11111
    p1 = v ^ (s & 1) ^ ((s >> 2)&1) ^ ((s >> 3)&1)
    p2 = v ^ ((s >> 1)&1) ^ ((s >> 3)&1)
    p3 = v ^ _bitpar(s)
    return ns, p1, p2, p3

@njit(cache=True)
def _encode_component(data):
    k = data.size
    sy = np.zeros(k + 4, np.uint8)
    pars = np.zeros((k + 4, 3), np.uint8)
    state = 0
    for t in range(k + 4):
        if t < k:
            u = int(data[t])
        else:
            u = ((state >> 2) ^ (state >> 3)) & 1  # trellis termination input
        sy[t] = u
        state, a, b, c = _transition(state, u)
        pars[t,0], pars[t,1], pars[t,2] = a,b,c
    assert state == 0
    return sy, pars


def encode(payload):
    """Return 640-bit systematic Turbo codeword (uint8)."""
    data = np.asarray(payload, dtype=np.uint8)
    if data.shape != (K,) or np.any(data > 1):
        raise ValueError(f'payload must be exactly {K} bits (0 or 1)')
    sy_a, pa = _encode_component(data)
    sy_b, pb = _encode_component(data[PI])
    out = np.empty((K + 4, 6), dtype=np.uint8)
    out[:,0] = sy_a
    out[:,1:4] = pa
    out[:,4] = pb[:,0]
    out[:,5] = pb[:,2]
    mother = out.ravel()
    assert mother.size == MOTHER_N
    return np.concatenate((mother, mother[REPEAT_POS]))

@njit(cache=True)
def _trellis():
    ns = np.empty((16,2),np.int32)
    parity = np.empty((16,2,3),np.uint8)
    for s in range(16):
        for u in range(2):
            nxt,a,b,c = _transition(s,u)
            ns[s,u] = nxt
            parity[s,u,0] = a
            parity[s,u,1] = b
            parity[s,u,2] = c
    return ns,parity

NEXT, PARITY = _trellis()

@njit(cache=True)
def _bcjr(sys_llr, parity_llr, parity_indices, apriori, tail_sys, tail_par,
          next_state, parbits):
    """16-state max-log-map constituent SISO with exact terminated end metric."""
    length = sys_llr.size
    # For each possible state after data, simulate four forced tail steps.
    beta_end = np.zeros(16, np.float64)
    for s0 in range(16):
        state = s0
        accum = 0.0
        for j in range(4):
            u = ((state >> 2) ^ (state >> 3)) & 1
            nxt, a,b,c = _transition(state, u)
            if tail_sys.size > 0:
                accum += 0.5 * (1-2*u) * tail_sys[j]
            for q in range(len(parity_indices)):
                pp = parbits[state,u,parity_indices[q]]
                accum += 0.5*(1-2*int(pp))*tail_par[j,q]
            state = nxt
        beta_end[s0] = accum
    alpha = np.full((length+1,16), -1e30, np.float64)
    alpha[0,0] = 0.0
    for t in range(length):
        for s in range(16):
            a0 = alpha[t,s]
            if a0 < -1e20: continue
            for u in range(2):
                metric = 0.5 * (1-2*u) * (sys_llr[t]+apriori[t])
                for q in range(len(parity_indices)):
                    pp = int(parbits[s,u,parity_indices[q]])
                    metric += 0.5*(1-2*pp)*parity_llr[t,q]
                sn = next_state[s,u]
                candidate = a0+metric
                if candidate > alpha[t+1,sn]: alpha[t+1,sn]=candidate
        mx = np.max(alpha[t+1,:])
        alpha[t+1,:] -= mx
    beta = beta_end.copy()
    extr = np.empty(length, np.float64)
    for t in range(length-1,-1,-1):
        metric0 = -1e30
        metric1 = -1e30
        newbeta = np.full(16,-1e30,np.float64)
        for s in range(16):
            for u in range(2):
                br = 0.5*(1-2*u)*(sys_llr[t]+apriori[t])
                for q in range(len(parity_indices)):
                    pp = int(parbits[s,u,parity_indices[q]])
                    br += 0.5*(1-2*pp)*parity_llr[t,q]
                candidate = alpha[t,s]+br+beta[next_state[s,u]]
                if u == 0:
                    if candidate > metric0: metric0=candidate
                else:
                    if candidate > metric1: metric1=candidate
                back = br + beta[next_state[s,u]]
                if back > newbeta[s]: newbeta[s]=back
        extr[t] = metric0-metric1 - sys_llr[t]-apriori[t]
        beta = newbeta - np.max(newbeta)
    return extr


def decode_llr(llr, iterations=12, damping=0.8):
    """Decode soft channel LLRs (positive implies bit 0), returning K hard bits."""
    llr = np.asarray(llr, np.float64)
    if llr.shape != (N,): raise ValueError(f'expected {N} LLRs')
    combined = llr[:MOTHER_N].copy()
    np.add.at(combined, REPEAT_POS, llr[MOTHER_N:])
    obs = combined.reshape(K+4,6)
    la_sys = obs[:K,0].copy()
    pa = np.ascontiguousarray(obs[:K,1:4])
    pb = np.ascontiguousarray(obs[:K,4:6])
    a_tail_par = np.ascontiguousarray(obs[K:,1:4])
    b_tail_par = np.ascontiguousarray(obs[K:,4:6])
    # B systematic is not transmitted: a priori/extrinsic of A is passed to B.
    zeros = np.zeros(K, dtype=np.float64)
    eb = np.zeros(K, dtype=np.float64)
    apriori_a = np.zeros(K, dtype=np.float64)
    idxa = np.array([0,1,2], dtype=np.int64)
    idxb = np.array([0,2], dtype=np.int64)
    for it in range(iterations):
        ea = _bcjr(la_sys,pa,idxa,apriori_a,obs[K:,0],a_tail_par,NEXT,PARITY)
        eb_new = _bcjr(zeros,pb,idxb,np.ascontiguousarray((la_sys+ea)[PI]),
                       np.zeros(4),b_tail_par,NEXT,PARITY)
        eb[:] = eb_new
        apriori_a[:] = damping*eb[INV_PI]
    return (la_sys+ea+apriori_a < 0).astype(np.uint8)


def decode(received, p=0.20, iterations=12, damping=0.8):
    """Hard-decision BSC decoder, p < 0.5."""
    if not (0 < p < 0.5): raise ValueError('p must be between 0 and 0.5')
    bits = np.asarray(received, dtype=np.uint8)
    if bits.shape != (N,) or np.any(bits>1): raise ValueError(f'expected {N} binary bits')
    amp = np.log((1-p)/p)
    return decode_llr((1.0 - 2.0*bits.astype(np.float64))*amp,iterations,damping)


def benchmark(blocks=1000, p=0.20, iterations=12, seed=42):
    rng=np.random.default_rng(seed)
    errors=0
    failed=0
    for t in range(blocks):
        x=rng.integers(0,2,K,dtype=np.uint8)
        tx=encode(x)
        rx=tx ^ (rng.random(N)<p).astype(np.uint8)
        y=decode(rx,p,iterations)
        e=int(np.count_nonzero(x!=y))
        errors += e
        failed += (e>0)
    return {'blocks':blocks, 'p':p, 'payload_bit_errors':errors,
            'failed_blocks':failed,'BER':errors/(blocks*K),'BLER':failed/blocks,
            'mean_payload_errors_per_block':errors/blocks}

if __name__ == '__main__':
    rng=np.random.default_rng(1)
    for i in range(20):
        x=rng.integers(0,2,K,dtype=np.uint8)
        assert np.array_equal(x,decode(encode(x),p=.2)), 'Noiseless decode failure'
    print('20 noiseless round trips OK')
    print(benchmark(blocks=1000,p=.20))
