#include "SQLite.h"
#include "../AUV150.h"
#include <string>
#include <iostream>
#include <sstream>

SQLite::SQLite(const std::string& dbPath) {
    try {
        int rc = sqlite3_open(dbPath.c_str(), &m_db);
        if (rc != SQLITE_OK) {
            std::cerr << "Can't open database: " << sqlite3_errmsg(m_db) << std::endl;
            m_db = nullptr;
        } else {
            std::cout << "Opened database successfully: " << dbPath << std::endl;
        }
        m_dbPath = dbPath;
    } catch (const std::exception& e) {
        std::cerr << "SQLite constructor exception: " << e.what() << std::endl;
        m_db = nullptr;
    }
}

SQLite::~SQLite()
{
    if (m_db) sqlite3_close(m_db);
}

bool SQLite::execSQL(const std::string& sql) {
    if (!m_db) {
        std::cerr << "Database not opened, cannot execute SQL" << std::endl;
        return false;
    }
    
    char* errMsg = nullptr;
    int rc = sqlite3_exec(m_db, sql.c_str(), nullptr, nullptr, &errMsg);
    if (rc != SQLITE_OK) {
        std::cerr << "SQL error (" << rc << "): " << (errMsg ? errMsg : "unknown error") << std::endl;
        std::cerr << "Failed SQL: " << sql << std::endl;
        if (errMsg) sqlite3_free(errMsg);
        return false;
    }
    return true;
}

bool SQLite::createTable() {
    if (!m_db) {
        std::cerr << "Database not opened, cannot create tables" << std::endl;
        return false;
    }

    std::cout << "Creating database tables in: " << m_dbPath << std::endl;

    std::string cmdTableSQL = R"(
        CREATE TABLE IF NOT EXISTS CommandFrame (
            id INTEGER PRIMARY KEY AUTOINCREMENT,
            timestamp DATETIME DEFAULT CURRENT_TIMESTAMP,
            frameHeader INTEGER,
            counter INTEGER,
            dataLength INTEGER,
            navMode INTEGER,
            mainThruster INTEGER,
            heading INTEGER,
            depth INTEGER,
            rudderUp INTEGER,
            rudderDown INTEGER,
            rudderLeft INTEGER,
            rudderRight INTEGER,
            led INTEGER,
            dvlSwitch INTEGER,
            payloadCtrl INTEGER,
            crc INTEGER,
            frameTail INTEGER
        );
    )";

    std::string feedbackTableSQL = R"(
        CREATE TABLE IF NOT EXISTS FeedbackFrame (
            id INTEGER PRIMARY KEY AUTOINCREMENT,
            timestamp DATETIME DEFAULT CURRENT_TIMESTAMP,
            frameHeader INTEGER,
            counter INTEGER,
            dataLength INTEGER,
            navModeFb INTEGER,
            altimeterHeight REAL,
            depthSensor REAL,
            trueHeading REAL,
            pitch REAL,
            roll REAL,
            velocityEast REAL,
            velocityNorth REAL,
            velocityDown REAL,
            insLongitude REAL,
            insLatitude REAL,
            insAltitude REAL,
            dvlVelX REAL,
            dvlVelY REAL,
            dvlVelZ REAL,
            thrusterRPM REAL,
            ledSwitch INTEGER,
            batteryVoltage REAL,
            batteryLevel INTEGER,
            batteryTemp REAL,
            leakStatus INTEGER,
            powerModule INTEGER,
            backupPower INTEGER,
            thrusterStatus INTEGER,
            reserved INTEGER,
            payloadStatus INTEGER,
            dvlStatus INTEGER,
            crc INTEGER,
            frameTail INTEGER
        );
    )";

    std::string statusTableSQL = R"(
        CREATE TABLE IF NOT EXISTS AUVStatus (
            id INTEGER PRIMARY KEY AUTOINCREMENT,
            timestamp DATETIME DEFAULT CURRENT_TIMESTAMP,
            navModeFb INTEGER,
            altimeterHeight REAL,
            depthSensor REAL,
            trueHeading REAL,
            pitch REAL,
            roll REAL,
            velocityEast REAL,
            velocityNorth REAL,
            velocityDown REAL,
            insLongitude REAL,
            insLatitude REAL,
            insAltitude REAL,
            dvlVelX REAL,
            dvlVelY REAL,
            dvlVelZ REAL,
            thrusterRPM REAL,
            ledSwitch INTEGER,
            batteryVoltage REAL,
            batteryLevel INTEGER,
            batteryTemp REAL,
            leakStatus INTEGER,
            powerModule INTEGER,
            backupPower INTEGER,
            thrusterStatus INTEGER,
            reserved INTEGER,
            payloadStatus INTEGER,
            dvlStatus INTEGER,
            x REAL,
            y REAL,
            z REAL,
            heading_error REAL,
            pitch_error REAL,
            depth_error REAL,
            desired_rudder REAL,
            desired_elevator REAL,
            desired_thrust REAL
        );
    )";

    return execSQL(cmdTableSQL) && 
           execSQL(feedbackTableSQL) && 
           execSQL(statusTableSQL);
}

bool SQLite::insertData(CommandFrame_150AUV data) {
    if (!m_db) {
        std::cerr << "Database not opened, cannot insert CommandFrame data" << std::endl;
        return false;
    }

    std::cout << "Inserting CommandFrame data into database" << std::endl;
    std::stringstream ss;
    ss << "INSERT INTO CommandFrame ("
       << "frameHeader, counter, dataLength, navMode, mainThruster, "
       << "heading, depth, rudderUp, rudderDown, rudderLeft, rudderRight, "
       << "led, dvlSwitch, payloadCtrl, crc, frameTail) VALUES ("
       << data.frameHeader << ", " << data.counter << ", " << (int)data.dataLength << ", "
       << (int)data.navMode << ", " << (int)data.mainThruster << ", "
       << data.heading << ", " << data.depth << ", " << (int)data.rudderUp << ", "
       << (int)data.rudderDown << ", " << (int)data.rudderLeft << ", " << (int)data.rudderRight << ", "
       << (int)data.led << ", " << (int)data.dvlSwitch << ", " << (int)data.payloadCtrl << ", "
       << (int)data.crc << ", " << data.frameTail << ");";

    return execSQL(ss.str());
}

bool SQLite::insertData(FeedbackFrame_150AUV data) {
    if (!m_db) {
        std::cerr << "Database not opened, cannot insert FeedbackFrame data" << std::endl;
        return false;
    }

    std::cout << "Inserting FeedbackFrame data into database" << std::endl;
    std::stringstream ss;
    ss << "INSERT INTO FeedbackFrame ("
       << "frameHeader, counter, dataLength, navModeFb, altimeterHeight, "
       << "depthSensor, trueHeading, pitch, roll, velocityEast, velocityNorth, "
       << "velocityDown, insLongitude, insLatitude, insAltitude, dvlVelX, "
       << "dvlVelY, dvlVelZ, thrusterRPM, ledSwitch, batteryVoltage, "
       << "batteryLevel, batteryTemp, leakStatus, powerModule, backupPower, "
       << "thrusterStatus, reserved, payloadStatus, dvlStatus, crc, frameTail) VALUES ("
       << data.frameHeader << ", " << data.counter << ", " << (int)data.dataLength << ", "
       << (int)data.navModeFb << ", " << data.altimeterHeight/100.0 << ", "
       << data.depthSensor/100.0 << ", " << data.trueHeading/100.0 << ", "
       << (short)data.pitch/100.0 << ", " << (short)data.roll/100.0 << ", "
       << (short)data.velocityEast/100.0 << ", " << (short)data.velocityNorth/100.0 << ", "
       << (short)data.velocityDown/100.0 << ", " << data.insLongitude/1e7 << ", "
       << data.insLatitude/1e7 << ", " << (short)data.insAltitude/100.0 << ", "
       << (short)data.dvlVelX/100.0 << ", " << (short)data.dvlVelY/100.0 << ", "
       << (short)data.dvlVelZ/100.0 << ", " << (short)data.thrusterRPM << ", "
       << (int)data.ledSwitch << ", " << data.batteryVoltage/1000.0 << ", "
       << (int)data.batteryLevel << ", " << data.batteryTemp/10.0 << ", "
       << data.leakStatus << ", " << (int)data.powerModule << ", "
       << (int)data.backupPower << ", " << (int)data.thrusterStatus << ", "
       << (int)data.reserved << ", " << (int)data.payloadStatus << ", "
       << (int)data.dvlStatus << ", " << (int)data.crc << ", " << data.frameTail << ");";

    return execSQL(ss.str());
}

bool SQLite::insertData(AUV150_Status data) {
    if (!m_db) {
        std::cerr << "Database not opened, cannot insert AUVStatus data" << std::endl;
        return false;
    }

    std::cout << "Inserting AUVStatus data into database" << std::endl;
    std::stringstream ss;
    ss << "INSERT INTO AUVStatus ("
       << "navModeFb, altimeterHeight, depthSensor, trueHeading, pitch, roll, "
       << "velocityEast, velocityNorth, velocityDown, insLongitude, insLatitude, "
       << "insAltitude, dvlVelX, dvlVelY, dvlVelZ, thrusterRPM, ledSwitch, "
       << "batteryVoltage, batteryLevel, batteryTemp, leakStatus, powerModule, "
       << "backupPower, thrusterStatus, reserved, payloadStatus, dvlStatus, "
       << "x, y, z, heading_error, pitch_error, depth_error, desired_rudder, "
       << "desired_elevator, desired_thrust) VALUES ("
       << (int)data.navModeFb << ", " << data.altimeterHeight << ", " << data.depthSensor << ", "
       << data.trueHeading << ", " << data.pitch << ", " << data.roll << ", "
       << data.velocityEast << ", " << data.velocityNorth << ", " << data.velocityDown << ", "
       << data.insLongitude << ", " << data.insLatitude << ", " << data.insAltitude << ", "
       << data.dvlVelX << ", " << data.dvlVelY << ", " << data.dvlVelZ << ", "
       << data.thrusterRPM << ", " << (int)data.ledSwitch << ", " << data.batteryVoltage << ", "
       << (int)data.batteryLevel << ", " << data.batteryTemp << ", " << data.leakStatus << ", "
       << (int)data.powerModule << ", " << (int)data.backupPower << ", "
       << (int)data.thrusterStatus << ", " << (int)data.reserved << ", "
       << (int)data.payloadStatus << ", " << (int)data.dvlStatus << ", "
       << data.x << ", " << data.y << ", " << data.z << ", " << data.heading_error << ", "
       << data.pitch_error << ", " << data.depth_error << ", " << data.desired_rudder << ", "
       << data.desired_elevator << ", " << data.desired_thrust << ");";

    return execSQL(ss.str());
}
