#include "shape.h"
#include "camera.h"
#include "debug.h"
#include "render.h"
#include <SDL2/SDL_render.h>
#include <stdlib.h>

struct Shape {
    Vector* points;
    size_t points_len;

    Vector mean;
    int is_convex;
};

static
Vector* _shape_get_furthest_in_direction(Shape* self, Vector direction) {
    // ensure direction is normalized
    direction = vnormalizedf(direction);
    Vector* end = self->points + self->points_len;
    float furthest_dot = vdotf(direction, vsubf(self->points[0], self->mean));
    Vector* furthest = self->points;

    float dot;
    for(Vector* point = self->points; point < end; ++point) {
        dot = vdotf(direction, vsubf(*point, self->mean));
        if(dot > furthest_dot) {
            furthest = point;
        }
    }

    return furthest;
}

// go through each point,
// in order to be convex, none of the points in the shape can be "inset".
// This means that if one of the points is not the furthest in it's own direction
// measured from the median.
static
int _shape_calculate_is_convex(Shape* self) {
    Vector* end = self->points + self->points_len;
    // point relative to mean
    Vector relative;
    for(Vector* point = self->points; point < end; ++point) {
        relative = vsubf(*point, self->mean);
        if(point != _shape_get_furthest_in_direction(self, relative)) {
            return 0;
        }
    }

    return 1;
}

static
Vector _shape_calculate_mean(Shape* self) {
    Vector* const end = self->points + self->points_len;
    Vector avg = ZeroVector;
    size_t count = 0;
    for(Vector* point = self->points; point < end; ++point) {
        ++count;
        avg = vaddf(avg, vmulff(*point, 1.0/count));
    }

    return avg;
}

Shape* shape_new(const Vector* points, size_t points_len) {
    // allocate required space for shape and points array
    Shape* self = malloc(sizeof(Shape));
    ASSERT_RETURN(self != NULL, NULL, "Failed to allocate enough space for a shape object.");

    self->points = malloc(points_len * sizeof(Vector));
    if(self->points == NULL) {
        free(self);
        RETURN_ERROR(NULL, "Failed to allocate enough space for %zu points", points_len);
    }

    // initialize data based on function arguments
    self->points_len = points_len;
    memcpy(self->points, points, points_len * sizeof(Vector));

    // derive metadata
    self->mean = _shape_calculate_mean(self);
    self->is_convex = _shape_calculate_is_convex(self);

    return self;
}

Shape* shape_new_square(Vector size) {
    return shape_new((Vector[4]){
        ZeroVector,
        (Vector){size.x, 0.f},
        size,
        (Vector){0.f, size.y},
    }, 4);
}

Shape* shape_clone(const Shape* source) {
    Shape* self = malloc(sizeof(Shape));
    ASSERT_RETURN(self != NULL, NULL, "Failed to allocate space for shape object.");

    self->points = malloc(source->points_len * sizeof(Vector));
    ASSERT_RETURN(self->points != NULL, NULL, "Failed to allocate space for shape object.");

    // copy data from source
    self->points_len = source->points_len;
    memcpy(self->points, source->points, source->points_len * sizeof(Vector));
    self->mean = source->mean;
    self->is_convex = source->is_convex;

    return self;
}

void shape_destroy(Shape* self) {
    free(self->points);
    free(self);
}

Vector* shape_get_start(Shape* self) {
    return self->points;
}

Vector* shape_get_end(Shape* self) {
    return self->points + self->points_len;
}

size_t shape_get_points_count(const Shape* self) {
    return self->points_len;
}

Vector shape_get_point(const Shape* self, size_t at) {
    ASSERT_RETURN(at < self->points_len, self->mean, "Point index %zu out of bounds for shape", at);
    return self->points[at];
}

Vector shape_get_point_transformed(const Shape* self, size_t at, Transform transform) {
    return transform_point(&transform, shape_get_point(self, at));
}

void shape_set_point(Shape* self, size_t at, Vector point) {
    ASSERT_RETURN(at < self->points_len,, "Point index %zu out of bounds for shape", at);
    self->points[at] = point;
}

void shape_add_point(Shape* self, Vector point) {
    Vector* new = realloc(self->points, (self->points_len + 1) * sizeof(Vector));
    ASSERT_RETURN(new != NULL,, "Failed to allocate space for new point in shape");
    
    new[self->points_len] = point;
    ++self->points_len;
    self->points = new;
}

void shape_insert_point(Shape* self, size_t at, Vector point) {
    ASSERT_RETURN(at < self->points_len + 1,, "Point index %zu out of bounds for shape", at);
    Vector* new = realloc(self->points, (self->points_len + 1) * sizeof(Vector));
    ASSERT_RETURN(new != NULL,, "Failed to allocate space to insert new point in shape");

    size_t after_at = at + 1;
    // both should add one
    // (len increased and we want to measure the difference starting offset by one from at)
    // which cancels out
    size_t dif = self->points_len - after_at;
    memmove(new + at, new + at + 1, dif);
    new[at] = point;
    self->points = new;
    ++self->points_len;
}

Vector shape_remove_point(Shape* self, size_t at) {
    ASSERT_RETURN(at < self->points_len, ZeroVector, "Point index %zu out of bounds for shape", at);
    Vector point = self->points[at];
    memmove(self->points + at, self->points + at + 1, (self->points_len - (at + 1)) * sizeof(Vector));
    Vector* new = realloc(self->points, (self->points_len - 1) * sizeof(Vector));
    ASSERT_RETURN(new != NULL, point, "Failed to shrink points array to %zu", self->points_len - 1);
    self->points = new;
    --self->points_len;
    return point;
}

Vector shape_get_median_point(Shape* self) {
    return self->mean;
}

int shape_is_convex(Shape* self) {
    return self->is_convex;
}

void shape_draw(Shape* self, Transform transform) {
    Vector lhs, rhs, normal;
    for(size_t i = 0; i < self->points_len; ++i) {
        lhs = shape_get_point_transformed(self, i, transform);
        rhs = shape_get_point_transformed(self, (i + 1) % self->points_len, transform);
        normal = vnormalizedf(vperpendicularf(vsubf(rhs, lhs)));
        lhs = camera_world_to_pixel_point(&g_camera, lhs);
        rhs = camera_world_to_pixel_point(&g_camera, rhs);
        normal = transform_direction(&g_camera.transform, normal);

        SDL_SetRenderDrawColor(g_renderer, 255, 255, 255, 255);
        SDL_RenderDrawLineF(g_renderer, lhs.x, lhs.y, rhs.x, rhs.y);

        lhs = vlerpf(lhs, rhs, 0.5f);
        rhs = vaddf(lhs, vmulff(normal, 10.f));
        SDL_SetRenderDrawColor(g_renderer, 0, 0, 255, 255);
        SDL_RenderDrawLineF(g_renderer, lhs.x, lhs.y, rhs.x, rhs.y);
    }
}