#include "mbed.h"
#include "MODSERIAL.h"
#include "MSCFileSystem.h"

  
//Serial uart1(p9, p10);  // tx, rx
//Serial uart2(p13, p14);
//Serial uart3(p28, p27);

#define UART1_BAUD 9600 // GPS G:
#define TX1_PIN p9
#define RX1_PIN p10

#define UART2_BAUD 115200 //EMS E:
#define TX2_PIN p13
#define RX2_PIN p14

#define UART3_BAUD 115200 //EFIS F:
#define TX3_PIN p28
#define RX3_PIN p27

#define BUFSIZE 512 // 256 is MODSERIAL default

// GLOBALS!!!
bool sync;
bool bufOVF;
bool done;
int msg1;
int msg2;
int msg3;
int n;

char RX1[BUFSIZE];
char RX2[BUFSIZE];
char RX3[BUFSIZE];

// * Note that \ for multiline macro is not supported yet
#define fileprint(fh, buffer, fmt, ...)  do { n=sprintf(buffer, fmt, ##__VA_ARGS__);  fh->write(buffer,n); } while (0)
//#define fileprint(fh, buffer, fmt)  do { n=sprintf(buffer, fmt);  fh->write(buffer,n); } while (0)    

DigitalOut led1(LED1);
DigitalOut led2(LED2);

Ticker tick1;

MODSERIAL pc(USBTX, USBRX);
MODSERIAL uart1(TX1_PIN, RX1_PIN, BUFSIZE);
MODSERIAL uart2(TX2_PIN, RX2_PIN, BUFSIZE);
MODSERIAL uart3(TX3_PIN, RX3_PIN, BUFSIZE);

//SDFileSystem sd(p5, p6, p7, p8, "sd", p22, SDFileSystem::SWITCH_NEG_NC); // mosi, miso, sclk, cs
MSCFileSystem fs("fs"); 
 
 // CALLBACKS...
 
void rxOvrflow(MODSERIAL_IRQ_INFO *sii) {
    //error("RX buffer overflow!");
    bufOVF=true;
}
 
void pcRX(MODSERIAL_IRQ_INFO *sii){
    done=true;
}
    
void uart1RX(MODSERIAL_IRQ_INFO *sii) {
     MODSERIAL *sp = sii->serial;
     msg1=sp->move(RX1,BUFSIZE);
}
 
void uart2RX(MODSERIAL_IRQ_INFO *sii) {
     MODSERIAL *sp = sii->serial;
     msg2=sp->move(RX2,BUFSIZE);
}

void uart3RX(MODSERIAL_IRQ_INFO *sii) {
     MODSERIAL *sp = sii->serial;
     msg3=sp->move(RX3,BUFSIZE);
}

void filesync(void) {
    sync=true;
}
int next_file_number() {
    FileHandle* fh;
    char fname[16];
    bool fileexists=true;
    int i=0;

    printf("Create filehandle to test if file exists...\n");
    sprintf(fname,"Log%03d.txt",i);
    printf("Testing for file %s\n",fname);
    do {
        sprintf(fname,"Log%03d.txt",i);
        fh = fs.open(fname, O_RDONLY);
        if (fh == NULL) {
            fileexists=false;
        } else {
            i++;
            fh->close();
        }
    } while (fileexists==true);   
    return(i);
}


int main() {
    int i;
    int n;
    char strbuf[BUFSIZE];
    char fname[16];
    FileHandle* fh;

// define usb-serial first since stdout (printf) goes here!
    pc.baud(115200);
    pc.attach(&pcRX, MODSERIAL::RxIrq);
 
// start filesystem before the input serial data
// so buffers don't overflow during setup!
    i=next_file_number();
    pc.printf("\r\n\r\nHData Logger Started...\r\n");   
    sprintf(fname,"Log%03d.txt",i);
    fh = fs.open(fname, O_WRONLY | O_CREAT | O_TRUNC);
    if(fh == NULL) {
        error("Could not open file for write\n");
    }
    fileprint(fh, strbuf, "Starting Log:\n");
    fileprint(fh, strbuf, "filename = %s\n",fname);
    //n=sprintf(strbuf,"Starting Log:\n");
    //fh->write(strbuf,n);

    msg1=0;
    msg2=0;
    msg3=0;
    sync=false;
    bufOVF=false;
    tick1.attach(&filesync, 5.0); // sync file system every 5 seconds
    
    uart1.baud(UART1_BAUD);
    uart1.attach(&uart1RX, MODSERIAL::RxAutoDetect);
    uart1.autoDetectChar('\x003');
    uart1.attach(&rxOvrflow, MODSERIAL::RxOvIrq);
    
    uart2.baud(UART2_BAUD);
    uart2.attach(&uart2RX, MODSERIAL::RxAutoDetect);
    uart2.autoDetectChar('\n');
    uart2.attach(&rxOvrflow, MODSERIAL::RxOvIrq);

    uart3.baud(UART3_BAUD);
    uart3.attach(&uart3RX, MODSERIAL::RxAutoDetect);
    uart3.autoDetectChar('\n');
    uart3.attach(&rxOvrflow, MODSERIAL::RxOvIrq);



    done=false;
    //while (!done) {
    while (true) { // always 
        if (msg1>0) {
            //pc.printf("msg1 handler\r\n");
            fileprint(fh, strbuf, "G:");
            //fh->write("G:",2);
            //n=sprintf(strbuf, RX1);
            //msg1=-1;
            fh->write(RX1,msg1);
            msg1=0;
            //fileprint(fh, strbuf, RX1);
            //fh->write(RX1,strlen(RX1));
            led1=!led1;
        }
        if (msg2>0) {
            fileprint(fh, strbuf, "E:");
            fh->write(RX2,msg2);
            msg2=0;
            led1=!led1;
        }
        if (msg3>0) {
            fileprint(fh, strbuf, "F:");
            fh->write(RX3,msg3);
            msg3=0;
            led1=!led1;            
        }
        if (sync) {
            sync=false;
            led2=!led2;
            fh->fsync();
        }
        if (bufOVF) {
            fileprint(fh, strbuf, "BUFFER OVERFLOW!\n");
            uart1.rxBufferFlush();
            uart2.rxBufferFlush();
            uart3.rxBufferFlush();
            bufOVF=false;
        }
    }
    //pc.printf("Done!\r\n");
}