"""
MMC5603

Micropython library for the MMC5603NJ（Magnetic）
=======================================================

#Preliminary composition                20221017

dahanzimin From the Mixly Team
"""
import time,math
from micropython import const

MMC5603_ADDRESS              = const(0x30)

MMC5603_REG_DATA             = const(0x00)
MMC5603_REG_TMP              = const(0x09)
MMC5603_REG_ODR              = const(0x1A)
MMC5603_REG_CTRL0            = const(0x1B)
MMC5603_REG_CTRL1            = const(0x1C)
MMC5603_REG_CTRL2            = const(0x1D)

MMC5603_REG_X_THD            = const(0x1E)
MMC5603_REG_Y_THD            = const(0x1F)
MMC5603_REG_Z_THD            = const(0x20)
MMC5603_REG_ST_X_VAL         = const(0x27)
MMC5603_REG_DEVICE_ID        = const(0x39)

class MMC5603:
    def __init__(self, i2c_bus):
        self._device = i2c_bus
        self._address = MMC5603_ADDRESS 
        self.raw_x = 0.0
        self.raw_y = 0.0
        self.raw_z = 0.0

        if self._chip_id() != 0x10:         #Check product ID
            raise AttributeError("Cannot find a MMC5603")
        try:                                #Read calibration file
            import magnetic_cal 
            self._offset_x = magnetic_cal._offset_x
            self._offset_y = magnetic_cal._offset_y
            self._offset_z = magnetic_cal._offset_z
        except:
            self._offset_x = 524288
            self._offset_y = 524288
            self._offset_z = 524288
        #print("offset:",self._offset_x ,self._offset_y,self._offset_z)

        self._auto_selftest()               #Auto self-test registers configuration
        self._set()
        self._continuous_mode(0x00, 50)     #Work mode setting
        time.sleep(0.05)
    
    def _wreg(self, reg, val):
        '''Write memory address'''
        self._device.writeto_mem(self._address,reg,val.to_bytes(1, 'little'))

    def _rreg(self, reg,nbytes=1):
        '''Read memory address'''
        return  self._device.readfrom_mem(self._address, reg, nbytes)[0] if nbytes<=1 else self._device.readfrom_mem(self._address, reg, nbytes)[0:nbytes]

    def _chip_id(self):  
        return self._rreg(MMC5603_REG_DEVICE_ID)
            
    def _auto_selftest(self):
        '''Auto self-test registers configuration'''
        st_thd_reg=[0,0,0]
        st_thr_data=[0,0,0]
        #/* Read trim data from reg 0x27-0x29 */
        _buffer=self._rreg(MMC5603_REG_ST_X_VAL,3)
        for i in range(0, 3, 1):
            st_thr_data[i]=(_buffer[i]-128)*32
            if st_thr_data[i] < 0:
                st_thr_data[i]=-st_thr_data[i]
            st_thd=(st_thr_data[i]-st_thr_data[i]//5)//8
            if st_thd > 255:
                st_thd_reg[i]=0xFF
            else:
                st_thd_reg[i]=st_thd
        #/* Write threshold into the reg 0x1E-0x20 */   
        self._wreg(MMC5603_REG_X_THD, st_thd_reg[0])    
        self._wreg(MMC5603_REG_Y_THD, st_thd_reg[1])    
        self._wreg(MMC5603_REG_Z_THD, st_thd_reg[2])
        
    def _set(self):
        '''Do SET operation'''
        self._wreg(MMC5603_REG_CTRL0, 0X08)
        time.sleep(0.005)

    def _continuous_mode(self,bandwith,sampling_rate):
        '''Work mode setting'''
        self._wreg(MMC5603_REG_CTRL1, bandwith)
        self._wreg(MMC5603_REG_ODR, sampling_rate)
        self._wreg(MMC5603_REG_CTRL0, 0X80|0X20)    
        self._wreg(MMC5603_REG_CTRL2, 0x10)
        
    def getdata(self): 
        _buf=self._rreg( MMC5603_REG_DATA,9)
        #/* Transform to unit Gauss */
        self.raw_x=(_buf[0] << 12) | (_buf[1] << 4) | (_buf[6] >> 4)
        self.raw_y=(_buf[2] << 12) | (_buf[3] << 4) | (_buf[7] >> 4)
        self.raw_z=(_buf[4] << 12) | (_buf[5] << 4) | (_buf[8] >> 4)
        return (-0.0625*(self.raw_x-self._offset_x), -0.0625*(self.raw_y-self._offset_y), -0.0625*(self.raw_z-self._offset_z))

    def calibrate(self):
        print("The magnetic field will be calibrated")
        print("Please pick up the board and rotate the '8' shape in the air")
        time.sleep(2)
        self.getdata()
        min_x = max_x = self.raw_x
        min_y = max_y = self.raw_y
        min_z = max_z = self.raw_z
        ticks_start = time.ticks_ms()
        while (time.ticks_diff(time.ticks_ms(), ticks_start) < 20000) :
            self.getdata()
            min_x = min(self.raw_x, min_x)
            max_x = max(self.raw_x, max_x)          
            min_y = min(self.raw_y, min_y)
            max_y = max(self.raw_y, max_y)
            min_z = min(self.raw_z, min_z)
            max_z = max(self.raw_z, max_z)
            time.sleep_ms(20)
            if time.ticks_diff(time.ticks_ms(), ticks_start) % 20 ==0:
                print("=",end ="")
        print(" 100%")
        self._offset_x = (max_x + min_x) / 2
        self._offset_y = (max_y + min_y) / 2
        self._offset_z = (max_z + min_z) / 2
        print("Save x_offset:{}, y_offset:{}, z_offset:{}".format(self._offset_x,self._offset_y,self._offset_z))
        s_f = open("magnetic_cal.py", "w+")
        s_f.write("_offset_x="+str(self._offset_x)+"\n")
        s_f.write("_offset_y="+str(self._offset_y)+"\n")
        s_f.write("_offset_z="+str(self._offset_z)+"\n")
        s_f.close()
        time.sleep(2)

    def getstrength(self):
        _x,_y,_z=self.getdata()
        return (math.sqrt(math.pow(_x, 2) + pow(_y, 2) + pow(_z, 2)))

    def getangle(self,upright=False):
        _x,_y,_z=self.getdata()
        if upright: #vertical 
            angle=math.atan2(_z, -_x)*(180 / math.pi) + 180 + 3
            return angle if angle<360 else angle-360
        else:       #horizontal
            angle=math.atan2(_y, -_x)*(180 / math.pi) + 180 + 3
            return angle if angle<360 else angle-360