import time
import setenv

from machine import I2C, Pin

ADDRESS     = 0x40
ZMCO        = 0x00
ZPOS        = 0x01
MPOS        = 0x03
MANG        = 0x05
CONF_REG    = 0x07
STATUS_REG  = 0x0B
RAWANGLE    = 0x0c
ANGLE_REG   = 0x0E
AGC_REG     = 0x1A
MAGNITUDE   = 0x1b
BURN        = 0xff


class AS5600L:

    def __init__(self,
        i2c_id = setenv.ENCODER_NUMBER,
        sda = setenv.ENCODER_SDA_PIN, 
        scl = setenv.ENCODER_SCL_PIN, 
        i2c_freq = setenv.ENCODER_I2C_FREQUENCE, 
        hyst = setenv.ENCODER_HYST, 
        power_mode = 0, 
        watchdog = 0,
        fast_filter_threshold = 0, 
        slow_filter = 0, 
        pwm_freq = 0, 
        output_stage = 0):
        """Init
        Args:
            i2c_id (int, optional): ID of the I2C bus. Defaults to 0.
            sda (int, optional) : gpio of the I2C bus. Defaults to 0.
            scl (int, optional) : gpio of clock for I2C bus. Defaults to 1.
            i2c_freq (int, optional): I2C Ferequency.  Default is max. Defaults to 1000000.
            hyst (int, optional): Hysteresis: 0->3. Defaults to 0.
            power_mode (int, optional): Power Mode: 0->3. Defaults to 0.
            watchdog (int, optional): Enable the watchdog. Defaults to 0.
            fast_filter_threshold (int, optional): Threshold value for the fast filter: 0->7. Defaults to 0.
            slow_filter (int, optional): Slow filter mode: 0->3. Defaults to 0.
            pwm_freq (int, optional): Output PWM frequency: 0->3. Defaults to 0.
            output_stage (int, optional): Output mode (see datasheet): 0->2. Defaults to 0.
        """
        self.i2c = I2C(i2c_id, scl=Pin(scl), sda=Pin(sda), freq=i2c_freq)
        print(self.i2c.scan())
        time.sleep_ms(100)
        c1 = 0x00
        c2 = 0x00
        # # set watchdog 0,1
        # if watchdog in range(0,2):
        #     c1 = c1 | (watchdog << 5)
        # # set fast filter threshold 0->7
        # if fast_filter_threshold in range(0,8):
        #     c1 = c1 | (fast_filter_threshold << 2)
        # # set the slow filter 0->3
        # if slow_filter in range(0,4):
        #     c1 = c1 | slow_filter
        # # set PWM frequency 0->3
        # if pwm_freq in range(0,4):
        #     c2 = c2 | (pwm_freq << 6)
        # # set output stage 0->2
        # if output_stage in range(0,2):
        #     c2 = c2 | (output_stage << 4)
        # # set power_mode 0->3
        # if power_mode in range(0, 4):
        #     c2 = c2 | power_mode
        # # set Hysteresis 0->3
        # if hyst in range(0, 4):
        #     c2 = c2 | (hyst << 2)

        self.i2c.writeto(ADDRESS, bytes([CONF_REG, c1, c2]))

    def get_status(self):
        """Get the status of the ASL560L

        Returns:
            Dict: Dictionary of the different statuses
        """
        self.i2c.writeto(ADDRESS, bytes([STATUS_REG]))
        status = int.from_bytes(self.i2c.readfrom(ADDRESS, 1), 'big')
        mh = bool(status & 0b00001000)
        ml = bool(status & 0b00010000)
        md = bool(status & 0b00100000)
        self.i2c.writeto(ADDRESS, bytes([AGC_REG]))
        agc = int.from_bytes(self.i2c.readfrom(ADDRESS, 1), 'big')

        return {
            "magnetDetected": md, 
            "magnetTooWeak": ml, 
            "magnetTooStrong": mh,
            "agc": agc
        }

    def is_ok(self) -> bool:
        """Reads the status values to determine if the magnet is able to be read.
        Returns:
            bool: True if the magnet can be detercted accurately
        """
        self.i2c.writeto(ADDRESS, bytes([STATUS_REG]))
        status = int.from_bytes(self.i2c.readfrom(ADDRESS, 1), 'big')
        mh = bool(status & 0b00001000)
        ml = bool(status & 0b00010000)
        md = bool(status & 0b00100000)

        if md and not mh and not ml:
            return True
        else:
            return False

    def get_angle(self) -> int:
        """Get the raw 12bit angle value
        Returns:
            int: 0->4095
        """
        self.i2c.writeto(ADDRESS, bytes([ANGLE_REG]))
        return int.from_bytes(self.i2c.readfrom(ADDRESS, 2), 'big')
        
        
    def get_angle_degrees(self):
        """Get the angle in degrees, IF the magnet can be read

        Returns:
            float: Degrees (0->359) if magnet can be read otherwise None.
        """
        if self.is_ok():
            return (float(self.get_angle()))
        else:
            # if the magnet cannot be read then the angle is unreliable, so return None.
            return None

    def get_angle_degrees_fast(self):
        """Get the angle in degrees

        Returns:
            float: Degrees (0->359) if magnet can be read otherwise None.
        """
        self.i2c.writeto(ADDRESS, b'\x0c')
        return (float(int.from_bytes(self.i2c.readfrom(ADDRESS, 2), 'big')))

    def get_conf(self):
        """Internal healper function to read the raw config value

        Returns:
            int: raw value
        """
        self.i2c.writeto(ADDRESS, bytes([CONF_REG]))
        return int.from_bytes(self.i2c.readfrom(ADDRESS, 2), 'big')