/* WiFi Example
 * Copyright (c) 2016 ARM Limited
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "mbed.h"
#include <stdio.h>
#include <cstring>
#include <fstream>

#define USE_RTK2GO

int NBR_CHAR_SENT;
int SEND_DATA_TRIES;
int REQUEST_RTK2GO_TRIES;

#define SERVER_PORT 2101
    
#define BUFFER_SIZE 1024*8
#define RX_TIMEOUT_MS 450

DigitalIn SWITCH_2OE(PA_12);            // hoko                    
DigitalIn SWITCH_3OE(PD_11);            // hoko                  
DigitalIn SWITCH_4OE(PD_12);            // hoko                    

typedef enum 
    {
        SM_STATE_WIFI_CNX,
        SM_STATE_SOCKET_CNX,
        SM_STATE_AUTHENTIFICATION,
        SM_STATE_PRE_CONNECTED,
        SM_STATE_CONNECTED,
    } SM_STATE_TYPEDEF;

WiFiInterface *wifi;

TCPSocket socket;

RawSerial UART1(PD_8, PD_9 , 460800);   // = UART 3

DigitalOut LedRed(LED_RED);     // red
DigitalOut LedGreen(LED_GREEN); // green
DigitalOut LedBlue(LED_BLUE);   // blue

DigitalOut R_LedRed(LED_RED);

char buf[BUFFER_SIZE];
unsigned int bufIndex=0;;
bool RxTriggred=false;
SM_STATE_TYPEDEF state;

uint32_t TotalRxCounter=0;
uint32_t TotalTxCounter=0;

// Read data from uart1
// Thread DataRxHandler;
void DataRx()
{
   
   while(UART1.readable())
   {
        const char x=UART1.getc();
        if (state==SM_STATE_CONNECTED)
        {
            buf[bufIndex]=x;
            if (bufIndex<BUFFER_SIZE)
                {
                    bufIndex++;
                    TotalRxCounter++;
                }
            else
            {
                bufIndex=0;
                bufIndex++;
                }
        }
        else
            bufIndex=0;
        
        RxTriggred=true;
    }
}

bool Sending=false;

// Send data to server
Thread DataTxHandler;
void DataTx()
{
   while(1)
    {
        if(Sending)
        {
            RxTriggred=false;
            wait_ms(RX_TIMEOUT_MS);  
            if ((RxTriggred==false)&&(bufIndex>0))
            {
                NBR_CHAR_SENT=socket.send(buf, bufIndex);
                if(NBR_CHAR_SENT<0){
                    SEND_DATA_TRIES++;
                }
                else{
                    TotalTxCounter=TotalTxCounter+bufIndex;
                    SEND_DATA_TRIES=0;
                }

                
                memset(&buf,0,BUFFER_SIZE);
                bufIndex=0;
                if(SEND_DATA_TRIES>0)
                    printf("Error sending to server = %d \n",SEND_DATA_TRIES);
                if(SEND_DATA_TRIES>=5)
                {
                    socket.close();
                    if(wifi->get_connection_status()!=1){
                        wifi->disconnect();
                        state=SM_STATE_WIFI_CNX;
                        printf("\nRe");
                        }
                    else
                    {
                        state=SM_STATE_SOCKET_CNX;
                        printf("\nRe-");
                    }
                    SEND_DATA_TRIES=0;
                    UART1.attach(0);
                    Sending=false;                   
                }
            }
        }
    }
    
}

typedef enum 
    {
        CLWHITE,
        CLYELLOW,
        CLCYAN,
        CLBLUE,
        CLGREEN,
        CLRED   
    } COLOR_TYPEDEF;

void LED_SetColor(COLOR_TYPEDEF Color)
{
    switch (Color)
    {
        case CLGREEN : LedRed=1;LedGreen=0;LedBlue=1; break;
        case CLCYAN  : LedRed=1;LedGreen=0;LedBlue=0; break;
        case CLBLUE  : LedRed=1;LedGreen=1;LedBlue=0; break;
        case CLWHITE : {
            DigitalOut W_lEDRed(LED_RED,0);
            DigitalOut W_LedGreen(LED_GREEN,0);
            DigitalOut W_LedBlue(LED_BLUE,0);
        }
        break;
        case CLYELLOW : {
            DigitalOut Y_lEDRed(LED_RED,0);
            DigitalOut Y_LedGreen(LED_GREEN,0);
            DigitalOut Y_LedBlue(LED_BLUE,1);
        }
        break;
        case CLRED : {
            DigitalOut R_LedRed(LED_RED,0);
            DigitalOut R_LedGreen(LED_GREEN,1);
            DigitalOut R_LedBlue(LED_BLUE,1);
        } 
         break;
        
        default : LedRed=1;LedGreen=1;LedBlue=1; break;
     }
}

/* Flash Control hoko*/
void _eraseFlash(void)
{
	FLASH_EraseInitTypeDef erase;
	erase.TypeErase = FLASH_TYPEERASE_SECTORS;  /* Select sector    */
	erase.Sector = FLASH_SECTOR_23;             /* Set sector 23    */
    erase.Banks = FLASH_BANK_2;                 /* Erase bank =2    */
	erase.NbSectors = 1;		                /* Erase sector num */
	erase.VoltageRange = FLASH_VOLTAGE_RANGE_3; /* Driver 3.3V      */

	uint32_t pageError = 0;

	HAL_FLASHEx_Erase(&erase, &pageError);	    /* Erase use by HAL_FLASHEx */
}

void writeFlash(uint32_t address, uint8_t *data, uint32_t size)
{
	HAL_FLASH_Unlock();		                    /* Unlock FLASH */
	_eraseFlash();			                    /* Erase sector 23 */
	do {
		HAL_FLASH_Program(FLASH_TYPEPROGRAM_BYTE, address, *data);
	} while (++address, ++data, --size);
	HAL_FLASH_Lock();		                    /* Lock FLASH */
}

void loadFlash(uint32_t address, uint8_t *data, uint32_t size)
{
	memcpy(data, (uint8_t*)address, size);
}

typedef struct {                                                    // hoko
	char    wifi_ssid[30]       = "MBED_CONF_APP_WIFI_SSID" ;
	char	wifi_passwd[30]     = "MBED_CONF_APP_WIFI_PASSWORD" ;
	char	ntrip_server[30]    = "SERVER_ADD" ;
    char	ntrip_user[30]      = "USER" ;
    char	ntrip_passwd[30]    = "RTK2GO_PASSWORD" ;
    char	ntrip_mountpt[30]   = "RTK2GO_MOUNT_POINT" ;
} NTRIP_Condition;

NTRIP_Condition get_ntrip_condition (){                             // hoko

    NTRIP_Condition ntrip_condition;

    printf("==============================  C099-F9P-NTRIP-SERVER-2 1110 -============================\n");
    printf("CASE\r\n"); 
    printf("ALL==1 [ZED-F9P UART1:RX] RXD_ZED ---+ FTDI_RXD_ZED  [PC USB-SERIAL:TX]\r\n");
    printf("2OE==0                               + TXD3_ODIN     [ODIN-W2 UART3:TX] -> Ntrip Client\r\n");
    printf("3OE==0                               + TXD_ODIN      [ODIN-W2 UART1:TX]\r\n");                            
    printf("4OE==0                               + RXD_ZED_ARD\r\n\r\n");                              
    printf("ALL==1 [ODIN-W2 UART1:RX] RXD_ODIN --+ FTDI_RXD_ODIN [PC USB-SERIAL:TX]\r\n");
    printf("2OE==0                               + FTDI_RXD_ODIN [PC USB-SERIAL:TX]\r\n");                            
    printf("3OE==0                               + TXD_ZED       [ODIN-W2 UART1:TX]\r\n");                            
    printf("4OE==0                               + RXD_ODIN_ARD  [Arduino Pin J9-3]\r\n\r\n");                             
    printf("2OE==1 [ODIN-W2 UART3:RX] RXD3_ODIN -+ TXD_ZED       [ZED-F9P UART1:TX] -> Ntrip Server\r\n");    
    printf("2OE==0                               + RXD3_ODIN_ARD [Ardiuno Pin J3-3]\r\n\r\n"); 
    printf("Setting for Board Rev .E \r\n"); 
    printf("2OE=%d 3OE=%d 4OE=%d \r\n",(SWITCH_2OE!=0),(SWITCH_3OE!=0),(SWITCH_4OE!=0));
    printf("==============================  C099-F9P NTRIP Setting Part  ==============================\n");

    mbed_stats_sys_t stats;
    mbed_stats_sys_get(&stats);

    printf("\n ---- ODIN-W2 Status -----\r\n");
    printf("Mbed OS version %d.%d.%d\r\n", MBED_MAJOR_VERSION, MBED_MINOR_VERSION, MBED_PATCH_VERSION);
    printf("CPU ID: 0x%x\r\n", stats.cpu_id);
    printf("Compiler ID: %d\r\n", stats.compiler_id);
    printf("Compiler Version: %ld\r\n", stats.compiler_version);

	const uint32_t address = 0x81E0000;

    loadFlash(address, (uint8_t*)&ntrip_condition, sizeof(NTRIP_Condition));
    printf (" -------------------------\r\n");
    printf ("| Current C099 conditions |\r\n");
    printf (" -------------------------\r\n");
    printf ("WiFi SSID      = %s \r\n" , ntrip_condition.wifi_ssid );
    printf ("WiFi Password  = %s \r\n" , ntrip_condition.wifi_passwd );
    printf ("Ntrip Server   = %s \r\n" , ntrip_condition.ntrip_server );
    printf ("Ntrip Password = %s \r\n" , ntrip_condition.ntrip_passwd );
    printf ("Ntrip Mounpt   = %s \r\n" , ntrip_condition.ntrip_mountpt );

    DigitalIn switch_in0(SW0);              // PUSH = 0 ;
    switch_in0.mode(PullUp);
    int x = switch_in0 && !(ntrip_condition.wifi_ssid[0]==0xff);
    int y = 1;

    char buffer[4]="OK";

    printf (" --------------------------------------------\r\n");
    printf ("| If you release RESET while turning on SW0, |\r\n");
    printf ("| C099 will enter the condition change mode. |\r\n");
    printf (" --------------------------------------------\r\n");

    while( (strstr(buffer,"OK") == NULL) || ( x == 0 ) ) // if SW0 is PUSH -> Change Condition
    {
        x = 1 ;
        y = 0 ;
        printf ("Please Input Condition\r\n");

        printf("\r\nWiFi SSID = ");
        scanf ("%s" , ntrip_condition.wifi_ssid );
        printf("%s\r\nWiFi Password = "              ,ntrip_condition.wifi_ssid);
        scanf ("%s" , ntrip_condition.wifi_passwd );
        printf("%s\r\nNtrip Server (ex:rtk2go.com)= ",ntrip_condition.wifi_passwd);
        scanf ("%s" , ntrip_condition.ntrip_server );
        printf("%s\r\nNtrip Server Passwd ="         ,ntrip_condition.ntrip_server);
        scanf ("%s" , ntrip_condition.ntrip_passwd );
        printf("%s\r\nNtrip Server Mount Point="     ,ntrip_condition.ntrip_passwd);
        scanf ("%s" , ntrip_condition.ntrip_mountpt );
        printf("%s\r\n"                              ,ntrip_condition.ntrip_mountpt);

        printf (" -------------------------\r\n");
        printf ("| Changed C099 conditions |\r\n");
        printf (" -------------------------\r\n");
        printf ("WiFi SSID      = %s \r\n" , ntrip_condition.wifi_ssid );
        printf ("WiFi Password  = %s \r\n" , ntrip_condition.wifi_passwd );
        printf ("Ntrip Server   = %s \r\n" , ntrip_condition.ntrip_server );
        printf ("Ntrip Password = %s \r\n" , ntrip_condition.ntrip_passwd );
        printf ("Ntrip Mounpt   = %s \r\n" , ntrip_condition.ntrip_mountpt );
        printf (" -----------------------------------------------------------------------\r\n");
        printf ("| If this is all right, enter \"OK\". If not, enter \"NG\". (and ENTER) |\r\n");
        printf (" -----------------------------------------------------------------------\r\n");

        scanf("%s", buffer);
        printf("%s\r\n",buffer);
    } 

	if ( y == 0 ) {
        printf ("Write changed condition\r\n");
        writeFlash(address, (uint8_t*)&ntrip_condition, sizeof(NTRIP_Condition));
    }

    return ntrip_condition;
}

bool exitLoop=false;
int main()
{
    NTRIP_Condition ntrip_condition = get_ntrip_condition();    // hoko

    LED_SetColor(CLWHITE);
    thread_sleep_for(2000);
      
    printf("\n\n===================  C099-F9P & RTK2GO example  ===================\n\n");
    state=SM_STATE_WIFI_CNX;
    NBR_CHAR_SENT=0;
    SEND_DATA_TRIES=0;
    REQUEST_RTK2GO_TRIES=0;
    
    while (!exitLoop){
        
        switch(state){
            case SM_STATE_WIFI_CNX:
                {
                    printf("\n>SM_STATE:SM_STATE_WIFI_CNX\n");
                    LED_SetColor(CLCYAN);
                    printf("connecting to %s...\n", ntrip_condition.wifi_ssid);
                    wifi = WiFiInterface::get_default_instance();
                    int ret=1;
                    ret = wifi->connect(ntrip_condition.wifi_ssid, ntrip_condition.wifi_passwd, NSAPI_SECURITY_WPA_WPA2);
                    if (ret != 0) {
                        printf("\nConnection error: %d\n", ret);
                    }
                    else {
                        printf("Success\n\n");
                        printf("MAC: %s\n", wifi->get_mac_address());
                        printf("IP: %s\n", wifi->get_ip_address());
                        printf("Netmask: %s\n", wifi->get_netmask());
                        printf("Gateway: %s\n", wifi->get_gateway());
                        printf("RSSI: %d\n\n", wifi->get_rssi());
                        state=SM_STATE_SOCKET_CNX;
                    }
                }
            break;
            
            case SM_STATE_SOCKET_CNX:
                {
                    printf("\n>SM_STATE:SM_STATE_SOCKET_CNX\n");
                    LED_SetColor(CLBLUE);
                    printf("opening socket\n");
                    if(wifi->get_connection_status()==1){
                        int result =socket.open(wifi);
                        if (result != 0) printf("Error! socket.open() returned: %d\n", result);
                        else{
                            printf("Connecting to socket\n");
                            nsapi_error_t err=NSAPI_ERROR_NO_CONNECTION;
                            err=socket.connect( ntrip_condition.ntrip_server , SERVER_PORT );
                            if (err != NSAPI_ERROR_OK){
                                printf("Unable to connect to (%s) on port (%d) err (%d)\n", ntrip_condition.ntrip_server , SERVER_PORT,err);
                                socket.close();
                                thread_sleep_for(2000);
                            }
                                else
                                {
                                    printf("Connected to Server at %s\n",ntrip_condition.ntrip_server);
                                    #ifdef USE_RTK2GO
                                        state=SM_STATE_AUTHENTIFICATION;
                                    #else
                                        state=SM_STATE_PRE_CONNECTED;
                                    #endif
                                }
                        }          
                    }
                    else state=SM_STATE_WIFI_CNX;
                }
            break;
                    
            case SM_STATE_AUTHENTIFICATION:
                {
                    char buffer[256];
                    printf("\n>SM_STATE:SM_STATE_AUTHENTIFICATION\n");
                    LED_SetColor(CLBLUE);
                    const string s= string("SOURCE ") +string(ntrip_condition.ntrip_passwd)+string(" ")+string(ntrip_condition.ntrip_mountpt)+string("\r\nSource-Agent: NtripServerCMD/1.1\r\n\r\n");
                    char dataToSend[s.size()+1];
                    s.copy(dataToSend, s.size()+1);
                    dataToSend[s.size()]='\0';
                    printf("Request confirmation from server\n");
                    socket.send(dataToSend, sizeof(dataToSend) - 1);
                    thread_sleep_for(3000);
                    int b = socket.recv(buffer, 256);
                    buffer[b] = '\0';
                    if (strstr(buffer,"ICY 200 OK")!=NULL)
                        {
                            printf("Response from server: '%s'\n", buffer);
                            state=SM_STATE_PRE_CONNECTED;
                        }
                    else
                    {
                        printf("Can't get the confirmation from server\n");
                        REQUEST_RTK2GO_TRIES++;
                        if(REQUEST_RTK2GO_TRIES>0)
                        {
                            if(REQUEST_RTK2GO_TRIES>=3)
                            {
                                printf("\nError getting confirmation from server\n");
                                thread_sleep_for(2000);
                                exitLoop=true;
                            }
                            else 
                            {
                                printf("\nRetry ");
                                thread_sleep_for(2000);
                            }
                        }
                    }                       
                }
            break;
            
            case SM_STATE_PRE_CONNECTED : // active read and write threads
                {
                    printf("\n>SM_STATE:SM_STATE_PRE_CONNECTED\n");
                    printf("Activating read && send data ...\n");
                    LED_SetColor(CLYELLOW);
                    UART1.attach(&DataRx);
                    Sending=true;
                    if(DataTxHandler.get_state()==16) DataTxHandler.start(&DataTx);
                    state=SM_STATE_CONNECTED;
                    thread_sleep_for(1000);
                }
            break;
            
            case SM_STATE_CONNECTED:
                {
                    printf(">SM_STATE:SM_STATE_CONNECTED TTX[%d] TRX[%d]\n",TotalTxCounter,TotalRxCounter);
                    LED_SetColor(CLGREEN);
                    if(wifi->get_connection_status()!=1){
                        printf("\nwifi disconnected \n");
                        socket.close();
                        wifi->disconnect();
                        UART1.attach(0);
                        Sending=false;
                        state=SM_STATE_WIFI_CNX;
                    } 
                }
            break;
            
        }
        
        thread_sleep_for(1000);       
    }
 
    LED_SetColor(CLRED);
    printf("Error !  Verify the server state then reset your device \n");
    
    while(1)
    {
        R_LedRed= !R_LedRed;
        thread_sleep_for(300);
    }
}