#include <memory.h>
#include "malunal/containers/vector.h"


typedef struct {
  container_t      container;
  allocator_mptr_t allocator;
  malunal_size_t   stride;
  malunal_size_t   count;
  malunal_size_t   capacity;
  malunal_mptr_t   buffer;
} impl_t;

typedef       impl_t* impl_mptr_t;
typedef const impl_t* impl_iptr_t;
_Static_assert(
  sizeof(vector_container_t) == sizeof(impl_t),
  "Vector container must be the size of its implementation"
);


static const
container_vtable_t vector_container_vtable = {
  .allocator = (container_allocator_pfn_t)&vector_container_allocator,
  .stride    = (container_stride_pfn_t)&vector_container_stride,
  .count     = (container_count_pfn_t)&vector_container_count,
  .capacity  = (container_capacity_pfn_t)&vector_container_capacity,
  .resize    = (container_resize_pfn_t)&vector_container_resize,
  .reserve   = (container_reserve_pfn_t)&vector_container_reserve,
  .append    = (container_append_pfn_t)&vector_container_append,
  .remove    = (container_remove_pfn_t)&vector_container_remove,
  .contains  = (container_contains_pfn_t)&vector_container_contains,
  .clear     = (container_clear_pfn_t)&vector_container_clear
};

static
malunal_size_t
power_2_ceil(malunal_size_t capacity) {
  if (capacity <= 1)
    return 1;

  capacity--;
  malunal_size_t power = 2;
  while (capacity >>= 1)
    power <<= 1;
  return power;
}

static
error_t
vector_container_realloc(
  impl_mptr_t    self,
  malunal_size_t capacity
) {
  if (capacity <= self->capacity)
    return NO_ERROR;

  malunal_mptr_t buffer;
  malunal_size_t newcap = power_2_ceil(capacity);
  malunal_size_t bytes  = self->stride * newcap;
  error_t        result = allocator_acquire(self->allocator, bytes, &buffer);
  if (result.domain != null)
    return result;

  bytes  = self->stride * self->capacity;
  buffer = memmove(buffer, self->buffer, bytes);
  result = allocator_dispose(self->allocator, self->buffer, bytes);
  self->buffer   = buffer;
  self->capacity = newcap;
  return NO_ERROR;
}

error_t
vector_container_init(
  malunal_size_t          stride,
  allocator_mptr_t        allocator,
  vector_container_mptr_t vector
) {
  if (vector == null)
    return (error_t) {
      .domain = &ERROR_DOMAIN_CONTAINER_T,
      .code   = CONTAINER_ERROR_NULL_CONTAINER
    };

  impl_mptr_t    self   = (impl_mptr_t)vector;
  malunal_size_t bytes  = stride * MALUNAL_CONTAINERS_VECTOR_INIT_CAPACITY;
  error_t        result = allocator_acquire(allocator, bytes, &self->buffer);
  if (result.domain != null)
    return result;

  self->container = (container_t){ &vector_container_vtable };
  self->allocator = allocator;
  self->stride    = stride;
  self->count     = 0;
  self->capacity  = MALUNAL_CONTAINERS_VECTOR_INIT_CAPACITY;
  return NO_ERROR;
}

error_t
vector_container_free(
  vector_container_mptr_t vector
) {
  if (vector == null)
    return (error_t) {
      .domain = &ERROR_DOMAIN_CONTAINER_T,
      .code   = CONTAINER_ERROR_NULL_CONTAINER
    };

  impl_mptr_t self   = (impl_mptr_t)vector;
  error_t     result = allocator_dispose(
    self->allocator,
    self->buffer,
    self->stride * self->capacity
  );

  if (result.domain != null)
    return result;

  self->stride   = 0;
  self->count    = 0;
  self->capacity = 0;
  self->buffer   = null;
  return NO_ERROR;
}

error_t
vector_container_allocator(
  vector_container_iptr_t vector,
  allocator_mptr_t*       out
) {
  if (vector == null)
    return (error_t) {
      .domain = &ERROR_DOMAIN_CONTAINER_T,
      .code   = CONTAINER_ERROR_NULL_CONTAINER
    };

  impl_iptr_t self = (impl_iptr_t)vector;
  *out = self->allocator;
  return NO_ERROR;
}

error_t
vector_container_stride(
  vector_container_iptr_t vector,
  malunal_size_t*         out
) {
  if (vector == null)
    return (error_t) {
      .domain = &ERROR_DOMAIN_CONTAINER_T,
      .code   = CONTAINER_ERROR_NULL_CONTAINER
    };

  impl_iptr_t self = (impl_iptr_t)vector;
  *out = self->stride;
  return NO_ERROR;
}

error_t
vector_container_count(
  vector_container_iptr_t vector,
  malunal_size_t*         out
) {
  if (vector == null)
    return (error_t) {
      .domain = &ERROR_DOMAIN_CONTAINER_T,
      .code   = CONTAINER_ERROR_NULL_CONTAINER
    };

  impl_iptr_t self = (impl_iptr_t)vector;
  *out = self->count;
  return NO_ERROR;
}

error_t
vector_container_capacity(
  vector_container_iptr_t vector,
  malunal_size_t*         out
) {
  if (vector == null)
    return (error_t) {
      .domain = &ERROR_DOMAIN_CONTAINER_T,
      .code   = CONTAINER_ERROR_NULL_CONTAINER
    };

  impl_iptr_t self = (impl_iptr_t)vector;
  *out = self->capacity;
  return NO_ERROR;
}

error_t
vector_container_resize(
  vector_container_mptr_t vector,
  malunal_size_t          capacity
) {
  if (vector == null)
    return (error_t) {
      .domain = &ERROR_DOMAIN_CONTAINER_T,
      .code   = CONTAINER_ERROR_NULL_CONTAINER
    };

  impl_mptr_t self   = (impl_mptr_t)vector;
  error_t     result = vector_container_realloc(self, capacity);
  if (result.domain != null)
    return result;

  self->count = capacity;
  return NO_ERROR;
}

error_t
vector_container_reserve(
  vector_container_mptr_t vector,
  malunal_size_t          capacity
) {
  if (vector == null)
    return (error_t) {
      .domain = &ERROR_DOMAIN_CONTAINER_T,
      .code   = CONTAINER_ERROR_NULL_CONTAINER
    };

  impl_mptr_t self = (impl_mptr_t)vector;
  return vector_container_realloc(self, capacity);
}

error_t
vector_container_append(
  vector_container_mptr_t vector,
  malunal_iptr_t          element
) {
  if (vector == null)
    return (error_t) {
      .domain = &ERROR_DOMAIN_CONTAINER_T,
      .code   = CONTAINER_ERROR_NULL_CONTAINER
    };

  error_t     result = {};
  impl_mptr_t self   = (impl_mptr_t)vector;
  if ((float)self->count / self->capacity >= 0.75f) {
    result = vector_container_realloc(self, self->capacity * 2);
    if (result.domain != null)
      return result;
  }

  malunal_uint8_t* buffer = self->buffer;
  buffer += self->stride * self->count;
  memcpy(buffer, element, self->stride);
  self->count++;
  return NO_ERROR;
}

error_t
vector_container_remove(
  vector_container_mptr_t vector,
  malunal_iptr_t          element
) {
  if (vector == null)
    return (error_t) {
      .domain = &ERROR_DOMAIN_CONTAINER_T,
      .code   = CONTAINER_ERROR_NULL_CONTAINER
    };

  malunal_size_t index;
  impl_mptr_t    self   = (impl_mptr_t)vector;
  error_t        result = vector_container_index_of(vector, element, &index);
  if (result.domain != null)
    return result;

  return index != (malunal_size_t)-1
   ? vector_container_remove_at(vector, index)
   : NO_ERROR;
}

error_t
vector_container_contains(
  vector_container_iptr_t vector,
  malunal_iptr_t          element
) {
  if (vector == null)
    return (error_t) {
      .domain = &ERROR_DOMAIN_CONTAINER_T,
      .code   = CONTAINER_ERROR_NULL_CONTAINER
    };

  malunal_size_t index;
  impl_iptr_t    self   = (impl_iptr_t)vector;
  error_t        result = vector_container_index_of(vector, element, &index);
  if (result.domain != null)
    return result;

  return index == (malunal_size_t)-1
    ? (error_t) {
        .domain = &ERROR_DOMAIN_CONTAINER_T,
        .code   = CONTAINER_ERROR_FAILURE
      }
    : NO_ERROR;
}

error_t
vector_container_clear(
  vector_container_mptr_t vector
) {
  if (vector == null)
    return (error_t) {
      .domain = &ERROR_DOMAIN_CONTAINER_T,
      .code   = CONTAINER_ERROR_NULL_CONTAINER
    };

  impl_mptr_t self = (impl_mptr_t)vector;
  self->count = 0;
  return NO_ERROR;
}

error_t
vector_container_get(
  vector_container_iptr_t vector,
  malunal_size_t          index,
  malunal_mptr_t          element
) {
  if (vector == null)
    return (error_t) {
      .domain = &ERROR_DOMAIN_CONTAINER_T,
      .code   = CONTAINER_ERROR_NULL_CONTAINER
    };

  impl_iptr_t self = (impl_iptr_t)vector;
  if (index >= self->count)
    return (error_t) {
      .domain = &ERROR_DOMAIN_CONTAINER_T,
      .code   = CONTAINER_ERROR_OUT_OF_BOUNDS
    };

  malunal_uint8_t* buffer = self->buffer;
  buffer += self->stride * index;
  memcpy(element, buffer, self->stride);
  return NO_ERROR;
}

error_t
vector_container_set(
  vector_container_mptr_t vector,
  malunal_size_t          index,
  malunal_iptr_t          element
) {
  if (vector == null)
    return (error_t) {
      .domain = &ERROR_DOMAIN_CONTAINER_T,
      .code   = CONTAINER_ERROR_NULL_CONTAINER
    };

  impl_mptr_t self = (impl_mptr_t)vector;
  if (index >= self->count)
    return (error_t) {
      .domain = &ERROR_DOMAIN_CONTAINER_T,
      .code   = CONTAINER_ERROR_OUT_OF_BOUNDS
    };

  malunal_uint8_t* buffer = self->buffer;
  buffer += self->stride * index;
  memcpy(buffer, element, self->stride);
  return NO_ERROR;
}

error_t
vector_container_insert_at(
  vector_container_mptr_t vector,
  malunal_size_t          index,
  malunal_iptr_t          element
) {
  if (vector == null)
    return (error_t) {
      .domain = &ERROR_DOMAIN_CONTAINER_T,
      .code   = CONTAINER_ERROR_NULL_CONTAINER
    };

  impl_mptr_t self = (impl_mptr_t)vector;
  if (index > self->count)
    return (error_t) {
      .domain = &ERROR_DOMAIN_CONTAINER_T,
      .code   = CONTAINER_ERROR_OUT_OF_BOUNDS
    };

  error_t result = {};
  if ((float)self->count / self->capacity > 0.75f) {
    result = vector_container_realloc(self, self->capacity * 2);
    if (result.domain != null)
      return result;
  }

  malunal_uint8_t* orig = self->buffer;
  orig += self->stride * index;

  malunal_uint8_t* dest = orig + self->stride;
  memmove(dest, orig, self->stride * (self->count - index));
  memcpy(orig, element, self->stride);
  self->count++;
  return NO_ERROR;
}

error_t
vector_container_remove_at(
  vector_container_mptr_t vector,
  malunal_size_t          index
) {
  if (vector == null)
    return (error_t) {
      .domain = &ERROR_DOMAIN_CONTAINER_T,
      .code   = CONTAINER_ERROR_NULL_CONTAINER
    };

  impl_mptr_t self = (impl_mptr_t)vector;
  if (self->count == 0 || index >= self->count)
    return (error_t) {
      .domain = &ERROR_DOMAIN_CONTAINER_T,
      .code   = CONTAINER_ERROR_OUT_OF_BOUNDS
    };

  malunal_uint8_t* dest = self->buffer;
  dest += self->stride * index;

  malunal_uint8_t* orig = dest + self->stride;
  memmove(dest, orig, self->stride * (self->count - index));
  self->count--;
  return NO_ERROR;
}

error_t
vector_container_index_of(
  vector_container_iptr_t vector,
  malunal_iptr_t          element,
  malunal_size_t*         outidx
) {
  if (vector == null)
    return (error_t) {
      .domain = &ERROR_DOMAIN_CONTAINER_T,
      .code   = CONTAINER_ERROR_NULL_CONTAINER
    };

  malunal_size_t   index  = 0;
  impl_iptr_t      self   = (impl_iptr_t)vector;
  malunal_uint8_t* buffer = self->buffer;
  while (index < self->count) {
    if (memcmp(buffer, element, self->stride) == 0) {
      *outidx = index;
      return NO_ERROR;
    }

    index  += 1;
    buffer += self->stride;
  }

  *outidx = (malunal_size_t)-1;
  return NO_ERROR;
}