#include "i2c.h"

#define PCA9685_ADDR    0x40

/* PCA9685 Registers */
#define PCA9685_MODE1       0x00
#define PCA9685_PRESCALE    0xFE
#define PCA9685_LED0_ON_L   0x06   /* Base register for channel 0 */

/* MODE1 bits */
#define MODE1_SLEEP         0x10
#define MODE1_AI            0x20   /* Auto-increment */

/* Timing */
#define PCA9685_OSC_HZ      25000000UL
#define PCA9685_PWM_FREQ_HZ 50UL
#define PCA9685_COUNTS      4096UL

/* Prescaler = round(osc / (4096 * freq)) - 1 */
#define PCA9685_PRESCALER   ((uint8_t)(((PCA9685_OSC_HZ + \
                             (PCA9685_COUNTS * PCA9685_PWM_FREQ_HZ / 2)) \
                             / (PCA9685_COUNTS * PCA9685_PWM_FREQ_HZ)) - 1))
                             /* = 121 = 0x79 */

/** Write a single register on the PCA9685 */
static oc_i2c_status_t pca9685_write_reg(uint8_t reg, uint8_t val)
{
    uint8_t buf[2] = { reg, val };
    return oc_i2c_write(PCA9685_ADDR, buf, 2);
}

/** Initialize PCA9685 for 50 Hz servo PWM */
oc_i2c_status_t pca9685_init(void)
{
    oc_i2c_status_t ret;

    /* 1. Put to sleep so we can write the prescaler */
    ret = pca9685_write_reg(PCA9685_MODE1, MODE1_SLEEP);
    if (ret != OC_I2C_OK) return ret;

    /* 2. Set prescaler for 50 Hz */
    ret = pca9685_write_reg(PCA9685_PRESCALE, PCA9685_PRESCALER);
    if (ret != OC_I2C_OK) return ret;

    /* 3. Wake up with auto-increment enabled */
    ret = pca9685_write_reg(PCA9685_MODE1, MODE1_AI);
    if (ret != OC_I2C_OK) return ret;

    /* 4. Wait >= 500 µs for oscillator to stabilise */
    /* Replace with your platform delay: delay_us(500); */
    // Fixme: Add proper delay
    for (volatile uint32_t i = 0; i < 5000; i++) {}

    return OC_I2C_OK;
}

/**
 * Set a servo channel PWM pulse width.
 *
 * @param i2c       OC I2C device handle
 * @param channel   PCA9685 channel (0–15)
 * @param pulse_us  Pulse width in microseconds (e.g. 1500 for center)
 */
oc_i2c_status_t pca9685_set_servo_us( uint8_t   channel,
                                      uint16_t  pulse_us)
{
    /* Convert µs to 12-bit count
     * period_us = 1,000,000 / 50 = 20,000 µs  → 4096 counts
     * count = pulse_us * 4096 / 20000
     */
    uint16_t off_count = (uint16_t)((uint32_t)pulse_us * PCA9685_COUNTS
                                    / (1000000UL / PCA9685_PWM_FREQ_HZ));

    uint8_t reg = (uint8_t)(PCA9685_LED0_ON_L + channel * 4u);

    uint8_t buf[5] = {
        reg,
        0x00,                           /* LEDn_ON_L  – ON at count 0  */
        0x00,                           /* LEDn_ON_H                   */
        (uint8_t)(off_count & 0xFF),    /* LEDn_OFF_L                  */
        (uint8_t)(off_count >> 8u),     /* LEDn_OFF_H                  */
    };

    return oc_i2c_write(PCA9685_ADDR, buf, 5);
}

/* -----------------------------------------------------------------------
 * Example usage
 * ----------------------------------------------------------------------- */
void servo_example(void)
{
    oc_i2c_init();
    pca9685_init();

    /* Center position: 1500 µs → count = 307 */
    pca9685_set_servo_us(0, 1500);

    /* Full left:  1000 µs → count = 204 */
    pca9685_set_servo_us(0, 1000);

    /* Full right: 2000 µs → count = 409 */
    pca9685_set_servo_us(0, 2000);
}