#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <inttypes.h>
#include <math.h>
#include <errno.h>

#include <xas/object.h>
#include <xas/vox.h>
#include <xas/spatial.h>

static inline float dist(xas_spatial_coord a, xas_spatial_coord b) {
    return powf(powf(b.x - a.x, 2.0f)
              + powf(b.y - a.y, 2.0f)
              + powf(b.z - a.z, 2.0f), 0.5f);
}

static float dotf(xas_spatial_coord a, xas_spatial_coord b) {
    return a.x * b.x
         + a.y * b.y
         + a.z * b.z;
}

static void diff(xas_spatial_coord *ret,
                 xas_spatial_coord a,
                 xas_spatial_coord b) {
    ret->x = a.x - b.x;
    ret->y = a.y - b.y;
    ret->z = a.z - b.z;
}

static float magnf(xas_spatial_coord point) {
    return sqrtf(point.x * point.x
               + point.y * point.y
               + point.z * point.z);
}

static void rotate(xas_spatial_coord rotation,
                   xas_spatial_coord *point) {
    double cosY = cos(rotation.y),
           sinY = sin(rotation.y),
           cosZ = cos(rotation.z),
           sinZ = sin(rotation.z),
           cosX = cos(rotation.x),
           sinX = sin(rotation.x);

    double Rx, Ry, Py, Pz, Yx, Yz;

    /*
     * Adjust vertex for Z axis rotation
     */
    Rx = point->x * cosZ - point->y * sinZ;
    Ry = point->y * cosZ + point->x * sinZ;

    /*
     * Adjust vertex for X axis rotation
     */
    Py = Ry * cosX - point->z * sinX;
    Pz = point->z * cosX - Ry * sinX;

    /*
     * Adjust vertex for Y axis rotation
     */
    Yx = Rx * cosY - Pz * sinY;
    Yz = Pz * cosY - Rx * sinY;

    point->x = Yx;
    point->y = Py;
    point->z = Yz;
}

static void move(xas_spatial_coord *point,
                 xas_spatial_coord heading,
                 float speed,
                 float interval) {
    float distance = speed * interval;

    xas_spatial_coord normal = { 0.0f, 0.0f, distance };

    rotate(heading, &normal);

    point->x += normal.x;
    point->y += normal.y;
    point->z += normal.z;
}

static int within_cone(xas_spatial_coord point,
                       xas_spatial_cone cone) {
    xas_spatial_coord apex_to_point_vect,
                        axis_vect,
                        rotated  = cone.apex,
                        opposite = {
                            .x = cone.rotation.x * -2.0f,
                            .y = cone.rotation.y * -2.0f,
                            .z = cone.rotation.z * -2.0f
                        };

    rotate(opposite, &rotated);

    diff(&apex_to_point_vect, cone.apex, point);
    diff(&axis_vect,          cone.apex, rotated);

    return dotf(apex_to_point_vect, axis_vect)
         / magnf(apex_to_point_vect)
         / magnf(axis_vect) <= cosf(cone.angle / 2.0f);
}

static int buffer_realloc(xas_spatial_scene *scene,
                          xas_spatial_buffer *buffer) {
    float seconds = scene->radius / scene->speed;

    size_t sample_rate = scene->format.sample_rate,
           stride      = scene->format.channels * scene->format.sample_size,
           count       = sample_rate * ceilf(seconds),
           total       = sizeof(xas_spatial_buffer) + stride * count;

    if ((buffer = realloc(buffer, total)) == NULL) {
        goto error_realloc_buffer;
    }

    scene->buffer        = buffer;
    scene->buffer->index = 0;
    scene->buffer->size  = count;

    memset(buffer + 1, '\0', stride * count);

    return 0;

error_realloc_buffer:
    return -1;
}

static inline size_t sample_delay(xas_spatial_scene *scene, float distance) {
    return floorf((distance / scene->speed) * (float)scene->format.sample_rate);
}

static inline int16_t sample_scale(int16_t value, float distance) {
    return (int16_t)roundf((float)value * (1.0f / distance));
}

static void buffer_zero(xas_spatial_buffer *buffer, size_t count) {
    int16_t *dest = (int16_t *)(buffer + 1);
    size_t i = buffer->index;

    if (count > buffer->size) {
        count = buffer->size;
    }

    while (count--) {
        if (i == buffer->size) {
            i = 0;
        }

        dest[XAS_AUDIO_STEREO*i]   = 0;
        dest[XAS_AUDIO_STEREO*i+1] = 0;

        i++;
    }
}

static void buffer_copy(xas_spatial_buffer *buffer,
                        int16_t *dest,
                        size_t count) {
    int16_t *src = (int16_t *)(buffer + 1);

    size_t i = buffer->index,
           o;

    if (count > buffer->size) {
        count = buffer->size;
    }

    for (o=0; o<count; o++) {
        if (i == buffer->size) {
            i = 0;
        }

        dest[XAS_AUDIO_STEREO*o]   = src[XAS_AUDIO_STEREO*i];
        dest[XAS_AUDIO_STEREO*o+1] = src[XAS_AUDIO_STEREO*i+1];

        i++;
    }
}

static void object_position(xas_spatial_scene *scene,
                            xas_spatial_object *object,
                            xas_spatial_coord point) {
    size_t delay_l_old = object->delay_l,
           delay_r_old = object->delay_r;

    object->point = point;

    object->distance_l = dist(scene->speaker_l, point);
    object->distance_r = dist(scene->speaker_r, point);
    object->delay_l    = sample_delay(scene, object->distance_l);
    object->delay_r    = sample_delay(scene, object->distance_r);
    object->shift_l    = object->delay_l - delay_l_old;
    object->shift_r    = object->delay_r - delay_r_old;
}

static void object_update_delays(xas_spatial_scene *scene) {
    xas_spatial_object *obj = scene->first;

    while (obj) {
        xas_spatial_object *next = obj->next;

        object_position(scene, obj, obj->point);

        obj = next;
    }
}

ssize_t scene_fill(xas_spatial_scene *scene,
                   int16_t *output,
                   size_t count) {
    xas_spatial_buffer *buffer = scene->buffer;
    xas_spatial_object *obj    = scene->first;

    float interval = 1.0f / (float)scene->format.sample_rate;

    size_t index_old = buffer->index;

    int16_t *dest = (int16_t *)(scene->buffer + 1);

    xas_spatial_cone cone_l = {
        .apex     = scene->speaker_l,
        .rotation = { 0.0f, M_PI * 1.5f, 0.0f },
        .angle    = M_PI / 4.0f
    }, cone_r = {
        .apex     = scene->speaker_r,
        .rotation = { 0.0f, M_PI * 0.5f, 0.0f },
        .angle    = M_PI / 4.0f
    };

    buffer_zero(buffer, count);

    while (obj) {
        int16_t *src;

        ssize_t readlen,
                i;

        if ((readlen = xas_audio_stream_read(obj->source,
                                               (void **)&src,
                                               count)) < 0) {
            goto error_audio_stream_read;
        }

        if (readlen > (ssize_t)buffer->size) {
            readlen = buffer->size;
        }

        for (i=0; i<readlen; i++) {
            int16_t value_l,
                    value_r;

            size_t index_l,
                   index_r;

            int within_cone_l = within_cone(obj->point, cone_l),
                within_cone_r = within_cone(obj->point, cone_r);

            if (obj->speed != 0.0f) {
                xas_spatial_coord point = obj->point;

                move(&point,
                     obj->heading,
                     obj->speed,
                     interval);

                object_position(scene, obj, point);
            }

            value_l = sample_scale(src[i], obj->distance_l);
            value_r = sample_scale(src[i], obj->distance_r);

            if (!within_cone_l) {
                if (within_cone_r) {
                    value_l /= 8;
                } else {
                    value_l /= 3;
                }
            }

            if (!within_cone_r) {
                if (within_cone_l) {
                    value_r /= 8;
                } else {
                    value_r /= 3;
                }
            }

            if (buffer->index == buffer->size) {
                buffer->index = 0;
            }

            index_l = index_r = buffer->index;

            if (obj->shift_l == 0) {
                dest[XAS_AUDIO_STEREO*index_l] += value_l;
            } else {
                while (obj->shift_l > 0) {
                    size_t index = index_l + obj->shift_l;

                    if (index < buffer->size) {
                        dest[XAS_AUDIO_STEREO*index] += value_l;
                    }

                    obj->shift_l--;
                }

                while (obj->shift_l < 0) {
                    dest[XAS_AUDIO_STEREO*index_l] += value_l;

                    obj->shift_l++;
                }
            }

            if (obj->shift_r == 0) {
                dest[XAS_AUDIO_STEREO*index_r+1] += value_r;
            } else {
                while (obj->shift_r > 0) {
                    size_t index = index_r + obj->shift_r;

                    if (index < buffer->size) {
                        dest[XAS_AUDIO_STEREO*index+1] += value_r;
                    }

                    obj->shift_r--;
                }

                while (obj->shift_r < 0) {
                    dest[XAS_AUDIO_STEREO*index_r+1] += value_r;

                    obj->shift_r++;
                }
            }

            buffer->index++;
        }

        buffer->index = index_old;
        obj           = obj->next;
    }

    buffer_copy(buffer, output, count);

    buffer->index  = index_old + count;
    buffer->index %= buffer->size;

    return count;

error_audio_stream_read:
    return -1;
}

xas_spatial_scene *xas_spatial_scene_new(xas_audio_format format,
                                             xas_spatial_coord speaker_l,
                                             xas_spatial_coord speaker_r) {
    xas_spatial_scene *scene;

    if ((scene = malloc(sizeof(*scene))) == NULL) {
        goto error_malloc_scene;
    }

    memset(scene, '\0', sizeof(*scene));

    scene->format    = format;
    scene->speaker_l = speaker_l;
    scene->speaker_r = speaker_r;
    scene->radius    = XAS_SPATIAL_DEFAULT_RADIUS;
    scene->speed     = XAS_SPATIAL_DEFAULT_MEDIUM_SPEED;

    if (buffer_realloc(scene, NULL) < 0) {
        goto error_buffer_realloc;
    }

    return scene;

error_buffer_realloc:
    free(scene);

error_malloc_scene:
    return NULL;
}

void xas_spatial_scene_destroy(xas_spatial_scene *scene) {
    xas_spatial_object *object = scene->first;

    while (object) {
        xas_spatial_object *next = object->next;

        if (object->flags & XAS_SPATIAL_OBJECT_MANAGED) {
            xas_audio_stream_destroy(object->source);
            xas_object_destroy(object->ctx);
        }

        free(object);

        object = next;
    }

    free(scene->buffer);
    free(scene);
}


void xas_spatial_scene_set_observer(xas_spatial_scene *scene,
                                      xas_spatial_coord point,
                                      xas_spatial_coord rotation,
                                      float width) {
    scene->observer.point    = point;
    scene->observer.rotation = rotation;
    scene->observer.width    = width;
}

void xas_spatial_scene_position_observer(xas_spatial_scene *scene,
                                           xas_spatial_coord point) {
    scene->observer.point = point;
}

void xas_spatial_scene_rotate_observer(xas_spatial_scene *scene,
                                         xas_spatial_coord rotation) {
    scene->observer.rotation = rotation;
}

void xas_spatial_scene_set_speaker_coords(xas_spatial_scene *scene,
                                            xas_spatial_coord speaker_l,
                                            xas_spatial_coord speaker_r) {
    scene->speaker_l = speaker_l;
    scene->speaker_r = speaker_r;

    object_update_delays(scene);
}

void xas_spatial_scene_rotate_speakers(xas_spatial_scene *scene,
                                         xas_spatial_coord rotation) {
    rotate(rotation, &scene->speaker_l);
    rotate(rotation, &scene->speaker_r);

    object_update_delays(scene);
}

int xas_spatial_scene_set_speed(xas_spatial_scene *scene, float speed) {
    scene->speed = speed;

    return buffer_realloc(scene, scene->buffer);
}

int xas_spatial_scene_set_radius(xas_spatial_scene *scene, float radius) {
    scene->radius = radius;

    return buffer_realloc(scene, scene->buffer);
}

xas_spatial_object *xas_spatial_scene_add_object(xas_spatial_scene *scene,
                                                     xas_spatial_coord point,
                                                     xas_audio_stream *source,
                                                     void *ctx) {
    xas_spatial_object *object;

    if (source->format.channels != XAS_AUDIO_MONO) {
        errno = EINVAL;

        goto error_invalid_source;
    }

    if ((object = malloc(sizeof(*object))) == NULL) {
        goto error_malloc_object;
    }

    memset(object, '\0', sizeof(*object));

    object->point  = point;
    object->source = source;
    object->ctx    = ctx;
    object->flags  = XAS_SPATIAL_OBJECT_NONE;
    object->speed  = XAS_SPATIAL_DEFAULT_OBJECT_SPEED;

    object_position(scene, object, point);

    if (scene->first == NULL) {
        scene->first = object;
    }

    if (scene->last) {
        scene->last->next = object;
    }

    scene->last = object;

    return object;

error_malloc_object:
error_invalid_source:
    return NULL;
}

xas_spatial_object *xas_spatial_scene_add_synth(xas_spatial_scene *scene,
                                                    xas_spatial_coord point,
                                                    enum xas_synth_type type) {
    xas_spatial_object *obj;
    xas_synth *synth;
    xas_audio_stream *stream;

    if ((synth = xas_synth_new(scene->format,
                                 scene->buffer->size,
                                 type)) == NULL) {
        goto error_synth_new;
    }

    if ((stream = xas_synth_stream_new(synth)) == NULL) {
        goto error_synth_stream_new;
    }

    if ((obj = xas_spatial_scene_add_object(scene,
                                              point,
                                              stream,
                                              synth)) == NULL) {
        goto error_scene_add_object;
    }

    obj->flags |= XAS_SPATIAL_OBJECT_MANAGED;

    return obj;

error_scene_add_object:
    xas_audio_stream_destroy(stream);

error_synth_stream_new:
    xas_synth_destroy(synth);

error_synth_new:
    return NULL;
}

xas_spatial_object *xas_spatial_scene_add_bank_player(xas_spatial_scene *scene,
                                                          xas_spatial_coord point,
                                                          xas_bank *bank) {
    xas_spatial_object *obj;
    xas_bank_player *player;
    xas_audio_stream *stream;

    if ((player = xas_bank_player_new(bank)) == NULL) {
        goto error_bank_player_new;
    }

    if ((stream = xas_bank_player_stream_new(player)) == NULL) {
        goto error_bank_player_stream_new;
    }

    if ((obj = xas_spatial_scene_add_object(scene,
                                              point,
                                              stream,
                                              player)) == NULL) {
        goto error_scene_add_object;
    }

    obj->flags |= XAS_SPATIAL_OBJECT_MANAGED;

    return obj;

error_scene_add_object:
    xas_audio_stream_destroy(stream);

error_bank_player_stream_new:
    xas_bank_player_destroy(player);

error_bank_player_new:
    return NULL;
}

xas_spatial_object *xas_spatial_scene_add_vox(xas_spatial_scene *scene,
                                                  xas_spatial_coord point,
                                                  const char *text2wave_path) {
    xas_spatial_object *obj;
    xas_vox *vox;
    xas_audio_stream *stream;

    if ((vox = xas_vox_new(scene->format,
                             scene->buffer->size,
                             text2wave_path)) == NULL) {
        goto error_vox_new;
    }

    if ((stream = xas_vox_stream_new(vox)) == NULL) {
        goto error_vox_stream_new;
    }

    if ((obj = xas_spatial_scene_add_object(scene,
                                              point,
                                              stream,
                                              vox)) == NULL) {
        goto error_scene_add_object;
    }

    obj->flags |= XAS_SPATIAL_OBJECT_MANAGED;

    return obj;

error_scene_add_object:
    xas_audio_stream_destroy(stream);

error_vox_stream_new:
    xas_vox_destroy(vox);

error_vox_new:
    return NULL;
}

void xas_spatial_scene_position_object(xas_spatial_scene *scene,
                                         xas_spatial_object *object,
                                         xas_spatial_coord point) {
    object->point = point;

    object_update_delays(scene);
}

void xas_spatial_object_get_point(xas_spatial_object *object,
                                    xas_spatial_coord *point) {
    point->x = object->point.x;
    point->y = object->point.y;
    point->z = object->point.z;
}

float xas_spatial_object_get_speed(xas_spatial_object *object) {
    return object->speed;
}

void xas_spatial_object_set_speed(xas_spatial_object *object, float speed) {
    object->speed = speed;
}

void xas_spatial_object_get_heading(xas_spatial_object *object,
                                      xas_spatial_coord *heading) {
    heading->x = object->heading.x;
    heading->y = object->heading.y;
    heading->z = object->heading.z;
}

void xas_spatial_object_set_heading(xas_spatial_object *object,
                                      xas_spatial_coord heading) {
    object->heading.x = heading.x;
    object->heading.y = heading.y;
    object->heading.z = heading.z;
}

int xas_spatial_object_start(xas_spatial_object *object) {
    return ((xas_object *)object->ctx)->start((xas_object *)object->ctx);
}

int xas_spatial_object_stop(xas_spatial_object *object) {
    return ((xas_object *)object->ctx)->stop((xas_object *)object->ctx);
}

xas_audio_stream *xas_spatial_scene_stream_new(xas_spatial_scene *scene,
                                                   size_t buffer_size) {
    return xas_audio_stream_new_source((xas_audio_fill)scene_fill,
                                         NULL,
                                         scene->format,
                                         buffer_size,
                                         scene);
}