#include "stdio.h"
#include "stdint.h"
#include "soft/debug.h"
#include "stddef.h"



/* table used for ffs and ffz operation */
const uint8_t iot_bitops_ffs_tab[256] = {
  0, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
  5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
  6, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
  5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
  7, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
  5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
  6, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
  5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
  8, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
  5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
  6, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
  5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
  7, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
  5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
  6, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1,
  5, 1, 2, 1, 3, 1, 2, 1, 4, 1, 2, 1, 3, 1, 2, 1
};

static inline uint8_t iot_bitops_ffs(uint8_t b)
{
    return iot_bitops_ffs_tab[b];
}

int gpio_find_first_int(uint8_t* bitmap, int bitmap_cnt, int start_bit) {
  uint32_t i, ret = 0;

  for (i = start_bit >> 3; i < bitmap_cnt; i++) {
      ret = iot_bitops_ffs(bitmap[i]);
      if (ret) {
          bitmap[i] &= ~(1 << (ret - 1));
          break;
      }
  }

  if (ret)
      ret += i << 3;

  return ret;
}


uint32_t iot_bitmap_ffs_from(uint8_t *bm, uint16_t size, uint32_t sp)
{
    uint32_t i, sp_byte, sp_bit, ret = 0;

    sp--;
    sp_byte = sp >> 3;
    sp_bit = sp & 0x7;
    if (sp_byte >= size) {
        sp_byte = 0;
        sp_bit = 0;
    }
    if (sp_bit) {
        for (i = sp_bit; i < 8; i++) {
            if (bm[sp_byte] & (1 << i)) {
                ret = i + 1;
                i = sp_byte;
                goto done;
            }
        }
        sp_byte++;
        sp_bit = 0;
    }

    for (i = sp_byte; i < size; i++) {
        ret = iot_bitops_ffs(bm[i]);
        if (ret) {
            goto done;
        }
    }

    /* wrap around and start from 0 again */
    for (i = 0; i < sp_byte; i++) {
        ret = iot_bitops_ffs(bm[i]);
        if (ret) {
            goto done;
        }
    }

done:

    if (ret)
        ret += i << 3;

    return ret;
}


int thread_fun(void* t)
{
  uint32_t bitmap[] = { 0,0,8 };
  uint32_t tmp[sizeof(bitmap) / sizeof(bitmap[0])];
  int ret = 0;
  for (int i = 0;i < sizeof(bitmap) / sizeof(bitmap[0]);i++) {
    tmp[i] = bitmap[i];
  }
  ret = gpio_find_first_int((uint8_t*)tmp, sizeof(tmp), 0);
  DBG_LOG("ret=%d\n", ret - 1);
  ret = gpio_find_first_int((uint8_t*)tmp, sizeof(tmp), ret);
  DBG_LOG("ret=%d\n", ret - 1);

  for (int i = 0;i < sizeof(bitmap) / sizeof(bitmap[0]);i++) {
    tmp[i] = bitmap[i];
  }
  ret = iot_bitmap_ffs_from((uint8_t*)tmp, sizeof(tmp), 0);
  DBG_LOG("ret=%d\n", ret - 1);
  ret = iot_bitmap_ffs_from((uint8_t*)tmp, sizeof(tmp), ret);
  DBG_LOG("ret=%d\n", ret - 1);
}