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

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

extern void WORK_formattedPrintArray(char* str, uint8_t* array, size_t size);

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->fifoWrite = fifo;
    worker->fifoRead = NULL;
    worker->isInitialized = true;
    worker->byteBuffer = buffer;
    
    FIFO_init(worker->fifoWrite);

    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 = HDLC_PACKET_LENGTH-2;
  // size_t structSize = sizeof(hdlcPacket_s);

  uint8_t tmpPtr = 0;
  uint8_t *packPtr = (uint8_t*)worker->packet;

  worker->byteBuffer[tmpPtr++] = HDLC_PACKET_FLAG;
  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];
  }

  worker->byteBuffer[tmpPtr] = HDLC_PACKET_FLAG;
  worker->byteBufferLength = tmpPtr+1;
  WORK_formattedPrintArray("prepared bytes:", 
    worker->byteBuffer, worker->byteBufferLength);
  
  return 0;
}


// Отправка пакета
int WORK_sendPackage(workerHost_s *worker)  {
  printf("WORKX: .. Sending package\n");

  if (worker->byteBufferHead > worker->byteBufferLength) {
    return -2;
  }

  if (FIFO_isUsable(worker->fifoWrite) == 0) {
    if (worker->fifoWrite->count < FIFO_DATA_SIZE) {
      FIFO_put(worker->fifoWrite, worker->byteBuffer[worker->byteBufferHead]); 
      if (worker->byteBufferHead < worker->byteBufferLength) {
        worker->byteBufferHead++;
        return 1;
      } else if (worker->byteBufferHead == worker->byteBufferLength) {
        // Передан весь буфер через фифо
        WORK_formattedPrintArray("sent bytes:", 
          worker->byteBuffer, worker->byteBufferLength);
        return 0;
      } else {
        worker->byteBufferHead = 0;
        return -3;
      }      
    } else {
      return -2;
    }
    return 2;
  }
  return -1;
}


// Ожидание пакета
int WORK_waitPackage(workerHost_s *worker) {
  printf("WORKX: .. Waiting for a package\n");
  if (FIFO_isUsable(worker->fifoRead) == 0) {
    if (worker->fifoRead->count > 0) {
      // Появились данные в fifo
      return 0;
    }
  }
  return -1;
}

int WORK_receivePackage(workerHost_s *worker) {
  printf("WORKX: .. Receiving package\n");
  if (FIFO_isUsable(worker->fifoRead) == 0) {
    if (worker->fifoRead->count > 0) {
      FIFO_get(worker->fifoRead, &worker->byteBuffer[worker->byteBufferHead]); 
      if (worker->byteBufferHead < WORK_BYTE_BUFFER_SIZE) {
        worker->byteBufferHead++;
        return 1;
      } else {
        worker->byteBufferHead = 0;
        return -2;
      }      
    } else {
      if (worker->byteBufferHead > 0) {
        // Приняли данные. Сейчас fifo пуст, можно обрабатывать пакет
        // fifo пуст, можно обрабатывать пакет
        WORK_formattedPrintArray("receives bytes:", 
          worker->byteBuffer, worker->byteBufferLength);
        return 0;
      }
      return 2;
    }
  }
  return -1;
}


int WORK_doGroupPackageBytes(hdlcPacket_s *packet, uint8_t *first, uint8_t *last, size_t bufLen) {
  if (last > first) {
    bufLen = last - first;
    // Так как может быть меньше в теории
    if (bufLen <= HDLC_PACKET_LENGTH - 2) {
      // Считываем адрес
      if (bufLen > 2) {  
        packet->address = ((uint16_t)*(first+1) << 8) | *(first);
        first += 2;
      }

      // Считываем управляющее поле
      if (bufLen > 4) { 
        packet->control = ((uint16_t)*(first+1) << 8) | *(first);
        first += 2;
      }

      // Считываем контрольную сумму
      if (bufLen > 6) { 
        printf("CRCRCR:%d,%d\n", *last, *(last-1));
        packet->fcs = ((uint16_t)*(last) << 8) | *(last-1);
        last -= 2;
      }

      // Считываем информационный пакет
      if ((bufLen > 8) && (last > first)) { 
        int j = 0;
        // От переполнения
        size_t infoLen = last - first + 1;
        if (infoLen > HDLC_INFO_LENGTH) {
          return -1;
        }
        // Заполнение данными
        for (uint8_t* i = first; i <= last; i++) {
          packet->info[j] = *i;
          j++;
        }
        // Чистка хвоста
        for (; j < HDLC_INFO_LENGTH; j++) {
          packet->info[j] = 0;
        }
      }
      
      return 0;

    } // bufLen <= HDLC_PACKET_LENGTH - 2
  } // (last > first)
  return -1;
}


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

  // Поиск пакета в массиве 
  uint8_t *first = NULL, *last = NULL;
  uint8_t tmpArr[WORK_BYTE_BUFFER_SIZE] = {0};
  size_t bufPtrI = 0, tmpArrI = 0, bufLen = 0;
  // Выносим из принятого массива только нагрузку в буфер
  for (bufPtrI = 0; bufPtrI < WORK_BYTE_BUFFER_SIZE; bufPtrI++) {
    // Заполняем временный буфер если нашли начало
    if (first) {
      tmpArr[tmpArrI] = worker->byteBuffer[bufPtrI];
      tmpArrI++;
    }

    // Найден escape-флаг и следующие значение - флаг
    if (worker->byteBuffer[bufPtrI] == HDLC_PACKET_ESCAPE) {
      if (worker->byteBuffer[bufPtrI+1] == HDLC_PACKET_FLAG) {
        tmpArr[tmpArrI-1] = HDLC_PACKET_FLAG;
        bufPtrI++;
      }
    } else 
    // Найден флаг
    if (worker->byteBuffer[bufPtrI] == HDLC_PACKET_FLAG) {
      if (!first) {
        first = &tmpArr[0];
      } else if (!last) {
        tmpArrI--;
        last = &tmpArr[tmpArrI-1];
        break;
      }
    }

  }
  
  WORK_formattedPrintArray("received bytes:", first, tmpArrI);

  // Разбор пакета в структуру, если найдены флаги
  if (first && last) {
    bufLen = last - first + 1;
    int ret = WORK_doGroupPackageBytes(worker->packet, first, last, bufLen);
    if (ret == 0) {
      // memset(worker->byteBuffer, 0, WORK_BYTE_BUFFER_SIZE);
      // memcpy(worker->byteBuffer, tmpArr, tmpArrI);
      
      // WORK_formattedPrintArray("parsed bytes:", worker->byteBuffer, bufLen);
      WORK_formattedPrintArray("parsed bytes:", tmpArr, bufLen);
      WORK_printBlocks(worker, 3);
    } 
    return ret;
  } else {
    // неправильный пакет
    printf("WORKX: .. WRONG PACKET\n");
    return -1;
  }
  return -1;
}


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

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

  return -1;
}



// Вызов функций обработчика хоста
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: .. Progerss (%d)\n", res);
  } else if (res < 0) {
    printf("WORKX: .. Error (%d)\n", res);  
  }
  
  return res;
}