#include "config.hpp"
#include "defines.hpp"
#include "serialResponse.hpp"

SerialPort *Config::mSerial = nullptr;
SerialCommand *Config::mCommand = nullptr;
uint16_t Config::mResolutionX = 0;
uint16_t Config::mResolutionY = 0;
float Config::mMaxZoom = 0.0f;
float Config::mCurrentYaw = 0.0f;
float Config::mCurrentPitch = 0.0f;
float Config::mCurrentRoll = 0.0f;
float Config::mCurrentZoom = 0.0f;

void Config::sanityCheck()
{
    if (mSerial == nullptr) {
        qCritical().noquote().nospace() << "Serial connection not available.";
    }

    if (mCommand == nullptr) {
        qCritical().noquote().nospace() << "Commands not available.";
    }
}

void Config::setInitalValues(SerialPort *serial, SerialCommand *command)
{
    mSerial = serial;
    mCommand = command;

    sanityCheck();

    // Setup some values that might be needed from camera
    QByteArray tempCommand;
    QByteArray tempResponse;
    QHash<QString, QVariant> responseValues;

    // Get resolution to reduce calls to camera
    tempCommand = mCommand->getCommandForInternal(COMMAND_ID::ACQUIRE_CAMERA_CODEC_SPECS);
    mSerial->sendCommand(tempCommand);
    tempResponse = mSerial->readResponse();
    responseValues = SerialResponse::getResponceValues(tempResponse);
    mResolutionX = responseValues["width"].toInt();
    mResolutionY = responseValues["height"].toInt();

    // Get max zoom value to reduce calls to camera
    tempCommand = mCommand->getCommandForInternal(COMMAND_ID::ACQUIRE_MAX_ZOOM_VALUE);
    mSerial->sendCommand(tempCommand);
    tempResponse = mSerial->readResponse();
    responseValues = SerialResponse::getResponceValues(tempResponse);
    mMaxZoom = responseValues["zoom"].toInt();

    // Debug printing
    qDebug().noquote().nospace() << "Camera resolution: " << mResolutionX << "*" << mResolutionY;
    qDebug().noquote().nospace() << "Camera max zoom:   " << mMaxZoom;
}

void Config::updateState()
{
    sanityCheck();
    QHash<QString, QVariant> responseValues;
    QByteArray tempCommand;
    QByteArray tempResponse;

    // Get current angles
    tempCommand = mCommand->getCommandForInternal(COMMAND_ID::ACQUIRE_ATTITUDE_DATA);
    Config::getSerial()->sendCommand(tempCommand);
    tempResponse = Config::getSerial()->readResponse();
    responseValues = SerialResponse::getResponceValues(tempResponse);
    mCurrentYaw = responseValues["yaw"].toFloat();
    mCurrentPitch = responseValues["pitch"].toFloat();
    mCurrentRoll = responseValues["roll"].toFloat();

    // Get current zoom
    tempCommand = mCommand->getCommandForInternal(COMMAND_ID::ACQUIRE_CURRENT_ZOOM);
    Config::getSerial()->sendCommand(tempCommand);
    tempResponse = Config::getSerial()->readResponse();
    responseValues = SerialResponse::getResponceValues(tempResponse);
    mCurrentZoom = responseValues["zoom"].toFloat();
}

SerialPort *Config::getSerial()
{
    sanityCheck();
    return mSerial;
}

SerialCommand *Config::getCommand()
{
    sanityCheck();
    return mCommand;
}

uint16_t Config::getResolutionWidth()
{
    sanityCheck();
    return mResolutionX;
}

uint16_t Config::getResolutionHeight()
{
    sanityCheck();
    return mResolutionY;
}

float Config::getMaxZoom()
{
    sanityCheck();
    return mMaxZoom;
}

float Config::getCurrentYaw()
{
    sanityCheck();
    return mCurrentYaw;
}

float Config::getCurrentPitch()
{
    sanityCheck();
    return mCurrentPitch;
}

float Config::getCurrentRoll()
{
    sanityCheck();
    return mCurrentRoll;
}

float Config::getCurrentZoom()
{
    sanityCheck();
    return (mCurrentZoom == 0.0f ? 1.0f : mCurrentZoom);
}