#!/usr/bin/python3
import smbus
import os
import sys
import datetime

# I2C channel
I2C_CHANNEL = 1

# I2C Addresses
MCP23008_ADDRESS = 0x20  # GPIO Expander
MCP4725_ADDRESS = 0x60   # DAC for variable voltage

# MCP23008 Registers
MCP23008_REG_IODIR = 0x00
MCP23008_REG_GPIO = 0x09

# MCP23008 GPIO pins
# As per hardware_interface.cpp
# POWER_CHANNEL_1: GPIO 6
# POWER_CHANNEL_2: GPIO 5
# POWER_CHANNEL_3: GPIO 4
# POWER_CHANNEL_4: GPIO 3
POWER_PINS = [3, 4, 5, 6]
# GPIO_VAR_CHANNEL: GPIO 1
VAR_VOLTAGE_PIN = 1

# PWM sysfs paths for RPI
PWM_PATHS = [
    "/sys/class/pwm/pwmchip0/pwm0/",
    "/sys/class/pwm/pwmchip0/pwm1/"
]

def log(message):
    """Log to stdout for systemd journal"""
    timestamp = datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S")
    print(f"{timestamp}: {message}")
    sys.stdout.flush()

def power_off_dc_and_var_enable():
    """Turns off all 4 DC power channels and the variable voltage enable pin on the MCP23008."""
    try:
        log("Starting DC power channels and Variable Voltage disable...")
        
        # Set all GPIO pins on the expander to output
        bus.write_byte_data(MCP23008_ADDRESS, MCP23008_REG_IODIR, 0x00)
        log("Set MCP23008 GPIO pins to output mode")

        # Read the current state of the GPIO register
        current_state = bus.read_byte_data(MCP23008_ADDRESS, MCP23008_REG_GPIO)
        log(f"Current MCP23008 GPIO state: 0x{current_state:02x}")
        
        # Calculate the new state by clearing the bits for power and var voltage
        new_state = current_state
        for pin in POWER_PINS:
            new_state &= ~(1 << pin)
        new_state &= ~(1 << VAR_VOLTAGE_PIN)
        
        log(f"New MCP23008 GPIO state will be: 0x{new_state:02x}")

        # Write the new state to the GPIO register
        bus.write_byte_data(MCP23008_ADDRESS, MCP23008_REG_GPIO, new_state)
        log("✓ DC power channels and Variable Voltage enable pin are off.")

    except Exception as e:
        log(f"✗ Failed to turn off DC/Var power channels via MCP23008: {e}")
        raise

def power_off_variable_dac():
    """Sets the variable voltage DAC output to 0V."""
    try:
        log("Starting Variable voltage DAC shutdown...")
        
        # MCP4725 command to write 0V to DAC register (not EEPROM)
        bus.write_i2c_block_data(MCP4725_ADDRESS, 0x40, [0x00, 0x00])
        log("✓ Variable voltage DAC set to 0V.")
        
    except Exception as e:
        log(f"✗ Failed to turn off variable voltage DAC: {e}")
        raise

def power_off_pwm():
    """Turns off both PWM channels by setting their duty cycle to 0."""
    log("Starting PWM channels shutdown...")
    
    for i, pwm_path in enumerate(PWM_PATHS):
        duty_cycle_path = os.path.join(pwm_path, "duty_cycle")
        try:
            if os.path.exists(duty_cycle_path):
                with open(duty_cycle_path, "w") as f:
                    f.write("0")
                log(f"✓ Set duty cycle to 0 for PWM channel {i} ({pwm_path})")
            else:
                log(f"⚠ Warning: PWM path not found, skipping: {duty_cycle_path}")
        except Exception as e:
            log(f"✗ Failed to turn off PWM at {pwm_path}: {e}")
            # Don't raise here - PWM failure shouldn't stop other shutdown operations

def check_i2c_devices():
    """Check if our I2C devices are accessible"""
    log("Checking I2C device accessibility...")
    
    try:
        # Test MCP23008 (GPIO expander)
        bus.read_byte_data(MCP23008_ADDRESS, MCP23008_REG_GPIO)
        log(f"✓ MCP23008 at 0x{MCP23008_ADDRESS:02x} accessible")
    except Exception as e:
        log(f"✗ MCP23008 at 0x{MCP23008_ADDRESS:02x} not accessible: {e}")
        return False
    
    try:
        # Test MCP4725 (DAC) - reading should work
        bus.read_i2c_block_data(MCP4725_ADDRESS, 0x00, 1)
        log(f"✓ MCP4725 at 0x{MCP4725_ADDRESS:02x} accessible")
    except Exception as e:
        log(f"✗ MCP4725 at 0x{MCP4725_ADDRESS:02x} not accessible: {e}")
        return False
    
    return True

if __name__ == "__main__":
    log("=== SM Pro Power-Off Sequence Starting ===")
    
    try:
        # Initialize I2C bus
        log(f"Initializing I2C bus {I2C_CHANNEL}...")
        bus = smbus.SMBus(I2C_CHANNEL)
        log("✓ I2C bus initialized")
        
        # Check device accessibility
        if not check_i2c_devices():
            log("✗ Some I2C devices not accessible, proceeding anyway...")
        
        # Execute power-off sequence
        power_off_dc_and_var_enable()
        power_off_variable_dac()
        power_off_pwm()
        
        # Clean up
        bus.close()
        log("✓ I2C bus closed")
        log("=== SM Pro Power-Off Sequence Complete Successfully ===")
        
    except Exception as e:
        log(f"✗ Critical error during power-off sequence: {e}")
        log(f"Error type: {type(e).__name__}")
        
        # Try to close bus even on error
        try:
            bus.close()
            log("I2C bus closed after error")
        except:
            pass
        
        sys.exit(1)