#include "pca9685.h"

extern I2C i2c;
DigitalOut PCA9685_PWM_nEN(PA_7);
DigitalOut PCA9685_VIN_EN(PB_1);
AnalogOut  PCA9685_VIN(PA_5);

void PCA9685_Init(void) {
    // Power up PCA9685 at full voltage with outputs disabled
    PCA9685_VIN.write(1);
    PCA9685_VIN_EN.write(1);
    PCA9685_PWM_nEN.write(1);

    char buffer[sizeof(REG_MODE1)+sizeof(REG_MODE2)+1] = {0};
    buffer[0] = FIELD_OFFSET(PCM9685_REGS, MODE1);
    
    REG_MODE1 *reg1 = (REG_MODE1*)&buffer[1];
    reg1->AI = 1;         // Turn on autoincrement
    reg1->SLEEP = 1;      // Start disabled
    reg1->ALLCALL = 1;    // Enable response to all call address
    
    REG_MODE2 *reg2 = (REG_MODE2*)&buffer[1+sizeof(REG_MODE1)];
    reg2->OUTDRV = 1;     // Configure output for totem pole drive
    
    i2c.write(PCM9685_All_Call << 1, buffer, sizeof(buffer));
    
    char pre_scale[2] = {0};
    pre_scale[0] = FIELD_OFFSET(PCM9685_REGS, PRE_SCALE);
    pre_scale[1] = 0x03;  // Set PWM frequency to 1526 Hz
    
    i2c.write(PCM9685_All_Call << 1, pre_scale, sizeof(pre_scale));
    
    PCA9685_SetDot(0);
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 10; j++) {
            PCA9685_SetDigit(i, j, 0);
        }
    }
    
    reg1->SLEEP = 0;
    
    i2c.write(PCM9685_All_Call << 1, buffer, sizeof(buffer));

    // Enable outputs after configuration has been completed
    PCA9685_PWM_nEN.write(0);
}

void PCA9685_SetVoltage(float Percent) {
    Percent = Percent * 0.6;
    PCA9685_VIN.write(Percent);
}

void PCA9685_SetDigit(int Tube, int Digit, int Brightness) {
    char buffer[sizeof(LED_CTRL)+1] = {0};

    buffer[0] = FIELD_OFFSET(PCM9685_REGS, LED0) + 
        (Tube_Mapping[Tube][Digit][MAP_PIN] * sizeof(LED_CTRL));

    LED_CTRL *reg = (LED_CTRL*)&buffer[1];
    if (Brightness >= PCA9685_Max_Brightness) {
        reg->ON_FULL = 1;
    } else if (Brightness == 0) {
        reg->OFF_FULL = 1;
    } else {
        reg->OFF = Brightness;
    }

    i2c.write(Tube_Mapping[Tube][Digit][MAP_ADDR] << 1, buffer, sizeof(buffer));
}

void PCA9685_SetDot(int Brightness) {
    char buffer[sizeof(LED_CTRL)+1] = {0};
    
    buffer[0] = FIELD_OFFSET(PCM9685_REGS, TUBE_DOT_PIN);
    
    LED_CTRL *reg = (LED_CTRL*)&buffer[1];

    if (Brightness >= PCA9685_Max_Brightness) {
        reg->ON_FULL = 1;
    } else if (Brightness == 0) {
        reg->OFF_FULL = 1;
    } else {
        reg->OFF = Brightness;
    }

    i2c.write(TUBE_DOT_ADDR << 1, buffer, sizeof(buffer));
}

void PCA9685_EnableOutput(bool Enabled) {
    if (Enabled) {
        PCA9685_PWM_nEN.write(0);
    } else {
        PCA9685_PWM_nEN.write(1);
    }
}