from migen import *
from misoc.interconnect.csr import *

from functools import reduce
from itertools import combinations
from operator import or_, and_

class Voter(Module):
    def __init__(self):
        self.data_4x = Signal(32)
        self.k_4x = Signal(4)
        
        # Section 9.2.2.1 (CXP-001-2021)
        # decoder should immune to single bit errors when handling duplicated characters
        self.char = Signal(8)
        self.k = Signal()


        # majority voting
        char = [[self.data_4x[i*8:(i+1)*8], self.k_4x[i]] for i in range(4)]
        voter = [Record([("data", 8), ("k", 1)]) for _ in range(4)]

        # stage 1
        for i, code in enumerate(combinations(char, 3)):
            self.sync += [
                voter[i].data.eq(reduce(and_, [c[0] for c in code])),
                voter[i].k.eq(reduce(and_, [c[1] for c in code])),
            ]

        # stage 2
        self.sync += [
            self.char.eq(reduce(or_, [v.data for v in voter])),
            self.k.eq(reduce(or_, [v.k for v in voter])),
        ]

        


dut = Voter()
def check_case(data_4x, k_4x, char, k):
  yield dut.data_4x.eq(data_4x)
  yield dut.k_4x.eq(k_4x)
  yield
  yield
  yield
  print(f"char = {yield dut.char:#X} k = {yield dut.k:#X}")
  assert (yield dut.char) == char and (yield dut.k) == k

def testbench():
  yield from check_case(0xFFFFFFFF, 0b1111, 0xFF, 1)
  yield from check_case(0xFFFFFF00, 0b1110, 0xFF, 1)
  yield from check_case(0xFFFFF00f, 0b0001, 0xFF, 0)
  yield from check_case(0xFFFFFFFF, 0b1111, 0xFF, 1)

run_simulation(dut, testbench())