#include "serialCommand.h"
#include "crc16.h"
#include <QDebug>
#include <iostream>

SerialCommand::SerialCommand() {
    /*
  Field    Index   Bytes       Description
  STX      0       2           0x6655: starting mark. Low byte in the front
  CTRL     2       1           0: need_ack (if the current data pack need “ack”)
  1: ack_pack (if the current data pack is an “ack” package) 2-7: reserved
  Data_len 3       2           Data field byte length. Low byte in the front
  SEQ      5       2           Frame sequence (0 ~ 65535). Low byte in the front
  CMD_ID   7       1           Command ID
  DATA     8       Data_len    Data
  CRC16            2           CRC16 check to the complete data package. Low
  byte in the front
  */

    mSerialCommands.append({createByteArray({0x00, 0x00}), "Exit program"});
    mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x04, 0x00, 0x00,
                                             0x00, 0x0E, 0x00, 0x00, 0x00, 0x00}),
                            "Degrees"});
    mSerialCommands.append(
        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01}),
         "Auto Centering"});
    mSerialCommands.append(
        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x01}),
         "Zoom +1"});
    mSerialCommands.append(
        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0xFF}),
         "Zoom -1"});
    mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x01,
                                             0x00, 0x0F, 0x04, 0x05}),
                            "4.5x"});
    mSerialCommands.append(
        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x06, 0x01}),
         "Manual Focus +1"});
    mSerialCommands.append(
        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x06, 0xff}),
         "Manual Focus -1"});
    mSerialCommands.append(
        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x04, 0x01}),
         "Auto Focus"});
    mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00,
                                             0x00, 0x07, 0x00, 0x2D}),
                            "Rotate Up"});
    mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00,
                                             0x00, 0x07, 0x00, -0x2D}),
                            "Rotate Down"});
    mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00,
                                             0x00, 0x07, 0x2D, 0x00}),
                            "Rotate Right"});
    mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00,
                                             0x00, 0x07, -0x2D, 0x00}),
                            "Rotate Left"});
    mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00,
                                             0x00, 0x07, 0x00, 0x00}),
                            "Stop rotation"});
    mSerialCommands.append(
        {createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x16}),
         "Acquire the Max Zoom Value"});
    mSerialCommands.append(
        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x00}),
         "Take Pictures"});
    mSerialCommands.append(
        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x02}),
         "Record Video"});
    mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00,
                                             0x00, 0x07, 0x64, 0x64}),
                            "Rotate 100 100"});
    mSerialCommands.append(
        {createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x0a}),
         "Gimbal Status Information"});
    mSerialCommands.append(
        {createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x02}),
         "Acquire Hardware ID"});
    mSerialCommands.append(
        {createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01}),
         "Acquire Firmware Version"});
    mSerialCommands.append(
        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x03}),
         "Lock Mode"});
    mSerialCommands.append(
        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x04}),
         "Follow Mode"});
    mSerialCommands.append(
        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x05}),
         "FPV Mode"});
    mSerialCommands.append(
        {createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x0d}),
         "Acquire Attitude Data"});
    mSerialCommands.append(
        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x06}),
         "Set Video Output as HDMI (Only available on A8 mini, restart to take "
         "effect)"});
    mSerialCommands.append(
        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x07}),
         "Set Video Output as CVBS (Only available on A8 mini, restart to take "
         "effect)"});
    mSerialCommands.append(
        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x08}),
         "Turn Off both CVBS and HDMI Output (Only available on A8 mini, restart "
         "to take effect)"});
    mSerialCommands.append(
        {createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x15}),
         "Read Range from Laser Rangefinder(Low byte in the front, high byte in "
         "the back, available on ZT30)"});
}

SerialCommand::~SerialCommand() {
    // Do something?
}

QByteArray
SerialCommand::createByteArray(const std::initializer_list<int> &bytes) {
    QByteArray byteArray;
    for (int byte : bytes) {
        byteArray.append(static_cast<char>(byte));
    }
    return byteArray;
}

void SerialCommand::printCommands() {
    uint8_t index = 0;
    foreach (Command command, mSerialCommands) {
        qInfo().noquote() << QString::number(index) + ": " + command.description;
        index++;
    }
}

void SerialCommand::setExtraValues(uint8_t number) {
    if (number == 1) {
        QByteArray command = mSerialCommands.at(number).command;
        int16_t degrees;

        std::cout << "Enter yaw degrees (-135 -> 135): ";
        std::cin >> degrees;
        degrees = degrees * 10;
        command[8] = degrees & 0xFF;
        command[9] = degrees >> 8;

        std::cout << "Enter pitch degrees (-90 -> 25): ";
        std::cin >> degrees;
        degrees = degrees * 10;
        command[10] = degrees & 0xFF;
        command[11] = degrees >> 8;

        mSerialCommands[number].command = command;
    }
}

QByteArray SerialCommand::getCommand(uint8_t number) {
    setExtraValues(number);

    QByteArray command = mSerialCommands.at(number).command;

    uint8_t crcBytes[2];
    CRC16::getCRCBytes(command, crcBytes);

    command.resize(command.size() +
                   2); // Increase array size to accommodate CRC bytes
    command[command.size() - 2] = crcBytes[0]; // Set LSB
    command[command.size() - 1] = crcBytes[1]; // Set MSB

    QString commandStr;
    for (int i = 0; i < command.size(); i++) {
        commandStr += QString("%1").arg(command.at(i), 2, 16, QChar('0')).toUpper();
    }
    qDebug() << "Command: " << commandStr;

    return command;
}

uint8_t SerialCommand::getCommandCount() { return mSerialCommands.size(); }