#import gpiozero.pins.mock
#from gpiozero import *
#from grove.factory import Factory
#from grove.grove_relay import GroveRelay
import aiomqtt
import asyncio
#from i2cpy import I2C
from smbus2 import SMBus
import toml
#import i2cpy
#i2cset -y 1 0x11 0x11 0x42
#set i2c address from 0x11 to 0x42

# ONLY FOR TESTING ON NON RPI
#Device.pin_factory = gpiozero.pins.mock.MockFactory()

#relays = [LED(17), LED(27), LED(22), LED(23), LED(5), LED(6), LED(24), LED(25)]
main_I2C_addr = 0 #default
side_I2C_addr = 0
room = ""
use_offset = False 

"""              0       1       2       3   """
relayMasks = [0b0001, 0b0010, 0b0100, 0b1000] #probably ne rabim

bus = SMBus(1)

relMapping = {
    'service_down': 0,
    'service_up': 1,
    'down': 2,
    'up': 3
}

currentState = {
    'main': 0b0000,
    'side': 0b0000
}

async def msgRelayBoard(projSelect, command, state: bool):
    #i2cAddr = relayBoardMain if projSelect == 'glavni' else relayBoardSide
    #TODO this is not optimal, check for more crap
    
    # register 0x10 za releje

    I2CAddr = 0 #glavni
    match projSelect:
        case 'main':
            I2CAddr = main_I2C_addr
        case 'side':
            I2CAddr = side_I2C_addr
            #return #TODO TEMPORARY, REMOVE LATER#
        case default: 
            return #ignore if unknown position

    maskShift = relMapping[command]
    mask = (1 << maskShift)
    if state:
        currentState[projSelect] = currentState[projSelect] | mask
    else:
        currentState[projSelect] = currentState[projSelect] & ~mask
    
    bus.write_byte_data(I2CAddr, 0x10, currentState[projSelect])
    print("Command sent")
    #print('testirovano jako')


"""
            SrvDwn  SrvUp  OpDwn  OpUp
MAIN: 0x42  0001    0010   0100   1000 
SIDE: 0x43  0001    0010   0100   1000
"""


#old board
"""
MAIN: SrvDwn SrvUp OpDwn OpUp
        0     1     2     3
SIDE:   4     5     6     7
"""
#dej like bolš to podukumentiraj or smth

async def task_command2relays(controlClient: aiomqtt.Client):
    global room
    #relayCtrl = lambda cmd, relay: relays[relay].on() if cmd == 1 else relays[relay].off()
    #relayCtrl = lambda  cmd, relay: [relays[r].on() if cmd == 1 and r == relay else relays[r].off for r in range(len(relays))]
    
    relayCtrl = lambda x, y: print(x, y)
    await controlClient.subscribe(f"{room}/projectors/+/lift/#")
    msgs = controlClient.messages
    async for mesg in msgs:
        mesg: aiomqtt.Message
        if mesg.topic.matches(f'{room}/projectors/+/lift/move/#'):
            msgTopicArr = mesg.topic.value.split('/')
            state =  mesg.payload.decode()
            print("Received: " + str(msgTopicArr) + " payload: [" + state + "]")
            #testCase = (msgTopicArr[2], msgTopicArr[4])
            projSel = msgTopicArr[2]
            if projSel != 'main' and projSel != 'side':
                continue #TODO error hnadling?
            command = msgTopicArr[5]
            await msgRelayBoard(projSel, command, state == '1')
            await controlClient.publish(f'{room}/projectors/{projSel}/lift/status', payload="", qos=1, retain=True)
            #print("Pushing \'off\' to other relays to prevent conflicts")
            await asyncio.sleep(0.01)



async def main():
    global main_I2C_addr, side_I2C_addr, room
    conf = toml.load('./malinaConfig.toml')
    projMotors = conf.get("projector_motors")
    mainMotor = projMotors.get("main")
    sideMotor = projMotors.get("side")
    main_I2C_addr = mainMotor['i2c_address']
    side_I2C_addr = sideMotor['i2c_address']

    room = conf["global"]["room"]

    async with aiomqtt.Client('localhost', 1883) as client:
        task_control = asyncio.create_task(task_command2relays(client))
        await asyncio.gather(task_control)

if __name__ == '__main__':
    asyncio.run(main())