/**
 * pca9685.c
 * PCA9685 16通道PWM驱动器完整实现
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <linux/i2c-dev.h>
#include <linux/i2c.h>
#include <math.h>
#include "pca9685.h"

/* SMBus辅助函数 */
static int i2c_smbus_write_byte_data(int fd, uint8_t reg, uint8_t value) {
    union i2c_smbus_data data;
    data.byte = value;
    struct i2c_smbus_ioctl_data args;
    args.read_write = I2C_SMBUS_WRITE;
    args.command = reg;
    args.size = I2C_SMBUS_BYTE_DATA;
    args.data = &data;
    return ioctl(fd, I2C_SMBUS, &args);
}

static int i2c_smbus_read_byte_data(int fd, uint8_t reg) {
    union i2c_smbus_data data;
    struct i2c_smbus_ioctl_data args;
    args.read_write = I2C_SMBUS_READ;
    args.command = reg;
    args.size = I2C_SMBUS_BYTE_DATA;
    args.data = &data;
    if (ioctl(fd, I2C_SMBUS, &args) < 0) return -1;
    return data.byte & 0xFF;
}

int pca9685_init(pca9685_t *dev, const char *i2c_device, uint8_t addr) {
    uint8_t mode1;
    
    if (dev == NULL || i2c_device == NULL) return PCA9685_ERR_PARAM;
    
    memset(dev, 0, sizeof(pca9685_t));
    dev->addr = addr;
    dev->frequency = 50.0f;
    
    dev->fd = open(i2c_device, O_RDWR);
    if (dev->fd < 0) {
        perror("Failed to open I2C device");
        return PCA9685_ERR_OPEN;
    }
    
    if (ioctl(dev->fd, I2C_SLAVE, addr) < 0) {
        perror("Failed to set I2C slave address");
        close(dev->fd);
        dev->fd = -1;
        return PCA9685_ERR_ADDR;
    }
    
    int ret = i2c_smbus_read_byte_data(dev->fd, PCA9685_MODE1);
    if (ret < 0) {
        fprintf(stderr, "Failed to read MODE1: %s\n", strerror(errno));
        close(dev->fd);
        dev->fd = -1;
        return PCA9685_ERR_INIT;
    }
    
    printf("PCA9685 detected at 0x%02X, MODE1=0x%02X\n", addr, ret);
    
    pca9685_reset(dev);
    usleep(10000);
    
    if (pca9685_write_byte(dev, PCA9685_MODE2, MODE2_OUTDRV | MODE2_OCH) < 0) {
        close(dev->fd);
        dev->fd = -1;
        return PCA9685_ERR_INIT;
    }
    
    mode1 = MODE1_RESTART | MODE1_AI | MODE1_ALLCALL;
    if (pca9685_write_byte(dev, PCA9685_MODE1, mode1) < 0) {
        close(dev->fd);
        dev->fd = -1;
        return PCA9685_ERR_INIT;
    }
    
    usleep(5000);
    pca9685_set_pwm_freq(dev, 50.0f);
    pca9685_all_off(dev);
    
    dev->is_open = true;
    printf("PCA9685 initialized\n");
    return PCA9685_OK;
}

void pca9685_close(pca9685_t *dev) {
    if (dev == NULL || dev->fd < 0) return;
    pca9685_all_off(dev);
    close(dev->fd);
    dev->fd = -1;
    dev->is_open = false;
}

int pca9685_reset(pca9685_t *dev) {
    if (dev == NULL || dev->fd < 0) return PCA9685_ERR_PARAM;
    uint8_t reset_buf[2] = {0x00, 0x06};
    int old_addr = dev->addr;
    ioctl(dev->fd, I2C_SLAVE, PCA9685_GENERAL_CALL);
    write(dev->fd, reset_buf, 2);
    ioctl(dev->fd, I2C_SLAVE, old_addr);
    usleep(10000);
    return PCA9685_OK;
}

int pca9685_write_byte(pca9685_t *dev, uint8_t reg, uint8_t data) {
    if (dev == NULL || dev->fd < 0) return PCA9685_ERR_PARAM;
    if (i2c_smbus_write_byte_data(dev->fd, reg, data) < 0) {
        fprintf(stderr, "Write failed: reg=0x%02X, %s\n", reg, strerror(errno));
        return PCA9685_ERR_WRITE;
    }
    return PCA9685_OK;
}

int pca9685_read_byte(pca9685_t *dev, uint8_t reg, uint8_t *data) {
    if (dev == NULL || dev->fd < 0 || data == NULL) return PCA9685_ERR_PARAM;
    int ret = i2c_smbus_read_byte_data(dev->fd, reg);
    if (ret < 0) {
        fprintf(stderr, "Read failed: reg=0x%02X, %s\n", reg, strerror(errno));
        return PCA9685_ERR_READ;
    }
    *data = (uint8_t)ret;
    return PCA9685_OK;
}

int pca9685_write_bytes(pca9685_t *dev, uint8_t reg, uint8_t *data, uint8_t len) {
    if (dev == NULL || data == NULL || len == 0) return PCA9685_ERR_PARAM;
    for (uint8_t i = 0; i < len; i++) {
        if (pca9685_write_byte(dev, reg + i, data[i]) < 0) return PCA9685_ERR_WRITE;
    }
    return PCA9685_OK;
}

int pca9685_set_pwm_freq(pca9685_t *dev, float freq) {
    uint8_t prescale, old_mode, new_mode;
    
    if (dev == NULL || dev->fd < 0) return PCA9685_ERR_PARAM;
    
    if (freq < PCA9685_MIN_FREQ) freq = PCA9685_MIN_FREQ;
    if (freq > PCA9685_MAX_FREQ) freq = PCA9685_MAX_FREQ;
    
    float prescale_val = (PCA9685_OSC_FREQ / (4096.0f * freq)) - 1.0f;
    prescale = (uint8_t)(prescale_val + 0.5f);
    if (prescale < 3) prescale = 3;
    
    if (pca9685_read_byte(dev, PCA9685_MODE1, &old_mode) < 0) return PCA9685_ERR_READ;
    
    new_mode = (old_mode & ~MODE1_RESTART) | MODE1_SLEEP;
    if (pca9685_write_byte(dev, PCA9685_MODE1, new_mode) < 0) return PCA9685_ERR_WRITE;
    if (pca9685_write_byte(dev, PCA9685_PRESCALE, prescale) < 0) return PCA9685_ERR_WRITE;
    if (pca9685_write_byte(dev, PCA9685_MODE1, old_mode) < 0) return PCA9685_ERR_WRITE;
    
    usleep(5000);
    if (pca9685_write_byte(dev, PCA9685_MODE1, old_mode | MODE1_RESTART | MODE1_AI) < 0) 
        return PCA9685_ERR_WRITE;
    
    dev->frequency = freq;
    printf("PWM frequency: %.1f Hz (prescale: %d)\n", freq, prescale);
    return PCA9685_OK;
}

float pca9685_get_pwm_freq(pca9685_t *dev) {
    return (dev == NULL) ? 0.0f : dev->frequency;
}

int pca9685_set_pwm(pca9685_t *dev, uint8_t channel, uint16_t on, uint16_t off) {
    uint8_t reg_base, data[4];
    
    if (dev == NULL || channel > 15) return PCA9685_ERR_PARAM;
    if (on > 4095) on = 4095;
    if (off > 4095) off = 4095;
    
    reg_base = PCA9685_LED0_ON_L + (channel * 4);
    data[0] = on & 0xFF;
    data[1] = (on >> 8) & 0x0F;
    data[2] = off & 0xFF;
    data[3] = (off >> 8) & 0x0F;
    
    return pca9685_write_bytes(dev, reg_base, data, 4);
}

int pca9685_set_pwm_duty(pca9685_t *dev, uint8_t channel, float duty_percent) {
    uint16_t off_value;
    if (duty_percent < 0.0f) duty_percent = 0.0f;
    if (duty_percent > 100.0f) duty_percent = 100.0f;
    off_value = (uint16_t)((duty_percent * 4096.0f) / 100.0f);
    if (off_value > 4095) off_value = 4095;
    return pca9685_set_pwm(dev, channel, 0, off_value);
}

int pca9685_set_pwm_percent(pca9685_t *dev, uint8_t channel, uint8_t percent) {
    return pca9685_set_pwm_duty(dev, channel, (float)percent);
}

int pca9685_led_on(pca9685_t *dev, uint8_t channel) {
    return pca9685_set_pwm(dev, channel, 0, 4095);
}

int pca9685_led_off(pca9685_t *dev, uint8_t channel) {
    return pca9685_set_pwm(dev, channel, 0, 0);
}

int pca9685_led_full_on(pca9685_t *dev, uint8_t channel) {
    uint8_t reg = PCA9685_LED0_ON_H + (channel * 4);
    uint8_t val;
    if (pca9685_read_byte(dev, reg, &val) < 0) return PCA9685_ERR_READ;
    val |= 0x10;
    return pca9685_write_byte(dev, reg, val);
}

int pca9685_led_full_off(pca9685_t *dev, uint8_t channel) {
    uint8_t reg = PCA9685_LED0_OFF_H + (channel * 4);
    uint8_t val;
    if (pca9685_read_byte(dev, reg, &val) < 0) return PCA9685_ERR_READ;
    val |= 0x10;
    return pca9685_write_byte(dev, reg, val);
}

int pca9685_led_brightness(pca9685_t *dev, uint8_t channel, uint8_t brightness) {
    uint16_t off_value = ((uint16_t)brightness * 4095) / 255;
    return pca9685_set_pwm(dev, channel, 0, off_value);
}

int pca9685_all_off(pca9685_t *dev) {
    if (dev == NULL) return PCA9685_ERR_PARAM;
    if (pca9685_write_byte(dev, PCA9685_ALL_LED_ON_L, 0) < 0) return PCA9685_ERR_WRITE;
    if (pca9685_write_byte(dev, PCA9685_ALL_LED_ON_H, 0) < 0) return PCA9685_ERR_WRITE;
    if (pca9685_write_byte(dev, PCA9685_ALL_LED_OFF_L, 0) < 0) return PCA9685_ERR_WRITE;
    if (pca9685_write_byte(dev, PCA9685_ALL_LED_OFF_H, 0x10) < 0) return PCA9685_ERR_WRITE;
    return PCA9685_OK;
}

int pca9685_all_on(pca9685_t *dev) {
    if (dev == NULL) return PCA9685_ERR_PARAM;
    if (pca9685_write_byte(dev, PCA9685_ALL_LED_ON_L, 0) < 0) return PCA9685_ERR_WRITE;
    if (pca9685_write_byte(dev, PCA9685_ALL_LED_ON_H, 0x10) < 0) return PCA9685_ERR_WRITE;
    if (pca9685_write_byte(dev, PCA9685_ALL_LED_OFF_L, 0) < 0) return PCA9685_ERR_WRITE;
    if (pca9685_write_byte(dev, PCA9685_ALL_LED_OFF_H, 0) < 0) return PCA9685_ERR_WRITE;
    return PCA9685_OK;
}

int pca9685_all_brightness(pca9685_t *dev, uint8_t brightness) {
    uint16_t off_value = ((uint16_t)brightness * 4095) / 255;
    if (dev == NULL) return PCA9685_ERR_PARAM;
    if (pca9685_write_byte(dev, PCA9685_ALL_LED_ON_L, 0) < 0) return PCA9685_ERR_WRITE;
    if (pca9685_write_byte(dev, PCA9685_ALL_LED_ON_H, 0) < 0) return PCA9685_ERR_WRITE;
    if (pca9685_write_byte(dev, PCA9685_ALL_LED_OFF_L, off_value & 0xFF) < 0) return PCA9685_ERR_WRITE;
    if (pca9685_write_byte(dev, PCA9685_ALL_LED_OFF_H, (off_value >> 8) & 0x0F) < 0) return PCA9685_ERR_WRITE;
    return PCA9685_OK;
}

int pca9685_set_mode2(pca9685_t *dev, uint8_t mode2_val) {
    return pca9685_write_byte(dev, PCA9685_MODE2, mode2_val);
}

int pca9685_set_open_drain(pca9685_t *dev, bool enable) {
    uint8_t mode2;
    if (pca9685_read_byte(dev, PCA9685_MODE2, &mode2) < 0) return PCA9685_ERR_READ;
    if (enable) mode2 &= ~MODE2_OUTDRV;
    else mode2 |= MODE2_OUTDRV;
    return pca9685_write_byte(dev, PCA9685_MODE2, mode2);
}

int pca9685_set_output_invert(pca9685_t *dev, bool invert) {
    uint8_t mode2;
    if (pca9685_read_byte(dev, PCA9685_MODE2, &mode2) < 0) return PCA9685_ERR_READ;
    if (invert) mode2 |= MODE2_INVRT;
    else mode2 &= ~MODE2_INVRT;
    return pca9685_write_byte(dev, PCA9685_MODE2, mode2);
}

void pca9685_print_status(pca9685_t *dev) {
    uint8_t mode1, mode2, prescale;
    if (dev == NULL || dev->fd < 0) {
        printf("Device not initialized\n");
        return;
    }
    pca9685_read_byte(dev, PCA9685_MODE1, &mode1);
    pca9685_read_byte(dev, PCA9685_MODE2, &mode2);
    pca9685_read_byte(dev, PCA9685_PRESCALE, &prescale);
    
    printf("\n=== PCA9685 Status ===\n");
    printf("Address: 0x%02X, Freq: %.1f Hz\n", dev->addr, dev->frequency);
    printf("MODE1: 0x%02X, MODE2: 0x%02X, PRESCALE: %d\n", mode1, mode2, prescale);
    printf("=====================\n\n");
}

int pca9685_verify_connection(pca9685_t *dev) {
    uint8_t mode1;
    if (dev == NULL || dev->fd < 0) return PCA9685_ERR_PARAM;
    if (pca9685_read_byte(dev, PCA9685_MODE1, &mode1) < 0) return PCA9685_ERR_READ;
    printf("Connection OK, MODE1=0x%02X\n", mode1);
    return PCA9685_OK;
}