#include "./mbed/mbed.h"
#include "SDFileSystem.h"
#include "ST7565_LCD.h"
#include "QEI.h"

#define BAT_GAIN    5.0     
#define BKL_TH_OFF  0.83    // Ambient ligh threshold for auto backlight off
#define BKL_TH_ON   0.81    // Ambient ligh threshold for auto backlight on
#define BKL_LOW     0.0     // Ambient ligh offset for auto backlight
#define FRM_ROW1    10      // Background frame, first horizontal line
#define FRM_ROW2    60
#define BUZZ_VOL    0.1     // Buzzer/speaker volume


PwmOut      BEEP        (D2);      // PA_10 Buzzer/speaker (PWM output)
PwmOut      BKL         (D3);      // PB_3 LCD backlight control (PMW output)
DigitalOut  KAL         (PC_8);    // PC_8 Keep-Alive/turn-off
DigitalOut  BTC         (PC_4);    // PC_4 Aux BT module control
DigitalIn   Button      (D4);      // PB_5 Pushbutton (digital input)
AnalogIn    BATT        (A0);      // PA_0 Battery monitor
AnalogIn    ALS         (A1);      // PA_1 Ambient Light sensor
AnalogIn    VCH1        (A2);      // PA_4 Analog input 1
AnalogIn    VCH2        (A3);      // PB_0 Analog input 2
AnalogIn    VCH3        (A4);      // PC_1 Analog input 3
AnalogIn    VCH4        (A5);      // PC_0 Analog input 4
InterruptIn TRIG1       (PC_13);   // PC_13 Counter 1 trigger
InterruptIn TRIG2       (PC_2);    // PC_2 Counter 2 trigger
InterruptIn TRIG3       (PA_15);   // PA_15 Counter 3 trigger
DigitalOut  PLED        (PC_3);    // PC_3 Pulse LED
DigitalOut  ALED        (PC_9);    // PC_9 Alarm LED
DigitalIn   CHRG        (PC_10);   // PC_10 Charge in progress
DigitalIn   EOCH        (PC_12);   // PC_12 Endo Of Charge
DigitalOut  SDPW        (PB_2);    // PB_2 SD-Card power enable

SDFileSystem sd(PB_15, PB_14, PB_13, PB_1, "sd"); // MOSI, MISO, SCK, CS

// Quadrature encoder
QEI Wheel(D6, D5, NC, 16); // PB_10, PB_4
// Tickers
Ticker Sec_Beat;            // Timer ticker
Ticker Display_Refresh;     // Display refresh ticker

//Serial ports
Serial PC(USBTX, USBRX);    // Virtual COM via USB

// GPS module
Serial GPS(PA_11, PA_12, 9600);    // PA_11=TX, PA_12=RX, default baud-rate 
I2C GPS_I2C(PB_9,PB_8);      // SDA=PB_9, SCL=PB_8

extern unsigned int buffer[128*64/8];   // RAM buffer used by LCD
time_t seconds;     // timestamp
char Text[40]="";   // Text string used by LCD 
float Vbatt, AmbLight;   // battery votage and ambient light level
uint16_t    CNT1, CNT2, CNT3;   // pulse counters
uint8_t FLASH_Status;
float V1, V2, V3, V4;
bool  Pulse=0, ExtPwr=0;

// used by GPS:
double latitude = 0.0;
double longitude = 0.0;
double altitude = 0.0;
int num_sat;
float hori_dilute;
float alt = 0.0;
float geoid, GPS_time;
char ns = 'N', ew='E';
char GPS_stream[256]="none";
char gu, hu;
//const int GPS_addr = 0x42;

// ------------------- Prototypes ----------------------- 
void Timer_tick(void);
void Update_Display(void);
void Set_Time(void);
void Read_Voltages(void);
void PowerOff(void);
void DrawFrame(void);
void CNT1_count(void);
void CNT2_count(void);
void CNT3_count(void);
uint8_t WriteEEPROM(uint32_t, uint8_t);
uint8_t ReadEEPROM(uint32_t);
void SDCard_test(void);
void EEPROM_test(void);
void Init_All(void);
int GPS_test(void);
void GPS_getline(void);
int GPS_get_stream(void);

int main() 
{
    Init_All();
   
    if(Button)  // if turn-on via pushbutton
    {   
        KAL = 1;    // self sustain power from battery
        ExtPwr=0;            
    }    
    else
        ExtPwr = 1;  // otherwise power comes from USB

    // debug: force 
    //ExtPwr = 0;


    // enable LCD refresh ticker        
    Display_Refresh.attach(&Update_Display, 0.1);
    
    wait(1.5);
    Clear_buffer(buffer);
        
    if(Button)   // if pushbutton still pressed after splash-screen
        Set_Time();  // set RTC time and date  
    
    // launch self-tests   
    SDCard_test();
    wait(1);
    //EEPROM_test();
    
    // draw background frame
    Clear_buffer(buffer);    
    DrawFrame();

    if(ExtPwr)      // if powered via USB, no need for backlight (recharge)
        BKL.write(0);

    // enable sec-beat ticker  
    Sec_Beat.attach(&Timer_tick, 1.113);

    //tm t = RTC::getDefaultTM();
    //RTC::attach(&Sec_Beat, RTC::Second);
    
    // enable & attach interrupts on rising edge of digital inputs
    TRIG1.rise(&CNT1_count);     
    TRIG2.rise(&CNT2_count);     
    TRIG3.rise(&CNT3_count);    
   
    while(1) 
    {   
    
        if(Button)   
            PowerOff();   // Power-off test 
        /*     
        // DEBUG: PC/GPS serial pass-through
        if(PC.readable()) 
            GPS.putc(PC.getc());
            
        if(GPS.readable()) 
            PC.putc(GPS.getc());
        */                   
                   
    } 
    
}


//===========================================================================

// ------------- Called every second ----------------------

void Timer_tick() 
{ 
    seconds = time(NULL); 
    strftime(Text, 50, "%d-%b-%Y  %H:%M:%S", localtime(&seconds));
    LCD_drawstring(buffer, 0, 0, Text);

    //TRIG1.rise(NULL);      // detach counters
    //TRIG2.rise(NULL);      //
    //TRIG3.rise(NULL);      //

    // read voltages 
    if(ExtPwr)
        KAL = 1;
    Read_Voltages();
    if(ExtPwr)
        KAL = 0;


    if(!ExtPwr)
        if(AmbLight>BKL_TH_OFF)
            BKL.write(BKL_LOW);
        else
            if(AmbLight<BKL_TH_ON)
                BKL.write(AmbLight+(1-BKL_TH_ON));        
    
    // write values to buffer
    sprintf(Text,"VBATT= %4.2f", Vbatt);
    LCD_drawstring(buffer, 0, 2, Text);    
    sprintf(Text,"%4.1f %4.1f %4.1f %4.1f", V1, V2, V3, V4 );
    LCD_drawstring(buffer, 0, 3, Text);
    sprintf(Text,"CPS1= %5d", CNT1);
    LCD_drawstring(buffer, 0, 4, Text);
    sprintf(Text,"CPS2= %5d", CNT2);
    LCD_drawstring(buffer, 0, 5, Text);
    sprintf(Text,"CPS3= %5d", CNT3);
    LCD_drawstring(buffer, 0, 6, Text); 
    
    CNT1=CNT2=CNT3=0;            

    //TRIG1.rise(&CNT1_count);      //attach CNT1_count(); to TRIG1
    //TRIG2.rise(&CNT2_count);      //attach CNT2_count(); to TRIG2
    //TRIG3.rise(&CNT3_count);      //attach CNT3_count(); to TRIG3
                                  
    GPS_get_stream(); // GPS test
          
    return;
}


//---------------------------------------------------------------------------
void Update_Display(void)
{

    if(Pulse)
    {
        PLED = 0;
        BEEP.write(0);
        Pulse = 0;
    }    
    LCD_write_buffer(buffer);   // LCD update 
    return;  
}

//---------------------------------------------------------------------------
void Set_Time(void)
{
    uint8_t Year=0, Month=0, Day=0, Hours=0, Mins=0, Secs=0;
    time_t seconds;
    struct tm t;

    Clear_buffer(buffer);
    sprintf(Text,"TIME & DATE SETTING");
    LCD_drawstring(buffer, 0, 0, Text); 
    
    // Set year
    while(Button);
    wait_ms(50);  

    while(!Button)
    { 
        if(int(Wheel.getPulses())<0)
            Wheel.reset();
        Year = (uint8_t)(Wheel.getPulses());
        
        if(Year>99) 
            Wheel.reset();
             
        sprintf(Text, "Year: %2d", Year);
        LCD_drawstring(buffer, 0, 2, Text);
   
    }

    // Set month
    while(Button);
    wait_ms(50);  
    Wheel.reset();
    while(!Button)
    { 
        if(int(Wheel.getPulses())<0)
            Wheel.reset();
        Month = (uint8_t)(Wheel.getPulses()/2);
        
        if(Month>11) 
            Wheel.reset();
            
        sprintf(Text, "Month: %2d", Month+1);
        LCD_drawstring(buffer, 0, 3, Text);
   
    }


    // Set day
    while(Button);
    wait_ms(50);  
    Wheel.reset();
    while(!Button)
    { 
        if(int(Wheel.getPulses())<0)
            Wheel.reset();
        Day = (uint8_t)(Wheel.getPulses()/2);
        
        if(Day>30) 
            Wheel.reset();
            
        sprintf(Text, "Day: %2d", Day+1);
        LCD_drawstring(buffer, 0, 4, Text);
   
    }

    // Set hours
    while(Button);
    wait_ms(50);  
    Wheel.reset();
    while(!Button)
    { 
        if(int(Wheel.getPulses())<0)
            Wheel.reset();
        Hours = (uint8_t)(Wheel.getPulses()/2);
        
        if(Hours>22) 
            Wheel.reset();
            
        sprintf(Text, "Hours: %2d", Hours);
        LCD_drawstring(buffer, 0, 5, Text);
   
    }
    //Hours++;

    // Set minutes
    while(Button);
    wait_ms(50);  
    Wheel.reset();
    while(!Button)
    { 
        if(int(Wheel.getPulses())<0)
            Wheel.reset();
        Mins = (uint8_t)(Wheel.getPulses()/2);
        
        if(Mins>59) 
            Wheel.reset();
            
        sprintf(Text, "Minutes: %2d", Mins);
        LCD_drawstring(buffer, 0, 6, Text);
   
    }

    t.tm_year = Year + 100;
    t.tm_mon  = Month;
    t.tm_mday = Day + 1;
    t.tm_hour = Hours;
    t.tm_min  = Mins;
    t.tm_sec  = Secs;

    seconds = mktime(&t);
    set_time(seconds);
    
    Clear_buffer(buffer);
        
    return;
}    


//---------------------------------------------------------------------------
void Read_Voltages(void)
{

    double ADC_value;
    uint8_t smooth = 10; // Number of samples to smooth
    uint8_t i;
    
    // Read battery voltage
    
    ADC_value = BATT.read();    // cleanup
    wait_ms(5);
    ADC_value = 0;    
    for(i=0;i<smooth;i++) 
        ADC_value += BATT.read();       

    ADC_value = ADC_value/smooth;
    Vbatt = (float)(ADC_value*BAT_GAIN);
    
    
    // Read Ambient Light Level
    
    ADC_value = ALS.read();    // cleanup
    wait_ms(5);
    ADC_value = 0;    
    for(i=0;i<smooth;i++) 
        ADC_value += ALS.read();       

    ADC_value = ADC_value/smooth;
    AmbLight = (float)(ADC_value);     
    
    
    // Read AIN1
    
    ADC_value = VCH1.read();    // cleanup
    wait_ms(5);
    ADC_value = 0;    
    for(i=0;i<smooth;i++) 
        ADC_value += VCH1.read();       

    ADC_value = ADC_value/smooth;
    V1 = (float)(ADC_value);   
    
    // Read AIN2
    
    ADC_value = VCH2.read();    // cleanup
    wait_ms(5);
    ADC_value = 0;    
    for(i=0;i<smooth;i++) 
        ADC_value += VCH2.read();       

    ADC_value = ADC_value/smooth;
    V2 = (float)(ADC_value);  
    
    // Read AIN3
    
    ADC_value = VCH3.read();    // cleanup
    wait_ms(5);
    ADC_value = 0;    
    for(i=0;i<smooth;i++) 
        ADC_value += VCH3.read();       

    ADC_value = ADC_value/smooth;
    V3 = (float)(ADC_value);      
    
    // Read AIN4
    
    ADC_value = VCH4.read();    // cleanup
    wait_ms(5);
    ADC_value = 0;    
    for(i=0;i<smooth;i++) 
        ADC_value += VCH4.read();       

    ADC_value = ADC_value/smooth;
    V4 = (float)(ADC_value);          
        
   
return;
}


//--------------------------------------------------------------------------- 
void PowerOff(void) 
{
    BKL.write(1);
    Display_Refresh.detach();
    Clear_buffer(buffer);
    sprintf(Text,"POWERING OFF");
    LCD_drawstring(buffer, 30, 3, Text); 
    LCD_write_buffer(buffer);
    wait(2);
    Clear_buffer(buffer);
    KAL = 0;
}


//---------------------------------------------------------------------------
void DrawFrame(void)
{
    uint8_t i;
    
    for(i=0; i<128; i++)
    {       
        LCD_setpixel(buffer, i, FRM_ROW1, 1);
        //LCD_setpixel(buffer, i, FRM_ROW2, 1);
    }
    return;  
}


//---------------------------------------------------------------------------
void CNT1_count(void)
{                       //function to call upon interrupt
    CNT1++;                      //increment counter object
    PLED = 1;
    BEEP.write(BUZZ_VOL);
    Pulse = 1;    
    return; 
}


//---------------------------------------------------------------------------
void CNT2_count(void)
{                       //function to call upon interrupt
    CNT2++;                      //increment counter object    
    return; 
}


//---------------------------------------------------------------------------
void CNT3_count(void)
{                       //function to call upon interrupt
    CNT3++;                      //increment counter object
    return; 
}


//---------------------------------------------------------------------------
uint8_t WriteEEPROM(uint32_t address, uint8_t data)
{                      
    HAL_StatusTypeDef  status;
    
    address = address + 0x08000000;
    HAL_FLASH_Unlock();
    status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_BYTE, address, data);
    HAL_FLASH_Lock();
    
    return status;        
}


//--------------------------------------------------------------------------- 
uint8_t ReadEEPROM(uint32_t address) 
{
    uint8_t tmp = 0;
    
    address = address + 0x08000000;
    tmp = *(__IO uint32_t*)address;
    
    return tmp;
}



//---------------------------------------------------------------------------
void SDCard_test(void)
{
    // SD-Card test  
    printf("SD-Card test... ");
    //mkdir("/sd/system", 0777);
    FILE *fp = fopen("/sd/system/version.dat", "a");
    if(fp == NULL) 
    {
        printf("ERROR\n");
        sprintf(Text,"SD-CARD ERROR");        
        LCD_drawstring(buffer, 15, 2, Text);  
        LCD_write_buffer(buffer); 
    }
    else
    {
        printf("OK\n");        
        sprintf(Text,"SD-CARD DETECTED");        
        LCD_drawstring(buffer, 15, 2, Text);  
        LCD_write_buffer(buffer);    
        fprintf(fp, "Geo Electronics 2107\n");
        fprintf(fp, __DATE__);
        fprintf(fp, "\t");        
        fprintf(fp, __TIME__);                
        fprintf(fp, "\n");
        fclose(fp); 
    }
    printf("SD-Card end of test\n");

    return;
}    


//---------------------------------------------------------------------------
void EEPROM_test(void)
{
    // internal EEPROM test
    PC.printf("Attempting to write to EEPROM...\n");

    for (uint32_t i = 0; i < 8; i++) 
    {
        FLASH_Status = WriteEEPROM(i,(uint8_t)(i));
        PC.printf("Writing %d at %d\n", i, i);
    }
    if(FLASH_Status == HAL_FLASH_ERROR_NONE) 
        {PC.printf("Success!!\r\n");}
    else
        {PC.printf("Failed!!\r\n");}
               
    for (uint32_t i = 0; i < 8; i++) 
    {    
            uint8_t storedValue = ReadEEPROM(i);
            PC.printf("Reading %d at %d\n", storedValue, i);
    } 
       
    // end of EEPROM test
    return;
    
}


//---------------------------------------------------------------------------
void Init_All(void)
{
    
    GPS_I2C.frequency(300000);    // I2C GPS speed (400k max)
    //Button.mode(PullUp);      // enable pushbutton pull-up
    BKL.period_ms(5);         // set LCD backlight PWM
    BKL.write(1.0);  
    BEEP.period_us(2000);      // set initial buzzer period and duty-cycle
    BEEP.write(0);
    Wheel.reset();        // clear encoder
    LCD_reset();   
    CNT1=CNT2=CNT3=0;   // clear counters
    SDPW = 0;   // Enable SC-card VDD
    // splash screen with date and time
    sprintf(Text,__DATE__);
    LCD_drawstring(buffer, 60, 5, Text);
    sprintf(Text,__TIME__);
    LCD_drawstring(buffer, 78, 6, Text);  
    LCD_write_buffer(buffer);
    
    // buzzer beep and blink LEDs
    BEEP.write(BUZZ_VOL);
    ALED = 1;
    wait(0.2);
    BEEP.period_us(1000);
    ALED = 0;
    PLED = 1;   
    wait(0.2);
    BEEP.write(0);         
    PLED = 0;
    // enable internal pull-ups for digital inputs
    TRIG1.mode(PullUp);           
    TRIG2.mode(PullUp);           
    TRIG3.mode(PullUp);          
       
    return;
    
}



//---------------------------------------------------------------------------
int GPS_test() 
{
    //float time, hori_dilute, alt,geoid;

// $GPGGA,104534.000,7791.0381,N,06727.4434,E,1,08,0.9,510.4,M,43.9,M,,*47
// $GPGGA,HHMMSS.SSS,latitude,N,longitude,E,FQ,NOS,HDP,altitude,M,height,M,,checksum data
   
    int lock, n;

    if(!GPS.readable())
    {       
        return -1;
    }    
        
    
    //PC.printf("Readable "); 

    GPS_stream[0] = '\0';
            
    for (n = 0; n < 456; n++) 
    {
        GPS_stream[n] = GPS.getc(); 
        if(GPS_stream[n] == '\r') 
        {
            GPS_stream[n] = '\0';
            PC.printf("%s\n", GPS_stream); 
            return;
        }        
               
    }    
       
       
 
       
    while(GPS.readable()) 
    {        
        
        GPS_getline();
        
        // Check if it is a GPGGA msg (matches both locked and non-locked msg)
        //wait(5);
        
        PC.printf("GPS: %s\n", GPS_stream);
        
        if(sscanf(GPS_stream, "GPGGA,%f,%f,%c,%f,%c,%d,%d,%f,%f,%c,%f,%c", &GPS_time, &latitude, &ns, &longitude, &ew, &lock, &num_sat, &hori_dilute, &alt, &hu, &geoid, &gu/*, &age_diff, &diff_ID*/) >= 1) 
        { 
            if(!lock) 
            {
                longitude = 0.0;
                latitude = 0.0;  
                ns = 'Z';
                ew = 'Z';
                alt = 0.0;      
                return 0;
            } 
            else 
            {
                //if(ns == 'S') {    latitude  *= -1.0; }
//                if(ew == 'W') {    longitude *= -1.0; }
//                float degrees = trunc(latitude / 100.0f);
//                float minutes = latitude - (degrees * 100.0f);
//                latitude = degrees + minutes / 60.0f;    
//                degrees = trunc(longitude / 100.0f * 0.01f);
//                minutes = longitude - (degrees * 100.0f);
//                longitude = degrees + minutes / 60.0f;
//                pc1.printf(msg);
                PC.printf("\n\rlongitude is %f\n\r", longitude);
                PC.printf("\n\rtime is %f\n\r", GPS_time);
                PC.printf("ns is %c\n\r", ns);
                PC.printf("ew is %c\n\r", ew);
                PC.printf("alt is %f\n\r", alt);
                
                latitude /= 100;
                longitude /= 100;
                
                return 1;
            }
        
        }
        
        return 0;
    }
    
 return(-1);
    
}


//---------------------------------------------------------------------------
void GPS_getline() 
{
    int i;
    char a;
    int n;
    
    //strcpy(GPS_stream, '\0');
    GPS_stream[0] = '\0';
      
            
    while(GPS.readable()) 
    {
    
        i = 0;
        a = GPS.getc();
        
        GPS_stream[i] = a;
        
        if (a == '$') 
        {
            //PC.printf("%c",a);
            a = GPS.getc();
            GPS_stream[i] = a;
            i++;
            if (a == 'G') 
            {
                //PC.printf("%c",a);
                a = GPS.getc();
                GPS_stream[i] = a;
                i++;
                if (a == 'P') 
                {
                    //PC.printf("%c",a);
                    a = GPS.getc();
                    GPS_stream[i] = a;
                    i++;
                    if (a == 'G') 
                    {
                        //PC.printf("%c",a);
                        a = GPS.getc();
                        GPS_stream[i] = a;
                        i++;
                        if (a == 'G') 
                        {
                            //PC.printf("%c",a);
                            a = GPS.getc();
                            GPS_stream[i] = a;
                            i++;
                            if (a == 'A') 
                            {
                                //PC.printf("%c",a);
                                //a = GPS.getc();
                                //msg[i] = a;
                                //PC.printf(msg);
                                //PC.printf("\r\n");
        
                                for (n = 5; n < 456; n++) 
                                {
                                    GPS_stream[n] = GPS.getc();
                                    //PC.printf("%c", GPS_stream[n]);
                                    if(GPS_stream[n] == '\r') 
                                    {
                                        GPS_stream[n] = '0';
                                        return;
                                    }
                            }
                         }
                     }
                 }
            }
       }
       }                          
//    while(GPS.getc() != '$') {
//        //char a = GPS.getc();
//        for(i = 0; i < 256; i++) {
//            msg[i] = GPS.getc();
//            pc1.printf("%c", msg[i]);
//            if(msg[i] == '\r') {
//             msg[i] = 0;
//             return;
//                }
//                }
//          
//         
//    }
//    while(GPS.getc() != '$');    // wait for the start of a line
//    for(int i=0; i<256; i++) {
//        msg[i] = GPS.getc();
//        if(msg[i] == '\r') {
//            msg[i] = 0;
//            return;
//        }
//    }
//    error("Overflowed message limit");
}

    return;
}


//---------------------------------------------------------------------------

int GPS_get_stream(void)
{
    char cmd[256];    
    int i;

    GPS_stream[0] = '\0';  // buffer cleanup
        
    cmd[0] = 0xFF;  
    GPS_I2C.write(0x42, cmd, 1);
    GPS_I2C.read(0x42, cmd, 100);
    //cmd[21] = '\0';    
    i=0;
    while((i<100))
    {    
        PC.printf("%c", cmd[i]); 
        //if(cmd[i]=='\n')
        //    i=100;
        //else
            i++;    
    }
    
    

    return 1;
}

// ==========================================================================
