/*
 * pq_fixed_trig.h
 *
 * Fixed-point trigonometric functions.
 *
 * (c) 2025 Sofian Audry        :: info(@)sofianaudry(.)com
 * 
 * sin32() and cos32() functions adapted from Teensy Audio library
 * (c) 2014 Paul Stoffregen     :: paul(@)pjrc.com
 * 
 * sin16() and cos16() functions adapted from FastLED library
 * https://fastled.io/
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef PQ_FIXED_TRIG_H_
#define PQ_FIXED_TRIG_H_

#include <stdint.h>
#include "pq_fixed_math.h"

namespace pq {

/** 
 * Fixed-point sin function with 32 bits precision.
 * Author: Paul Stoffregen (from Teensy Audio library)
 * Range: [ -2147483532, +2147483532 ]
 */
static inline int32_t sin32(uint32_t ph) __attribute__((unused));
static inline int32_t sin32(uint32_t ph)
{
	int32_t angle, sum, p1, p2, p3, p5, p7, p9, p11;

	if (ph >= 0xC0000000 || ph < 0x40000000) {                            // ph:  0.32
		angle = (int32_t)ph; // valid from -90 to +90 degrees
	} else {
		angle = (int32_t)(0x80000000u - ph);                        // angle: 2.30
	}
	p1 =  multiply_32x32_rshift32_rounded(angle, 1686629713) << 2;        // p1:  2.30
	p2 =  multiply_32x32_rshift32_rounded(p1, p1) << 1;                   // p2:  3.29
	p3 =  multiply_32x32_rshift32_rounded(p2, p1) << 2;                   // p3:  3.29
	sum = multiply_subtract_32x32_rshift32_rounded(p1, p3, 1431655765);   // sum: 2.30
	p5 =  multiply_32x32_rshift32_rounded(p3, p2);                        // p5:  6.26
	sum = multiply_accumulate_32x32_rshift32_rounded(sum, p5, 572662306);
	p7 =  multiply_32x32_rshift32_rounded(p5, p2);                        // p7:  9.23
	sum = multiply_subtract_32x32_rshift32_rounded(sum, p7, 109078534);
	p9 =  multiply_32x32_rshift32_rounded(p7, p2);                        // p9: 12.20
	sum = multiply_accumulate_32x32_rshift32_rounded(sum, p9, 12119837);
	p11 = multiply_32x32_rshift32_rounded(p9, p2);                       // p11: 15.17
	sum = multiply_subtract_32x32_rshift32_rounded(sum, p11, 881443);
	return sum <<= 1;                                                 // return:  1.31
}

/// Fixed-point cos function with 32 bits precision.
static inline int32_t cos32(uint32_t ph) __attribute__((always_inline, unused));
static inline int32_t cos32(uint32_t ph) {
    return sin32(ph + 0x40000000);
}

// Fast trigonometric functions for AVR.
// Source: http://fastled.io/docs/3.1/trig8_8h_source.html

/// Fast 8 and 16-bit approximations of sin(x) and cos(x).
///        Don't use these approximations for calculating the
///        trajectory of a rocket to Mars, but they're great
///        for art projects and LED displays.
///
///        On Arduino/AVR, the 16-bit approximation is more than
///        10X faster than floating point sin(x) and cos(x), while
///        the 8-bit approximation is more than 20X faster.

static inline int16_t sin16( uint16_t theta ) __attribute__ ((unused));

#if defined(__AVR__)

/// Fast 16-bit approximation of sin(x). This approximation never varies more than
/// 0.69% from the floating point value you'd get by doing
///
///     float s = sin(x) * 32767.0;
///
/// @param theta input angle from 0-65535
/// @returns sin of theta, value between -32767 to 32767.
static inline int16_t sin16(uint16_t theta)
{
    static const uint8_t data[] =
    { 0,         0,         49, 0, 6393%256,   6393/256, 48, 0,
      12539%256, 12539/256, 44, 0, 18204%256, 18204/256, 38, 0,
      23170%256, 23170/256, 31, 0, 27245%256, 27245/256, 23, 0,
      30273%256, 30273/256, 14, 0, 32137%256, 32137/256,  4 /*,0*/ };

    uint16_t offset = (theta & 0x3FFF);

    // AVR doesn't have a multi-bit shift instruction,
    // so if we say "offset >>= 3", gcc makes a tiny loop.
    // Inserting empty volatile statements between each
    // bit shift forces gcc to unroll the loop.
    offset >>= 1; // 0..8191
    asm volatile("");
    offset >>= 1; // 0..4095
    asm volatile("");
    offset >>= 1; // 0..2047

    if( theta & 0x4000 ) offset = 2047 - offset;

    uint8_t sectionX4;
    sectionX4 = offset / 256;
    sectionX4 *= 4;

    uint8_t m;

    union {
        uint16_t b;
        struct {
            uint8_t blo;
            uint8_t bhi;
        };
    } u;

    //in effect u.b = blo + (256 * bhi);
    u.blo = data[ sectionX4 ];
    u.bhi = data[ sectionX4 + 1];
    m     = data[ sectionX4 + 2];

    uint8_t secoffset8 = (uint8_t)(offset) / 2;
    
    uint16_t mx = m * secoffset8;

    int16_t  y  = mx + u.b;
    if( theta & 0x8000 ) y = -y;

    return y;
}

#else
/// Fast 16-bit approximation of sin(x). This approximation never varies more than
/// 0.69% from the floating point value you'd get by doing
///
///     float s = sin(x) * 32767.0;
///
/// @param theta input angle from 0-65535
/// @returns sin of theta, value between -32767 to 32767.
static inline int16_t sin16(uint16_t theta)
{
    static const uint16_t base[] =
    { 0, 6393, 12539, 18204, 23170, 27245, 30273, 32137 };
    static const uint8_t slope[] =
    { 49, 48, 44, 38, 31, 23, 14, 4 };

    uint16_t offset = (theta & 0x3FFF) >> 3; // 0..2047
    if( theta & 0x4000 ) offset = 2047 - offset;

    uint8_t section = offset / 256; // 0..7
    uint16_t b   = base[section];
    uint8_t  m   = slope[section];

    uint8_t secoffset8 = (uint8_t)(offset) / 2;

    uint16_t mx = m * secoffset8;
    int16_t  y  = mx + b;

    if( theta & 0x8000 ) y = -y;

    return y;
}

#endif

/// Fast 16-bit approximation of cos(x). This approximation never varies more than
/// 0.69% from the floating point value you'd get by doing
///
///     float s = cos(x) * 32767.0;
///
/// @param theta input angle from 0-65535
/// @returns sin of theta, value between -32767 to 32767.
static inline int16_t cos16(uint16_t theta) __attribute__ ((unused));
static inline int16_t cos16(uint16_t theta)
{
    return sin16( theta + 16384);
}

// ///////////////////////////////////////////////////////////////////////

// // sin8 & cos8
// //        Fast 8-bit approximations of sin(x) & cos(x).
// //        Input angle is an unsigned int from 0-255.
// //        Output is an unsigned int from 0 to 255.
// //
// //        This approximation can vary to to 2%
// //        from the floating point value you'd get by doing
// //          float s = (sin( x ) * 128.0) + 128;
// //
// //        Don't use this approximation for calculating the
// //        "real" trigonometric calculations, but it's great
// //        for art projects and LED displays.
// //
// //        On Arduino/AVR, this approximation is more than
// //        20X faster than floating point sin(x) and cos(x)

// const uint8_t b_m16_interleave[] = { 0, 49, 49, 41, 90, 27, 117, 10 };

// #if defined(__AVR__) && !defined(LIB8_ATTINY)
// #define sin8 sin8_avr


// /// Fast 8-bit approximation of sin(x). This approximation never varies more than
// /// 2% from the floating point value you'd get by doing
// ///
// ///     float s = (sin(x) * 128.0) + 128;
// ///
// /// @param theta input angle from 0-255
// /// @returns sin of theta, value between 0 and 255
// __attribute__ ((unused)) static inline uint8_t  sin8_avr( uint8_t theta)
// {
//     uint8_t offset = theta;

//     asm volatile(
//                  "sbrc %[theta],6         \n\t"
//                  "com  %[offset]           \n\t"
//                  : [theta] "+r" (theta), [offset] "+r" (offset)
//                  );

//     offset &= 0x3F; // 0..63

//     uint8_t secoffset  = offset & 0x0F; // 0..15
//     if( theta & 0x40) secoffset++;

//     uint8_t m16; uint8_t b;

//     uint8_t section = offset >> 4; // 0..3
//     uint8_t s2 = section * 2;

//     const uint8_t* p = b_m16_interleave;
//     p += s2;
//     b   = *p;
//     p++;
//     m16 = *p;

//     uint8_t mx;
//     uint8_t xr1;
//     asm volatile(
//                  "mul %[m16],%[secoffset]   \n\t"
//                  "mov %[mx],r0              \n\t"
//                  "mov %[xr1],r1             \n\t"
//                  "eor  r1, r1               \n\t"
//                  "swap %[mx]                \n\t"
//                  "andi %[mx],0x0F           \n\t"
//                  "swap %[xr1]               \n\t"
//                  "andi %[xr1], 0xF0         \n\t"
//                  "or   %[mx], %[xr1]        \n\t"
//                  : [mx] "=d" (mx), [xr1] "=d" (xr1)
//                  : [m16] "d" (m16), [secoffset] "d" (secoffset)
//                  );

//     int8_t y = mx + b;
//     if( theta & 0x80 ) y = -y;

//     y += 128;

//     return y;
// }

// #else
// #define sin8 sin8_C


// /// Fast 8-bit approximation of sin(x). This approximation never varies more than
// /// 2% from the floating point value you'd get by doing
// ///
// ///     float s = (sin(x) * 128.0) + 128;
// ///
// /// @param theta input angle from 0-255
// /// @returns sin of theta, value between 0 and 255
// __attribute__ ((unused)) static inline uint8_t sin8_C( uint8_t theta)
// {
//     uint8_t offset = theta;
//     if( theta & 0x40 ) {
//         offset = (uint8_t)255 - offset;
//     }
//     offset &= 0x3F; // 0..63

//     uint8_t secoffset  = offset & 0x0F; // 0..15
//     if( theta & 0x40) secoffset++;

//     uint8_t section = offset >> 4; // 0..3
//     uint8_t s2 = section * 2;
//     const uint8_t* p = b_m16_interleave;
//     p += s2;
//     uint8_t b   =  *p;
//     p++;
//     uint8_t m16 =  *p;

//     uint8_t mx = (m16 * secoffset) >> 4;

//     int8_t y = mx + b;
//     if( theta & 0x80 ) y = -y;

//     y += 128;

//     return y;
// }

// #endif

// /// Fast 8-bit approximation of cos(x). This approximation never varies more than
// /// 2% from the floating point value you'd get by doing
// ///
// ///     float s = (cos(x) * 128.0) + 128;
// ///
// /// @param theta input angle from 0-255
// /// @returns sin of theta, value between 0 and 255
// __attribute__ ((unused)) static inline uint8_t cos8( uint8_t theta)
// {
//     return sin8( theta + 64);
// }

// ///@}

} // namespace pq

#endif
