#include "system.h"
#include "usb.h"
#include "i2c.h"

#pragma config CPUDIV = NOCLKDIV    // CPU Clock Divider: NOCLKDIV ==> No CPU System Clock divide, CLKDIV2 ==> CPU System Clock divided by 2
#pragma config USBDIV = OFF         // USB Clock Divider: ON ==> USB clock comes from the OSC1/OSC2 divided by 2
#pragma config FOSC   = HS          // Oscillator: HS ==> High Speed
#pragma config PLLEN  = ON          // Phase Lock Loop: ON ==> Oscillator multiplied by 4
#pragma config FCMEN  = OFF         // Fail-Safe Clock Monitor disabled
#pragma config IESO   = OFF         // Oscillator Switchover mode disabled
#pragma config PWRTEN = OFF         // Power-up Timer disabled
#pragma config BOREN  = ON          // Brown-out Reset enabled in hardware and software
#pragma config BORV   = 30          // Brown-out Reset Voltage bits: VBOR set to 3.0 V nominal
#pragma config WDTEN  = OFF         // Watch dog timer: WDT is controlled by SWDTEN bit of the WDTCON register
#pragma config WDTPS  = 32768       // Watchdog Timer Postscale Select bits: 1:32768
#pragma config MCLRE  = OFF         // MCLR Pin Enable bit:RA3 input pin enabled; MCLR disabled
#pragma config HFOFST = OFF         // HFINTOSC Fast Start-up bit: The system clock is held off until the HFINTOSC is stable.
#pragma config STVREN = ON          // Stack Full/Underflow Reset Enable bit: Stack full/underflow will cause Reset
#pragma config LVP    = OFF         // Single-Supply ICSP Enable bit: Single-Supply ICSP disabled
#pragma config XINST  = OFF         // Extended Instruction Set Enable bit: Instruction set extension and Indexed Addressing mode disabled (Legacy mode)
#pragma config BBSIZ  = OFF         // Boot Block Size Select bit: 1kW boot block size
#pragma config CP0    = OFF         // Code Protection bit: Block 0 not write-protected
#pragma config CP1    = OFF         //
#pragma config CPB    = OFF         // Boot Block Code Protection bit: not write-protected
#pragma config WRT0   = OFF         // Table Write Protection bit: Block 1 not write-protected
#pragma config WRT1   = OFF         //
#pragma config WRTB   = OFF         // Boot Block Write Protection bit: not write-protected
#pragma config WRTC   = OFF         // Configuration Register Write Protection bit: not write-protected
#pragma config EBTR0  = OFF         // Table Read Protection bit: Block 0 not write-protected
#pragma config EBTR1  = OFF         //
#pragma config EBTRB  = OFF         // Boot Block Table Read Protection bit: not write-protected

void SYSTEM_Init(){
    //ANSEL = 0;  //enable digital input buffers
    //ANSELH = 0; //enable digital input buffers
    //WPUA = 0x00; //Disable Pull UPs
    
    // LED init
    LED_1 = 0;
    LED_2 = 0;
    LED_1_TRIS = 0;
    LED_2_TRIS = 0;
    
    // GestIC
    MGC_MCLR = 0;
    MGC_MCLR_TRIS = 0;
    MGC_SCL = 1;
    MGC_SCL_TRIS = 1;
    MGC_SDA_LAT = 1;
    MGC_SDA_TRIS = 1;
    MGC_TS_LAT = 1;
    MGC_TS_TRIS = 1;
    
    // Timer 0 - MGC3030 countdown timer (~1.4 kHz)
    T0CONbits.T0CS = 0; //source is internal instruction cycle
    T0CONbits.T0PS = 0b100; //1:32 prescale
    T0CONbits.PSA = 0; //assign prescale
    T0CONbits.T08BIT = 1; //8bit timer
    
    //MGC_Init(); //Setup MGC3030
}

void MGC_Init(){
    //i2c configuration
    MGC_SSPCON1_REG = 0x28; // I2C master mode
    MGC_SSPSTAT_REG = 0x00; // Slew rate control enabled
    MGC_SSPCON2_REG = 0x28; // Clear Control Bits
    
    MGC_SSPADD_REG = 0x18;  // BRG divider for 400kHz I2C clock
    
    i2c_setbidirectional(); // configure TS line depending on build configuration
    MGC_SSP_BCL_IF = 0;
    MGC_SSP_BCL_IE = 1;     // enable BCL interrupt
    MGC_SSP_IF = 0;
    MGC_SSP_IE = 1;         // enable SSP interrupt
    __delay_ms(2);          // wait ~2ms

    i2cTmr_reset();         //set counters and enable ints
    i2cTmr_clrInt();
    i2cTmr_Int_enable();

    //release and handle MGC reset line
    MGC_MCLR = 1;
    i2c_handleGestICReset();
}

void interrupt SYS_InterruptHigh(void)
{
    USBDeviceTasks();
}

void interrupt isr(void) {
    //MGC SSP interrupt
    if(mgc_checkSSPint()) {
        mgc_clrSSPint();
        mgcSSPintFlag = 1;
    }

    //MGC I2C bus collision
    if(mgc_checkBCLint()) {
        mgc_clrBCLint();
        mgcBCLintFlag++;
    }

    //Timer 1 interrupt routine for MGC reset and TS timing - timer only running when needed
    if(i2cTmr_checkInterrupt()) {
         i2cTmr_reset();
        i2cTmr_clrInt();
		
        if(waitTSline > 1 || countDownI2C > 1 || countDownReset > 1) {
			i2cTmr_start();
		}
        else {
			i2cTmr_stop();
		}
		
        i2cTimerIntFlag = 1;

    }
}