import sys

from can import Message as CAN_Message
from can.interfaces.socketcan_constants import *  # CAN_RAW

from Global_CAN import *
import os

import socket
import struct
import time
import fcntl

import queue
from threading import Thread

class PythonCAN(CANDriver):
    def __init__(self, br = 250):

        self.channel = 'can1'
        if br == 250:
            self._can = socket.socket(socket.AF_CAN, socket.SOCK_RAW, socket.CAN_RAW)
            self._can.bind((self.channel,))
        else:#tbd
            pass

        self.rcvSocketQ = queue.Queue()  
        rxS = Thread(target = self._RxSocket)  
        self.rxSocketRun = True
        rxS.start()
        return super(PythonCAN, self).__init__()

    def __del__(self):
        self.taskRxRunning = False
        self.rxSocketRun = False

    def _RxSocket(self):
        while self.rxSocketRun:
            cf, addr = self._can.recvfrom(16)
            self.rcvSocketQ.put(cf)
                                            
    def _RxTask(self):
        if self.rcvSocketQ.empty() == True:
            return
        while True:
            cf = self.rcvSocketQ.get_nowait()
            can_id, can_dlc, data = struct.unpack("=IB3x8s", cf)
            CAN_EFF_FLAG = bool(can_id & 0x80000000)
            
            if CAN_EFF_FLAG:
                arbitration_id = can_id & 0x1FFFFFFF
            else:
                arbitration_id = can_id & 0x000007FF
            
            res = fcntl.ioctl(self._can, SIOCGSTAMP, struct.pack("@LL", 0, 0))
            seconds, microseconds = struct.unpack("@LL", res)
            timestamp = seconds + microseconds / 1000000
    
            message = CAN_Message(timestamp=timestamp,arbitration_id=arbitration_id, data=data, extended_id=CAN_EFF_FLAG)
            self.rxQ.put(message)
            self.msgC += 1
            if self.rcvSocketQ.empty() == True:
                break

    def _SendMessage(self, m):
      data = m.data.ljust(8, b'\x00')
      arbitration_id = m.arbitration_id
      if m.id_type:
          arbitration_id |= 0x80000000
            
      msg = struct.pack("=IB3x8s", arbitration_id, m.dlc, data)
      self._can.send(msg)

    def _Reset(self):
        self._can.close()