#include "driver/i2c.h"
 
#define FUSB302_ADDR 0x22
#define I2C_PORT I2C_NUM_0
 
void initI2C() {
    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = SDA_PIN,
        .scl_io_num = SCL_PIN,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = 400000  // 400kHz
    };
    
    i2c_param_config(I2C_PORT, &conf);
    i2c_driver_install(I2C_PORT, conf.mode, 0, 0, 0);
}

uint8_t fusb302_read(uint8_t reg) {
    uint8_t data;
    i2c_master_write_read_device(I2C_PORT, FUSB302_ADDR, 
                                  &reg, 1, &data, 1, 
                                  pdMS_TO_TICKS(100));
    return data;
}
 
void fusb302_write(uint8_t reg, uint8_t data) {
    uint8_t buf[2] = {reg, data};
    i2c_master_write_to_device(I2C_PORT, FUSB302_ADDR, 
                                buf, 2, 
                                pdMS_TO_TICKS(100));
}
 
// Verify FUSB302 is connected
bool fusb302_detect() {
    uint8_t device_id = fusb302_read(0x01);  // Device ID register
    return (device_id & 0xF0) == 0x90;  // FUSB302 ID
}

void fusb302_init_sink() {
    // Reset the chip
    fusb302_write(0x0C, 0x01);  // RESET register
    vTaskDelay(pdMS_TO_TICKS(10));
    
    // Power on internal oscillator and enable receive
    fusb302_write(0x0B, 0x0F);  // POWER register
    
    // Enable CC1 and CC2 with pull-downs (sink mode)
    fusb302_write(0x02, 0x07);  // SWITCHES0 - enable CC1, CC2, measure CC
    
    // Enable auto-GoodCRC response
    fusb302_write(0x03, 0x04);  // SWITCHES1
    
    // Enable interrupts for messages
    fusb302_write(0x0E, 0x00);  // MASK - unmask all interrupts
    fusb302_write(0x0F, 0x00);  // MASKA
    fusb302_write(0x10, 0x01);  // MASKB
    
    // Reset PD logic
    fusb302_write(0x0C, 0x02);  // RESET - reset PD logic
}

typedef enum {
    PD_STATE_UNATTACHED,
    PD_STATE_ATTACHED,
    PD_STATE_WAIT_CAPS,
    PD_STATE_REQUESTING,
    PD_STATE_READY,
    PD_STATE_ERROR
} pd_state_t;
 
pd_state_t pd_state = PD_STATE_UNATTACHED;
uint32_t source_caps[7];  // Up to 7 PDOs
uint8_t num_pdos = 0;
 
void pd_run() {
    switch (pd_state) {
        case PD_STATE_UNATTACHED:
            if (check_attach()) {
                pd_state = PD_STATE_ATTACHED;
                printf("USB-C attached, waiting for Source Capabilities...\n");
            }
            break;
            
        case PD_STATE_ATTACHED:
            // Enable message reception
            fusb302_write(0x03, 0x25);  // Enable receive
            pd_state = PD_STATE_WAIT_CAPS;
            break;
            
        case PD_STATE_WAIT_CAPS:
            if (check_message_received()) {
                parse_source_capabilities();
                pd_state = PD_STATE_REQUESTING;
            }
            break;
            
        case PD_STATE_REQUESTING:
            // Request the voltage we want
            if (request_voltage(20)) {  // Request 20V
                pd_state = PD_STATE_READY;
                printf("20V negotiated successfully!\n");
            } else {
                printf("Request failed, falling back to 5V\n");
                pd_state = PD_STATE_READY;
            }
            break;
            
        case PD_STATE_READY:
            // Power is available at requested voltage
            // Monitor for disconnection or hard reset
            if (!check_attach()) {
                pd_state = PD_STATE_UNATTACHED;
            }
            break;
    }
}

typedef struct {
    uint8_t type;       // 0=Fixed, 1=Battery, 2=Variable, 3=PPS
    uint16_t voltage;   // mV
    uint16_t current;   // mA
} pdo_t;
 
pdo_t available_pdos[7];
 
void parse_source_capabilities() {
    // Read message from FUSB302 FIFO
    uint8_t header[2];
    fusb302_read_fifo(header, 2);
    
    num_pdos = (header[0] >> 4) & 0x07;  // Number of data objects
    
    for (int i = 0; i < num_pdos; i++) {
        uint32_t pdo;
        fusb302_read_fifo((uint8_t*)&pdo, 4);
        
        available_pdos[i].type = (pdo >> 30) & 0x03;
        
        if (available_pdos[i].type == 0) {  // Fixed supply
            available_pdos[i].voltage = ((pdo >> 10) & 0x3FF) * 50;  // 50mV units
            available_pdos[i].current = (pdo & 0x3FF) * 10;          // 10mA units
            
            printf("PDO %d: %dmV @ %dmA\n", 
                   i+1, available_pdos[i].voltage, available_pdos[i].current);
        }
    }
}

bool request_voltage(uint16_t target_voltage_v) {
    // Find matching PDO
    int pdo_index = -1;
    uint16_t target_mv = target_voltage_v * 1000;
    
    for (int i = 0; i < num_pdos; i++) {
        if (available_pdos[i].voltage == target_mv) {
            pdo_index = i;
            break;
        }
    }
    
    if (pdo_index < 0) {
        printf("Requested voltage not available\n");
        return false;
    }
    
    // Build request message
    uint32_t request = 0;
    request |= (pdo_index + 1) << 28;  // Object position (1-indexed)
    request |= (available_pdos[pdo_index].current / 10) << 10;  // Operating current
    request |= (available_pdos[pdo_index].current / 10);         // Max current
    
    // Send request
    send_pd_message(0x02, &request, 1);  // Message type 0x02 = Request
    
    // Wait for Accept
    if (!wait_for_accept(500)) {
        return false;
    }
    
    // Wait for PS_Ready (voltage has stabilized)
    if (!wait_for_ps_ready(1000)) {
        return false;
    }
    
    return true;
}

// Monitor VBUS voltage via ADC
#define VBUS_ADC_PIN 4
#define VBUS_DIVIDER_RATIO 11.0  // 100k/10k divider
 
float read_vbus_voltage() {
    int raw = analogRead(VBUS_ADC_PIN);
    return (raw * 3.3 / 4095.0) * VBUS_DIVIDER_RATIO;
}
 
// In main loop
float vbus = read_vbus_voltage();
if (vbus > 22.0) {  // 10% over max expected
    // EMERGENCY: Disable power path
    digitalWrite(POWER_ENABLE_PIN, LOW);
    printf("OVERVOLTAGE! VBUS = %.1fV\n", vbus);
}

// Using INA228 or similar for current monitoring
if (current_ma > 3500) {  // 3.5A limit
    disable_output();
    printf("OVERCURRENT! I = %dmA\n", current_ma);
}

void handle_pd_error() {
    // Try soft reset first
    send_pd_message(0x0D, NULL, 0);  // Soft Reset
    vTaskDelay(pdMS_TO_TICKS(100));
    
    if (!check_attach()) {
        // Hard reset - full reinitialization
        fusb302_init_sink();
        pd_state = PD_STATE_UNATTACHED;
    }
}