1use crate::NQuin;
8
9#[cfg(target_arch = "x86_64")]
10use std::arch::x86_64::_MM_HINT_T0;
11
12#[inline(always)]
24pub fn resolve_aligned_byte_offset(start_index: usize) -> (u64, u16) {
25 let exact_byte_offset = (start_index * 8) as u64;
26
27 let page_aligned_offset = exact_byte_offset & !0xFFF;
30
31 let remainder = (exact_byte_offset - page_aligned_offset) as u16;
34
35 (page_aligned_offset, remainder)
36}
37
38#[inline(always)]
41pub fn pack_f32_pair(step: f32, comp: f32) -> u64 {
42 let step_bits = step.to_bits() as u64;
43 let comp_bits = comp.to_bits() as u64;
44 (step_bits << 32) | comp_bits
45}
46
47#[inline(always)]
49pub fn unpack_f32_pair(packed: u64) -> (f32, f32) {
50 let step_bits = (packed >> 32) as u32;
51 let comp_bits = (packed & 0xFFFFFFFF) as u32;
52 (f32::from_bits(step_bits), f32::from_bits(comp_bits))
53}
54
55#[derive(Debug, Clone, Copy, PartialEq, Eq)]
58pub enum CalculusError {
59 AlignmentError(AlignmentError),
60 InvalidOffset,
61 InsufficientData,
62 InvalidStepSize,
63 NonFiniteInput,
64 SimpsonRequiresEvenPanels,
65}
66
67#[derive(Debug, Clone, Copy, PartialEq, Eq)]
68pub enum AlignmentError {
69 MisalignedPointer,
70 MisalignedLength,
71 MisalignedOffset,
72}
73
74pub struct ContinuousGrid<'a> {
81 data: &'a [f64],
82}
83
84impl<'a> ContinuousGrid<'a> {
85 pub fn new(raw_slice: &'a [u8], points: usize) -> Result<Self, AlignmentError> {
92 let byte_len = points
93 .checked_mul(core::mem::size_of::<f64>())
94 .ok_or(AlignmentError::MisalignedLength)?;
95
96 if raw_slice.len() < byte_len {
97 return Err(AlignmentError::MisalignedLength);
98 }
99
100 if raw_slice.as_ptr() as usize % 8 != 0 {
102 return Err(AlignmentError::MisalignedPointer);
103 }
104
105 if byte_len % 8 != 0 {
107 return Err(AlignmentError::MisalignedLength);
108 }
109
110 let float_slice =
112 unsafe { core::slice::from_raw_parts(raw_slice.as_ptr() as *const f64, points) };
113
114 Ok(Self { data: float_slice })
115 }
116
117 pub fn resume_from_quin(
122 raw_slice: &'a [u8],
123 quin: &NQuin,
124 ) -> Result<(Self, usize), CalculusError> {
125 let offset = quin.object as usize;
126
127 if offset % 8 != 0 {
129 return Err(CalculusError::AlignmentError(
130 AlignmentError::MisalignedOffset,
131 ));
132 }
133
134 if offset >= raw_slice.len() {
135 return Err(CalculusError::InvalidOffset);
136 }
137
138 let grid = Self::new(&raw_slice[offset..], (raw_slice.len() - offset) / 8)
139 .map_err(CalculusError::AlignmentError)?;
140
141 Ok((grid, offset))
142 }
143
144 pub fn len(&self) -> usize {
146 self.data.len()
147 }
148
149 pub fn is_empty(&self) -> bool {
151 self.data.is_empty()
152 }
153
154 pub fn as_slice(&self) -> &[f64] {
156 self.data
157 }
158}
159
160#[derive(Debug, Clone, Copy, PartialEq, Eq)]
163pub enum SimdWidth {
164 Scalar,
165 Neon2, Avx2, }
168
169pub fn detect_simd_width() -> SimdWidth {
170 #[cfg(target_arch = "x86_64")]
171 {
172 if std::is_x86_feature_detected!("avx2") {
173 SimdWidth::Avx2
174 } else {
175 SimdWidth::Scalar
176 }
177 }
178
179 #[cfg(target_arch = "aarch64")]
180 {
181 SimdWidth::Neon2
182 }
183
184 #[cfg(not(any(target_arch = "x86_64", target_arch = "aarch64")))]
185 {
186 SimdWidth::Scalar
187 }
188}
189
190pub fn detect_cache_line_size() -> usize {
191 #[cfg(target_arch = "x86_64")]
192 {
193 #[cfg(target_arch = "x86_64")]
194 {
195 64
197 }
198 }
199
200 #[cfg(target_arch = "aarch64")]
201 {
202 64 }
204
205 #[cfg(not(any(target_arch = "x86_64", target_arch = "aarch64")))]
206 {
207 64 }
209}
210
211pub fn integrate_simpsons_kahan(
219 grid: &ContinuousGrid,
220 step_size: f32,
221) -> Result<(f64, f32), CalculusError> {
222 validate_simpson_inputs(grid.data, step_size as f64)?;
223
224 let mut sum = 0.0f64;
225 let mut compensation = 0.0f64;
226 let chunk_size = calculate_optimal_chunk_size();
227
228 for (chunk_index, chunk) in grid.data.chunks(chunk_size).enumerate() {
229 let start = chunk_index * chunk_size;
230 let chunk_sum = process_simpson_weighted_chunk(chunk, start, grid.data.len());
231
232 let y = chunk_sum - compensation;
234 let t = sum + y;
235 compensation = (t - sum) - y;
236 sum = t;
237 }
238
239 let scale = step_size as f64 / 3.0;
240 Ok((sum * scale, (compensation * scale) as f32))
241}
242
243pub fn integrate_simpsons_chunked(
247 grid: &ContinuousGrid,
248 step_size: f64,
249) -> Result<f64, CalculusError> {
250 validate_simpson_inputs(grid.data, step_size)?;
251
252 let mut accumulator = 0.0f64;
253 let chunk_size = calculate_optimal_chunk_size();
254 let prefetch_distance = chunk_size * 2;
255
256 let chunks = grid.data.chunks(chunk_size);
257 for (i, chunk) in chunks.enumerate() {
258 if let Some(future_data) = grid.data.get(i * chunk_size + prefetch_distance) {
260 issue_prefetch(future_data);
261 }
262
263 accumulator += process_simpson_weighted_chunk(chunk, i * chunk_size, grid.data.len());
264 }
265
266 Ok(accumulator * (step_size / 3.0))
267}
268
269pub fn integrate_trapezoidal_chunked(
271 grid: &ContinuousGrid,
272 step_size: f64,
273) -> Result<f64, CalculusError> {
274 validate_common_inputs(grid.data, step_size, 2)?;
275
276 let mut accumulator = 0.0f64;
277 let chunk_size = calculate_optimal_chunk_size();
278
279 for (chunk_index, chunk) in grid.data.chunks(chunk_size).enumerate() {
280 accumulator += process_trapezoidal_chunk(chunk, chunk_index * chunk_size, grid.data.len());
281 }
282
283 Ok(accumulator * (step_size / 2.0))
284}
285
286fn calculate_optimal_chunk_size() -> usize {
289 let simd_width = detect_simd_width();
290 let cache_line_size = detect_cache_line_size();
291
292 let base = match simd_width {
294 SimdWidth::Scalar => 1,
295 SimdWidth::Neon2 => 2,
296 SimdWidth::Avx2 => 4,
297 };
298
299 let f64_per_cache_line = cache_line_size / 8;
301
302 let target = f64_per_cache_line * 2;
304
305 ((target + base - 1) / base) * base
307}
308
309#[inline]
310fn simpson_weight(index: usize, total_len: usize) -> f64 {
311 if index == 0 || index + 1 == total_len {
312 1.0
313 } else if index & 1 == 1 {
314 4.0
315 } else {
316 2.0
317 }
318}
319
320fn validate_common_inputs(
321 data: &[f64],
322 step_size: f64,
323 minimum_len: usize,
324) -> Result<(), CalculusError> {
325 if data.len() < minimum_len {
326 return Err(CalculusError::InsufficientData);
327 }
328 if !step_size.is_finite() || step_size == 0.0 {
329 return Err(CalculusError::InvalidStepSize);
330 }
331 if data.iter().any(|value| !value.is_finite()) {
332 return Err(CalculusError::NonFiniteInput);
333 }
334 Ok(())
335}
336
337fn validate_simpson_inputs(data: &[f64], step_size: f64) -> Result<(), CalculusError> {
338 validate_common_inputs(data, step_size, 3)?;
339 if data.len() & 1 == 0 {
340 return Err(CalculusError::SimpsonRequiresEvenPanels);
341 }
342 Ok(())
343}
344
345fn process_simpson_weighted_chunk(chunk: &[f64], global_start: usize, total_len: usize) -> f64 {
350 #[cfg(target_arch = "x86_64")]
351 {
352 if std::is_x86_feature_detected!("avx2") {
353 return unsafe { process_simpson_weighted_chunk_avx2(chunk, global_start, total_len) };
356 }
357 }
358
359 #[cfg(target_arch = "aarch64")]
360 {
361 return process_simpson_weighted_chunk_neon(chunk, global_start, total_len);
362 }
363
364 process_simpson_weighted_chunk_scalar(chunk, global_start, total_len)
365}
366
367fn process_simpson_weighted_chunk_scalar(
368 chunk: &[f64],
369 global_start: usize,
370 total_len: usize,
371) -> f64 {
372 chunk
373 .iter()
374 .enumerate()
375 .map(|(offset, value)| simpson_weight(global_start + offset, total_len) * value)
376 .sum()
377}
378
379#[cfg(target_arch = "x86_64")]
381#[target_feature(enable = "avx2")]
382unsafe fn process_simpson_weighted_chunk_avx2(
383 chunk: &[f64],
384 global_start: usize,
385 total_len: usize,
386) -> f64 {
387 use core::arch::x86_64::*;
388
389 let mut sum = 0.0f64;
390 let len = chunk.len();
391
392 let simd_chunks = len / 4;
393 for i in 0..simd_chunks {
394 let idx = i * 4;
395 let global = global_start + idx;
396 let vals = _mm256_loadu_pd(chunk.as_ptr().add(idx));
397 let weights = _mm256_set_pd(
398 simpson_weight(global + 3, total_len),
399 simpson_weight(global + 2, total_len),
400 simpson_weight(global + 1, total_len),
401 simpson_weight(global, total_len),
402 );
403 let weighted = _mm256_mul_pd(vals, weights);
404 let mut lanes = [0.0_f64; 4];
405 _mm256_storeu_pd(lanes.as_mut_ptr(), weighted);
406 sum += lanes[0] + lanes[1] + lanes[2] + lanes[3];
407 }
408
409 for i in (simd_chunks * 4)..len {
410 sum += simpson_weight(global_start + i, total_len) * chunk[i];
411 }
412
413 sum
414}
415
416#[cfg(target_arch = "aarch64")]
418fn process_simpson_weighted_chunk_neon(
419 chunk: &[f64],
420 global_start: usize,
421 total_len: usize,
422) -> f64 {
423 use core::arch::aarch64::*;
424
425 let mut sum = 0.0f64;
426 let len = chunk.len();
427
428 let simd_chunks = len / 2;
430 for i in 0..simd_chunks {
431 let idx = i * 2;
432 unsafe {
433 let vals = vld1q_f64(chunk.as_ptr().add(idx));
434 let global = global_start + idx;
435 let weights = [
436 simpson_weight(global, total_len),
437 simpson_weight(global + 1, total_len),
438 ];
439 let weighted = vmulq_f64(vals, vld1q_f64(weights.as_ptr()));
440 sum += vgetq_lane_f64::<0>(weighted) + vgetq_lane_f64::<1>(weighted);
441 }
442 }
443
444 for i in (simd_chunks * 2)..len {
446 sum += simpson_weight(global_start + i, total_len) * chunk[i];
447 }
448
449 sum
450}
451
452fn process_trapezoidal_chunk(chunk: &[f64], global_start: usize, total_len: usize) -> f64 {
454 chunk
455 .iter()
456 .enumerate()
457 .map(|(offset, value)| {
458 let index = global_start + offset;
459 let weight = if index == 0 || index + 1 == total_len {
460 1.0
461 } else {
462 2.0
463 };
464 weight * value
465 })
466 .sum()
467}
468
469fn issue_prefetch(data: &f64) {
471 #[cfg(target_arch = "x86_64")]
472 {
473 use core::arch::x86_64::_mm_prefetch;
474 unsafe {
475 _mm_prefetch(data as *const f64 as *const i8, _MM_HINT_T0);
476 }
477 }
478
479 #[cfg(target_arch = "aarch64")]
480 {
481 let _ = data; }
483
484 #[cfg(not(any(target_arch = "x86_64", target_arch = "aarch64")))]
485 {
486 core::hint::black_box(data);
488 }
489}
490
491#[cfg(test)]
494mod tests {
495 use super::*;
496
497 #[test]
498 fn test_simpsons_integration() {
499 #[repr(C, align(4096))]
500 struct TestBuffer {
501 data: [f64; 101],
502 }
503
504 let buffer = TestBuffer {
505 data: [1.0f64; 101],
506 };
507
508 let raw_bytes: &[u8] = unsafe {
509 core::slice::from_raw_parts(buffer.data.as_ptr() as *const u8, buffer.data.len() * 8)
510 };
511
512 let grid = ContinuousGrid::new(raw_bytes, 101).unwrap();
513 let result = integrate_simpsons_chunked(&grid, 0.02).unwrap();
514 assert_eq!(result, 2.0);
515 }
516
517 #[test]
518 fn test_alignment_safety() {
519 #[repr(C, align(4096))]
522 struct TestBuffer {
523 data: [u8; 8192], }
525
526 let buffer = TestBuffer { data: [0u8; 8192] };
527 let result = ContinuousGrid::new(&buffer.data[1..], 2);
528 assert!(matches!(result, Err(AlignmentError::MisalignedPointer)));
529 }
530
531 #[test]
532 fn test_resolve_aligned_byte_offset() {
533 let (aligned, remainder) = resolve_aligned_byte_offset(0);
535 assert_eq!(aligned, 0);
536 assert_eq!(remainder, 0);
537
538 let (aligned, remainder) = resolve_aligned_byte_offset(512);
540 assert_eq!(aligned, 4096);
541 assert_eq!(remainder, 0);
542
543 let (aligned, remainder) = resolve_aligned_byte_offset(513);
545 assert_eq!(aligned, 4096);
546 assert_eq!(remainder, 8);
547
548 let (aligned, remainder) = resolve_aligned_byte_offset(1023);
550 assert_eq!(aligned, 4096);
551 assert_eq!(remainder, 4088);
552
553 let (aligned, remainder) = resolve_aligned_byte_offset(1024);
555 assert_eq!(aligned, 8192);
556 assert_eq!(remainder, 0);
557 }
558
559 #[test]
560 fn test_pack_unpack_f32_pair() {
561 let step = 0.001f32;
562 let comp = 0.0f32;
563 let packed = pack_f32_pair(step, comp);
564 let (unpacked_step, unpacked_comp) = unpack_f32_pair(packed);
565 assert_eq!(step, unpacked_step);
566 assert_eq!(comp, unpacked_comp);
567 }
568
569 #[test]
570 fn test_state_suspension() {
571 let mut quin = NQuin::default();
573 quin.object = 1024; quin.metadata = f64::to_bits(42.5); let offset = quin.object;
577 let accumulator = f64::from_bits(quin.metadata);
578
579 assert_eq!(offset, 1024);
580 assert_eq!(accumulator, 42.5);
581 }
582
583 #[test]
584 fn test_resume_from_quin() {
585 let mut data = [0.0f64; 100];
586 for i in 0..100 {
587 data[i] = i as f64;
588 }
589
590 let raw_bytes: &[u8] =
591 unsafe { core::slice::from_raw_parts(data.as_ptr() as *const u8, data.len() * 8) };
592
593 let mut quin = NQuin::default();
594 quin.object = 64; let (grid, offset) = ContinuousGrid::resume_from_quin(raw_bytes, &quin).unwrap();
597 assert_eq!(offset, 64);
598 assert_eq!(grid.len(), 92); }
600
601 #[test]
602 fn test_resume_from_quin_misaligned() {
603 let data = [0u8; 100];
604 let mut quin = NQuin::default();
605 quin.object = 63; let result = ContinuousGrid::resume_from_quin(&data, &quin);
608 assert!(matches!(
609 result,
610 Err(CalculusError::AlignmentError(
611 AlignmentError::MisalignedOffset
612 ))
613 ));
614 }
615
616 #[test]
617 fn test_simd_width_detection() {
618 let width = detect_simd_width();
619 match width {
621 SimdWidth::Scalar | SimdWidth::Neon2 | SimdWidth::Avx2 => {}
622 }
623 }
624
625 #[test]
626 fn test_cache_line_size_detection() {
627 let size = detect_cache_line_size();
628 assert!(size == 32 || size == 64 || size == 128);
630 }
631
632 #[test]
633 fn test_kahan_summation() {
634 let mut data = [0.0f64; 1001];
636 for i in 0..1001 {
637 data[i] = 1e-10; }
639
640 let raw_bytes: &[u8] =
641 unsafe { core::slice::from_raw_parts(data.as_ptr() as *const u8, data.len() * 8) };
642
643 let grid = ContinuousGrid::new(raw_bytes, 1001).unwrap();
644 let (sum, _compensation) = integrate_simpsons_kahan(&grid, 0.001).unwrap();
645
646 assert!(sum > 0.0);
648 }
649
650 #[test]
651 fn simpson_is_exact_for_cubic_across_cache_chunks() {
652 #[repr(C, align(64))]
653 struct Buffer {
654 data: [f64; 101],
655 }
656 let mut buffer = Buffer { data: [0.0; 101] };
657 for (i, value) in buffer.data.iter_mut().enumerate() {
658 let x = i as f64 / 100.0;
659 *value = x * x * x;
660 }
661 let bytes = unsafe {
662 core::slice::from_raw_parts(
663 buffer.data.as_ptr().cast::<u8>(),
664 buffer.data.len() * core::mem::size_of::<f64>(),
665 )
666 };
667 let grid = ContinuousGrid::new(bytes, buffer.data.len()).unwrap();
668 let integral = integrate_simpsons_chunked(&grid, 0.01).unwrap();
669 assert!((integral - 0.25).abs() <= 8.0 * f64::EPSILON);
670 }
671
672 #[test]
673 fn scalar_quadrature_exactness_holds_for_all_small_legal_lengths() {
674 #[repr(C, align(64))]
675 struct Buffer {
676 data: [f64; 257],
677 }
678 let mut buffer = Buffer { data: [0.0; 257] };
679 let h = 1.0 / 256.0;
680 for (i, value) in buffer.data.iter_mut().enumerate() {
681 let x = i as f64 * h;
682 *value = x * x * x;
683 }
684 let bytes = unsafe {
685 core::slice::from_raw_parts(
686 buffer.data.as_ptr().cast::<u8>(),
687 buffer.data.len() * core::mem::size_of::<f64>(),
688 )
689 };
690
691 for len in (3..=257).step_by(2) {
692 let grid = ContinuousGrid::new(bytes, len).unwrap();
693 let upper = (len - 1) as f64 * h;
694 let expected = upper.powi(4) / 4.0;
695 let actual = integrate_simpsons_chunked(&grid, h).unwrap();
696 assert!(
697 (actual - expected).abs() <= 128.0 * f64::EPSILON * expected.max(1.0),
698 "len={len}: expected {expected}, got {actual}"
699 );
700 }
701
702 for (i, value) in buffer.data.iter_mut().enumerate() {
703 *value = 3.0 * i as f64 * h - 2.0;
704 }
705 for len in 2..=257 {
706 let grid = ContinuousGrid::new(bytes, len).unwrap();
707 let upper = (len - 1) as f64 * h;
708 let expected = 1.5 * upper * upper - 2.0 * upper;
709 let actual = integrate_trapezoidal_chunked(&grid, h).unwrap();
710 assert!(
711 (actual - expected).abs() <= 128.0 * f64::EPSILON * expected.abs().max(1.0),
712 "len={len}: expected {expected}, got {actual}"
713 );
714 }
715 }
716
717 #[test]
718 fn trapezoid_is_exact_for_affine_data_across_cache_chunks() {
719 #[repr(C, align(64))]
720 struct Buffer {
721 data: [f64; 101],
722 }
723 let mut buffer = Buffer { data: [0.0; 101] };
724 for (i, value) in buffer.data.iter_mut().enumerate() {
725 *value = i as f64 / 100.0;
726 }
727 let bytes = unsafe {
728 core::slice::from_raw_parts(
729 buffer.data.as_ptr().cast::<u8>(),
730 buffer.data.len() * core::mem::size_of::<f64>(),
731 )
732 };
733 let grid = ContinuousGrid::new(bytes, buffer.data.len()).unwrap();
734 let integral = integrate_trapezoidal_chunked(&grid, 0.01).unwrap();
735 assert!((integral - 0.5).abs() <= 4.0 * f64::EPSILON);
736 }
737
738 #[test]
739 fn simpson_rejects_invalid_panel_count_and_non_finite_data() {
740 let even = [1.0_f64; 4];
741 let even_bytes = unsafe {
742 core::slice::from_raw_parts(
743 even.as_ptr().cast::<u8>(),
744 even.len() * core::mem::size_of::<f64>(),
745 )
746 };
747 let even_grid = ContinuousGrid::new(even_bytes, even.len()).unwrap();
748 assert_eq!(
749 integrate_simpsons_chunked(&even_grid, 1.0),
750 Err(CalculusError::SimpsonRequiresEvenPanels)
751 );
752
753 let non_finite = [0.0, f64::NAN, 1.0];
754 let non_finite_bytes = unsafe {
755 core::slice::from_raw_parts(
756 non_finite.as_ptr().cast::<u8>(),
757 non_finite.len() * core::mem::size_of::<f64>(),
758 )
759 };
760 let non_finite_grid = ContinuousGrid::new(non_finite_bytes, non_finite.len()).unwrap();
761 assert_eq!(
762 integrate_simpsons_chunked(&non_finite_grid, 0.5),
763 Err(CalculusError::NonFiniteInput)
764 );
765 assert_eq!(
766 integrate_simpsons_chunked(&non_finite_grid, 0.0),
767 Err(CalculusError::InvalidStepSize)
768 );
769 }
770
771 #[cfg(target_arch = "x86_64")]
772 #[test]
773 fn runtime_avx2_weighted_sum_matches_forced_scalar() {
774 if !std::is_x86_feature_detected!("avx2") {
775 return;
776 }
777 let mut data = [0.0_f64; 80];
778 for (index, value) in data.iter_mut().enumerate() {
779 *value = (index as f64 * 0.37).sin() * (1.0 + index as f64);
780 }
781 for offset in 0..8 {
782 for len in 0..=64 {
783 let slice = &data[offset..offset + len];
784 let scalar = process_simpson_weighted_chunk_scalar(slice, offset + 3, 97);
785 let avx2 = unsafe { process_simpson_weighted_chunk_avx2(slice, offset + 3, 97) };
786 let scale = scalar.abs().max(1.0);
787 assert!(
788 (scalar - avx2).abs() <= 64.0 * f64::EPSILON * scale,
789 "offset={offset}, len={len}, scalar={scalar}, avx2={avx2}"
790 );
791 }
792 }
793 }
794
795 #[cfg(target_arch = "aarch64")]
796 #[test]
797 fn neon_weighted_sum_matches_forced_scalar() {
798 let mut data = [0.0_f64; 80];
799 for (index, value) in data.iter_mut().enumerate() {
800 *value = (index as f64 * 0.37).sin() * (1.0 + index as f64);
801 }
802 for offset in 0..8 {
803 for len in 0..=64 {
804 let slice = &data[offset..offset + len];
805 let scalar = process_simpson_weighted_chunk_scalar(slice, offset + 3, 97);
806 let neon = process_simpson_weighted_chunk_neon(slice, offset + 3, 97);
807 let scale = scalar.abs().max(1.0);
808 assert!((scalar - neon).abs() <= 64.0 * f64::EPSILON * scale);
809 }
810 }
811 }
812}