#include "decoder.h"
#include <QTime>
#include <QtDebug>

Decoder::Decoder() {
  this->img = Image();
  this->receivedData = 0;
  this->rec = none;
  this->newCommand = true;
  this->timer = new QTime();
  this->lastTime = 0;
}

Decoder::~Decoder() { delete this->timer; }

void Decoder::sendDummyData(uint32_t width, uint32_t heigth, bool rgb) {
  this->img.setParams(width, heigth, rgb);
  this->img.setData(
      std::vector<uint8_t>((rgb ? 3 : 1) * (width * heigth), 120));
  for (uint32_t i = 0; i < width; i++) {
    for (uint32_t j = 0; j < heigth; j++) {
      this->img.setDataAt(i, j, (i * j) % static_cast<uint8_t>(255));
    }
  }
  emit imageData(&this->img);
}

void Decoder::processData(const QByteArray &data) {
  // qDebug() << "received data " << data.length();
  if (this->newCommand && data.length() > 7) {
    this->decodeCommand(data);
  } else {
    this->continueReceive(data);
  }
}

void Decoder::decodeCommand(const QByteArray &data) {
  switch (data[0]) {
  case 's':
    switch (data[1]) {
    case 'i': {
      this->rec = image_data;
      this->newCommand = false;

      uint8_t width[2] = {static_cast<uint8_t>(data[3]),
                          static_cast<uint8_t>(data[4])};
      uint8_t height[2] = {static_cast<uint8_t>(data[5]),
                           static_cast<uint8_t>(data[6])};
      this->img.setParams(*reinterpret_cast<uint16_t *>(width),
                          *reinterpret_cast<uint16_t *>(height),
                          data[2] == 'c' ? true : false);
      this->img.clearData();
      this->expectedLength =
          static_cast<int>(this->img.getWidth() * this->img.getHeight());
      this->receivedData = 0;
      this->receiveImage(static_cast<QByteArray>(data).remove(0, 8));
      break;
    }
    case 'e': {
      uint8_t width[2] = {static_cast<uint8_t>(data[3]),
                          static_cast<uint8_t>(data[4])};
      uint8_t height[2] = {static_cast<uint8_t>(data[5]),
                           static_cast<uint8_t>(data[6])};
      bool rgb = data[2] == 'c' ? true : false;
      if (*reinterpret_cast<uint16_t *>(width) == this->img.getWidth() &&
          *reinterpret_cast<uint16_t *>(height) == this->img.getHeight() &&
          rgb == this->img.getColors()) {
        if (this->img.getColors()) {
          this->img.image2rgb();
        }
        int time_ms = QTime::currentTime().msec();
        qDebug() << "Image received: " << QTime::currentTime().second() << "."
                 << time_ms;

        if (time_ms > this->lastTime) {
          emit fps(static_cast<double>(1000) / (time_ms - this->lastTime));
        } else {
          emit fps(static_cast<double>(1000) /
                   ((1000 + time_ms) - this->lastTime));
        }
        this->lastTime = time_ms;
        emit imageData(&this->img);
      }
      break;
    }
    case 's': {
      this->rec = sensor_data;
      this->newCommand = false;

      this->sensorDataType = static_cast<uint8_t>(data[2]);
      this->expectedLength = static_cast<uint8_t>(data[3]);
      this->currentData.clear();
      this->receivedData = 0;
      this->receiveSensorData(static_cast<QByteArray>(data).remove(0, 4));
      break;
    }
    case 'r': {
      uint8_t add = static_cast<uint8_t>(data[2]);
      uint8_t d[2] = {static_cast<uint8_t>(data[3]),
                      static_cast<uint8_t>(data[4])};
      emit registerData(add, *reinterpret_cast<uint16_t *>(d));
      break;
    }
    case 'm': { // speed measurement
      this->rec = speed_measurement;
      this->newCommand = false;
      uint8_t length[4] = {
          static_cast<uint8_t>(data[2]), static_cast<uint8_t>(data[3]),
          static_cast<uint8_t>(data[4]), static_cast<uint8_t>(data[5])};
      this->expectedLength = *reinterpret_cast<int *>(length);
      this->receivedData = 0;
      this->timer->start();
      this->receiveSpeedMeas(static_cast<QByteArray>(data).remove(0, 8));
      break;
    }
    }
    break;
  default:
    break;
  }
}

void Decoder::continueReceive(const QByteArray &data) {
  switch (this->rec) {
  case image_data:
    this->receiveImage(data);
    break;
  case sensor_data:
    this->receiveSensorData(data);
    break;
  case speed_measurement:
    this->receiveSpeedMeas(data);
    break;
  case none:
    break;
  }
}

void Decoder::receiveSensorData(const QByteArray &data) {
  int missingData = this->expectedLength - this->receivedData;
  if (missingData > data.length()) {
    this->currentData.append(data);
    this->receivedData += data.length();
  } else {
    this->currentData.append(data, missingData);
    emit sensorData(this->sensorDataType, this->currentData);
    this->rec = none;
    this->newCommand = true;
    qDebug() << "Sensor data done" << this->sensorDataType;
    if (missingData < data.length()) {
      this->processData(static_cast<QByteArray>(data).remove(0, missingData));
    }
  }
}

void Decoder::receiveImage(const QByteArray &data) {
  int missingData = this->expectedLength - this->receivedData;
  if (missingData > data.length()) {
    this->img.appendData(data);
    this->receivedData += data.length();
  } else {
    this->img.appendData(data, static_cast<uint32_t>(missingData));
    this->rec = none;
    this->newCommand = true;
    if (missingData < data.length()) {
      this->processData(static_cast<QByteArray>(data).remove(0, missingData));
    }
  }
}

void Decoder::receiveSpeedMeas(const QByteArray &data) {
  int missingData = this->expectedLength - this->receivedData;
  if (missingData > data.length()) {
    this->receivedData += data.length();
  } else {
    double speed =
        static_cast<double>(this->expectedLength) / this->timer->elapsed();
    qDebug() << "Transfer speed = " << speed << "kB/s";
    this->rec = none;
    this->newCommand = true;
    if (missingData < data.length()) {
      this->processData(static_cast<QByteArray>(data).remove(0, missingData));
    }
  }
}

void Decoder::clearAll() {
  this->receivedData = 0;
  this->rec = none;
  this->newCommand = true;
}
