#include "mbed.h"
#include "main.h"
#include "SWO.h"
#include "pca9685.h"
#include "tusb322.h"
#include "ds3231.h"
#include "ioc.h"
#include "animation.h"

I2C i2c(PA_10, PA_9);
// SWO_Channel swo("swo");

bool Tick = false;

void RtcCallback(void) {
    Tick = true;
}

int main() {
    
    // Start I2C at 400kHz for DS3231 
    i2c.frequency(400000);
    
    // Start with HV PSU disabled
    HV_EnableOutput(false);
    
    TUSB322_Init();
    PCA9685_Init();
    DS3231_Init(RtcCallback);
    
    // Enable HV PSU
    HV_EnableOutput(true);
    
    // Set PCA9685 input voltage to highest possible
    PCA9685_SetVoltage(1.0);
    
    // swo.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
    
    // Bump I2C frequency to 1MHz
    i2c.frequency(1000000);

    // DS3231_SetTime(0, 55, 6, true);
    // DS3231_SetDate(0, 2, 12, 18, 0);

    int currMinute = 0, currHour = 0;
    int nextSecond, nextMinute, nextHour;
    int rngTick = 0;

    while(1) {

        // Animate_Cycle_Basic();
        // Animate_Cycle_Analog();
        // Animate_Cycle_Low_Pwm();
        // Animate_Cycle_Pwm();
        // Animate_Cycle_Fade();
        // Animate_Cycle_Fade_Random();
        // Animate_Cycle_Fast();
        // Animate_Cycle_Fast_Random();

        rngTick = rand() % 60;

        if (Tick) {
            
            Tick = false;

            rngTick--;
            if (rngTick == 0) {



                rngTick = rand() % 60;
            }

            DS3231_GetTime(&nextSecond, &nextMinute, &nextHour);

            // Fade dot/digits to show the updated time
            for (int i = 0; i < PCA9685_Max_Brightness; i += 4) {

                if (nextMinute % 10 != currMinute % 10) {
                    PCA9685_SetDigit(3, currMinute % 10, PCA9685_Max_Brightness - i);
                    PCA9685_SetDigit(3, nextMinute % 10, PCA9685_Min_Brightness + i);
                }

                if (nextMinute / 10 != currMinute / 10) {
                    PCA9685_SetDigit(2, currMinute / 10, PCA9685_Max_Brightness - i);
                    PCA9685_SetDigit(2, nextMinute / 10, PCA9685_Min_Brightness + i);
                }

                if (nextHour % 10 != currHour % 10 ) {
                    PCA9685_SetDigit(1, currHour % 10, PCA9685_Max_Brightness - i);
                    PCA9685_SetDigit(1, nextHour % 10, PCA9685_Min_Brightness + i);
                }

                if (nextHour / 10 != currHour / 10) {
                    PCA9685_SetDigit(0, currHour / 10, PCA9685_Max_Brightness - i);
                    if (nextHour / 10 != 0) {
                        PCA9685_SetDigit(0, nextHour / 10, PCA9685_Min_Brightness + i);
                    }
                }

                PCA9685_SetDot(nextSecond % 2 ? i : PCA9685_Max_Brightness - i);

                wait(0.0007);
            }

            currMinute = nextMinute;
            currHour = nextHour;
        }

        // Delay a bit to avoid constant polling
        wait(0.001);
    }
}

void I2C_Write(int DeviceAddress, char RegAddress, char *Data, int Length) {
    char buffer[I2C_MAX_BUFFER+1] = {0};

    if (Length > I2C_MAX_BUFFER) LED_Fault(1);

    buffer[0] = RegAddress;
    memcpy(&buffer[1], Data, Length);

    i2c.write(DeviceAddress << 1, buffer, Length + 1);
}

void I2C_Read(int DeviceAddress, char RegAddress, char *Data, int Length) {
    
    i2c.write(DeviceAddress << 1, &RegAddress, 1);
    i2c.read(DeviceAddress << 1, Data, Length);
}