#include "sprs.h"
#include <stdlib.h>

void sprs_init(sprs* sprs){
  sprs->capacity = 0;
  sprs->size = 0;
  sprs->dense = NULL;
  sprs->sparse = NULL;
}
libds_result sprs_setup(sprs* sprs, size_t size){
  libds_result result = LIBDS_SUCCESS;
  sprs->sparse = malloc(sizeof(*(sprs->sparse)) * size);
  sprs->dense = malloc(sizeof(*(sprs->dense)) * size);
  if(sprs->sparse == NULL || sprs->dense == NULL){
    free(sprs->sparse);
    free(sprs->dense);
    sprs->sparse = NULL;
    sprs->dense = NULL;
    result = LIBDS_MALLOC;
  } else {
    sprs->capacity = size;
  }
  return result;
}
void sprs_free(sprs* sprs){
  free(sprs->sparse);
  free(sprs->dense);
  sprs->capacity = 0;
  sprs->size = 0;
  sprs->dense = NULL;
  sprs->sparse = NULL;
}
void sprs_put(sprs* sprs, size_t index, void* data){
  if(index < sprs->capacity && sprs->size < sprs->capacity){
    if(!sprs_contains(sprs, index)){
      sprs->dense[sprs->size++] = index;
      sprs->sparse[index].index = sprs->size - 1;
      sprs->sparse[index].data = data;
    } else {
      sprs->sparse[index].data = data;
    }
  }
}
libds_result sprs_put_grow(sprs* sprs, size_t index, void* data) {
  libds_result result = LIBDS_SUCCESS;
  if(!sprs_contains(sprs, index) && (index >= sprs->capacity || sprs->size >= sprs->capacity)) {
    int increase_required = 0;
    size_t ratio = index / sprs->capacity;
    size_t new_size;
    void* new_sparse;
    void* new_dense;
    while(ratio >>= 1) {
      increase_required++;
    }
    new_size = sprs->capacity * (1 << (increase_required + 1));
    new_sparse = realloc(sprs->sparse, sizeof(*(sprs->sparse)) * new_size);
    new_dense = realloc(sprs->dense, sizeof(*(sprs->dense)) * new_size);
    if(new_sparse == NULL || new_dense == NULL){
      free(new_sparse);
      free(new_dense);
      result = LIBDS_MALLOC;
    } else {
      sprs->sparse = new_sparse;
      sprs->dense = new_dense;
      sprs->capacity = new_size;
    }
  }
  if(result == LIBDS_SUCCESS) {
    sprs_put(sprs, index, data);
  }
  return result;
}
int sprs_contains(const sprs* sprs, size_t index){
  return sprs->sparse[index].index < sprs->size && sprs->dense[sprs->sparse[index].index] == index;
}
void* sprs_get(const sprs* sprs, size_t index){
  void* data = NULL;
  if(sprs_contains(sprs, index)){
    data = sprs->sparse[index].data;
  }
  return data;
}
void* sprs_find(const sprs* sprs, void* data, compare_func compare){
  int index = 0;
  void* to_return = NULL;
  for(; index < sprs->size && to_return == NULL; index++){
    if(compare(data, sprs->sparse[sprs->dense[index]].data)) {
      to_return = sprs->sparse[sprs->dense[index]].data;
    }
  }
  return to_return;
}
int sprs_foreach(const sprs* sprs, void* data, compare_func compare, foreach_func foreach, ...){
  int index = 0;
  int return_code = 0;
  va_list args;
  for(; index < sprs->size && return_code == 0; index++){
    if(compare(data, sprs->sparse[sprs->dense[index]].data)) {
      va_start(args, foreach);
      return_code = foreach(sprs->sparse[sprs->dense[index]].data, args);
      va_end(args);
    }
  }
  return return_code;
}