
#define _SUPPRESS_PLIB_WARNING
#define _DISABLE_OPENADC10_CONFIGPORT_WARNING
#include <plib.h>
#include "gpsserial.h"
#include "parsegps.h"
#include "util.h"
#include "Timezone/timezones.h"
#include "mainmenu.h"

int gps_fix, gps_numsats, gps_time, gps_fractime, gps_date, gps_lat, gps_lon, gps_detected, gps_fixgood, gps_datetimelatlongood, gps_fixgoodfor, gps_good_counter;
char gps_alt[16];
char gpsbuf[256];
int gpsbuf_pos;
int tz_searching, timezone_found = -1, tz_override = -1;
unsigned long serial_rx_bytes_since_last_valid;

extern unsigned char time_set, date_set;

__attribute__((mips16)) static int get_U1BR() {
    return get_PBCLK() / ((U1MODEbits.BRGH ? 4 : 16) * (U1BRG+1));
}
__attribute__((mips16)) static int calc_U1BRG(int desired_baud_rate) {
    return get_PBCLK() / ((U1MODEbits.BRGH ? 4 : 16) * desired_baud_rate) - 1;
}

static const unsigned short baud_rates[6] = { 1200, 2400, 4800, 9600, 19200, 38400 };

__attribute__((mips16)) void setup_serial(unsigned char invert, unsigned int baud_rate) {
    LATBbits.LATB13 = 0;
    TRISBbits.TRISB13 = 1;
    ANSELBbits.ANSB13 = 0;
    // map RB13 (3) to U1RX
    U1RXR = 3;

    U1MODE = 0;
    U1BRG = calc_U1BRG(baud_rate);
    U1STAbits.URXEN = 1;
    U1MODEbits.RXINV = invert;
    U1MODEbits.ON = 1;
}
__attribute__((mips16)) void close_serial() {
    while( !U1STAbits.TRMT )
        ;
    ANSELBbits.ANSB13 = 1;
    U1STAbits.URXEN = 0;
    U1MODEbits.ON = 0;

    // unmap RB13 (3) from U1RX
    U1RXR = 0;
}
__attribute__((mips16)) static unsigned short get_next_baud_rate(unsigned short baud_rate) {
    int i;
    for( i = 0; i < sizeof(baud_rates)/sizeof(*baud_rates)-1; ++i )
        if( baud_rate < ((int)baud_rates[i]+(int)baud_rates[i+1])/2 )
            return baud_rates[i+1];
    return baud_rates[0];
}

__attribute__((mips16)) static unsigned char dec_to_bcd(unsigned char dec) {
    return ((dec/10)<<4)|(dec%10);
}
__attribute__((mips16)) static unsigned char bcd_to_dec(unsigned char bcd) {
    return (bcd>>4)*10 + (bcd&0x0F);
}

__attribute__((mips16)) void decimal_time_date_to_bcd(rtccTime* timehere, rtccDate* datehere, int dectime, int decdate) {
    int mday, mon, hour, min;//, year;

    mday = decdate/10000;
    decdate -= mday * 10000;
    mon = decdate/100;
    decdate -= mon * 100;

    hour = dectime / 10000;
    dectime -= hour * 10000;
    min = dectime / 100;
    dectime -= min * 100;

    datehere->year = dec_to_bcd(decdate);
    datehere->mon = dec_to_bcd(mon);
    datehere->mday = dec_to_bcd(mday);
    timehere->sec = dec_to_bcd(dectime);
    timehere->min = dec_to_bcd(min);
    timehere->hour = dec_to_bcd(hour);
}

__attribute__((mips16)) void bcd_time_date_to_decimal(int* timehere, int* datehere, const rtccTime* bcdtime, const rtccDate* bcddate) {
    *datehere = (bcd_to_dec(bcddate->mday) * 100 + bcd_to_dec(bcddate->mon)) * 100 + bcd_to_dec(bcddate->year);
    *timehere = (bcd_to_dec(bcdtime->hour) * 100 + bcd_to_dec(bcdtime->min)) * 100 + bcd_to_dec(bcdtime->sec);
}

static inline __attribute__((mips16)) unsigned char RtccTimeEqual(const rtccTime* a, const rtccTime* b) {
    return a->sec == b->sec && a->min == b->min && a->hour == b->hour;
}
static inline __attribute__((mips16)) unsigned char RtccDateEqual(const rtccDate* a, const rtccDate* b) {
    return a->mday == b->mday && a->mon == b->mon && a->year == b->year;
}

static unsigned char gps_skip_second_disable;
static int gps_good_time, num_consecutive_off_by_one_sec;
__attribute__((mips16)) int serial_read(int max_read, const signed long* tzinfo, const daylight_savings_info* dstinfo) {
    int bytes_read = 0;
    if( U1STAbits.OERR ) {
        U1STAbits.OERR = 0;
        return -1;
    }
    if( U1STAbits.URXDA ) {
        while( U1STAbits.URXDA && max_read-- ) {
            ++bytes_read;
            ++serial_rx_bytes_since_last_valid;
            if( serial_rx_bytes_since_last_valid >= 1024 && 0 ) {
                // If we get more than 1000 characters of serial data but no valid GPS messages,
                // try re-initialising the serial port at a different baud rate (4800/9600) or
                // inverted. One of the four combos should work.
                unsigned char inverted = U1MODEbits.RXINV;
                unsigned int baud_rate = get_U1BR();
                close_serial();
                setup_serial(!inverted, inverted ? baud_rate : get_next_baud_rate(baud_rate));
                gpsbuf_pos = 0;
                serial_rx_bytes_since_last_valid = 0;
            }
            if( gpsbuf_pos ) {
                if( gpsbuf_pos == sizeof(gpsbuf)-2 ) {
                    gpsbuf_pos = 0;
                } else {
                    gpsbuf[gpsbuf_pos] = U1RXREG;
                    ++gpsbuf_pos;
                    if( gpsbuf_pos > 3 && gpsbuf[gpsbuf_pos-2] == '\r' && gpsbuf[gpsbuf_pos-1] == '\n' ) {
                        gpsbuf[gpsbuf_pos] = '\0';
                        gpsbuf_pos = 0;
                        switch( ParseGPS(gpsbuf, &gps_fix, &gps_numsats, gps_alt, &gps_time, &gps_fractime, &gps_date, &gps_lat, &gps_lon) ) {
                            case GPS_MSG_FIXANDALT:
                                gps_detected = 1;
                                if( gps_fix > 0 || !GPS_LOCK_REQUIRED ) {
                                    gps_fixgood = 10;
                                    gps_good_time = RtccGetTime();
                                }
                                serial_rx_bytes_since_last_valid = 0;
                                break;
                            case GPS_MSG_TIMEDATELATLON:
                                gps_detected = 1;
                                gps_datetimelatlongood = 10;
                                if( timezone_found != -1 ) {
                                    int gtime = gps_time, gdate = gps_date;
                                    rtccTime time, rtime;
                                    rtccDate date, rdate;

                                    RtccGetTimeDate(&time, &date);
                                    decimal_time_date_to_bcd(&rtime, &rdate, gtime, gdate);
                                    if( !RtccTimeEqual(&rtime, &time) || !RtccDateEqual(&rdate, &date) ) {
                                        if( times_are_one_second_apart(&time, &date, &rtime, &rdate) && num_consecutive_off_by_one_sec < 30 ) {
                                            // if we're getting time updates before the clock has rolled over, speed the clock up a bit
                                            if( !gps_skip_second_disable ) {
                                                gps_skip_second_disable = 255;
                                                if( OPTS_XTAL_TRIM < 511 )
                                                    RtccSetCalibration(++OPTS_XTAL_TRIM);
                                            }

                                            ++num_consecutive_off_by_one_sec;
                                        } else {
                                            RtccFixupDOW(&rdate);
                                            RtccSetTimeDate(rtime.l, rdate.l);
                                            time_set = 1;
                                            date_set = 1;
                                            num_consecutive_off_by_one_sec = 0;
                                        }
                                    } else {
                                        num_consecutive_off_by_one_sec = 0;
                                        if( RTCCONbits.HALFSEC ) {
                                            // if we're getting time updates after the clock has rolled over, slow the clock up a bit
                                            if( !gps_skip_second_disable ) {
                                                if( OPTS_XTAL_TRIM > -512 )
                                                    RtccSetCalibration(--OPTS_XTAL_TRIM);
                                                gps_skip_second_disable = 255;
                                            }
                                        }
                                    }
                                }
                                serial_rx_bytes_since_last_valid = 0;
                                break;
                        }
                    }
                }
            } else {
                if( U1RXREG == '$' ) {
                    gpsbuf[0] = '$';
                    gpsbuf_pos = 1;
                }
            }
        }
    } else if( gps_fixgood && gps_datetimelatlongood ) {
        int ret;
        if( !tz_searching ) {
            tz_searching = 1;
            init_get_timezone(gps_lat, gps_lon);
        }
        ret = continue_get_timezone(8);
        if( ret != -1 ) {
            timezone_found = ret;
            tz_searching = 0;
        }
        if( gps_fixgoodfor < 10 )
            ++gps_fixgoodfor;
        gps_good_counter = 8*3600;
    } else {
        gps_fixgoodfor = 0;
    }

    if( gps_good_time != RtccGetTime() ) {
        gps_good_time = RtccGetTime();
        if( gps_fixgood )
            --gps_fixgood;
        if( gps_datetimelatlongood )
            --gps_datetimelatlongood;
    }

    return bytes_read;
}

__attribute__((mips16)) void save_serial_state_and_close(serial_state* pState) {
    pState->inverted = U1MODEbits.RXINV;
    pState->baud_rate = get_U1BR();
    close_serial();
}

__attribute__((mips16)) void restore_serial_state(const serial_state* pState) {
    setup_serial(pState->inverted, pState->baud_rate);
}

__attribute__((mips16)) void RtccFixupDOW(rtccDate* date) {
    date->wday = RtccWeekDay(2000 + bcd_to_dec(date->year), bcd_to_dec(date->mon), bcd_to_dec(date->mday));
}

__attribute__((mips16)) void time_date_add_one_second(rtccTime* time, rtccDate* date) {
    if( ((++time->sec)&15) == 10 ) {
        time->sec += 6;
        if( time->sec == 0x60 ) {
            time->sec = 0;
            if( ((++time->min)&15) == 10 ) {
                time->min += 6;
                if( time->min == 0x60 ) {
                    time->min = 0;
                    if( ((++time->hour)&15) == 10 ) {
                        time->hour += 6;
                    if( time->hour == 0x24 ) {
                        time->hour = 0;
                        if( ++date->wday == 7 )
                            date->wday = 0;
                        if( ((++date->mday)&15) == 10 )
                            date->mday += 6;

                            if( bcd_to_dec(date->mday) > get_days_in_month(bcd_to_dec(date->mon), 2000+bcd_to_dec(date->year)) ) {
                                date->mday = 1;
                                if( (++date->year&15) == 10 )
                                    date->year += 6;
                            }
                        }
                    }
                }
            }
        }
    }
}

__attribute__((mips16)) unsigned char times_are_one_second_apart(const rtccTime* time1, const rtccDate* date1, const rtccTime* time2, const rtccDate* date2) {
    rtccTime time;
    rtccDate date;
    time.l = time1->l;
    date.l = date1->l;
    time_date_add_one_second(&time, &date);
    if( RtccTimeEqual(&time, time2) && RtccDateEqual(&date, date2) )
        return 1;
    time.l = time2->l;
    date.l = date2->l;
    time_date_add_one_second(&time, &date);
    time.rsvd = time1->rsvd;
    date.wday = date1->wday;
    return RtccTimeEqual(&time, time1) && RtccDateEqual(&date, date1);
}
