#define Version 100     //Version 1.00
#define _SUPPRESS_PLIB_WARNING                                      // required for XC1.33  Later compiler versions will need PLIB to be installed
#include <plib.h>                                                   // the pre Harmony peripheral libraries
#include "CFunctions.h"

#define T1_OVERFLOW        0
#define T1_OVERFLOW2       1
#define T2_OVERFLOW        2
#define T4_OVERFLOW        3
#define OLD_T2CON          4
#define OLD_TMR2           5
#define OLD_PR2            6
#define OLD_T3CON          7
#define OLD_TMR3           8
#define OLD_PR3            9
#define OLD_T4CON          10
#define OLD_TMR4           11
#define OLD_PR4            12
#define OLD_T5CON          13
#define OLD_TMR5           14
#define OLD_PR5            15

void T1Int(void) {
    CFuncRam[T1_OVERFLOW] += 0x10000;
    if( !CFuncRam[T1_OVERFLOW] )
        ++CFuncRam[T1_OVERFLOW2];
}

__attribute__((noinline)) void getFPC(void *a, void *b, volatile unsigned int *c) {
    *c = (unsigned int) (__builtin_return_address (0) - (b -a)) ;
}

long long int main(long long int* function, long long int* parameter){
//    long long ret;
    if( *function == 0 ) { // initialise timers 2-3 to count TCXO pulses and set up T1 interrupt
        // set up T1Int for 1kHz
        volatile unsigned int libAddr;
        getFPC(NULL,&&getFPCLab,&libAddr) ; // warning can be ignored, stupid editor
        getFPCLab: { }
        CFuncT1=(unsigned int)&T1Int + libAddr;
        mT1SetIntPriority(1);
        mT1ClearIntFlag();
        mT1IntEnable(1);

        CFuncRam[OLD_T2CON] = T2CON;
        CFuncRam[OLD_TMR2] = TMR2;
        CFuncRam[OLD_PR2] = PR2;
        CFuncRam[OLD_T3CON] = T3CON;
        CFuncRam[OLD_TMR3] = TMR3;
        CFuncRam[OLD_PR3] = PR3;
        CFuncRam[OLD_T4CON] = T4CON;
        CFuncRam[OLD_TMR4] = TMR4;
        CFuncRam[OLD_PR4] = PR4;
        CFuncRam[OLD_T5CON] = T5CON;
        CFuncRam[OLD_TMR5] = TMR5;
        CFuncRam[OLD_PR5] = PR5;

        T2CON = 0;
        T3CON = 0;
        TMR2 = 0;
        TMR3 = 0;
        PR2 = 0xFFFF;
        PR3 = 0xFFFF;
        IFS0bits.T3IF = 0;
        CFuncRam[T2_OVERFLOW] = 0;

        T2CKR = 0; // set RPD2 = T2CK
        T2CON = 0x800A;
        T3CON = 0;
    } else if( *function == 1 ) { // initialise timer 1 (with interrupt overflow) for external clock and timer 4/5 for CPU clock
        T1CON = 0;
        T4CON = 0;
        // clear variables
        CFuncRam[T1_OVERFLOW] = 0;
        CFuncRam[T1_OVERFLOW2] = 0;
        TMR1 = 0;
        TMR4 = 0;
        TMR5 = 0;
        PR1 = 0xFFFF;
        PR4 = 0xFFFF;
        PR5 = 0xFFFF;
        IFS0bits.T5IF = 0;
        CFuncRam[T4_OVERFLOW] = 0;

        // set up timer1
        T1CON = 0x8002;

        // set up timer4/5
        T4CKR = 2; // set RPB8 = T4CK
        T4CON = 0x8008;
        T5CON = 0;
    } else if( *function == 2 ) { // initialise timer 1 (with interrupt overflow) for CPU clock and timer 4/5 for external clock
        T1CON = 0;
        T4CON = 0;
        // clear variables
        CFuncRam[T1_OVERFLOW] = 0;
        CFuncRam[T1_OVERFLOW2] = 0;
        TMR1 = 0;
        TMR4 = 0;
        TMR5 = 0;
        PR1 = 0xFFFF;
        PR4 = 0xFFFF;
        PR5 = 0xFFFF;
        IFS0bits.T5IF = 0;
        CFuncRam[T4_OVERFLOW] = 0;

        // set up timer1
        T1CON = 0x8000;

        // set up timer4/5
        T4CKR = 2; // set RPB8 = T4CK
        T4CON = 0x800A;
        T5CON = 0;
    } else if( *function == 3 ) { // read timer counts
        unsigned char if1b, if1a, if2b, if2a, if4b, if4a;
        mT1IntEnable(0);
        if1b = IFS0bits.T1IF;
        if2b = IFS0bits.T3IF;
        if4b = IFS0bits.T5IF;
        parameter[0] = TMR1 | (unsigned long)CFuncRam[T1_OVERFLOW] | (((unsigned long long)CFuncRam[T1_OVERFLOW2]) << 32);
        parameter[1] = TMR2;
        parameter[2] = TMR4;
        if1a = IFS0bits.T1IF;
        if2a = IFS0bits.T3IF;
        if4a = IFS0bits.T5IF;
        mT1IntEnable(1);
        if( if1a && !if1b )
            parameter[0] = (parameter[0] & 0xFFFFFFFFFFFF0000UL) + 0x10000;
        if( if2a ) {
            if( !if2b )
                parameter[1] = 0;
            ++CFuncRam[T2_OVERFLOW];
            IFS0bits.T3IF = 0;
        }
        if( if4a ) {
            if( !if4b )
                parameter[2] = 0;
            ++CFuncRam[T4_OVERFLOW];
            IFS0bits.T5IF = 0;
        }
        parameter[1] |= ((unsigned long long)CFuncRam[T2_OVERFLOW]) << 32;
        parameter[2] |= ((unsigned long long)CFuncRam[T4_OVERFLOW]) << 32;
        return 3;
    } else if( *function == 4 ) { // set up OC1 - parameter[0] is start count for pulse, parameter[1] is finish and parameter[2] is 1 for continuous and 0 for single
        if( parameter[0] && parameter[1] ) {
            OC1CON = 0;
            OC1R = parameter[0];
            OC1RS = parameter[1];
            IFS0bits.OC1IF = 0;
            RPB14R = 12; // RB14 = OC1
            OC1CON = parameter[2] ? 0x8025 : 0x8024;
        } else {
            OC1CON = 0;
            RPB14R = 0; // RB14 != OC1
            IFS0bits.OC1IF = 0;
        }
    } else if( *function == 5 ) { // set up OC2 - parameter[0] is start count for pulse, parameter[1] is finish and parameter[2] is 1 for continuous and 0 for single
        if( parameter[0] && parameter[1] ) {
            OC2CON = 0;
            OC2R = parameter[0];
            OC2RS = parameter[1];
            IFS0bits.OC2IF = 0;
            RPD1R = 11; // RD1 = OC2
            OC2CON = parameter[2] ? 0x8025 : 0x8024;
        } else {
            OC2CON = 0;
            RPD1R = 0; // RD1 != OC2
            IFS0bits.OC2IF = 0;
        }
    } else if( *function == 6 ) { // get OC1/OC2 status
        unsigned char a = IFS0bits.OC1IF, b = IFS0bits.OC2IF;
        return a | (b<<1);
    } else if( *function == 7 ) { // set up input capture
        IC4R = 0; // IC4 = RPD3
        IFS0bits.IC4IF = 0;
        switch(parameter[0]) {
            case 1:
                IC4CON = 0x8103;
                return 1;
            case 2:
                IC4CON = 0x8102;
                return 2;
            default:
                IC4CON = 0;
                break;
        }
    } else if( *function == 8 ) { // get input capture
        if( IFS0bits.IC4IF || IC4CONbits.ICBNE ) {
            unsigned long ic4buf = IC4BUF;
            parameter[0] = ic4buf | ((unsigned long long)CFuncRam[T2_OVERFLOW]) << 32;
            if( IFS0bits.T3IF && ic4buf < 0x80000000UL )
                parameter[0] += 0x100000000ULL;
            else if( !IFS0bits.T3IF && TMR2 < ic4buf )
                parameter[0] -= 0x100000000ULL;
            IFS0bits.IC4IF = 0;
            return 1;
        } else {
            return 0;
        }
    } else if( *function == 9 ) { // set backlight PWM
        RPD15R = 0xB; // RD15 = OC4
        OC4CON = 0x8006;
        OC4RS = parameter[0] * 655;
    } else if( *function == 10 ) { // restore old TMR2/3
        T2CON = 0;
        IFS0bits.T2IF = 0;
        PR2 = CFuncRam[OLD_PR2];
        TMR2 = CFuncRam[OLD_TMR2];
        T2CON = CFuncRam[OLD_T2CON];
        T3CON = 0;
        IFS0bits.T3IF = 0;
        PR3 = CFuncRam[OLD_PR3];
        TMR3 = CFuncRam[OLD_TMR3];
        T3CON = CFuncRam[OLD_T3CON];
    } else if( *function == 11 ) { // restore old TMR4/5
        T4CON = 0;
        IFS0bits.T4IF = 0;
        PR4 = CFuncRam[OLD_PR4];
        TMR4 = CFuncRam[OLD_TMR4];
        T4CON = CFuncRam[OLD_T4CON];
        T5CON = 0;
        IFS0bits.T5IF = 0;
        PR5 = CFuncRam[OLD_PR5];
        TMR5 = CFuncRam[OLD_TMR5];
        T5CON = CFuncRam[OLD_T5CON];
    }
    return 0;
 }
