#include "mbed.h"
#include "SWO.h"
#include "pca9685.h"
#include "tusb322.h"
#include "ds3231.h"

DigitalIn nFault(PA_3);
DigitalIn Usb(PB_4);
DigitalOut HvEn(PA_2);
DigitalOut nNixieEn(PA_7);
DigitalOut AmpEn(PB_1);
DigitalOut LED(PB_6);
AnalogIn Imon(PA_0);
AnalogIn Vout(PA_1);
AnalogOut Aout(PA_5);
I2C i2c(PA_10, PA_9);
SWO_Channel swo("swo");

PCM9685_REGS Regs[3] = {0};

// Calculate the byte offset of a field in a structure of type type.
#define FIELD_OFFSET(type, field) ((uint32_t)(uint32_t*)&(((type *)0)->field))

void Clock_Update(int tube, int digit, int brightness) {
    char buffer[sizeof(LED_CTRL)+1] = {0};
    
    if (!nFault.read()) { LED.write(1); while(1); }

    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 >= 1000) {
        reg->ON_FULL = 1;
    } else if (brightness == 0) {
        reg->OFF_FULL = 1;
    } else {
        reg->OFF = brightness * 4;
    }

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

void Dot_Update(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 == 0) {
        reg->OFF_FULL = 1;
    } else {
        reg->OFF = brightness;
    }

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

void Clock_Init() {
    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));
    
    Dot_Update(0);
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 10; j++) {
            Clock_Update(i, j, 0);
        }
    }
    
    reg1->SLEEP = 0;
    
    i2c.write(PCM9685_All_Call << 1, buffer, sizeof(buffer));
}

void Set_Analog(float voltage) {
    voltage = voltage * 0.6;
    Aout.write(voltage);
}

void USB_Init() {
    char buffer[sizeof(TUSB322_REGS)] = {0};
    
    // Disable UFP accessory support (otherwise IC stays in DRP mode)
    buffer[0] = FIELD_OFFSET(TUSB322_REGS, Status2);
    CONN_STATUS2 *stat = (CONN_STATUS2*)&buffer[1];
    stat->UFP_ACCESSORY = 0x1;
    i2c.write(TUSB322_ADDR << 1, buffer, 2);

    // Disable CC termination to change to UFP mode
    buffer[0] = FIELD_OFFSET(TUSB322_REGS, Control);
    CTRL *ctrl = (CTRL*)&buffer[1];
    ctrl->DISABLE_TERM = 0x1;
    i2c.write(TUSB322_ADDR << 1, buffer, 2);

    // For operation in UFP mode
    ctrl->MODE_SELECT = 0x01;
    i2c.write(TUSB322_ADDR << 1, buffer, 2);

    // Reenable CC termination
    ctrl->DISABLE_TERM = 0x0;
    i2c.write(TUSB322_ADDR << 1, buffer, 2);
}

//#define CURRENT_TEST
#define CYCLE_DISPLAY
#define CYCLE_BASIC
//#define CYCLE_ANALOG
#define CYCLE_PWM
#define CYCLE_FADE
#define CYCLE_FADE_RANDOM
#define CYCLE_FAST
#define CYCLE_FAST_RANDOM

int main() {
    
    // Initialize pins
    i2c.frequency(1000000);
    
    // Power up PCA9685 at 3.3V with outputs disabled
    Aout.write(1);
    AmpEn.write(1);
    nNixieEn.write(1);
    
    // Start with HV PSU disabled
    HvEn.write(0);
    
    USB_Init();
    
    LED.write(1);
    wait(1);
    LED.write(0);
    
    Clock_Init();
    nNixieEn.write(0);
    
    // Enable HV PSU
    HvEn.write(1);
    
    // Reduce PCA9685 voltage
    Set_Analog(1);
    
    swo.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
    
#ifdef CURRENT_TEST
    Dot_Update(100);  
   Clock_Update(3, 1, 100);
   Clock_Update(2, 3, 100);
   Clock_Update(1, 5, 100);
   Clock_Update(0, 7, 100);
#endif

    while(1) {
            
        int i_curr[4];
        int i_next[4];
            
#ifdef CYCLE_DISPLAY
        Set_Analog(1);
            
#ifdef CYCLE_BASIC
        // Switch each digit from 0 to 100%
        for (int i = 0; i < 10; i++) {
            if (i % 2 == 0) {
                Dot_Update(0);
            } else {
                Dot_Update(1000);
            }
            
            for (int j = 0; j < 4; j++) {
                Clock_Update(j, i, 1000);
            }      
            
            wait(0.5);
            
            for (int j = 0; j < 4; j++) {
                Clock_Update(j, i, 0);
            }
        }
#endif
      
#ifdef CYCLE_ANALOG  
        for (int i = 0; i < 10; i++) {
            
            nNixieEn.write(1);
            
            for (int j = 0; j < 4; j++) {    
                Clock_Update(j, i, 1000);
                Dot_Update(1000);
            }
            
            for (double k = 1; k >= 0.1; k -= 0.001) {
                Set_Analog(k);
                wait(0.0001);
            }
            
            nNixieEn.write(0);
            
            for (double k = 0.1; k <= 1; k += 0.001) {
                Set_Analog(k);
                wait(0.001);
            }
            wait(0.2);
            for (double k = 1; k >= 0.1; k -= 0.001) {
                Set_Analog(k);
                wait(0.001);
            }

            nNixieEn.write(1);

            for (double k = 0.1; k <= 1; k += 0.001) {
                Set_Analog(k);
                wait(0.0001);
            }
            
            for (int j = 0; j < 4; j++) {
                Clock_Update(j, i, 0);
                Dot_Update(0);
            }
        }
        
        nNixieEn.write(0);
        Set_Analog(1);
#endif
        
#ifdef CYCLE_PWM
        for (int i = 0; i < 10; i++) {
            for (int k = 0; k <= 1000; k++) {
                for (int j = 0; j < 4; j++) {
                    Clock_Update(j, i, k);
                    Dot_Update(k);
                }
                wait(0.0001);
            }      
            for (int k = 0; k <= 1000; k++) {
                for (int j = 0; j < 4; j++) {
                    Clock_Update(j, i, 1000-k);
                    Dot_Update(1000-k);;
                }
                wait(0.0001);
            }      
            wait(0.2);
        }
#endif
        
#ifdef CYCLE_FADE
        
        Set_Analog(1);
        Dot_Update(0);
        
        for (int i = 0; i < 10; i++) {
            int i_next = (i == 9) ? 0 : i+1;
            for (int k = 0; k <= 1000; k++) {
                for (int j = 0; j < 4; j++) {
                    Clock_Update(j, i, 1000-k);
                    Clock_Update(j, i_next, k);
                }
                if (i % 2 == 0) {
                    Dot_Update(k);
                } else {
                    Dot_Update(1000-k);
                }
                wait(0.00005);
            }        
            wait(0.2);
        }
        
        for (int i = 0; i < 4; i++) {
            for (int j = 0; j < 10; j++) {
                Clock_Update(i, j, 0);
            }
        }
#endif
      
#ifdef CYCLE_FADE_RANDOM  

        Set_Analog(1);
        Dot_Update(0);
        
        for (int i = 0; i < 4; i++) {
            i_curr[i] = rand() % 10;
        }
        
        for (int k = 0; k <= 1000; k++) {
            for (int j = 0; j < 4; j++) {
                Clock_Update(j, i_curr[j], k);
            }
            wait(0.00005);
        }
            
        for (int iter = 0; iter < 10; iter++) {
            
            for (int i = 0; i < 4; i++) {
                do {
                    i_next[i] = rand() % 10;
                } while (i_next[i] == i_curr[i]);
            }
            
            for (int k = 0; k <= 1000; k++) {
                for (int j = 0; j < 4; j++) {
                    Clock_Update(j, i_curr[j], 1000-k);
                    Clock_Update(j, i_next[j], k);
                }
                if (iter % 2 == 0) {
                    Dot_Update(k);
                } else {
                    Dot_Update(1000-k);
                }
                wait(0.00005);
            }
            wait(0.2);
            
            for (int i = 0; i < 4; i++) {
                i_curr[i] = i_next[i];
            }
        }
        for (int k = 1000; k >= 0; k--) {
            for (int j = 0; j < 4; j++) {
                Clock_Update(j, i_curr[j], k);
            }
            wait(0.00005);
        }
#endif

#ifdef CYCLE_FAST

        Set_Analog(1);
        Dot_Update(0);

        for (int k = 0; k < 10; k++) {
            for (int i = 0; i < 10; i++) {
                for (int j = 0; j < 4; j++) {
                    Clock_Update(j, i, 1000);
                }
                if (i % 2 == 0) {
                    Dot_Update(1000);
                } else {
                    Dot_Update(0);
                }
                wait(0.1);
                for (int j = 0; j < 4; j++) {
                    Clock_Update(j, i, 0);
                }
            }
        }
#endif

#ifdef CYCLE_FAST_RANDOM

        Set_Analog(1);
        Dot_Update(0);
        
        for (int i = 0; i < 4; i++) {
            i_curr[i] = rand() % 10;
        }

        for (int k = 0; k < 100; k++) {
            
            for (int i = 0; i < 4; i++) {
                do {
                    i_next[i] = rand() % 10;
                } while (i_next[i] == i_curr[i]);
            }
            
            for (int j = 0; j < 4; j++) {
                Clock_Update(j, i_next[j], 1000);
            }
            
            if (k % 2 == 0) {
                Dot_Update(1000);
            } else {
                Dot_Update(0);
            }
            
            wait(0.1);
            for (int j = 0; j < 4; j++) {
                Clock_Update(j, i_next[j], 0);
            }
            
            for (int i = 0; i < 4; i++) {
                i_curr[i] = i_next[i];
            }
        }

#endif
        
#endif
        
    }
}