/**
 * @file NTRIPClient.cpp
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Contains the implementation of the class NTRIPClient.
 * @version 0.1
 * @date 2022-09-18
 * 
 * @copyright Copyright (c) 2022
 * 
 */

#include "ntripClient.h"


NTRIPClient::NTRIPClient(SFE_UBLOX_GNSS* gps, const char* host, uint16_t port, const char* mountPoint, const char* user, const char* password) {
    this->gps = gps;
    strcpy(this->host, host);
    this->port = port;
    strcpy(this->mountPoint, mountPoint);
    strcpy(this->user, user);
    strcpy(this->password, password);

    this->ntripClient = new WiFiClient;
    this->state = NTRIPClientStates::closeConnection;
}

NTRIPClient::~NTRIPClient() {
    delete this->ntripClient;
}

void NTRIPClient::loop() {
    this->pushGPGGA();

    if (millis() - this->lastLoopTime < this->delayTime)
        return;
    this->lastLoopTime = millis();

    switch (this->state) {
        case NTRIPClientStates::openConnection:
            if (!this->activated) {
                this->state = NTRIPClientStates::closeConnection;
                break;
            }

            std::cout << "Connecting to the NTRIP caster..." << std::endl;
            if (this->beginClient()) {
                std::cout << "Connected to the NTRIP caster!" << std::endl;
                this->state = NTRIPClientStates::pushData;
            } else {
                std::cout << "Failed!" << std::endl;
                this->state = NTRIPClientStates::wait;
                this->activated = false;
            }
            break;

        case NTRIPClientStates::pushData:
            if (!processConnection() || !this->activated)
                this->state = NTRIPClientStates::closeConnection;
            break;

        case NTRIPClientStates::closeConnection:
            std::cout << "Closing the connection to the NTRIP caster..." << std::endl;
            this->closeConnection();
            state = NTRIPClientStates::wait;
            break;

        case NTRIPClientStates::wait:
            if (this->activated)
                this->state = NTRIPClientStates::openConnection;
            else
                this->checkAutoReconnect();
            break;

        case NTRIPClientStates::notAvailable:
            break;

        default:
            std::cout << "Wrong state in NTRIPClient.cpp..." << std::endl;
            this->state = NTRIPClientStates::closeConnection;
            break;
    }
}

void NTRIPClient::gpsConfiguration() {
    this->gps->setSPIOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
    this->gps->setPortInput(COM_PORT_SPI, COM_TYPE_UBX | COM_TYPE_NMEA | COM_TYPE_RTCM3);
    // Set the differential mode - ambiguities are fixed whenever possible
    this->gps->setDGNSSConfiguration(SFE_UBLOX_DGNSS_MODE_FIXED);
    this->gps->setMainTalkerID(SFE_UBLOX_MAIN_TALKER_ID_GP);
    this->gps->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10);
}

bool NTRIPClient::setActivated(bool state) {
    // std::cout << "NTRIPClient::setActivated: b - " << b << std::endl;
    if (state && this->state != NTRIPClientStates::notAvailable)
        this->activated = true;
    else if (state)
        return false;
    else {
        this->activated = false;
        this->autoReconnect = false;
    }
    return true;
}

void NTRIPClient::setAutoReconnect(bool state) {
    if (state) {
        this->reconnectAttemps = 0;
        this->autoReconnect = true;
        return;
    }

    this->autoReconnect = false;
}

bool NTRIPClient::beginClient() {
    std::cout << "Opening socket to " << this->host << std::endl;

    char serverRequest[this->bufferSize];
    char credentials[this->bufferSize];

    if (!this->ntripClient->connect(this->host, this->port)) {
        std::cout << "Connection to caster failed" << std::endl;
        return false;
    } else {
        std::cout << "Connected to " << this->host << " : " << this->port << std::endl;
        std::cout << "Requesting NTRIP Data from mount point " << this->mountPoint << std::endl;

        // Generate the server request (GET)
        snprintf(serverRequest,
                 this->bufferSize,
                 "GET /%s HTTP/1.0\r\nUser-Agent: NTRIP SparkFun u-blox Client v1.0\r\n",
                 this->mountPoint);

        // Credentials
        uint8_t userCredentialsLength = strlen(this->user) + strlen(this->password) + 2;
        char* userCredentials = new char[userCredentialsLength];
        snprintf(userCredentials, userCredentialsLength, "%s:%s", this->user, this->password);

        std::cout << "Sending credentials: " << userCredentials << std::endl;

        //Encode
        base64 b;
        String strEncodedCredentials = b.encode(userCredentials);
        delete userCredentials;
        char encodedCredentials[strEncodedCredentials.length() + 1];
        strEncodedCredentials.toCharArray(encodedCredentials, sizeof(encodedCredentials));

        snprintf(credentials, sizeof(credentials), "Authorization: Basic %s\r\n", encodedCredentials);
    }

    // Add the encoded credentials to the server request
    strncat(serverRequest, credentials, this->bufferSize);
    strncat(serverRequest, "\r\n", this->bufferSize);

    std::cout   << "serverRequest size: " 
                << strlen(serverRequest) 
                << " of " 
                << this->bufferSize 
                << " bytes available" 
                << std::endl;

    // Send the server request
    std::cout << "Sending server request: " << serverRequest << std::endl;
    this->ntripClient->write(serverRequest, strlen(serverRequest));

    //Wait up to 5 seconds for response
    uint32_t lastMillis = millis();
    while (!ntripClient->available()) {
        if (millis() - lastMillis > this->timeOut) {
            std::cout << "Caster timed out!" << std::endl;
            this->ntripClient->stop();
            return false;
        }
        delay(10);
    }

    //Check reply
    uint16_t httpStatusCode = 0;
    char response[this->bufferSize];
    uint16_t responseIndex = 0;
    while (this->ntripClient->available()) {
        if (responseIndex == sizeof(response))
            break;

        response[responseIndex++] = ntripClient->read();

        if (httpStatusCode == 0) {
            if (strstr(response, "200") != nullptr)
                httpStatusCode = 200;
            if (strstr(response, "401") != nullptr)
                httpStatusCode = 401;
        }
    }
    response[responseIndex] = '\0';

    // std::cout << "Caster response: " << response << std::endl;

    if (httpStatusCode != 200) {
        std::cout << "Failed to connect to " << this->host << " - HTTP Code: " << (int) httpStatusCode 
                  << " Length of Response: " << responseIndex << std::endl;

        if (httpStatusCode == 0)
            std::cout << "Response: " << response << std::endl;
        else if (httpStatusCode == 401)
            std::cout << "Statuscode 401 - Unauthorized" << std::endl;
        return false;
    }

    std::cout << "Connected to: " << this->host << std::endl;
    this->lastReceivedRtcmTime = millis();
    return true;
}

void NTRIPClient::closeConnection() {
    if (this->ntripClient->connected())
        this->ntripClient->stop();
    this->activated = false;
    std::cout << "NtripClient disconnected from: " << this->host << std::endl;
}

bool NTRIPClient::processConnection() {
    if (this->ntripClient->connected()) {
        uint8_t rtcmData[this->bufferSize * 4];
        uint16_t rtcmCount = 0;

        while (this->ntripClient->available()) {
            rtcmData[rtcmCount++] = ntripClient->read();
            if (rtcmCount == sizeof(rtcmData))
                break;
        }

        if (rtcmCount > 0) {
            this->lastReceivedRtcmTime = millis();
            this->gps->pushRawData(rtcmData, rtcmCount);
            // std::cout << "Pushed " << rtcmCount << " RTCM bytes to ZED." << std::endl;
        }
    } else {
        std::cout << "Connection to " << this->host << " dropped!" << std::endl;
        return false;
    }

    if (millis() - this->lastReceivedRtcmTime > this->timeOut) {
        std::cout << "RTCM timeout!" << std::endl;
        return false;
    }

    return true;
}

void NTRIPClient::checkAutoReconnect() {
    if (!this->autoReconnect)
        return;

    if (millis() - this->lastReconnectTime < this->reconnectDelayTime)
        return;
    this->lastReconnectTime = millis();

    if (this->reconnectAttemps >= this->maxReconnectAttemps) {
        this->autoReconnect = false;
        return;
    }

    this->activated = true;
    this->reconnectAttemps++;
}

void NTRIPClient::pushGPGGA() {
    if (!this->transmitLocation && !this->activated)
        return;

    if (millis() - this->lastGPGGAPushTime < this->pushGPGGATime)
        return;
    this->lastGPGGAPushTime = millis();

    if (!this->ntripClient->connected())
        std::cout << "Failed to pushing GGA to server: " << std::endl;
    
    NMEA_GGA_data_t *data = new NMEA_GGA_data_t;
    uint8_t res = this->gps->getLatestNMEAGPGGA(data);
    if (res == 2)
        this->ntripClient->print((const char *)data);
    delete data;
}

bool NTRIPClient::isConnected() {
    if (this->state == NTRIPClientStates::pushData)
        return true;
    return false;
}