import rp2
from machine import Pin
from rp2 import PIO
import time

# RailCom encoding:
# - 6 bit data payload => 8 bit signal
#
railcom_signaling = {
    "ACK":   0b00111100,
    "NACK1": 0b00001111,
    "NACK2": 0b11110000
}

railcom_encoding = [
    0b10101100,
    0b10101010,
    0b10101001,
    0b10100101,
    0b10100011,
    0b10100110,
    0b10011100,
    0b10011010,
    0b10011001,
    0b10010101,
    0b10010011,
    0b10010110,
    0b10001110,
    0b10001101,
    0b10001011,
    0b10110001,
    0b10110010,
    0b10110100,
    0b10111000,
    0b01110100,
    0b01110010,
    0b01101100,
    0b01101010,
    0b01101001,
    0b01100101,
    0b01100011,
    0b01100110,
    0b01011100,
    0b01011010,
    0b01011001,
    0b01010101,
    0b01010011,
    0b01010110,
    0b01001110,
    0b01001101,
    0b01001011,
    0b01000111,
    0b01110001,
    0b11101000,
    0b11100100,
    0b11100010,
    0b11010001,
    0b11001001,
    0b11000101,
    0b11011000,
    0b11010100,
    0b11010010,
    0b11001010,
    0b11000110,
    0b11001100,
    0b01111000,
    0b00010111,
    0b00011011,
    0b00011101,
    0b00011110,
    0b00101110,
    0b00110110,
    0b00111010,
    0b00100111,
    0b00101011,
    0b00101101,
    0b00110101,
    0b00111001,
    0b00110011
]

def conv6to8bits(six_bit_in):

    eight_bit_out = railcom_encoding[six_bit_in]
    return eight_bit_out

def conv12to16bit(id_4, data_8):

    # convert id[3..0] + data[7..6] bits
    eight_bit_out[0] = railcom_encoding[six_bit_in]

    # convert data[5..0]
    eight_bit_out[1] = railcom_encoding[six_bit_in]

    return eight_bit_out


@rp2.asm_pio(set_init=rp2.PIO.OUT_LOW, out_init=rp2.PIO.OUT_LOW, out_shiftdir=rp2.PIO.SHIFT_RIGHT, autopull=True, pull_thresh=8)
def echo2():
    wrap_target()
    label("bitloop")                        # Main Loop
    set(pins, 1)                     [20]   # Set DCC to '1'                  (20+1 cycles)
    out(x, 1)                               # Read next value                 (   1 cycle)
    jmp(not_x, "do_zero")                   # IF bit == '0' THEN run longer   (   1 cycle)
    set(pins, 0)                     [21]   # Set DCC to '0'                  (21+1 cycles)
    jmp("bitloop")                          # restart over
    label("do_zero")                        # ... IF bit == '0' THEN ...
    nop()                            [16]   # Run '1' longer                  (16+1 cycles)
    set(pins, 0)                     [30]   # Set DCC to '0'                  (30+1 cycles)
    nop()                             [8]   # Stay low for longer             ( 8+1 cycles)  
    wrap()


sm = rp2.StateMachine(0, echo2, freq=1_000_000, set_base=Pin(7))
sm.active(1)

print("Hello World 2")

print(sm.tx_fifo())
sm.put(railcom_encoding[0b])
sm.put(railcom_encoding[55])
print(sm.tx_fifo())
sm.put(170)
sm.put(85)
print(sm.tx_fifo())

print("Hello World 3")