#![no_std]
#![no_main]

mod fmt;

use core::mem;
use core::ops::{AddAssign, SubAssign};
use defmt::{error, Format};
#[cfg(not(feature = "defmt"))]
use panic_halt as _;
#[cfg(feature = "defmt")]
use {defmt_rtt as _, panic_probe as _};
#[cfg(feature = "defmt")]
use defmt::info;

use embassy_executor::Spawner;
use embassy_futures::join::{join3, join4};
use embassy_nrf::gpio::{AnyPin, Input, OutputDrive, Pull};
use embassy_nrf::interrupt::{InterruptExt, Priority};
use embassy_nrf::pwm::{DutyCycle, Prescaler, SimplePwm};
use embassy_nrf::{bind_interrupts, interrupt, saadc, Peri};
use embassy_nrf::peripherals::SAADC;
use embassy_nrf::saadc::{ChannelConfig, Gain, Reference, Saadc};
use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex;
use embassy_sync::mutex::Mutex;
use embassy_sync::signal::Signal;
use embassy_time::{with_timeout, Duration, Timer};
use futures::future::{select, Either};
use futures::pin_mut;
use nrf_softdevice::{raw, Softdevice};
use nrf_softdevice::ble::advertisement_builder::{Flag, LegacyAdvertisementBuilder, LegacyAdvertisementPayload, ServiceList, ServiceUuid16};
use nrf_softdevice::ble::{peripheral, gatt_server, Connection};

#[derive(Copy, Clone, Eq, PartialEq, Format)]
enum SysAction {
    PresentenceOn,
    PresentenceOff,
    ConstantOn,
}

#[derive(Copy, Clone, Eq, PartialEq, Format)]
enum TextLightType {
    White,
    ConstantColor,
    Hue
}

#[derive(Copy, Clone, Eq, PartialEq, Format)]
enum HeartLightType {
    ConstantColor,
    Hue,
    HeartBeat
}

#[derive(Copy, Clone, Eq, PartialEq, Format)]
pub enum TouchAction {
    Tap,
    DoubleTap,
    TripleTap,
    Hold,
}

#[derive(Copy, Clone, Eq, PartialEq, Format)]
pub enum AnimationSignal<T> {
    NextAnimation,
    SetMode(T),
    SetColor(u16),
}

static TOUCH_SIGNAL: Signal<ThreadModeRawMutex, TouchAction> = Signal::new();
static RADAR_SIGNAL: Signal<ThreadModeRawMutex, u8> = Signal::new();
static TEXT_ANIMATION_SIGNAL: Signal<ThreadModeRawMutex, AnimationSignal<TextLightType>> = Signal::new();
static HEART_ANIMATION_SIGNAL: Signal<ThreadModeRawMutex, AnimationSignal<HeartLightType>> = Signal::new();
static SYS_ACTION: Mutex<ThreadModeRawMutex, SysAction> = Mutex::new(SysAction::PresentenceOn);
static TEXT_ANIMATION: Mutex<ThreadModeRawMutex, TextLightType> = Mutex::new(TextLightType::Hue);
static HEART_ANIMATION: Mutex<ThreadModeRawMutex, HeartLightType> = Mutex::new(HeartLightType::HeartBeat);

#[nrf_softdevice::gatt_service(uuid = "9e7312e0-2354-11eb-9f10-fbc30a62cf38")]
struct AnimationService {
    #[characteristic(uuid = "9e7312e0-2354-11eb-9f10-fbc30a63cf38", read, write, notify, indicate)]
    text_mode: u16,
    #[characteristic(uuid = "9e7312e0-2354-11eb-9f10-fbc30a64cf38", read, write, notify, indicate)]
    text_color: u16,
    #[characteristic(uuid = "9e7312e0-2354-11eb-9f10-fbc30a65cf38", read, write, notify, indicate)]
    heart_mode: u16,
    #[characteristic(uuid = "9e7312e0-2354-11eb-9f10-fbc30a66cf38", read, write, notify, indicate)]
    heart_color: u16,

}

#[nrf_softdevice::gatt_service(uuid = "180f")]
struct BatteryService {
    #[characteristic(uuid = "2a19", read, notify)]
    battery_level: u8,
}

#[nrf_softdevice::gatt_server]
struct Server {
    bas: BatteryService,
    light: AnimationService,
}

bind_interrupts!(struct AdcIrqs {
    SAADC => saadc::InterruptHandler;
});

#[embassy_executor::main]
async fn main(spawner: Spawner) {
    let mut config = embassy_nrf::config::Config::default();
    config.gpiote_interrupt_priority = Priority::P2;
    config.time_interrupt_priority = Priority::P2;
    let mut p = embassy_nrf::init(config);

    let config = nrf_softdevice::Config {
        clock: Some(raw::nrf_clock_lf_cfg_t {
            source: raw::NRF_CLOCK_LF_SRC_RC as u8,
            rc_ctiv: 16,
            rc_temp_ctiv: 2,
            accuracy: raw::NRF_CLOCK_LF_ACCURACY_500_PPM as u8,
        }),
        conn_gap: Some(raw::ble_gap_conn_cfg_t {
            conn_count: 6,
            event_length: 24,
        }),
        conn_gatt: Some(raw::ble_gatt_conn_cfg_t { att_mtu: 256 }),
        gatts_attr_tab_size: Some(raw::ble_gatts_cfg_attr_tab_size_t {
            attr_tab_size: raw::BLE_GATTS_ATTR_TAB_SIZE_DEFAULT,
        }),
        gap_role_count: Some(raw::ble_gap_cfg_role_count_t {
            adv_set_count: 1,
            periph_role_count: 3,
            central_role_count: 3,
            central_sec_count: 0,
            _bitfield_1: raw::ble_gap_cfg_role_count_t::new_bitfield_1(0),
        }),
        gap_device_name: Some(raw::ble_gap_cfg_device_name_t {
            p_value: b"Odesa!" as *const u8 as _,
            current_len: 9,
            max_len: 9,
            write_perm: unsafe { mem::zeroed() },
            _bitfield_1: raw::ble_gap_cfg_device_name_t::new_bitfield_1(raw::BLE_GATTS_VLOC_STACK as u8),
        }),
        ..Default::default()
    };

    let sd = Softdevice::enable(&config);
    let server = Server::new(sd).expect("failed to enable softdevice");

    let mut channel_config = ChannelConfig::single_ended(p.P0_31.reborrow());
    interrupt::SAADC.set_priority(Priority::P3);
    channel_config.gain = Gain::GAIN1_6;
    channel_config.reference = Reference::INTERNAL;
    let saadc = Saadc::new(p.SAADC, AdcIrqs, saadc::Config::default(), [channel_config]);
    saadc.calibrate().await;

    let pwm_text = SimplePwm::new_3ch(p.PWM0, p.P1_00, p.P1_04, p.P1_06, &Default::default());
    let pwm_heart = SimplePwm::new_3ch(p.PWM2, p.P0_17, p.P0_20, p.P0_22, &Default::default());
    spawner.spawn(radar_task()).expect("failed to spawn radar task");
    spawner.spawn(touch_button(p.P0_08.into())).expect("failed to spawn touch task");
    spawner.spawn(actions_task()).expect("failed to spawn actions task");
    spawner.spawn(softdevice_task(sd)).expect("failed to spawn softdevice task");
    // spawner.spawn(battery_task(saadc)).expect("failed to spawn softdevice task");

    join4(gatt_task(server, saadc, sd), moving_radar(p.P0_06.into()), heartbeat_task(pwm_heart), light_task(pwm_text)).await;
}


async fn gatt_task(server: Server, mut saadc: Saadc<'static, 1>, sd: &'static Softdevice) {
    static ADV_DATA: LegacyAdvertisementPayload = LegacyAdvertisementBuilder::new()
        .flags(&[Flag::GeneralDiscovery, Flag::LE_Only])
        .services_16(ServiceList::Complete, &[ServiceUuid16::BATTERY])
        .full_name("Odesa!")
        .build();

    static SCAN_DATA: LegacyAdvertisementPayload = LegacyAdvertisementBuilder::new()
        .services_128(
            ServiceList::Complete,
            &[0x9e7312e0_2354_11eb_9f10_fbc30a62cf38_u128.to_le_bytes()],
        )
        .build();

    loop {
        let config = peripheral::Config::default();
        let adv = peripheral::ConnectableAdvertisement::ScannableUndirected {
            adv_data: &ADV_DATA,
            scan_data: &SCAN_DATA,
        };
        let conn = peripheral::advertise_connectable(sd, adv, &config).await.expect("failed to advertise");

        info!("advertising done!");


        let battery_fut = battery_task(&mut saadc, &server, &conn);
        let gatt_fut = gatt_server::run(&conn, &server, |e| match e {
            ServerEvent::Bas(e) => match e {
                BatteryServiceEvent::BatteryLevelCccdWrite { notifications } => {
                    info!("battery notifications: {}", notifications)
                }
            },
            ServerEvent::Light(e) => match e {
                AnimationServiceEvent::TextModeWrite(val) => {
                    let val = val % 3;
                    info!("wrote text mode: {}", val);
                    if let Err(e) = server.light.text_mode_notify(&conn, &(val)) {
                        info!("send notification error: {:?}", e);
                    }
                    let next = match val {
                        0 => TextLightType::Hue,
                        1 => TextLightType::ConstantColor,
                        2 => TextLightType::White,
                        _ => TextLightType::Hue,
                    };
                    TEXT_ANIMATION_SIGNAL.signal(AnimationSignal::SetMode(next));
                }
                AnimationServiceEvent::TextColorWrite(val) => {
                    let val = val % 1536;
                    info!("wrote text color: {}", val);
                    if let Err(e) = server.light.text_color_notify(&conn, &(val)) {
                        info!("send notification error: {:?}", e);
                    }
                    TEXT_ANIMATION_SIGNAL.signal(AnimationSignal::SetColor(val));
                }
                AnimationServiceEvent::HeartModeWrite(val) => {
                    let val = val % 3;
                    info!("wrote heart mode: {}", val);
                    if let Err(e) = server.light.heart_mode_notify(&conn, &(val)) {
                        info!("send notification error: {:?}", e);
                    }
                    let next = match val {
                        0 => HeartLightType::HeartBeat,
                        1 => HeartLightType::Hue,
                        2 => HeartLightType::ConstantColor,
                        _ => HeartLightType::HeartBeat,
                    };
                    HEART_ANIMATION_SIGNAL.signal(AnimationSignal::SetMode(next));
                }
                AnimationServiceEvent::HeartColorWrite(val) => {
                    let val = val % 1536;
                    info!("wrote heart color: {}", val);
                    if let Err(e) = server.light.heart_color_notify(&conn, &(val)) {
                        info!("send notification error: {:?}", e);
                    }
                    HEART_ANIMATION_SIGNAL.signal(AnimationSignal::SetColor(val));
                }
                AnimationServiceEvent::TextModeCccdWrite {
                    indications,
                    notifications,
                } => {
                    info!("text mode indications: {}, notifications: {}", indications, notifications)
                }
                AnimationServiceEvent::TextColorCccdWrite {
                    indications,
                    notifications,
                } => {
                    info!("text color indications: {}, notifications: {}", indications, notifications)
                }
                AnimationServiceEvent::HeartModeCccdWrite {
                    indications,
                    notifications,
                } => {
                    info!("text mode indications: {}, notifications: {}", indications, notifications)
                }
                AnimationServiceEvent::HeartColorCccdWrite {
                    indications,
                    notifications,
                } => {
                    info!("text color indications: {}, notifications: {}", indications, notifications)
                }
            },
        });

        pin_mut!(battery_fut);
        pin_mut!(gatt_fut);

        let _ = match select(battery_fut, gatt_fut).await {
            Either::Left((_, _)) => {
                info!("Battery encountered an error and stopped!")
            }
            Either::Right((e, _)) => {
                info!("GATT run exited with error: {:?}", e);
            }
        };
    }
}

#[embassy_executor::task]
async fn softdevice_task(sd: &'static Softdevice) -> ! {
    sd.run().await
}

#[embassy_executor::task]
async fn touch_button(pin: Peri<'static, AnyPin>) {
    let mut button = Input::new(pin, Pull::Down);

    loop {
        info!("Wait for button");
        button.wait_for_high().await;
        let mut tap_count = 0;
        let mut is_hold = false;

        info!("Wait for low 1");
        match with_timeout(Duration::from_millis(400), button.wait_for_low()).await {
            Err(_) => {
                info!("Look holding");
                is_hold = true;
                TOUCH_SIGNAL.signal(TouchAction::Hold);
                button.wait_for_low().await;
            }
            Ok(_) => {
                tap_count = 1;

                info!("Wait for next tap ({})", tap_count);
                loop {
                    match with_timeout(Duration::from_millis(300), button.wait_for_high()).await {
                        Ok(_) => {
                            info!("Wait for tap release ({})", tap_count);
                            button.wait_for_low().await;
                            tap_count += 1;
                            if tap_count >= 3 { break; }
                        }
                        Err(_) => {
                            break;
                        }
                    }
                }
            }
        }

        // 4. Dispatch the action if it wasn't a hold
        if !is_hold {
            match tap_count {
                1 => TOUCH_SIGNAL.signal(TouchAction::Tap),
                2 => TOUCH_SIGNAL.signal(TouchAction::DoubleTap),
                3 => TOUCH_SIGNAL.signal(TouchAction::TripleTap),
                _ => {}
            }
        }

        // Debounce / Cool-down
        Timer::after_millis(100).await;
    }
}

async fn battery_task<'a>(saadc: &'a mut Saadc<'_, 1>, server: &'a Server, connection: &'a Connection) {
    const REFERENCE: f32 = 3.6;
    let mut measurements = [0i16; 4];
    let mut index = 0;

    loop {
        let mut buf = [0; 1];
        saadc.sample(&mut buf).await;
        let voltage = ((buf[0] as f32 * REFERENCE) / 2.03f32) as i16;
        measurements[index] = voltage;
        index = (index + 1) % measurements.len();
        if measurements.iter().any(|v| *v == 0) {
            Timer::after_millis(1000).await;
        } else {
            let total = (measurements.iter().sum::<i16>() / measurements.len() as i16) as u16;
            let percent = battery_mv_to_percent(total);
            info!("Battery: {=i16}, {=u16}, {}%", voltage, total, percent);
            if let Err(e) = server.bas.battery_level_notify(connection, &percent) {
                error!("Error notifying battery level: {:?}", e);
            }
            Timer::after_millis(60000).await;
        }
    }
}

fn battery_mv_to_percent(mv: u16) -> u8 {
    match mv {
        mv if mv >= 4150 => 100,
        mv if mv <= 3000 => 0,
        _ => {
            let percent = (mv as u32 - 3000) * 100 / (4150 - 3000);
            percent as u8
        }
    }
}

#[embassy_executor::task]
async fn actions_task() {
    loop {
        let signal = TOUCH_SIGNAL.wait().await;
        info!("Action: {:?}", signal);
        match signal {
            // constant on vs presentence
            TouchAction::Tap => {
                let mut action = SYS_ACTION.lock().await;
                match *action {
                    SysAction::PresentenceOn |
                    SysAction::PresentenceOff => {
                        *action = SysAction::ConstantOn;
                        info!("Constant light");
                    }
                    SysAction::ConstantOn => {
                        *action = SysAction::PresentenceOn;
                        info!("Light by presentence");
                    }
                }
                info!("Light mode {:?}", *action);
            }
            // change text animation
            TouchAction::DoubleTap => {
                TEXT_ANIMATION_SIGNAL.signal(AnimationSignal::NextAnimation);
            }
            // change anchor animation
            TouchAction::TripleTap => {
                HEART_ANIMATION_SIGNAL.signal(AnimationSignal::NextAnimation);
            }
            // enter pairing mode
            TouchAction::Hold => {
            }
        }
    }
}

#[embassy_executor::task]
async fn radar_task() {
    const MAX_TIME: usize = 480;
    let mut time = MAX_TIME;
    loop {
        let action = {
            let current = SYS_ACTION.lock().await;
            *current
        };
        if action != SysAction::ConstantOn {
            if let Err(_) = with_timeout(Duration::from_secs(1), RADAR_SIGNAL.wait()).await {
                time = time.saturating_sub(1);
                if time == 0 {
                    let mut action = SYS_ACTION.lock().await;
                    if *action == SysAction::PresentenceOn {
                        *action = SysAction::PresentenceOff;
                        info!("Off light due to no presentence");
                    }
                }
            } else {
                if time == 0 {
                    let mut action = SYS_ACTION.lock().await;
                    if *action == SysAction::PresentenceOff {
                        *action = SysAction::PresentenceOn;
                        info!("On light due to presentence");
                    }
                }
                time = MAX_TIME;
            }
        } else {
            time = MAX_TIME;
            Timer::after_millis(500).await;
        }
    }
}

async fn moving_radar(pin: Peri<'static, AnyPin>) {
    let mut radar = Input::new(pin, Pull::Down);
    loop {
        info!("Waiting for radar");
        radar.wait_for_high().await;
        RADAR_SIGNAL.signal(0);
        while let Err(_) = with_timeout(Duration::from_secs(1), radar.wait_for_low()).await {
            info!("Moving!!");
            RADAR_SIGNAL.signal(0);
        }
    }
}

async fn light_task(mut pwm: SimplePwm<'static>) {
    info!("Starting light task");
    const MAX_DUTY: u16 = 32767;

    pwm.set_prescaler(Prescaler::Div1);
    pwm.set_max_duty(MAX_DUTY);
    pwm.set_ch1_drive(OutputDrive::Standard);
    pwm.set_ch2_drive(OutputDrive::Standard);
    pwm.set_ch3_drive(OutputDrive::Standard);
    pwm.set_all_duties([DutyCycle::normal(0), DutyCycle::normal(0), DutyCycle::normal(0), DutyCycle::normal(0)]);

    let mut intensity = 0i32;
    let mut hue: u16 = 0;
    let mut animation_type = TextLightType::Hue;

    loop {
        let action = {
            let guard = SYS_ACTION.lock().await;
            guard.clone()
        };
        match action {
            SysAction::PresentenceOn if intensity < 100 => {
                intensity.add_assign(1);
                info!("Increase intensity to {}", intensity);
            },
            SysAction::PresentenceOff if intensity > 0 => {
                intensity.sub_assign(1);
                info!("Decrease intensity to {}", intensity);
            },
            _ => {
                // ignore
            }
        }

        if let Ok(val) = with_timeout(Duration::from_millis(10), TEXT_ANIMATION_SIGNAL.wait()).await {
            match val {
                AnimationSignal::NextAnimation => {
                    animation_type = match animation_type {
                        TextLightType::Hue => {
                            TextLightType::ConstantColor
                        }
                        TextLightType::ConstantColor => {
                            TextLightType::White
                        }
                        TextLightType::White => {
                            TextLightType::Hue
                        }
                    };
                }
                AnimationSignal::SetMode(val) => {
                    animation_type = val;
                }
                AnimationSignal::SetColor(val) => {
                    hue = val;
                }
            }
        }
        match animation_type {
            TextLightType::Hue => {
                let (r, g, b) = hue_to_rgb(hue, MAX_DUTY);

                pwm.set_all_duties([
                    DutyCycle::normal(((r as i32) * intensity / 100) as u16),
                    DutyCycle::normal(((g as i32) * intensity / 100) as u16),
                    DutyCycle::normal(((b as i32) * intensity / 100) as u16),
                    DutyCycle::normal(0), // Ch4 unused
                ]);

                hue = (hue + 1) % 1536;
            }
            TextLightType::White => {
                let r = 12288;  // Lowered significantly
                let g = 16384;
                let b = 24576;

                // Set them once. No loop, no flicker.
                pwm.set_all_duties([
                    DutyCycle::normal(((r) * intensity / 100) as u16),
                    DutyCycle::normal(((g) * intensity / 100) as u16),
                    DutyCycle::normal(((b) * intensity / 100) as u16),
                    DutyCycle::normal(0),
                ]);
            }
            TextLightType::ConstantColor => {
                let (r, g, b) = hue_to_rgb(hue, MAX_DUTY);

                pwm.set_all_duties([
                    DutyCycle::normal(((r as i32) * intensity / 100) as u16),
                    DutyCycle::normal(((g as i32) * intensity / 100) as u16),
                    DutyCycle::normal(((b as i32) * intensity / 100) as u16),
                    DutyCycle::normal(0), // Ch4 unused
                ]);
            }
        }
    }
}

fn hue_to_rgb(hue: u16, max: u16) -> (u16, u16, u16) {
    let sector = (hue / 256) as u8;
    let rising = (hue % 256) as u32 * max as u32 / 255;
    let falling = max as u32 - rising;

    let (r, g, b) = match sector {
        0 => (max as u32, rising, 0),        // Red to Yellow
        1 => (falling, max as u32, 0),       // Yellow to Green
        2 => (0, max as u32, rising),        // Green to Cyan
        3 => (0, falling, max as u32),       // Cyan to Blue
        4 => (rising, 0, max as u32),        // Blue to Magenta
        _ => (max as u32, 0, falling),       // Magenta to Red
    };

    (r as u16, g as u16, b as u16)
}

async fn heartbeat_task(mut pwm: SimplePwm<'static>) {
    const MAX: u16 = 16384;
    pwm.set_max_duty(MAX);
    pwm.set_prescaler(Prescaler::Div1);


    let mut intensity = 0i32;
    let mut animation_type = HeartLightType::HeartBeat;
    let mut hue: u16 = 0;
    let mut heart_beat = 0;

    loop {
        let action = {
            let guard = SYS_ACTION.lock().await;
            guard.clone()
        };
        match action {
            SysAction::PresentenceOn if intensity < 100 => {
                intensity.add_assign(1);
                info!("Increase intensity to {}", intensity);
            },
            SysAction::PresentenceOff if intensity > 0 => {
                intensity.sub_assign(1);
                info!("Decrease intensity to {}", intensity);
            },
            _ => {
                // ignore
            }
        }

        if let Ok(val) = with_timeout(Duration::from_millis(10), HEART_ANIMATION_SIGNAL.wait()).await {
            match val {
                AnimationSignal::NextAnimation => {
                    animation_type = match animation_type {
                        HeartLightType::HeartBeat => {
                            HeartLightType::ConstantColor
                        }
                        HeartLightType::ConstantColor => {
                            HeartLightType::Hue
                        }
                        HeartLightType::Hue => {
                            HeartLightType::HeartBeat
                        }
                    };
                }
                AnimationSignal::SetMode(val) => {
                    animation_type = val;
                }
                AnimationSignal::SetColor(val) => {
                    hue = val;
                }
            }
        }

        match animation_type {
            HeartLightType::ConstantColor => {
                let (r, g, b) = hue_to_rgb(hue, MAX);

                pwm.set_all_duties([
                    DutyCycle::normal(((r as i32) * intensity / 100) as u16),
                    DutyCycle::normal(((g as i32) * intensity / 100) as u16),
                    DutyCycle::normal(((b as i32) * intensity / 100) as u16),
                    DutyCycle::normal(0), // Ch4 unused
                ]);
            }
            HeartLightType::Hue => {
                let (r, g, b) = hue_to_rgb(hue, MAX);

                pwm.set_all_duties([
                    DutyCycle::normal(((r as i32) * intensity / 100) as u16),
                    DutyCycle::normal(((g as i32) * intensity / 100) as u16),
                    DutyCycle::normal(((b as i32) * intensity / 100) as u16),
                    DutyCycle::normal(0), // Ch4 unused
                ]);

                hue = (hue + 1) % 1536;
            }
            HeartLightType::HeartBeat => {
                let beat = get_heartbeat(heart_beat);

                pwm.set_all_duties([
                    DutyCycle::normal(((beat as i32) * intensity / 100) as u16),
                    DutyCycle::normal(0),
                    DutyCycle::normal(0),
                    DutyCycle::normal(0), // Ch4 unused
                ]);

                heart_beat += 10;
                if heart_beat >= 32768 {
                    heart_beat = 0;
                }
            }
        }
    }
}

const BPM: u32 = 24;
const MIN_GLOW: u32 = 2048;
const MAX_VAL: u32 = 32768;
const PERIOD_MS: u32 = 60_000 / BPM;

/// Heartbeat logic optimized for MCU (no floats, precalculated constants)
pub fn get_heartbeat(sys_time_ms: u32) -> u16 {
    let time_tick = (((sys_time_ms % PERIOD_MS) as u64 * MAX_VAL as u64) / PERIOD_MS as u64) as u32;

    // 1. Calculate the pulse intensity (0..MAX_VAL)
    let pulse = if time_tick > 2000 && time_tick < 7000 {
        scale_pulse(time_tick, 2000, 4000, 7000, MAX_VAL)
    } else if time_tick > 8500 && time_tick < 12500 {
        scale_pulse(time_tick, 8500, 10000, 12500, MAX_VAL / 2)
    } else {
        0
    };

    // 2. Square it for that organic "ease-in" feel
    let eased_pulse = (pulse * pulse) >> 15;

    // 3. Map the 0..MAX_VAL pulse into the MIN_GLOW..MAX_VAL range
    // Formula: MIN + (eased_pulse * (MAX - MIN) / MAX)
    let range = MAX_VAL - MIN_GLOW;
    let final_val = MIN_GLOW + ((eased_pulse * range) >> 15);

    final_val as u16
}

#[inline(always)]
fn scale_pulse(t: u32, start: u32, peak: u32, end: u32, max: u32) -> u32 {
    if t <= peak {
        ((t - start) * max) / (peak - start)
    } else {
        ((end - t) * max) / (end - peak)
    }
}