"""
Apollo Subcarrier Oscillator (SCO) Demodulator — FM analog telemetry.

In FM downlink mode, the Pre-Modulation Processor generates 9 subcarrier
oscillators (SCOs) that encode analog sensor voltages (0-5V DC) as frequency
deviations of +/-7.5% around each channel's center frequency.

The SCOs are present in the composite FM modulating signal alongside PCM and
voice subcarriers. This block extracts one SCO channel and recovers the
original 0-5V sensor value.

Receiver side (this block):
    PM demod output -> subcarrier_extract(sco_freq, BW=15% of center)
    -> quadrature_demod (FM discriminator)
    -> DC offset + scale to 0-5V

The mapping is linear:
    0V input  -> center_freq - 7.5% = low  frequency
    2.5V input -> center_freq (nominal)
    5V input  -> center_freq + 7.5% = high frequency

Reference: IMPLEMENTATION_SPEC.md section 4.3
"""

import math

from gnuradio import analog, blocks, gr

from apollo.constants import (
    SAMPLE_RATE_BASEBAND,
    SCO_DEVIATION_PERCENT,
    SCO_FREQUENCIES,
    SCO_INPUT_RANGE_V,
)
from apollo.subcarrier_extract import subcarrier_extract


class sco_demod(gr.hier_block2):
    """Extract and demodulate one SCO channel to a 0-5V sensor reading.

    Only valid in FM downlink mode (not PM mode).

    Inputs:
        float -- PM demodulator output (composite subcarrier signal)

    Outputs:
        float -- recovered sensor voltage (0.0 to 5.0 V)
    """

    def __init__(
        self,
        sco_number: int = 1,
        sample_rate: float = SAMPLE_RATE_BASEBAND,
    ):
        gr.hier_block2.__init__(
            self,
            "apollo_sco_demod",
            gr.io_signature(1, 1, gr.sizeof_float),
            gr.io_signature(1, 1, gr.sizeof_float),
        )

        if sco_number not in SCO_FREQUENCIES:
            raise ValueError(
                f"SCO number must be 1-9, got {sco_number}. "
                f"Valid channels: {sorted(SCO_FREQUENCIES.keys())}"
            )

        self._sco_number = sco_number
        self._sample_rate = sample_rate

        center_freq = SCO_FREQUENCIES[sco_number]
        self._center_freq = center_freq

        # BPF bandwidth = 15% of center frequency (per IMPL_SPEC 4.3:
        # the deviation is +/-7.5%, so 15% total bandwidth captures the
        # full FM swing)
        bw = 0.15 * center_freq
        self._bandwidth = bw

        # Frequency deviation in Hz: +/-7.5% of center
        deviation_hz = center_freq * (SCO_DEVIATION_PERCENT / 100.0)
        self._deviation_hz = deviation_hz

        # Decimation: SCOs range from 14.5 kHz to 165 kHz. We need at
        # least 2x the BW after decimation. Be conservative.
        min_rate = bw * 3.0  # 3x bandwidth for margin
        decimation = max(1, int(sample_rate / min_rate))
        self._decimation = decimation
        extracted_rate = sample_rate / decimation

        # Stage 1: Extract the SCO to complex baseband
        self.extract = subcarrier_extract(
            center_freq=center_freq,
            bandwidth=bw,
            sample_rate=sample_rate,
            decimation=decimation,
        )

        # Stage 2: FM discriminator
        # Gain: sample_rate / (2 * pi * max_deviation)
        # This gives output in units of (deviation_hz / deviation_hz) = 1.0
        # at full deviation. We then scale to voltage.
        fm_gain = extracted_rate / (2.0 * math.pi * deviation_hz)
        self.fm_demod = analog.quadrature_demod_cf(fm_gain)

        # Stage 3: Scale and offset to 0-5V range
        # The FM demod output is proportional to instantaneous frequency offset:
        #   -deviation -> demod output ≈ -1.0  -> 0V
        #   0          -> demod output ≈  0.0  -> 2.5V
        #   +deviation -> demod output ≈ +1.0  -> 5V
        #
        # voltage = (demod_output + 1.0) * 2.5
        # Implemented as: multiply by 2.5, then add 2.5
        v_min, v_max = SCO_INPUT_RANGE_V
        v_range = v_max - v_min  # 5.0
        v_mid = (v_max + v_min) / 2.0  # 2.5

        self.scale = blocks.multiply_const_ff(v_range / 2.0)
        self.offset = blocks.add_const_ff(v_mid)

        # Connect the chain
        self.connect(
            self,
            self.extract,
            self.fm_demod,
            self.scale,
            self.offset,
            self,
        )

    @property
    def center_freq(self) -> float:
        """Center frequency of this SCO channel in Hz."""
        return self._center_freq

    @property
    def deviation_hz(self) -> float:
        """FM deviation in Hz (+/- from center)."""
        return self._deviation_hz

    @property
    def output_sample_rate(self) -> float:
        """Sample rate of the output stream."""
        return self._sample_rate / self._decimation