#include <string.h>
#include "main.h"
#include "worker.h"

#include "hdlc.h"
#include "fifo.h"


workerHost_s* WORK_workerInit (workerHost_s* worker,
  hdlcPacket_s* packet, fifo_s* fifo, uint8_t* buffer) {
  if (!worker->isInitialized) {
    printf("WORKX: Starting master worker initialization\n");
    worker->packet = packet;
    worker->fifo = fifo;
    worker->isInitialized = true;
    worker->byteBuffer = buffer;
    
    FIFO_init(worker->fifo);

    if (DO_UNIT_TESTS){WORK_testFifo(worker->fifo);} 

    printf("WORKX: Master initialization done\n");
    WORK_printBlocks(worker, 3);
    
    return worker;
  }

  // Ошибка в пакете, повторная инициализация
  worker->isInitialized = false;
  return NULL;

}


int WORK_waitClient (fifo_s *clientFifo) {
  printf("WORKX: .. Waiting for a client\n");
  if (FIFO_isUsable(clientFifo) == 0) {
    return 0;
  }
  return -1;
}


// Подготовка пакета для отправки
int WORK_preparePackage(workerHost_s *worker) {
  printf("WORKX: .. Preparing package\n");
  
  // Перемещение структуры в массив на передачу
  size_t structSize = sizeof(hdlcPacket_s);
  uint8_t tmpPtr = 0;
  uint8_t *packPtr = (uint8_t*)&worker->packet;
  for (size_t s = 0; s < structSize; s++, tmpPtr++) {
    // Нашли флаг в теле сообщения
    if (packPtr[s] == HDLC_PACKET_FLAG) {
      worker->byteBuffer[tmpPtr] = HDLC_PACKET_ESCAPE;
      tmpPtr++;
    }
    worker->byteBuffer[tmpPtr] = packPtr[s];
  }

  return 0;
}


// Отправка пакета
int WORK_sendPackage(workerHost_s *worker)  {
  printf("WORKX: .. Sending package\n");
  if (FIFO_isUsable(worker->fifo) == 0) {
    if (worker->fifo->count > 0) {
      FIFO_put(worker->fifo, &worker->byteBuffer[worker->byteBufferHead]); 
      if (worker->byteBufferHead < WORK_BYTE_BUFFER_SIZE) {
        worker->byteBufferHead++;
      } else {
        worker->byteBufferHead = 0;
        return -2;
      }      
    } else {
      // fifo опустошился
      return 1;
    }
    return 0;
  }
  return 0;
}


// Ожидание пакета
inline int WORK_waitPackage(workerHost_s *worker) {
  printf("WORKX: .. Waiting for a package\n");
  if (FIFO_isUsable(worker->fifo) == 0) {
    if (worker->fifo->count > 0) {
      FIFO_get(worker->fifo, &worker->byteBuffer[worker->byteBufferHead]); 
      if (worker->byteBufferHead < WORK_BYTE_BUFFER_SIZE) {
        worker->byteBufferHead++;
      } else {
        worker->byteBufferHead = 0;
        return -2;
      }      
    } else {
      return 1;
    }
    return 0;
  }
  return -1;
}


inline int WORK_receivePackage(workerHost_s *worker) {
  printf("WORKX: .. Receiving package\n");
  // Receiving in buffer
  
  static int fifoEmptyCounter = 0;
  static int bufPtr = 0;
  uint8_t buf = 0;
  int res = FIFO_get(worker->fifo, &buf);
  // Байт из fifo забран
  if (res == 0) {
    fifoEmptyCounter = 0;
    bufPtr++;
  }
  // fifo опустошён
  if (res == 1) {
    fifoEmptyCounter++;
  }
  if (fifoEmptyCounter > 3) {
    fifoEmptyCounter = 0;
    bufPtr = 0;
    return 0;
  }
  
  return -1;
}


int WORK_parsePackage(workerHost_s *worker) {
  printf("WORKX: .. Parsing package\n");

  // Поиск пакета в массиве 
  uint8_t *first = NULL, *last = NULL;
  uint8_t tmpBuf[WORK_BYTE_BUFFER_SIZE] = {0};
  uint8_t bufPtr=0;
  for (size_t s = 0; s < WORK_BYTE_BUFFER_SIZE; s++, bufPtr++) {
    // Найден флаг
    if (worker->byteBuffer[bufPtr] == HDLC_PACKET_FLAG) {
      if (!first) {
        first = &tmpBuf[s];
        first++; // Скипаем байт флага
      } else if (!last) {
        last = &tmpBuf[s];
        last--;
      }
    }

    // Найден escape-флаг
    if (worker->byteBuffer[bufPtr] == HDLC_PACKET_ESCAPE) {
      if (worker->byteBuffer[bufPtr+1] == HDLC_PACKET_FLAG) {
        bufPtr++;
      }
    }
    tmpBuf[s] = worker->byteBuffer[bufPtr];
  }
  

  // Разбор пакета в структуру
  if (first && last) {
    if (last > first) {
      size_t len = last - first;
      // Так как может быть меньше в теории
      if (len <= HDLC_PACKET_LENGTH) {
        if (len > 2) {  // Считываем адрес
          worker->packet->address = ((uint16_t)*first << 8) | *(first+1);
          first += 2;
        }
        if (len > 4) { // Считываем управляющее поле
          worker->packet->control = ((uint16_t)*first << 8) | *(first+1);
          first += 2;
        }
        if (len > 6) { // Считываем контрольную сумму
          worker->packet->fcs = ((uint16_t)*(last-1) << 8) | *last;
          last -= 2;
        }
        if ((len > 8) && (last > first)) { // Считываем информационный пакет
          int j = 0;
          // От переполнения
          if (last-first >= HDLC_INFO_LENGTH) {
            return -1;
          }

          // Заполнение данными
          for (uint8_t* i = first; i <= last; ++i) {
            worker->packet->info[j] = *i;
            j++;
          }

          // Чистка хвоста
          for (; j < HDLC_INFO_LENGTH; ++j) {
            worker->packet->info[j] = 0;
          }
        }
      }
    } // (last > first)
  } // (first && last)

  memset(worker->byteBuffer, 0, WORK_BYTE_BUFFER_SIZE);
  memcpy(worker->byteBuffer, tmpBuf, WORK_BYTE_BUFFER_SIZE);

  for (uint8_t i = 0; i < WORK_BYTE_BUFFER_SIZE/2; i++) {
    printf("0x%d, ", worker->byteBuffer[i]);
  }
  printf("\n");

  // Проверка контрольной суммы разобранного пакета
  uint16_t crc = worker->packet->fcs;
  worker->packet->fcs = 0;
  if (CRC16_compute((uint8_t*) worker->packet, sizeof(workerHost_s)) == crc) {
    printf("WORKX: .. CRC OK");
  } else {
    printf("WORKX: .. WRONG CRC");
    return -1;
  }

  return -1;
}


int WORK_checkPackage(workerHost_s *worker) {
  printf("WORKX: .. Checking package\n");
  int res = 0;
  return 0;
  res = HDLC_isControlFieldValid(worker->packet->control);
  // Проверка поля управления
  if (res != 0) {
    return res;
  }
  printf("WORKX: .. Package is info ...\n");

  // Проверка контрольной суммы

  printf("WORKX: .. Package checked\n");
  return 0;
}



// Вызов функций обработчика хоста
int WORK_execute(int (*callback)(workerHost_s*), workerHost_s *worker) {
  if (callback == NULL) {
    printf("WORKX: .. Wrong object\n");
    return -1;
  }

  int res = callback(worker);

  if (res != 0) {
    printf("WORKX: .. Error\n");
  }
  
  return res;
}