import machine
import utime
import ustruct
import sys
import max7219

#look at more display driving examples
#https://docs.wokwi.com/parts/wokwi-max7219-matrix#simulator-examples
#

###############################################################################
# Constants

# Registers
REG_DEVID = 0x00
REG_POWER_CTL = 0x2D
REG_DATAX0 = 0x32

# Other constants
DEVID = 0x00
SENSITIVITY_2G = 1.0 / 256  # (g/LSB)
EARTH_GRAVITY = 9.80665     # Earth's gravity in [m/s^2]

X_SEGMENTS = 1
Y_SEGMENTS = 1
NUM_SEGMENTS = X_SEGMENTS * Y_SEGMENTS
fb = [0]*8*NUM_SEGMENTS
YSENSITIVITY_2G = 1.0 / 256  # (g/LSB)
EARTH_GRAVITY = 9.80665     # Earth's gravity in [m/s^2]

print("x-segs",X_SEGMENTS)
print("y-segs",Y_SEGMENTS)
print("NUM-segs",NUM_SEGMENTS)
print("FB",fb)


###############################################################################
# Settings

# Assign init pin to help with digital signal recording readout
init = machine.Pin(0, machine.Pin.OUT)

# Assign chip select (CS) pin (and start it high)
cs = machine.Pin(5, machine.Pin.OUT)

# Initialize SPI
spi = machine.SPI(0,
                  baudrate=1000000,
                  polarity=1,
                  phase=1,
                  bits=8,
                  firstbit=machine.SPI.MSB,
                  sck=machine.Pin(2),
                  mosi=machine.Pin(3),
                  miso=machine.Pin(4))

###############################################################################
# Functions

def reg_write(spi, cs, reg, data):
    """
    Write 1 byte to the specified register.
    """
    
    # Construct message (set ~W bit low, MB bit low)
    msg = bytearray()
    msg.append(0x00 | reg)
    msg.append(data)
    
    # Send out SPI message
    cs.value(0)
    spi.write(msg)
    cs.value(1)


def shiftAll(reg, data):
    """
    Write all bytes to display / byte to the specified register.
    """
    for y in range(0,NUM_SEGMENTS):
        # Construct message (set ~W bit low, MB bit low)
        msg = bytearray()
        msg.append(0x00 | reg)
        msg.append(data)
        # Send out SPI message
        cs.value(0)
        spi.write(msg)
        cs.value(1)
    
def reg_read(spi, cs, reg, nbytes=1):
    """
    Read byte(s) from specified register. If nbytes > 1, read from consecutive
    registers.
    """
    
    # Determine if multiple byte (MB) bit should be set
    if nbytes < 1:
        return bytearray()
    elif nbytes == 1:
        mb = 0
    else:
        mb = 1
    
    # Construct message (set ~W bit high)
    msg = bytearray()
    msg.append(0x80 | (mb << 6) | reg)
    
    # Send out SPI message and read
    cs.value(0)
    spi.write(msg)
    data = spi.read(nbytes)
    cs.value(1)
    
    return data

###############################################################################
# Main


# Start CS pin high
cs.value(1)

# Workaround: perform throw-away read to make SCK idle high
reg_read(spi, cs, REG_DEVID)

# Read device ID to make sure that we can communicate with the ADXL343
data = reg_read(spi, cs, REG_DEVID)
print("Display id is:")
print(data)

if (data != bytearray((DEVID,))):
    print("ERROR: Could not communicate with Display")
    sys.exit()

#################################################################################
# init displays

# Send out INIT before init procedure
init.value(1)
init.value(0)


#// Setup each MAX7219
shiftAll(0x0f, 0x00) #; //display test register - test mode off
shiftAll(0x0b, 0x07) #; //scan limit register - display digits 0 thru 7
shiftAll(0x0c, 0x01) #; //shutdown register - normal operation
shiftAll(0x0a, 0x0f) #; //intensity register - max brightness
shiftAll(0x09, 0x00) #; //decode mode register - No decode

# Send out INIT after init procedure
init.value(0)
init.value(1)
init.value(0)


shiftAll(0x01, 0xff) #; //digit0 write xFF 
shiftAll(0x02, 0x1f) #; //digit0 write xFF 
shiftAll(0x03, 0x3f) #; //digit0 write xFF 
shiftAll(0x04, 0x5f) #; //digit0 write xFF 
shiftAll(0x05, 0x7f) #; //digit0 write xFF 
shiftAll(0x06, 0x9f) #; //digit0 write xFF 
shiftAll(0x07, 0xaf) #; //digit0 write xFF 
shiftAll(0x08, 0xcf) #; //digit0 write xFF 

shiftAll(0x11, 0xff) #; //digit0 write xFF 
shiftAll(0x12, 0x1f) #; //digit0 write xFF 
shiftAll(0x13, 0x3f) #; //digit0 write xFF 
shiftAll(0x14, 0x5f) #; //digit0 write xFF 
shiftAll(0x05, 0x7f) #; //digit0 write xFF 
shiftAll(0x06, 0x9f) #; //digit0 write xFF 
shiftAll(0x07, 0xaf) #; //digit0 write xFF 
shiftAll(0x08, 0xcf) #; //digit0 write xFF 

#reg_write(spi, cs, 0x0f , 0x00) # //display test register - test mode off
#reg_write(spi, cs, 0x0b , 0x07) # //scan limit register - display digits 0 thru 7
#reg_write(spi, cs, 0x0c , 0x01) # //shutdown register - normal operation
#reg_write(spi, cs, 0x0a , 0x0f) # //intensity register - max brightness
#reg_write(spi, cs, 0x09 , 0x00) # //decode mode register - No decode

delay = 10.0
print("Display is initialized  ",delay,"s delay")
# Delay after init
utime.sleep(delay)




# Read Power Control register
data = reg_read(spi, cs, REG_POWER_CTL)
print(data)

# Tell ADXL343 to start taking measurements by setting Measure bit to high
data = int.from_bytes(data, "big") | (1 << 3)
reg_write(spi, cs, REG_POWER_CTL, data)

# Test: read Power Control register back to make sure Measure bit was set
data = reg_read(spi, cs, REG_POWER_CTL)
print(data)

# Wait before taking measurements
utime.sleep(2.0)

# Run forever
while True:
    
    # Read X, Y, and Z values from registers (16 bits each)
    data = reg_read(spi, cs, REG_DATAX0, 6)

    # Convert 2 bytes (little-endian) into 16-bit integer (signed)
    acc_x = ustruct.unpack_from("<h", data, 0)[0]
    acc_y = ustruct.unpack_from("<h", data, 2)[0]
    acc_z = ustruct.unpack_from("<h", data, 4)[0]

    # Convert measurements to [m/s^2]
    acc_x = acc_x * SENSITIVITY_2G * EARTH_GRAVITY
    acc_y = acc_y * SENSITIVITY_2G * EARTH_GRAVITY
    acc_z = acc_z * SENSITIVITY_2G * EARTH_GRAVITY

    # Print results
    print("X:", "{:.2f}".format(acc_x), \
          "| Y:", "{:.2f}".format(acc_y), \
          "| Z:", "{:.2f}".format(acc_z))
    
    utime.sleep(0.1)

print("Hello, Pi Pico!")