1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96

/*
 * File: modulation.rs
 * Project: operation
 * Created Date: 08/01/2023
 * Author: Shun Suzuki
 * -----
 * Last Modified: 07/03/2023
 * Modified By: Shun Suzuki (suzuki@hapis.k.u-tokyo.ac.jp)
 * -----
 * Copyright (c) 2023 Shun Suzuki. All rights reserved.
 *
 */

use super::Operation;
use crate::{
    CPUControlFlags, DriverError, TxDatagram, MOD_BUF_SIZE_MAX, MOD_HEADER_INITIAL_DATA_SIZE,
    MOD_HEADER_SUBSEQUENT_DATA_SIZE, MOD_SAMPLING_FREQ_DIV_MIN,
};
use anyhow::Result;

pub struct Modulation {
    mod_data: Vec<u8>,
    sent: usize,
    freq_div: u32,
}

impl Modulation {
    pub fn new(mod_data: Vec<f64>, freq_div: u32) -> Self {
        Self {
            mod_data: mod_data.into_iter().map(Self::to_duty).collect(),
            sent: 0,
            freq_div,
        }
    }

    pub fn to_duty(amp: f64) -> u8 {
        (amp.clamp(0., 1.).asin() * 2.0 / std::f64::consts::PI * 255.0) as u8
    }
}

impl Operation for Modulation {
    fn pack(&mut self, tx: &mut TxDatagram) -> Result<()> {
        if self.mod_data.len() > MOD_BUF_SIZE_MAX {
            return Err(DriverError::ModulationSizeOutOfRange(self.mod_data.len()).into());
        }

        let is_first_frame = self.sent == 0;
        let max_size = if is_first_frame {
            MOD_HEADER_INITIAL_DATA_SIZE
        } else {
            MOD_HEADER_SUBSEQUENT_DATA_SIZE
        };
        let mod_size = (self.mod_data.len() - self.sent).min(max_size);
        if mod_size == 0 {
            tx.header_mut().cpu_flag.remove(CPUControlFlags::MOD);
            return Ok(());
        }
        let is_last_frame = self.sent + mod_size == self.mod_data.len();

        tx.header_mut().cpu_flag.set(CPUControlFlags::MOD, true);
        tx.header_mut().cpu_flag.remove(CPUControlFlags::MOD_BEGIN);
        tx.header_mut().cpu_flag.remove(CPUControlFlags::MOD_END);
        tx.header_mut().size = mod_size as _;

        if is_first_frame {
            if self.freq_div < MOD_SAMPLING_FREQ_DIV_MIN {
                return Err(DriverError::ModFreqDivOutOfRange(self.freq_div).into());
            }
            tx.header_mut()
                .cpu_flag
                .set(CPUControlFlags::MOD_BEGIN, true);
            tx.header_mut().mod_initial_mut().freq_div = self.freq_div;
            tx.header_mut().mod_initial_mut().data[0..mod_size]
                .copy_from_slice(&self.mod_data[self.sent..]);
        } else {
            tx.header_mut().mod_subsequent_mut().data[0..mod_size]
                .copy_from_slice(&self.mod_data[self.sent..]);
        }

        if is_last_frame {
            tx.header_mut().cpu_flag.set(CPUControlFlags::MOD_END, true);
        }

        self.sent += mod_size;

        Ok(())
    }

    fn init(&mut self) {
        self.sent = 0;
    }

    fn is_finished(&self) -> bool {
        self.sent == self.mod_data.len()
    }
}