#include "../include/math.h"

// Stolen from grumpycoder
// this is from here : https://github.com/grumpycoders/Balau/blob/master/tests/test-Handles.cc#L20-L102
// precalc costable
static int m_cosTable[512];                     
static const unsigned int DC_2PI = 2048;        
static const unsigned int DC_PI  = 1024;
static const unsigned int DC_PI2 = 512;
// f(n) = cos(n * 2pi / 2048) <- 2048 is == DC_2PI value
// f(n) = 2 * f(1) * f(n - 1) - f(n - 2)
void generateTable(void){
    m_cosTable[0] = 16777216;               // 2^24 * cos(0 * 2pi / 2048) => 2^24 * 1 = 2^24 : here, 2^24 defines the precision we want after the decimal point
    static const long long C = 16777137;    // 2^24 * cos(1 * 2pi / 2048) = C = f(1);
    m_cosTable[1] = C;
    for (int i = 2; i < 511; i++){
        m_cosTable[i] = ((C * m_cosTable[i - 1]) >> 23) - m_cosTable[i - 2];
    }
    for (int i = 0; i < 512; i++){
        m_cosTable[i] >>= 12;
    }
};
int ncos(unsigned int t) {
    t %= DC_2PI;
    int r;
    if (t < DC_PI2) {
        r = m_cosTable[t];
    } else if (t < DC_PI) {
        r = -m_cosTable[DC_PI - 1 - t];
    } else if (t < (DC_PI + DC_PI2)) {
        r = -m_cosTable[t - DC_PI];
    } else {
        r = m_cosTable[DC_2PI - 1 - t];
    };
    return r;
};
// sin(x) = cos(x - pi / 2)
int nsin(unsigned int t) {
    t %= DC_2PI;
    if (t < DC_PI2){
        return ncos(t + DC_2PI - DC_PI2);
    };
    return ncos(t - DC_PI2);
};
// https://github.com/Arsunt/TR2Main/blob/411cacb35914c616cb7960c0e677e00c71c7ee88/3dsystem/phd_math.cpp#L432
long long patan(long x, long y){
    long long result;
    int swapBuf;
    int flags = 0;
    // if either x or y are 0, return 0
    if( x == 0 && y == 0){
        return 0;
    }
    if( x < 0 ) {
        flags |= 4; 
        x = -x;
    }
    if ( y < 0 ) {
        flags |= 2;
        y = -y;
    }
    if ( y > x ) {
        flags |= 1;
        SWAP(x, y ,swapBuf);
    }
    result = AtanBaseTable[flags] + AtanAngleTable[0x800 * y / x];
    if ( result < 0 ){
        result = -result;
    return result;
    }
};
u_int psqrt(u_int n){
    u_int result = 0;
    u_int base = 0x40000000;
    u_int basedResult;
    for( ; base != 0; base >>= 2 ) {
        for( ; base != 0; base >>= 2 ) {
            basedResult = base + result;
            result >>= 1;
            if( basedResult > n ) {
                break;
            }
            n -= basedResult;
            result |= base;
        }
    }
    return result;
};
// From : https://github.com/grumpycoders/pcsx-redux/blob/7438e9995833db5bc1e14da735bbf9dc78300f0b/src/mips/shell/math.h
int32_t dMul(int32_t a, int32_t b) {
    long long r = a;
    r *= b;
    return r >> 24;
};
// standard lerp function
//  s = source, an arbitrary number up to 2^24
//  d = destination, an arbitrary number up to 2^24
//  p = position, a number between 0 and 256, inclusive
//  p = 0 means output = s
//  p = 256 means output = d
uint32_t lerpU(uint32_t start, uint32_t dest, unsigned pos) {
    return (start * (256 - pos) + dest * pos) >> 8; 
};
int32_t lerpS(int32_t start, int32_t dest, unsigned pos) {
    return (start * (256 - pos) + dest * pos) >> 8;
};
// start, dest and pos have to be << x, then the result has to be >> x where x defines precision: 
// precision = 2^24 - 2^x
// << x : 0 < pos < precision
// https://discord.com/channels/642647820683444236/646765703143227394/811318550978494505
// my angles are between 0 and 2048 (full circle), so 2^11 for the range of angles; with numbers on a 8.24 representation, 
// a 1.0 angle (or 2pi) means it's 2^24, so to "convert" my angles from 8.24 to my internal discrete cos, I only have to shift by 13
int32_t lerpD(int32_t start, int32_t dest, int32_t pos) {
     return dMul(start, 16777216 - pos) + dMul(dest, pos);
};
long long lerpL(long long start, long long dest, long long pos) {
    return dMul( (start << 12), 16777216 - (pos << 12) ) + dMul((dest << 12), (pos << 12) ) >> 12;
};
int lerp(int start, int end, int factor){
    // lerp interpolated cam movement
    // InBetween = Value 1 + ( ( Value2 - Value1 ) * lerpValue ) ;
    // lerpValue should be a int between 17 and 256.
    return ( ( start ) + ( ( end - start ) * factor ) ) >> 12;
};
long long easeIn(long long i){
    return ((i << 7) * (i << 7) * (i << 7) / 32 ) >> 19;
};
int easeOut(int i){
    return (4096 >> 7) - ((4096 - (i << 7)) * (4096 - (i << 7))) >> 12;
};
//~ int easeInOut(int i, int div){
    //~ return lerp(easeIn(i, div), easeOut(i) , i);
//~ };
SVECTOR SVlerp(SVECTOR start, SVECTOR end, int factor){
    SVECTOR output = {0,0,0,0};
    output.vx = lerp(start.vx, end.vx, factor);
    output.vy = lerp(start.vy, end.vy, factor);
    output.vz = lerp(start.vz, end.vz, factor);
    return output;
};
VECTOR getVectorTo( VECTOR actor, VECTOR target ) {
    // Returns a normalized vector that points from actor to target
    VECTOR direction = { subVector(target, actor) };
    VECTOR Ndirection = {0,0,0,0};
    u_int distSq = (direction.vx * direction.vx) + (direction.vz * direction.vz);
    direction.pad = psqrt(distSq);
    VectorNormal(&direction, &Ndirection);
    return Ndirection ;
};

int32_t round( int32_t n){
    // GRS - Action
    // 0xx - round down = do nothing (x means any bit value, 0 or 1)
    // 100 - this is a tie: round up if the mantissa's bit just before G is 1, else round down=do nothing
    // 101 - round up
    // 110 - round up
    // 111 - round up
    // source : https://stackoverflow.com/a/8984135
    // e.g : n == 106 150 == 0000 0000 0000 0001 1001 1110 1010 0110
    // Get GRS bits
    // 0xe00 ==                                  0000 1110 0000 0000
    int8_t grs = ( n & 0xe00) >> 8 ;       //         1110 0000 0000 >> 8
    // GRS == 111(0)
    // Get G value - 0x8 == 1000
    if (grs & 0x8){
        // GRS = 1xx
        if ( // Get R value - 0x4 == 0100
             // GRS == 11x ; round up
             grs & 0x4 ||
             // Get S value - 0x2 == 0010
             // GRS == 101 ; round up
             ( !(grs & 0x4) && grs & 0x2) 
        ) {
            n += 0x800;
        } else if ( !(n & 0x1000) ) {
            // Get mantissa lsb - 0x1000 == 0001 0000 0000
            // GRS == 100 ; tie, round up if mantissa lsb is 1
            n += 0x800;
        }
    }
    return n;
};