#include "ksk_math.h"
#include <math.h>

// Normalize to [0, 2π)
double normalize_angle_0_2pi(double theta) {
    double result = fmod(theta, 2.0 * M_PI);
    if (result < 0) {
        result += 2.0 * M_PI;
    }
    return result;
}

// Normalize to [-π, π)
double normalize_angle_pmpi(double theta) {
    double result = fmod(theta + M_PI, 2.0 * M_PI);
    if (result < 0) {
        result += 2.0 * M_PI;
    }
    return result - M_PI;
}

// Sawtooth from 0 to 1 (rising)
double sawtooth(double time, double fsw) {
    double period = 1.0 / fsw;
    double value = fmod(time, period) / period;
    return value;  // Returns value in range [0, 1)
}

// Symmetric triangle wave (0->1->0) at frequency fsw
double triangle_wave(double time, double fsw) {
    double period = 1.0 / fsw;
    double t_mod = fmod(time, period);
    double half_period = period / 2.0;
    
    // Using ternary operator
    return (t_mod <= half_period) ? 
           (t_mod / half_period) :           // Rising: 0 to 1
           (2.0 - t_mod / half_period);      // Falling: 1 to 0
}

// Basic comparator: outputs 1 if inp > inn, else 0
int comparator(double inp, double inn) {
    return (inp > inn) ? 1 : 0;
}