import protocol_kx
import can
import cantools
import os
import numpy as np
import struct
import time


K = np.zeros(protocol_kx.KX_K_COUNT)


class KX:
    def __init__(self, id, value, sbl_id, can):
        K[id] = value
        self.sbl_id = sbl_id
        self.can0 = can


    def KXInit(self, id, value):
        K[id] = value


    def reset(self):
        resetMsg = can.Message(arbitration_id=0xA, data=[99, self.sbl_id], is_extended_id=False)
        try:
            self.can0.send(resetMsg)
            print(f"Message sent on{self.can0.channel_info}")
        except self.can0.CanError:
            print("Message NOT sent")



    def sendK(self):
        i = 0
        for element in K:
            ba = bytearray(struct.pack("f", element))
            messageK = can.Message(arbitration_id=0x2BB, data=[self.sbl_id, i, 0,
                                                             protocol_kx.KX_FLAG_SEND, ba[0], ba[1], ba[2], ba[3]], is_extended_id=False)
            try:
                self.can0.send(messageK)
                time.sleep(0.01)
                print(f"Message sent on{self.can0.channel_info}")
            except self.can0.CanError:
                print("Message NOT sent")
            i = i + 1



    def writeToFlashK(self):
        msg = protocol_kx.CANMsgK(self.sbl_id, 0, protocol_kx.KX_FLAG_WRITE_REQ, K)
        messageK = can.Message(arbitration_id=0x2BB, data=[self.sbl_id, 0, 0,
                                                             protocol_kx.KX_FLAG_WRITE_REQ], is_extended_id=False)
        try:
            self.can0.send(messageK)
            time.sleep(0.01)
            print(f"Flash sent on{self.can0.channel_info}")
        except self.can0.CanError:
            print("Flash NOT sent")


    def send_test_source_enable(self, x, y, xVel, yVel):
        resetMsg = can.Message(
            arbitration_id=0xA,
            data=[
                0x53,
                0xFF,
                struct.pack('b', int(x))[0],
                struct.pack('B', int(y))[0],
                struct.pack('b', int(xVel))[0],
                struct.pack('b', int(yVel))[0],
                0x68,
                0xA4
            ],
            is_extended_id=False
        )
        self.can0.send(resetMsg)