#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 RtcTick = false;
bool RefreshTick = false;

void RtcCallback(void) {
    RtcTick = true;
}

void RefreshCallback(void) {
    RefreshTick = true;
}

typedef union {
    struct {
        char Hour10;
        char Hour1;
        char Minute10;
        char Minute1;
    };
} DigitValue;

DigitValue DigitPrev, DigitNext;
int DigitNextValue[4][10] = {0};
int DigitPrevValue[4][10] = {0};
int DotNextValue, DotPrevValue;

bool StartupFlag = true;
bool RuntimeFlag = false;
bool DotIncrement = false;

Ticker StartupTicker;
int StartupTickerIter = 1024;

void StartupTickerCallback(void) {

    DigitNextValue[3][DigitNext.Minute1] += 4;
    DigitNextValue[2][DigitNext.Minute10] += 4;
    DigitNextValue[1][DigitNext.Hour1] += 4;
    if (DigitNext.Hour10 != 0) {
        DigitNextValue[0][DigitNext.Hour10] += 4;
    }
    DotNextValue += 4;

    if (--StartupTickerIter == 0) {
        StartupTicker.detach();
        memcpy(&DigitPrev, &DigitNext, sizeof(DigitValue));
        RuntimeFlag = true;
    }
}

Ticker RuntimeTicker;
int RuntimeTickerIter = 1024;

void RuntimeTickerCallback(void) {

    if (DigitPrev.Minute1 != DigitNext.Minute1) {
        DigitNextValue[3][DigitPrev.Minute1] -= 4;
        DigitNextValue[3][DigitNext.Minute1] += 4;
    }

    if (DigitPrev.Minute10 != DigitNext.Minute10) {
        DigitNextValue[2][DigitPrev.Minute10] -= 4;
        DigitNextValue[2][DigitNext.Minute10] += 4;
    }

    if (DigitPrev.Hour1 != DigitNext.Hour1) {
        DigitNextValue[1][DigitPrev.Hour1] -= 4;
        DigitNextValue[1][DigitNext.Hour1] += 4;
    }

    if (DigitPrev.Hour10 != DigitNext.Hour10) {
        DigitNextValue[0][DigitPrev.Hour10] -= 4;
        if (DigitNext.Hour10 != 0) {
            DigitNextValue[0][DigitNext.Hour10] += 4;
        }
    }

    DotNextValue = DotIncrement ? DotNextValue + 4 : DotNextValue - 4;

    if (--RuntimeTickerIter == 0) {
        RuntimeTicker.detach();
        memcpy(&DigitPrev, &DigitNext, sizeof(DigitValue));
        DotIncrement = !DotIncrement;
    }
}

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);

    // Setup a ticker to refresh the display at 1kHz
    Ticker refreshTicker;
    refreshTicker.attach_us(RefreshCallback, 1000);

    // DS3231_SetTime(0, 55, 6, true);
    // DS3231_SetDate(0, 2, 12, 18, 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();

        if (RefreshTick) {

            RefreshTick = false;

            // On refresh, update the display with new values

            for (int i = 0; i < 4; i++) {
                for (int j = 0; j < 10; j++) {
                    if (DigitNextValue[i][j] != DigitPrevValue[i][j]) {
                        PCA9685_SetDigit(i, j, DigitNextValue[i][j]);
                    }
                }
            }
            if (DotNextValue != DotPrevValue) {
                PCA9685_SetDot(DotNextValue);
            }

            memcpy(DigitPrevValue, DigitNextValue, sizeof(DigitPrevValue));
            DotPrevValue = DotNextValue;
        }

        if (RtcTick) {
            
            RtcTick = false;

            // On RTC 1Hz ticks, get the time and begin update

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

            DigitNext.Minute1 = nextMinute % 10;
            DigitNext.Minute10 = nextMinute / 10;
            DigitNext.Hour1 = nextHour % 10;
            DigitNext.Hour10 = nextHour / 10;

            if (StartupFlag) {

                // Fade in the current time
                StartupTicker.attach_us(StartupTickerCallback, 900);
                StartupFlag = false;
            } 

            if (RuntimeFlag) {

                // Fade in the new time
                RuntimeTickerIter = 1024;
                RuntimeTicker.attach_us(RuntimeTickerCallback, 900);
            }
        }
    }
}

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);
}