/*
 * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */
#ifndef _AFFINE_TRANSFORM_H_
#define _AFFINE_TRANSFORM_H_

// Stolen from RISCBoy

#include <stdint.h>
#include "pico/platform.h"

// Store unpacked affine transforms as signed 16.16 fixed point in the following order:
// a00, a01, b0,   a10, a11, b1
// i.e. the top two rows of the matrix
// [ a00 a01 b0 ]
// [ a01 a11 b1 ]
// [ 0   0   1  ]
// Then pack integers appropriately 

typedef int32_t affine_transform_t[6];
static const int32_t AF_ONE = 1 << 16;

static inline __attribute__((always_inline)) int32_t mul_fp1616(int32_t x, int32_t y) {
    // TODO this results in an aeabi call?!
    int64_t result = (int64_t) x * y;
    return result >> 16;
}

// result can not be == left or right
static inline void affine_mul(affine_transform_t result, const affine_transform_t left,
                              const affine_transform_t right) {
    result[0] = mul_fp1616(left[0], right[0]) + mul_fp1616(left[1], right[3]);
    result[1] = mul_fp1616(left[0], right[1]) + mul_fp1616(left[1], right[4]);
    result[2] = mul_fp1616(left[0], right[2]) + mul_fp1616(left[1], right[5]) + left[2];
    result[3] = mul_fp1616(left[3], right[0]) + mul_fp1616(left[4], right[3]);
    result[4] = mul_fp1616(left[3], right[1]) + mul_fp1616(left[4], right[4]);
    result[5] = mul_fp1616(left[3], right[2]) + mul_fp1616(left[4], right[5]) + left[5];
}

static inline void affine_copy(affine_transform_t dst, const affine_transform_t src) {
    for (int i = 0; i < 6; ++i)
        dst[i] = src[i];
}

// User is describing a sequence of transformations from texture space to
// screen space, which are applied by premultiplying a column vector. However,
// hardware transforms *from* screenspace *to* texture space, so we want the
// inverse of the transform the user is building. Therefore our functions each
// produce the inverse of the requested transform, and we apply transforms by
// *post*-multiplication.
static inline void affine_identity(affine_transform_t current_trans) {
    int32_t tmp[6] = {
            AF_ONE, 0, 0,
            0, AF_ONE, 0
    };
    affine_copy(current_trans, tmp);
}

static inline void affine_translate(affine_transform_t current_trans, int32_t x, int32_t y) {
    int32_t tmp[6];
    int32_t transform[6] = {
            AF_ONE, 0, -AF_ONE * x,
            0, AF_ONE, -AF_ONE * y
    };
    affine_mul(tmp, current_trans, transform);
    affine_copy(current_trans, tmp);
}

// TODO this is shit
static const int32_t __not_in_flash("atrans") sin_lookup_fp1616[256] = {
        0x0, 0x648, 0xc8f, 0x12d5, 0x1917, 0x1f56, 0x2590, 0x2bc4, 0x31f1, 0x3817,
        0x3e33, 0x4447, 0x4a50, 0x504d, 0x563e, 0x5c22, 0x61f7, 0x67bd, 0x6d74,
        0x7319, 0x78ad, 0x7e2e, 0x839c, 0x88f5, 0x8e39, 0x9368, 0x987f, 0x9d7f,
        0xa267, 0xa736, 0xabeb, 0xb085, 0xb504, 0xb968, 0xbdae, 0xc1d8, 0xc5e4,
        0xc9d1, 0xcd9f, 0xd14d, 0xd4db, 0xd848, 0xdb94, 0xdebe, 0xe1c5, 0xe4aa,
        0xe76b, 0xea09, 0xec83, 0xeed8, 0xf109, 0xf314, 0xf4fa, 0xf6ba, 0xf853,
        0xf9c7, 0xfb14, 0xfc3b, 0xfd3a, 0xfe13, 0xfec4, 0xff4e, 0xffb1, 0xffec,
        0x10000, 0xffec, 0xffb1, 0xff4e, 0xfec4, 0xfe13, 0xfd3a, 0xfc3b, 0xfb14,
        0xf9c7, 0xf853, 0xf6ba, 0xf4fa, 0xf314, 0xf109, 0xeed8, 0xec83, 0xea09,
        0xe76b, 0xe4aa, 0xe1c5, 0xdebe, 0xdb94, 0xd848, 0xd4db, 0xd14d, 0xcd9f,
        0xc9d1, 0xc5e4, 0xc1d8, 0xbdae, 0xb968, 0xb504, 0xb085, 0xabeb, 0xa736,
        0xa267, 0x9d7f, 0x987f, 0x9368, 0x8e39, 0x88f5, 0x839c, 0x7e2e, 0x78ad,
        0x7319, 0x6d74, 0x67bd, 0x61f7, 0x5c22, 0x563e, 0x504d, 0x4a50, 0x4447,
        0x3e33, 0x3817, 0x31f1, 0x2bc4, 0x2590, 0x1f56, 0x1917, 0x12d5, 0xc8f, 0x648,
        0x0, 0xfffff9b8, 0xfffff371, 0xffffed2b, 0xffffe6e9, 0xffffe0aa, 0xffffda70,
        0xffffd43c, 0xffffce0f, 0xffffc7e9, 0xffffc1cd, 0xffffbbb9, 0xffffb5b0,
        0xffffafb3, 0xffffa9c2, 0xffffa3de, 0xffff9e09, 0xffff9843, 0xffff928c,
        0xffff8ce7, 0xffff8753, 0xffff81d2, 0xffff7c64, 0xffff770b, 0xffff71c7,
        0xffff6c98, 0xffff6781, 0xffff6281, 0xffff5d99, 0xffff58ca, 0xffff5415,
        0xffff4f7b, 0xffff4afc, 0xffff4698, 0xffff4252, 0xffff3e28, 0xffff3a1c,
        0xffff362f, 0xffff3261, 0xffff2eb3, 0xffff2b25, 0xffff27b8, 0xffff246c,
        0xffff2142, 0xffff1e3b, 0xffff1b56, 0xffff1895, 0xffff15f7, 0xffff137d,
        0xffff1128, 0xffff0ef7, 0xffff0cec, 0xffff0b06, 0xffff0946, 0xffff07ad,
        0xffff0639, 0xffff04ec, 0xffff03c5, 0xffff02c6, 0xffff01ed, 0xffff013c,
        0xffff00b2, 0xffff004f, 0xffff0014, 0xffff0000, 0xffff0014, 0xffff004f,
        0xffff00b2, 0xffff013c, 0xffff01ed, 0xffff02c6, 0xffff03c5, 0xffff04ec,
        0xffff0639, 0xffff07ad, 0xffff0946, 0xffff0b06, 0xffff0cec, 0xffff0ef7,
        0xffff1128, 0xffff137d, 0xffff15f7, 0xffff1895, 0xffff1b56, 0xffff1e3b,
        0xffff2142, 0xffff246c, 0xffff27b8, 0xffff2b25, 0xffff2eb3, 0xffff3261,
        0xffff362f, 0xffff3a1c, 0xffff3e28, 0xffff4252, 0xffff4698, 0xffff4afc,
        0xffff4f7b, 0xffff5415, 0xffff58ca, 0xffff5d99, 0xffff6281, 0xffff6781,
        0xffff6c98, 0xffff71c7, 0xffff770b, 0xffff7c64, 0xffff81d2, 0xffff8753,
        0xffff8ce7, 0xffff928c, 0xffff9843, 0xffff9e09, 0xffffa3de, 0xffffa9c2,
        0xffffafb3, 0xffffb5b0, 0xffffbbb9, 0xffffc1cd, 0xffffc7e9, 0xffffce0f,
        0xffffd43c, 0xffffda70, 0xffffe0aa, 0xffffe6e9, 0xffffed2b, 0xfffff371,
        0xfffff9b8
};

static inline int32_t sin_fp1616(uint8_t theta) {
    return sin_lookup_fp1616[theta];
}

static inline int32_t cos_fp1616(uint8_t theta) {
    return sin_lookup_fp1616[(theta + 64) & 0xff];
}

// Appears as a counterclockwise rotation (when viewed from texture space to screen space)
// Units of angle are 256 = one turn
static inline void affine_rotate(affine_transform_t current_trans, uint8_t theta) {
    int32_t tmp[6];
    int32_t transform[6] = {
            cos_fp1616(theta), -sin_fp1616(theta), 0,
            sin_fp1616(theta), cos_fp1616(theta), 0
    };
    affine_mul(tmp, current_trans, transform);
    affine_copy(current_trans, tmp);
}

static inline void affine_scale(affine_transform_t current_trans, int32_t sx, int32_t sy) {
    int32_t sx_inv = ((int64_t) AF_ONE * AF_ONE) / sx;
    int32_t sy_inv = ((int64_t) AF_ONE * AF_ONE) / sy;
    int32_t tmp[6];
    int32_t transform[6] = {
            sx_inv, 0, 0,
            0, sy_inv, 0
    };
    affine_mul(tmp, current_trans, transform);
    affine_copy(current_trans, tmp);
}

#endif // _AFFINE_TRANSFORM_H_