#include <stdio.h>
#include "fifo_impl.h"

typedef enum test_t{
  FAIL=0,
  PASS=1
}test_t;

#define FIFO_SIZE   4

unsafe test_t test_is_empty(mem_fifo_short_t * unsafe fifo){
  unsigned fill = fifo_get_fill_short(fifo);
  short data[1];
  fifo_ret_t ret = fifo_block_pop_short(fifo, data, 1);
  if (fill == 0 && ret == FIFO_EMPTY) return PASS;
  return FAIL;
}

unsafe test_t test_push_one(mem_fifo_short_t * unsafe fifo){
  unsigned fill_0 = fifo_get_fill_short(fifo);
  short data[] = {123};
  fifo_ret_t ret = fifo_block_push_short(fifo, data, 1);
  unsigned fill_1 = fifo_get_fill_short(fifo);

  if (ret != FIFO_SUCCESS || fill_0 + 1 != fill_1) return FAIL;
  return PASS;
}

unsafe test_t test_partial_fill(mem_fifo_short_t * unsafe fifo){
  unsigned fill_0 = fifo_get_fill_short(fifo);
  short data[] = {80, 90, 100};
  fifo_ret_t ret = fifo_block_push_short(fifo, data, 3);
  unsigned fill_1 = fifo_get_fill_short(fifo);

  if (ret != FIFO_SUCCESS || fill_0 != 0 || fill_1 != 3) return FAIL;
  return PASS;
}

unsafe test_t test_fill_level(mem_fifo_short_t * unsafe fifo){
  unsigned fill = fifo_get_fill_short(fifo);
  if (fill != 3) return FAIL;
  return PASS;
}

unsafe test_t test_pop_one(mem_fifo_short_t * unsafe fifo, short expect){
  unsigned fill_0 = fifo_get_fill_short(fifo);
  short data[1] = {0xffff};
  fifo_ret_t ret = fifo_block_pop_short(fifo, data, 1);
  unsigned fill_1 = fifo_get_fill_short(fifo);
  if (ret != FIFO_SUCCESS || fill_0 != fill_1 + 1 || data[0] != expect) {
    printf("grr %d\n", data[0]);
    return FAIL;
  }
  return PASS;
}

unsafe test_t test_pop_three_fail(mem_fifo_short_t * unsafe fifo){
  unsigned fill_0 = fifo_get_fill_short(fifo);
  short data[3] = {0xffff, 0xffff, 0xffff};
  fifo_ret_t ret = fifo_block_pop_short(fifo, data, 3);
  unsigned fill_1 = fifo_get_fill_short(fifo);
  if (ret == FIFO_SUCCESS || fill_0 != 2 || fill_1 != 2) return FAIL;
  return PASS;
}

unsafe test_t test_partial_fill_fast(mem_fifo_short_t * unsafe fifo){
  unsigned fill_0 = fifo_get_fill_short(fifo);
  short data[] = {20, 30, 40};
  fifo_ret_t ret = fifo_block_push_short_fast(fifo, data, 3);
  unsigned fill_1 = fifo_get_fill_short(fifo);

  if (ret != FIFO_SUCCESS || fill_0 != 0 || fill_1 != 3) return FAIL;
  return PASS;
}

unsafe test_t test_pop_block_fast(mem_fifo_short_t * unsafe fifo, short expect[], unsigned n){
  unsigned fill_0 = fifo_get_fill_short(fifo);
  short data[10] = {0xffff};
  fifo_ret_t ret = fifo_block_pop_short_fast(fifo, data, n);
  unsigned fill_1 = fifo_get_fill_short(fifo);
  unsigned data_ok = memcmp(data, expect, n * sizeof(short)) == 0;
  if (ret != FIFO_SUCCESS || fill_0 != fill_1 + n || !data_ok) return FAIL;
  return PASS;
}

unsafe void print_fifo(mem_fifo_short_t * unsafe fifo){
  for (int i = fifo->size - 1; i >= 0; i--){
    printf("FIFO[%d]: %hd %s %s\n", i, fifo->data_base_ptr[i], fifo->read_idx == i ? "RD" : "", fifo->write_idx == i ? "WR" : "");
  }
}


int main(void){
  unsafe{
    short test_fifo_storage[FIFO_SIZE];
    mem_fifo_short_t test_fifo = {sizeof(test_fifo_storage)/sizeof(test_fifo_storage[0]), test_fifo_storage, 0, 0};
    volatile mem_fifo_short_t * unsafe test_fifo_ptr = &test_fifo;

    // print_fifo(test_fifo_ptr);
    printf("test_is_empty: %d\n", test_is_empty(test_fifo_ptr));
    printf("test_partial_fill: %d\n", test_partial_fill(test_fifo_ptr));
    // print_fifo(test_fifo_ptr);

    printf("test_fill_level: %d\n", test_fill_level(test_fifo_ptr));
    printf("test_pop_one: %d\n", test_pop_one(test_fifo_ptr, 80));
    printf("test_pop_three_fail: %d\n", test_pop_three_fail(test_fifo_ptr));
    printf("test_pop_one: %d\n", test_pop_one(test_fifo_ptr, 90));
    printf("test_pop_one: %d\n", test_pop_one(test_fifo_ptr, 100));
    printf("test_is_empty: %d\n", test_is_empty(test_fifo_ptr));

    printf("test_partial_fill_fast: %d\n", test_partial_fill_fast(test_fifo_ptr));

    printf("test_pop_one: %d\n", test_pop_one(test_fifo_ptr, 20));
    // print_fifo(test_fifo_ptr);

    short td[] = {30, 40};

    printf("test_pop_block_fast: %d\n", test_pop_block_fast(test_fifo_ptr, td, 2)); //no wrap

    printf("test_is_empty: %d\n", test_is_empty(test_fifo_ptr));
    printf("test_push_one: %d\n", test_push_one(test_fifo_ptr));
    printf("test_push_one: %d\n", test_push_one(test_fifo_ptr));
    printf("test_pop_three_fail: %d\n", test_pop_three_fail(test_fifo_ptr));
    printf("test_pop_one: %d\n", test_pop_one(test_fifo_ptr, 123));
    printf("test_pop_one: %d\n", test_pop_one(test_fifo_ptr, 123));

    printf("test_partial_fill_fast: %d\n", test_partial_fill_fast(test_fifo_ptr)); //no wrap
    short te[] = {20, 30, 40};
    printf("test_pop_block_fast: %d\n", test_pop_block_fast(test_fifo_ptr, te, 3));// no wrap
    printf("test_partial_fill_fast: %d\n", test_partial_fill_fast(test_fifo_ptr)); //wrap
    printf("test_pop_block_fast: %d\n", test_pop_block_fast(test_fifo_ptr, te, 3));// wrap
    // print_fifo(test_fifo_ptr);




    return 0;
  }
}