src/lib.rs

   1 /*!
   2 An algorithm library based on C/C++ language
   3
   4 ## Installation
   5
   6 Add this to your `Cargo.toml`:
   7
   8 ```toml
   9 [dependencies]
  10 liba = "0.1"
  11 ```
  12 */
  13
  14 #![warn(missing_docs)]
  15 #![allow(non_camel_case_types)]
  16 #![cfg_attr(not(features = "std"), no_std)]
  17
  18 /// Equivalent to C’s int type.
  19 pub type int = core::ffi::c_int;
  20 /// Equivalent to C’s unsigned int type.
  21 pub type uint = core::ffi::c_uint;
  22 /// floating-point number stored using `f64`
  23 #[cfg(not(feature = "float"))]
  24 pub type float = f64;
  25 /// floating-point number stored using `f32`
  26 #[cfg(feature = "float")]
  27 pub type float = f32;
  28
  29 extern "C" {
  30     fn a_u32_sqrt(x: u32) -> u16;
  31     fn a_u64_sqrt(x: u64) -> u32;
  32 }
  33
  34 /// square root of an unsigned integer
  35 #[inline(always)]
  36 pub fn u32_sqrt(x: u32) -> u16 {
  37     unsafe { a_u32_sqrt(x) }
  38 }
  39 /// square root of an unsigned integer
  40 #[inline(always)]
  41 pub fn u64_sqrt(x: u64) -> u32 {
  42     unsafe { a_u64_sqrt(x) }
  43 }
  44
  45 extern "C" {
  46     fn a_f32_rsqrt(x: f32) -> f32;
  47     fn a_f64_rsqrt(x: f64) -> f64;
  48 }
  49
  50 /// reciprocal of square-root, `\frac{1}{\sqrt{x}}`
  51 #[inline(always)]
  52 pub fn f32_rsqrt(x: f32) -> f32 {
  53     unsafe { a_f32_rsqrt(x) }
  54 }
  55 /// reciprocal of square-root, `\frac{1}{\sqrt{x}}`
  56 #[inline(always)]
  57 pub fn f64_rsqrt(x: f64) -> f64 {
  58     unsafe { a_f64_rsqrt(x) }
  59 }
  60
  61 extern "C" {
  62     fn a_hash_bkdr_(pdata: *const u8, nbyte: usize, value: u32) -> u32;
  63     fn a_hash_sdbm_(pdata: *const u8, nbyte: usize, value: u32) -> u32;
  64 }
  65
  66 /// a hash function whose prime number is 131
  67 #[inline(always)]
  68 pub fn hash_bkdr(block: &[u8], value: u32) -> u32 {
  69     unsafe { a_hash_bkdr_(block.as_ptr(), block.len(), value) }
  70 }
  71 /// a hash function whose prime number is 65599
  72 #[inline(always)]
  73 pub fn hash_sdbm(block: &[u8], value: u32) -> u32 {
  74     unsafe { a_hash_sdbm_(block.as_ptr(), block.len(), value) }
  75 }
  76
  77 mod test {
  78     extern crate std;
  79     #[test]
  80     fn usqrt() {
  81         std::println!("{}", crate::u32_sqrt(u32::MAX));
  82         std::println!("{}", crate::u64_sqrt(u64::MAX));
  83     }
  84     #[test]
  85     fn rsqrt() {
  86         std::println!("1/sqrt({})={}", 4, crate::f32_rsqrt(4.0));
  87         std::println!("1/sqrt({})={}", 4, crate::f64_rsqrt(4.0));
  88     }
  89     #[test]
  90     fn hash() {
  91         let text: [u8; 10] = *b"0123456789";
  92         std::println!("{}", crate::hash_bkdr(&text, 0));
  93         std::println!("{}", crate::hash_sdbm(&text, 0));
  94     }
  95 }
  96
  97 /// Cyclic Redundancy Check for 8 bits
  98 #[repr(C)]
  99 pub struct crc8 {
 100     /// Cyclic Redundancy Check comparison table
 101     pub table: [u8; 0x100],
 102 }
 103
 104 extern "C" {
 105     fn a_crc8m_init(table: *mut u8, poly: u8);
 106     fn a_crc8l_init(table: *mut u8, poly: u8);
 107     fn a_crc8(table: *const u8, pdate: *const u8, nbyte: usize, value: u8) -> u8;
 108 }
 109
 110 impl crc8 {
 111     /// initialize for MSB CRC-8
 112     #[inline(always)]
 113     pub fn new_msb(poly: u8) -> Self {
 114         let mut ctx: Self = Self { table: [0; 0x100] };
 115         unsafe { a_crc8m_init(ctx.table.as_mut_ptr(), poly) };
 116         ctx
 117     }
 118     /// initialize for LSB CRC-8
 119     #[inline(always)]
 120     pub fn new_lsb(poly: u8) -> Self {
 121         let mut ctx: Self = Self { table: [0; 0x100] };
 122         unsafe { a_crc8l_init(ctx.table.as_mut_ptr(), poly) };
 123         ctx
 124     }
 125     /// generate for MSB CRC-8
 126     #[inline(always)]
 127     pub fn gen_msb(&mut self, poly: u8) -> &mut Self {
 128         unsafe { a_crc8m_init(self.table.as_mut_ptr(), poly) };
 129         self
 130     }
 131     /// generate for LSB CRC-8
 132     #[inline(always)]
 133     pub fn gen_lsb(&mut self, poly: u8) -> &mut Self {
 134         unsafe { a_crc8l_init(self.table.as_mut_ptr(), poly) };
 135         self
 136     }
 137     /// calculate for CRC-8
 138     #[inline(always)]
 139     pub fn eval(self, block: &[u8], value: u8) -> u8 {
 140         unsafe { a_crc8(self.table.as_ptr(), block.as_ptr(), block.len(), value) }
 141     }
 142 }
 143
 144 #[test]
 145 fn crc8() {
 146     extern crate std;
 147     {
 148         let ctx = crate::crc8::new_msb(0x07);
 149         assert_eq!(ctx.eval(b"0123456789", 0), 0x45);
 150     }
 151     {
 152         let ctx = crate::crc8::new_lsb(0x31);
 153         assert_eq!(ctx.eval(b"0123456789", 0), 0x75);
 154     }
 155 }
 156
 157 /// Cyclic Redundancy Check for 16 bits
 158 #[repr(C)]
 159 pub struct crc16 {
 160     /// Cyclic Redundancy Check comparison table
 161     pub table: [u16; 0x100],
 162     eval: unsafe extern "C" fn(*const u16, *const u8, usize, u16) -> u16,
 163 }
 164
 165 extern "C" {
 166     fn a_crc16m_init(table: *mut u16, poly: u16);
 167     fn a_crc16l_init(table: *mut u16, poly: u16);
 168     fn a_crc16m(table: *const u16, pdate: *const u8, nbyte: usize, value: u16) -> u16;
 169     fn a_crc16l(table: *const u16, pdate: *const u8, nbyte: usize, value: u16) -> u16;
 170 }
 171
 172 impl crc16 {
 173     /// initialize for MSB CRC-16
 174     #[inline(always)]
 175     pub fn new_msb(poly: u16) -> Self {
 176         let mut ctx: Self = Self {
 177             table: [0; 0x100],
 178             eval: a_crc16m,
 179         };
 180         unsafe { a_crc16m_init(ctx.table.as_mut_ptr(), poly) };
 181         ctx
 182     }
 183     /// initialize for LSB CRC-16
 184     #[inline(always)]
 185     pub fn new_lsb(poly: u16) -> Self {
 186         let mut ctx: Self = Self {
 187             table: [0; 0x100],
 188             eval: a_crc16l,
 189         };
 190         unsafe { a_crc16l_init(ctx.table.as_mut_ptr(), poly) };
 191         ctx
 192     }
 193     /// generate for MSB CRC-16
 194     #[inline(always)]
 195     pub fn gen_msb(&mut self, poly: u16) -> &mut Self {
 196         unsafe { a_crc16m_init(self.table.as_mut_ptr(), poly) };
 197         self.eval = a_crc16m;
 198         self
 199     }
 200     /// generate for LSB CRC-16
 201     #[inline(always)]
 202     pub fn gen_lsb(&mut self, poly: u16) -> &mut Self {
 203         unsafe { a_crc16l_init(self.table.as_mut_ptr(), poly) };
 204         self.eval = a_crc16l;
 205         self
 206     }
 207     /// calculate for CRC-16
 208     #[inline(always)]
 209     pub fn eval(self, block: &[u8], value: u16) -> u16 {
 210         unsafe { (self.eval)(self.table.as_ptr(), block.as_ptr(), block.len(), value) }
 211     }
 212 }
 213
 214 #[test]
 215 fn crc16() {
 216     extern crate std;
 217     {
 218         let ctx = crate::crc16::new_msb(0x1021);
 219         assert_eq!(ctx.eval(b"0123456789", 0), 0x9C58);
 220     }
 221     {
 222         let ctx = crate::crc16::new_lsb(0x8005);
 223         assert_eq!(ctx.eval(b"0123456789", 0), 0x443D);
 224     }
 225 }
 226
 227 /// Cyclic Redundancy Check for 32 bits
 228 #[repr(C)]
 229 pub struct crc32 {
 230     /// Cyclic Redundancy Check comparison table
 231     pub table: [u32; 0x100],
 232     eval: unsafe extern "C" fn(*const u32, *const u8, usize, u32) -> u32,
 233 }
 234
 235 extern "C" {
 236     fn a_crc32m_init(table: *mut u32, poly: u32);
 237     fn a_crc32l_init(table: *mut u32, poly: u32);
 238     fn a_crc32m(table: *const u32, pdate: *const u8, nbyte: usize, value: u32) -> u32;
 239     fn a_crc32l(table: *const u32, pdate: *const u8, nbyte: usize, value: u32) -> u32;
 240 }
 241
 242 impl crc32 {
 243     /// initialize for MSB CRC-32
 244     #[inline(always)]
 245     pub fn new_msb(poly: u32) -> Self {
 246         let mut ctx: Self = Self {
 247             table: [0; 0x100],
 248             eval: a_crc32m,
 249         };
 250         unsafe { a_crc32m_init(ctx.table.as_mut_ptr(), poly) };
 251         ctx
 252     }
 253     /// initialize for LSB CRC-32
 254     #[inline(always)]
 255     pub fn new_lsb(poly: u32) -> Self {
 256         let mut ctx: Self = Self {
 257             table: [0; 0x100],
 258             eval: a_crc32l,
 259         };
 260         unsafe { a_crc32l_init(ctx.table.as_mut_ptr(), poly) };
 261         ctx
 262     }
 263     /// generate for MSB CRC-32
 264     #[inline(always)]
 265     pub fn gen_msb(&mut self, poly: u32) -> &mut Self {
 266         unsafe { a_crc32m_init(self.table.as_mut_ptr(), poly) };
 267         self.eval = a_crc32m;
 268         self
 269     }
 270     /// generate for LSB CRC-32
 271     #[inline(always)]
 272     pub fn gen_lsb(&mut self, poly: u32) -> &mut Self {
 273         unsafe { a_crc32l_init(self.table.as_mut_ptr(), poly) };
 274         self.eval = a_crc32l;
 275         self
 276     }
 277     /// calculate for CRC-32
 278     #[inline(always)]
 279     pub fn eval(self, block: &[u8], value: u32) -> u32 {
 280         unsafe { (self.eval)(self.table.as_ptr(), block.as_ptr(), block.len(), value) }
 281     }
 282 }
 283
 284 #[test]
 285 fn crc32() {
 286     extern crate std;
 287     {
 288         let ctx = crate::crc32::new_msb(0x1EDC6F41);
 289         assert_eq!(ctx.eval(b"0123456789", 0), 0x512B456E);
 290     }
 291     {
 292         let ctx = crate::crc32::new_lsb(0x04C11DB7);
 293         assert_eq!(ctx.eval(b"0123456789", 0), 0x450EAFB0);
 294     }
 295 }
 296
 297 /// Cyclic Redundancy Check for 64 bits
 298 #[repr(C)]
 299 pub struct crc64 {
 300     /// Cyclic Redundancy Check comparison table
 301     pub table: [u64; 0x100],
 302     eval: unsafe extern "C" fn(*const u64, *const u8, usize, u64) -> u64,
 303 }
 304
 305 extern "C" {
 306     fn a_crc64m_init(table: *mut u64, poly: u64);
 307     fn a_crc64l_init(table: *mut u64, poly: u64);
 308     fn a_crc64m(table: *const u64, pdate: *const u8, nbyte: usize, value: u64) -> u64;
 309     fn a_crc64l(table: *const u64, pdate: *const u8, nbyte: usize, value: u64) -> u64;
 310 }
 311
 312 impl crc64 {
 313     /// initialize for MSB CRC-64
 314     #[inline(always)]
 315     pub fn new_msb(poly: u64) -> Self {
 316         let mut ctx: Self = Self {
 317             table: [0; 0x100],
 318             eval: a_crc64m,
 319         };
 320         unsafe { a_crc64m_init(ctx.table.as_mut_ptr(), poly) };
 321         ctx
 322     }
 323     /// initialize for LSB CRC-64
 324     #[inline(always)]
 325     pub fn new_lsb(poly: u64) -> Self {
 326         let mut ctx: Self = Self {
 327             table: [0; 0x100],
 328             eval: a_crc64l,
 329         };
 330         unsafe { a_crc64l_init(ctx.table.as_mut_ptr(), poly) };
 331         ctx
 332     }
 333     /// generate for MSB CRC-64
 334     #[inline(always)]
 335     pub fn gen_msb(&mut self, poly: u64) -> &mut Self {
 336         unsafe { a_crc64m_init(self.table.as_mut_ptr(), poly) };
 337         self.eval = a_crc64m;
 338         self
 339     }
 340     /// generate for LSB CRC-64
 341     #[inline(always)]
 342     pub fn gen_lsb(&mut self, poly: u64) -> &mut Self {
 343         unsafe { a_crc64l_init(self.table.as_mut_ptr(), poly) };
 344         self.eval = a_crc64l;
 345         self
 346     }
 347     /// calculate for CRC-64
 348     #[inline(always)]
 349     pub fn eval(self, block: &[u8], value: u64) -> u64 {
 350         unsafe { (self.eval)(self.table.as_ptr(), block.as_ptr(), block.len(), value) }
 351     }
 352 }
 353
 354 #[test]
 355 fn crc64() {
 356     extern crate std;
 357     {
 358         let ctx = crate::crc64::new_msb(0x000000000000001B);
 359         assert_eq!(ctx.eval(b"0123456789", 0), 0xE4FFBEA588AFC790);
 360     }
 361     {
 362         let ctx = crate::crc64::new_lsb(0x42F0E1EBA9EA3693);
 363         assert_eq!(ctx.eval(b"0123456789", 0), 0xDA60676A5CDE0008);
 364     }
 365 }
 366
 367 #[allow(clippy::excessive_precision)]
 368 const TAU: float = 6.28318530717958647692528676655900577;
 369
 370 /// High Pass Filter
 371 #[repr(C)]
 372 pub struct hpf {
 373     /// filter coefficient
 374     pub alpha: float,
 375     /// filter output
 376     pub output: float,
 377     /// filter input
 378     pub input: float,
 379 }
 380
 381 impl hpf {
 382     /// initialize for High Pass Filter
 383     #[inline(always)]
 384     pub fn new(fc: float, ts: float) -> Self {
 385         Self {
 386             alpha: 1.0 / (TAU * fc * ts + 1.0),
 387             output: 0.0,
 388             input: 0.0,
 389         }
 390     }
 391     /// generate for High Pass Filter
 392     #[inline(always)]
 393     pub fn gen(&mut self, fc: float, ts: float) -> &mut Self {
 394         self.alpha = 1.0 / (TAU * fc * ts + 1.0);
 395         self
 396     }
 397     /// calculate for High Pass Filter
 398     #[inline(always)]
 399     pub fn iter(&mut self, x: float) -> float {
 400         self.output = self.alpha * (self.output + x - self.input);
 401         self.input = x;
 402         self.output
 403     }
 404     /// zeroing for High Pass Filter
 405     #[inline(always)]
 406     pub fn zero(&mut self) -> &mut Self {
 407         self.output = 0.0;
 408         self.input = 0.0;
 409         self
 410     }
 411 }
 412
 413 #[test]
 414 fn hpf() {
 415     extern crate std;
 416     let mut a = crate::hpf::new(10.0, 0.01);
 417     a.gen(10.0, 0.01).zero().iter(1.0);
 418 }
 419
 420 #[allow(clippy::excessive_precision)]
 421 const _1_TAU: float = 0.159154943091895335768883763372514362;
 422
 423 /// Low Pass Filter
 424 #[repr(C)]
 425 pub struct lpf {
 426     /// filter coefficient
 427     pub alpha: float,
 428     /// filter output
 429     pub output: float,
 430 }
 431
 432 impl lpf {
 433     /// initialize for Low Pass Filter
 434     #[inline(always)]
 435     pub fn new(fc: float, ts: float) -> Self {
 436         Self {
 437             alpha: ts / (_1_TAU / fc + ts),
 438             output: 0.0,
 439         }
 440     }
 441     /// generate for Low Pass Filter
 442     #[inline(always)]
 443     pub fn gen(&mut self, fc: float, ts: float) -> &mut Self {
 444         self.alpha = ts / (_1_TAU / fc + ts);
 445         self
 446     }
 447     /// calculate for Low Pass Filter
 448     #[inline(always)]
 449     pub fn iter(&mut self, x: float) -> float {
 450         self.output *= 1.0 - self.alpha;
 451         self.output += x * self.alpha;
 452         self.output
 453     }
 454     /// zeroing for Low Pass Filter
 455     #[inline(always)]
 456     pub fn zero(&mut self) -> &mut Self {
 457         self.output = 0.0;
 458         self
 459     }
 460 }
 461
 462 #[test]
 463 fn lpf() {
 464     extern crate std;
 465     let mut a = crate::lpf::new(10.0, 0.01);
 466     a.gen(10.0, 0.01).zero().iter(1.0);
 467 }
 468
 469 /// membership function
 470 pub mod mf {
 471     use crate::int;
 472     /// none
 473     pub const NUL: int = 0;
 474     /// gaussian membership function
 475     pub const GAUSS: int = 1;
 476     /// gaussian combination membership function
 477     pub const GAUSS2: int = 2;
 478     /// generalized bell-shaped membership function
 479     pub const GBELL: int = 3;
 480     /// sigmoidal membership function
 481     pub const SIG: int = 4;
 482     /// difference between two sigmoidal membership functions
 483     pub const DSIG: int = 5;
 484     /// product of two sigmoidal membership functions
 485     pub const PSIG: int = 6;
 486     /// trapezoidal membership function
 487     pub const TRAP: int = 7;
 488     /// triangular membership function
 489     pub const TRI: int = 8;
 490     /// linear s-shaped saturation membership function
 491     pub const LINS: int = 9;
 492     /// linear z-shaped saturation membership function
 493     pub const LINZ: int = 10;
 494     /// s-shaped membership function
 495     pub const S: int = 11;
 496     /// z-shaped membership function
 497     pub const Z: int = 12;
 498     /// pi-shaped membership function
 499     pub const PI: int = 13;
 500
 501     use crate::float;
 502     extern "C" {
 503         fn a_mf_gauss(x: float, sigma: float, c: float) -> float;
 504         fn a_mf_gauss2(x: float, sigma1: float, c1: float, sigma2: float, c2: float) -> float;
 505         fn a_mf_gbell(x: float, a: float, b: float, c: float) -> float;
 506         fn a_mf_sig(x: float, a: float, c: float) -> float;
 507         fn a_mf_dsig(x: float, a1: float, c1: float, a2: float, c2: float) -> float;
 508         fn a_mf_psig(x: float, a1: float, c1: float, a2: float, c2: float) -> float;
 509         fn a_mf_trap(x: float, a: float, b: float, c: float, d: float) -> float;
 510         fn a_mf_tri(x: float, a: float, b: float, c: float) -> float;
 511         fn a_mf_lins(x: float, a: float, b: float) -> float;
 512         fn a_mf_linz(x: float, a: float, b: float) -> float;
 513         fn a_mf_s(x: float, a: float, b: float) -> float;
 514         fn a_mf_z(x: float, a: float, b: float) -> float;
 515         fn a_mf_pi(x: float, a: float, b: float, c: float, d: float) -> float;
 516     }
 517
 518     /// gaussian membership function
 519     #[inline(always)]
 520     pub fn gauss(x: float, sigma: float, c: float) -> float {
 521         unsafe { a_mf_gauss(x, sigma, c) }
 522     }
 523     /// gaussian combination membership function
 524     #[inline(always)]
 525     pub fn gauss2(x: float, sigma1: float, c1: float, sigma2: float, c2: float) -> float {
 526         unsafe { a_mf_gauss2(x, sigma1, c1, sigma2, c2) }
 527     }
 528     /// generalized bell-shaped membership function
 529     #[inline(always)]
 530     pub fn gbell(x: float, a: float, b: float, c: float) -> float {
 531         unsafe { a_mf_gbell(x, a, b, c) }
 532     }
 533     /// sigmoidal membership function
 534     #[inline(always)]
 535     pub fn sig(x: float, a: float, c: float) -> float {
 536         unsafe { a_mf_sig(x, a, c) }
 537     }
 538     /// difference between two sigmoidal membership functions
 539     #[inline(always)]
 540     pub fn dsig(x: float, a1: float, c1: float, a2: float, c2: float) -> float {
 541         unsafe { a_mf_dsig(x, a1, c1, a2, c2) }
 542     }
 543     /// product of two sigmoidal membership functions
 544     #[inline(always)]
 545     pub fn psig(x: float, a1: float, c1: float, a2: float, c2: float) -> float {
 546         unsafe { a_mf_psig(x, a1, c1, a2, c2) }
 547     }
 548     /// trapezoidal membership function
 549     #[inline(always)]
 550     pub fn trap(x: float, a: float, b: float, c: float, d: float) -> float {
 551         unsafe { a_mf_trap(x, a, b, c, d) }
 552     }
 553     /// triangular membership function
 554     #[inline(always)]
 555     pub fn tri(x: float, a: float, b: float, c: float) -> float {
 556         unsafe { a_mf_tri(x, a, b, c) }
 557     }
 558     /// linear s-shaped saturation membership function
 559     #[inline(always)]
 560     pub fn lins(x: float, a: float, b: float) -> float {
 561         unsafe { a_mf_lins(x, a, b) }
 562     }
 563     /// linear z-shaped saturation membership function
 564     #[inline(always)]
 565     pub fn linz(x: float, a: float, b: float) -> float {
 566         unsafe { a_mf_linz(x, a, b) }
 567     }
 568     /// s-shaped membership function
 569     #[inline(always)]
 570     pub fn s(x: float, a: float, b: float) -> float {
 571         unsafe { a_mf_s(x, a, b) }
 572     }
 573     /// z-shaped membership function
 574     #[inline(always)]
 575     pub fn z(x: float, a: float, b: float) -> float {
 576         unsafe { a_mf_z(x, a, b) }
 577     }
 578     /// pi-shaped membership function
 579     #[inline(always)]
 580     pub fn pi(x: float, a: float, b: float, c: float, d: float) -> float {
 581         unsafe { a_mf_pi(x, a, b, c, d) }
 582     }
 583
 584     #[cfg(test)]
 585     mod test {
 586         extern crate std;
 587         #[test]
 588         fn gauss2() {
 589             for i in -4..=4 {
 590                 let x = i as crate::float;
 591                 std::println!("{:+} {}", x, crate::mf::gauss2(x, 1.0, -1.0, 1.0, 1.0));
 592             }
 593         }
 594         #[test]
 595         fn gbell() {
 596             for i in -4..=4 {
 597                 let x = i as crate::float;
 598                 std::println!("{:+} {}", x, crate::mf::gbell(x, 2.0, 4.0, 0.0));
 599             }
 600         }
 601         #[test]
 602         fn sig() {
 603             for i in -4..=4 {
 604                 let x = i as crate::float;
 605                 std::println!("{:+} {}", x, crate::mf::sig(x, 2.0, 0.0));
 606             }
 607         }
 608         #[test]
 609         fn dsig() {
 610             for i in -4..=4 {
 611                 let x = i as crate::float;
 612                 std::println!("{:+} {}", x, crate::mf::dsig(x, 5.0, -2.0, 5.0, 2.0));
 613             }
 614         }
 615         #[test]
 616         fn psig() {
 617             for i in -4..=4 {
 618                 let x = i as crate::float;
 619                 std::println!("{:+} {}", x, crate::mf::psig(x, 5.0, -2.0, -5.0, 2.0));
 620             }
 621         }
 622         #[test]
 623         fn trap() {
 624             for i in -3..=3 {
 625                 let x = i as crate::float;
 626                 std::println!("{:+} {}", x, crate::mf::trap(x, -2.0, -1.0, 1.0, 2.0));
 627             }
 628         }
 629         #[test]
 630         fn tri() {
 631             for i in -2..=2 {
 632                 let x = i as crate::float;
 633                 std::println!("{:+} {}", x, crate::mf::tri(x, -1.0, 0.0, 1.0));
 634             }
 635         }
 636         #[test]
 637         fn lins() {
 638             for i in -2..=2 {
 639                 let x = i as crate::float;
 640                 std::println!("{:+} {}", x, crate::mf::lins(x, -1.0, 1.0));
 641             }
 642         }
 643         #[test]
 644         fn linz() {
 645             for i in -2..=2 {
 646                 let x = i as crate::float;
 647                 std::println!("{:+} {}", x, crate::mf::linz(x, -1.0, 1.0));
 648             }
 649         }
 650         #[test]
 651         fn s() {
 652             for i in -2..=2 {
 653                 let x = i as crate::float;
 654                 std::println!("{:+} {}", x, crate::mf::s(x, -1.0, 1.0));
 655             }
 656         }
 657         #[test]
 658         fn z() {
 659             for i in -2..=2 {
 660                 let x = i as crate::float;
 661                 std::println!("{:+} {}", x, crate::mf::z(x, -1.0, 1.0));
 662             }
 663         }
 664         #[test]
 665         fn pi() {
 666             for i in -3..=3 {
 667                 let x = i as crate::float;
 668                 std::println!("{:+} {}", x, crate::mf::pi(x, -2.0, -1.0, 1.0, 2.0));
 669             }
 670         }
 671     }
 672 }
 673
 674 /// proportional integral derivative controller
 675 #[repr(C)]
 676 pub struct pid {
 677     /// proportional constant
 678     pub kp: float,
 679     /// integral constant
 680     pub ki: float,
 681     /// derivative constant
 682     pub kd: float,
 683     /// maximum integral output
 684     pub summax: float,
 685     /// minimum integral output
 686     pub summin: float,
 687     /// controller integral output
 688     pub sum: float,
 689     /// maximum final output
 690     pub outmax: float,
 691     /// minimum final output
 692     pub outmin: float,
 693     /// controller final output
 694     pub out: float,
 695     /// cache variable
 696     pub(crate) var: float,
 697     /// cache feedback
 698     pub fdb: float,
 699     /// cache error
 700     pub err: float,
 701 }
 702
 703 impl Default for pid {
 704     #[inline(always)]
 705     fn default() -> Self {
 706         Self {
 707             kp: 0.0,
 708             ki: 0.0,
 709             kd: 0.0,
 710             summax: float::INFINITY,
 711             summin: -float::INFINITY,
 712             sum: 0.0,
 713             outmax: float::INFINITY,
 714             outmin: -float::INFINITY,
 715             out: 0.0,
 716             var: 0.0,
 717             fdb: 0.0,
 718             err: 0.0,
 719         }
 720     }
 721 }
 722
 723 extern "C" {
 724     fn a_pid_kpid(ctx: *mut pid, kp: float, ki: float, kd: float);
 725     fn a_pid_run(ctx: *mut pid, set: float, fdb: float) -> float;
 726     fn a_pid_pos(ctx: *mut pid, set: float, fdb: float) -> float;
 727     fn a_pid_inc(ctx: *mut pid, set: float, fdb: float) -> float;
 728     fn a_pid_zero(ctx: *mut pid);
 729 }
 730
 731 impl pid {
 732     /// initialize for PID controller
 733     #[inline(always)]
 734     pub fn new() -> Self {
 735         Self::default()
 736     }
 737     /// set proportional integral derivative constant for PID controller
 738     #[inline(always)]
 739     pub fn kpid(&mut self, kp: float, ki: float, kd: float) -> &mut Self {
 740         unsafe { a_pid_kpid(self, kp, ki, kd) };
 741         self
 742     }
 743     /// calculate for PID controller
 744     #[inline(always)]
 745     pub fn run(&mut self, set: float, fdb: float) -> float {
 746         unsafe { a_pid_run(self, set, fdb) }
 747     }
 748     /// calculate for positional PID controller
 749     #[inline(always)]
 750     pub fn pos(&mut self, set: float, fdb: float) -> float {
 751         unsafe { a_pid_pos(self, set, fdb) }
 752     }
 753     /// calculate for incremental PID controller
 754     #[inline(always)]
 755     pub fn inc(&mut self, set: float, fdb: float) -> float {
 756         unsafe { a_pid_inc(self, set, fdb) }
 757     }
 758     /// zeroing for PID controller
 759     #[inline(always)]
 760     pub fn zero(&mut self) -> &mut Self {
 761         unsafe { a_pid_zero(self) };
 762         self
 763     }
 764 }
 765
 766 #[test]
 767 fn pid() {
 768     extern crate std;
 769     let mut a = crate::pid::new();
 770     a.kpid(10.0, 0.1, 1.0);
 771     std::println!("{}", a.zero().pos(1.0, 0.0));
 772     std::println!("{}", a.zero().inc(1.0, 0.0));
 773 }
 774
 775 /// fuzzy PID controller operator
 776 pub mod fuzzy {
 777     use crate::uint;
 778     /// min(a,b)
 779     pub const CAP: uint = 1;
 780     /// a*b
 781     pub const CAP_ALGEBRA: uint = 2;
 782     /// max(a+b-1,0)
 783     pub const CAP_BOUNDED: uint = 3;
 784     /// max(a,b)
 785     pub const CUP: uint = 4;
 786     /// a+b-a*b
 787     pub const CUP_ALGEBRA: uint = 5;
 788     /// min(a+b,1)
 789     pub const CUP_BOUNDED: uint = 6;
 790     /// sqrt(a,b)*sqrt(1-(1-a)*(1-b))
 791     pub const EQU: uint = 0;
 792 }
 793
 794 #[macro_export]
 795 /// compute size of memory block for fuzzy PID controller
 796 macro_rules! PID_FUZZY_BLOCK {
 797     ($n:expr) => {{
 798         core::mem::size_of::<uint>() * $n * 2 + core::mem::size_of::<float>() * $n * (2 + $n)
 799     }};
 800 }
 801
 802 /// fuzzy proportional integral derivative controller
 803 #[repr(C)]
 804 pub struct pid_fuzzy {
 805     /// proportional integral derivative controller
 806     pub pid: pid,
 807     pub(crate) me: *const float,
 808     pub(crate) mec: *const float,
 809     pub(crate) mkp: *const float,
 810     pub(crate) mki: *const float,
 811     pub(crate) mkd: *const float,
 812     pub(crate) idx: *mut uint,
 813     pub(crate) val: *mut float,
 814     /// fuzzy relational operator
 815     pub(crate) op: extern "C" fn(float, float) -> float,
 816     /// base proportional constant
 817     pub kp: float,
 818     /// base integral constant
 819     pub ki: float,
 820     /// base derivative constant
 821     pub kd: float,
 822     /// number of order in the square matrix
 823     pub(crate) order: uint,
 824     /// maximum number triggered by the rule
 825     pub(crate) block: uint,
 826 }
 827
 828 impl Default for pid_fuzzy {
 829     #[inline(always)]
 830     fn default() -> Self {
 831         Self {
 832             pid: pid::default(),
 833             me: core::ptr::null(),
 834             mec: core::ptr::null(),
 835             mkp: core::ptr::null(),
 836             mki: core::ptr::null(),
 837             mkd: core::ptr::null(),
 838             idx: core::ptr::null_mut(),
 839             val: core::ptr::null_mut(),
 840             op: unsafe { a_pid_fuzzy_op(fuzzy::EQU) },
 841             kp: 0.0,
 842             ki: 0.0,
 843             kd: 0.0,
 844             order: 0,
 845             block: 0,
 846         }
 847     }
 848 }
 849
 850 extern "C" {
 851     fn a_pid_fuzzy_op(op: uint) -> extern "C" fn(float, float) -> float;
 852     fn a_pid_fuzzy_set_op(ctx: *mut pid_fuzzy, op: uint);
 853     fn a_pid_fuzzy_rule(
 854         ctx: *mut pid_fuzzy,
 855         num: uint,
 856         me: *const float,
 857         mec: *const float,
 858         mkp: *const float,
 859         mki: *const float,
 860         mkd: *const float,
 861     );
 862     fn a_pid_fuzzy_block(ctx: *mut pid_fuzzy) -> *mut u8;
 863     fn a_pid_fuzzy_set_block(ctx: *mut pid_fuzzy, ptr: *mut u8, num: usize);
 864     fn a_pid_fuzzy_kpid(ctx: *mut pid_fuzzy, kp: float, ki: float, kd: float);
 865     fn a_pid_fuzzy_run(ctx: *mut pid_fuzzy, set: float, fdb: float) -> float;
 866     fn a_pid_fuzzy_pos(ctx: *mut pid_fuzzy, set: float, fdb: float) -> float;
 867     fn a_pid_fuzzy_inc(ctx: *mut pid_fuzzy, set: float, fdb: float) -> float;
 868     fn a_pid_fuzzy_zero(ctx: *mut pid_fuzzy);
 869 }
 870
 871 impl pid_fuzzy {
 872     /// initialize for fuzzy PID controller
 873     #[inline(always)]
 874     pub fn new() -> Self {
 875         Self::default()
 876     }
 877     /// set rule base for fuzzy PID controller
 878     #[inline(always)]
 879     pub fn rule(
 880         &mut self,
 881         col: usize,
 882         me: &[float],
 883         mec: &[float],
 884         mkp: &[float],
 885         mki: &[float],
 886         mkd: &[float],
 887     ) -> &mut Self {
 888         unsafe {
 889             a_pid_fuzzy_rule(
 890                 self,
 891                 col as uint,
 892                 me.as_ptr(),
 893                 mec.as_ptr(),
 894                 mkp.as_ptr(),
 895                 mki.as_ptr(),
 896                 mkd.as_ptr(),
 897             )
 898         };
 899         self
 900     }
 901     /// set proportional integral derivative constant for fuzzy PID controller
 902     #[inline(always)]
 903     pub fn kpid(&mut self, kp: float, ki: float, kd: float) -> &mut Self {
 904         unsafe { a_pid_fuzzy_kpid(self, kp, ki, kd) };
 905         self
 906     }
 907     /// set memory block for fuzzy PID controller
 908     #[inline(always)]
 909     pub fn set_block(&mut self, ptr: &mut [u8], num: usize) -> &mut Self {
 910         unsafe { a_pid_fuzzy_set_block(self, ptr.as_mut_ptr(), num) };
 911         self
 912     }
 913     /// get memory block for fuzzy PID controller
 914     #[inline(always)]
 915     pub fn block(&mut self) -> &mut [u8] {
 916         unsafe {
 917             core::slice::from_raw_parts_mut(
 918                 a_pid_fuzzy_block(self),
 919                 PID_FUZZY_BLOCK!(self.block as usize),
 920             )
 921         }
 922     }
 923     /// set fuzzy relational operator for fuzzy PID controller
 924     #[inline(always)]
 925     pub fn op(&mut self, op: uint) -> &mut Self {
 926         unsafe { a_pid_fuzzy_set_op(self, op) };
 927         self
 928     }
 929     /// calculate for fuzzy PID controller
 930     #[inline(always)]
 931     pub fn run(&mut self, set: float, fdb: float) -> float {
 932         unsafe { a_pid_fuzzy_run(self, set, fdb) }
 933     }
 934     /// calculate for positional fuzzy PID controller
 935     #[inline(always)]
 936     pub fn pos(&mut self, set: float, fdb: float) -> float {
 937         unsafe { a_pid_fuzzy_pos(self, set, fdb) }
 938     }
 939     /// calculate for incremental fuzzy PID controller
 940     #[inline(always)]
 941     pub fn inc(&mut self, set: float, fdb: float) -> float {
 942         unsafe { a_pid_fuzzy_inc(self, set, fdb) }
 943     }
 944     /// zeroing for fuzzy PID controller
 945     #[inline(always)]
 946     pub fn zero(&mut self) -> &mut Self {
 947         unsafe { a_pid_fuzzy_zero(self) };
 948         self
 949     }
 950 }
 951
 952 #[test]
 953 #[allow(non_snake_case)]
 954 fn pid_fuzzy() {
 955     extern crate std;
 956     let NL: float = -3.0;
 957     let NM: float = -2.0;
 958     let NS: float = -1.0;
 959     let ZO: float = 0.0;
 960     let PS: float = 1.0;
 961     let PM: float = 2.0;
 962     let PL: float = 3.0;
 963     let me = [
 964         [crate::mf::TRI as float, NL, NL, NM],
 965         [crate::mf::TRI as float, NL, NM, NS],
 966         [crate::mf::TRI as float, NM, NS, ZO],
 967         [crate::mf::TRI as float, NS, ZO, PS],
 968         [crate::mf::TRI as float, ZO, PS, PM],
 969         [crate::mf::TRI as float, PS, PM, PL],
 970         [crate::mf::TRI as float, PM, PL, PL],
 971     ];
 972     let NL: float = -3.0;
 973     let NM: float = -2.0;
 974     let NS: float = -1.0;
 975     let ZO: float = 0.0;
 976     let PS: float = 1.0;
 977     let PM: float = 2.0;
 978     let PL: float = 3.0;
 979     let mec = [
 980         [crate::mf::TRI as float, NL, NL, NM],
 981         [crate::mf::TRI as float, NL, NM, NS],
 982         [crate::mf::TRI as float, NM, NS, ZO],
 983         [crate::mf::TRI as float, NS, ZO, PS],
 984         [crate::mf::TRI as float, ZO, PS, PM],
 985         [crate::mf::TRI as float, PS, PM, PL],
 986         [crate::mf::TRI as float, PM, PL, PL],
 987     ];
 988     let NL: float = -15.0;
 989     let NM: float = -10.0;
 990     let NS: float = -5.0;
 991     let ZO: float = 0.0;
 992     let PS: float = 5.0;
 993     let PM: float = 10.0;
 994     let PL: float = 15.0;
 995     let mkp = [
 996         [NL, NL, NM, NM, NS, ZO, ZO],
 997         [NL, NL, NM, NS, NS, ZO, PS],
 998         [NM, NM, NM, NS, ZO, PS, PS],
 999         [NM, NM, NS, ZO, PS, PM, PM],
1000         [NS, NS, ZO, PS, PS, PM, PM],
1001         [NS, ZO, PS, PM, PM, PM, PL],
1002         [ZO, ZO, PM, PM, PM, PL, PL],
1003     ];
1004     let NL: float = -3.0;
1005     let NM: float = -2.0;
1006     let NS: float = -1.0;
1007     let ZO: float = 0.0;
1008     let PS: float = 1.0;
1009     let PM: float = 2.0;
1010     let PL: float = 3.0;
1011     let mki = [
1012         [PL, PL, PM, PM, PS, ZO, ZO],
1013         [PL, PL, PM, PS, PS, ZO, ZO],
1014         [PL, PM, PS, PS, ZO, NS, NS],
1015         [PM, PM, PS, ZO, NS, NM, NM],
1016         [PM, PS, ZO, NS, NS, NM, NL],
1017         [ZO, ZO, NS, NS, NM, NL, NL],
1018         [ZO, ZO, NS, NM, NM, NL, NL],
1019     ];
1020     let mkd = [
1021         [NS, PS, PL, PL, PL, PM, NS],
1022         [NS, PS, PL, PM, PM, PS, ZO],
1023         [ZO, PS, PM, PM, PS, PS, ZO],
1024         [ZO, PS, PS, PS, PS, PS, ZO],
1025         [ZO, ZO, ZO, ZO, ZO, ZO, ZO],
1026         [NL, NS, NS, NS, NS, NS, NL],
1027         [NL, NM, NM, NM, NS, NS, NL],
1028     ];
1029     let mut block = [0u8; crate::PID_FUZZY_BLOCK!(2)];
1030     let mut a = pid_fuzzy::new();
1031     a.rule(
1032         me.len(),
1033         &me.concat(),
1034         &mec.concat(),
1035         &mkp.concat(),
1036         &mki.concat(),
1037         &mkd.concat(),
1038     )
1039     .kpid(10.0, 0.1, 1.0)
1040     .set_block(&mut block, 2);
1041     a.op(crate::fuzzy::EQU).zero();
1042     std::println!("{} {}", a.pos(1.0, 0.0), a.pos(1.0, 0.0));
1043     a.op(crate::fuzzy::EQU).zero();
1044     std::println!("{} {}", a.inc(1.0, 0.0), a.inc(1.0, 0.0));
1045 }
1046
1047 /// single neuron proportional integral derivative controller
1048 #[repr(C)]
1049 pub struct pid_neuro {
1050     /// proportional integral derivative controller
1051     pub pid: pid,
1052     /// proportional coefficient
1053     pub k: float,
1054     /// proportional weight
1055     pub wp: float,
1056     /// integral weight
1057     pub wi: float,
1058     /// derivative weight
1059     pub wd: float,
1060     /// error change
1061     pub ec: float,
1062 }
1063
1064 impl Default for pid_neuro {
1065     #[inline(always)]
1066     fn default() -> Self {
1067         Self {
1068             pid: pid::default(),
1069             k: 0.0,
1070             wp: 0.0,
1071             wi: 0.0,
1072             wd: 0.0,
1073             ec: 0.0,
1074         }
1075     }
1076 }
1077
1078 extern "C" {
1079     fn a_pid_neuro_kpid(ctx: *mut pid_neuro, k: float, kp: float, ki: float, kd: float);
1080     fn a_pid_neuro_wpid(ctx: *mut pid_neuro, wp: float, wi: float, wd: float);
1081     fn a_pid_neuro_run(ctx: *mut pid_neuro, set: float, fdb: float) -> float;
1082     fn a_pid_neuro_inc(ctx: *mut pid_neuro, set: float, fdb: float) -> float;
1083     fn a_pid_neuro_zero(ctx: *mut pid_neuro);
1084 }
1085
1086 impl pid_neuro {
1087     /// initialize for single neuron PID controller
1088     #[inline(always)]
1089     pub fn new() -> Self {
1090         Self::default()
1091     }
1092     /// set proportional integral derivative constant for single neuron PID controller
1093     #[inline(always)]
1094     pub fn kpid(&mut self, k: float, kp: float, ki: float, kd: float) -> &mut Self {
1095         unsafe { a_pid_neuro_kpid(self, k, kp, ki, kd) };
1096         self
1097     }
1098     /// set proportional integral derivative weight for single neuron PID controller
1099     #[inline(always)]
1100     pub fn wpid(&mut self, wp: float, wi: float, wd: float) -> &mut Self {
1101         unsafe { a_pid_neuro_wpid(self, wp, wi, wd) };
1102         self
1103     }
1104     /// calculate for single neuron PID controller
1105     #[inline(always)]
1106     pub fn run(&mut self, set: float, fdb: float) -> float {
1107         unsafe { a_pid_neuro_run(self, set, fdb) }
1108     }
1109     /// calculate for incremental single neuron PID controller
1110     #[inline(always)]
1111     pub fn inc(&mut self, set: float, fdb: float) -> float {
1112         unsafe { a_pid_neuro_inc(self, set, fdb) }
1113     }
1114     /// zeroing for single neuron PID controller
1115     #[inline(always)]
1116     pub fn zero(&mut self) -> &mut Self {
1117         unsafe { a_pid_neuro_zero(self) };
1118         self
1119     }
1120 }
1121
1122 #[test]
1123 fn pid_neuro() {
1124     extern crate std;
1125     let mut a = crate::pid_neuro::new();
1126     a.kpid(10.0, 1.0, 0.1, 1.0).wpid(1.0, 0.0, 0.0);
1127     std::println!("{}", a.inc(1.0, 0.0));
1128     a.zero();
1129 }
1130
1131 /// transfer function
1132 #[repr(C)]
1133 pub struct tf {
1134     /// input
1135     input: *mut float,
1136     /// output
1137     output: *mut float,
1138     /// numerator
1139     num_p: *const float,
1140     /// denominator
1141     den_p: *const float,
1142     /// numerator number
1143     num_n: uint,
1144     /// denominator number
1145     den_n: uint,
1146 }
1147
1148 extern "C" {
1149     fn a_tf_set_num(ctx: *mut tf, num_n: uint, num_p: *const float, input: *mut float);
1150     fn a_tf_set_den(ctx: *mut tf, den_n: uint, den_p: *const float, output: *mut float);
1151     fn a_tf_init(
1152         ctx: *mut tf,
1153         num_n: uint,
1154         num_p: *const float,
1155         input: *mut float,
1156         den_n: uint,
1157         den_p: *const float,
1158         output: *mut float,
1159     );
1160     fn a_tf_iter(ctx: *const tf, x: float) -> float;
1161     fn a_tf_zero(ctx: *const tf);
1162 }
1163
1164 impl tf {
1165     /// initialize for transfer function
1166     #[inline(always)]
1167     pub fn new(num: &[float], input: &mut [float], den: &[float], output: &mut [float]) -> Self {
1168         let mut ctx: Self = Self {
1169             input: core::ptr::null_mut(),
1170             output: core::ptr::null_mut(),
1171             num_p: core::ptr::null(),
1172             den_p: core::ptr::null(),
1173             num_n: 0,
1174             den_n: 0,
1175         };
1176         unsafe {
1177             a_tf_init(
1178                 &mut ctx,
1179                 num.len() as uint,
1180                 num.as_ptr(),
1181                 input.as_mut_ptr(),
1182                 den.len() as uint,
1183                 den.as_ptr(),
1184                 output.as_mut_ptr(),
1185             )
1186         };
1187         ctx
1188     }
1189     /// calculate for transfer function
1190     #[inline(always)]
1191     pub fn iter(&mut self, x: float) -> float {
1192         unsafe { a_tf_iter(self, x) }
1193     }
1194     /// zeroing for transfer function
1195     #[inline(always)]
1196     pub fn zero(&mut self) -> &mut Self {
1197         unsafe { a_tf_zero(self) };
1198         self
1199     }
1200     /// get input for transfer function
1201     #[inline(always)]
1202     pub fn input(&self) -> &[float] {
1203         unsafe { core::slice::from_raw_parts(self.input, self.num_n as usize) }
1204     }
1205     /// get numerator for transfer function
1206     #[inline(always)]
1207     pub fn num(&self) -> &[float] {
1208         unsafe { core::slice::from_raw_parts(self.num_p, self.num_n as usize) }
1209     }
1210     /// set numerator for transfer function
1211     #[inline(always)]
1212     pub fn set_num(&mut self, num: &[float], input: &mut [float]) -> &mut Self {
1213         unsafe { a_tf_set_num(self, num.len() as uint, num.as_ptr(), input.as_mut_ptr()) };
1214         self
1215     }
1216     /// get output for transfer function
1217     #[inline(always)]
1218     pub fn output(&self) -> &[float] {
1219         unsafe { core::slice::from_raw_parts(self.output, self.den_n as usize) }
1220     }
1221     /// get denominator for transfer function
1222     #[inline(always)]
1223     pub fn den(&self) -> &[float] {
1224         unsafe { core::slice::from_raw_parts(self.den_p, self.den_n as usize) }
1225     }
1226     /// set denominator for transfer function
1227     #[inline(always)]
1228     pub fn set_den(&mut self, den: &[float], output: &mut [float]) -> &mut Self {
1229         unsafe { a_tf_set_den(self, den.len() as uint, den.as_ptr(), output.as_mut_ptr()) };
1230         self
1231     }
1232 }
1233
1234 #[test]
1235 fn tf() {
1236     extern crate std;
1237     let num = [6.59492796e-05, 6.54019884e-05];
1238     let den = [-1.97530991, 0.97530991];
1239     let mut input = [0.0; 2];
1240     let mut output = [0.0; 2];
1241     let mut a = crate::tf::new(&num, &mut input, &den, &mut output);
1242     a.set_num(&num, &mut input).set_den(&den, &mut output);
1243     std::println!("{} {}", a.iter(10.0), a.iter(10.0));
1244     std::println!("{:?} {:?}", a.num(), a.input());
1245     std::println!("{:?} {:?}", a.den(), a.output());
1246     a.zero();
1247 }
1248
1249 /// bell-shaped velocity trajectory
1250 #[repr(C)]
1251 pub struct trajbell {
1252     /// total duration
1253     pub t: float,
1254     /// constant velocity phase
1255     pub tv: float,
1256     /// acceleration phase
1257     pub ta: float,
1258     /// deceleration phase
1259     pub td: float,
1260     /// time-interval in which the jerk is constant (j max or j min ) during the acceleration phase
1261     pub taj: float,
1262     /// time-interval in which the jerk is constant (j max or j min ) during the deceleration phase
1263     pub tdj: float,
1264     /// initial position
1265     pub p0: float,
1266     /// final position
1267     pub p1: float,
1268     /// initial velocity
1269     pub v0: float,
1270     /// final velocity
1271     pub v1: float,
1272     /// maximum velocity
1273     pub vm: float,
1274     /// maximum jerk
1275     pub jm: float,
1276     /// maximum acceleration
1277     pub am: float,
1278     /// maximum deceleration
1279     pub dm: float,
1280 }
1281
1282 impl Default for trajbell {
1283     #[inline(always)]
1284     fn default() -> Self {
1285         Self {
1286             t: 0.0,
1287             tv: 0.0,
1288             ta: 0.0,
1289             td: 0.0,
1290             taj: 0.0,
1291             tdj: 0.0,
1292             p0: 0.0,
1293             p1: 0.0,
1294             v0: 0.0,
1295             v1: 0.0,
1296             vm: 0.0,
1297             jm: 0.0,
1298             am: 0.0,
1299             dm: 0.0,
1300         }
1301     }
1302 }
1303
1304 extern "C" {
1305     fn a_trajbell_gen(
1306         ctx: *mut trajbell,
1307         jm: float,
1308         am: float,
1309         vm: float,
1310         p0: float,
1311         p1: float,
1312         v0: float,
1313         v1: float,
1314     ) -> float;
1315     fn a_trajbell_pos(ctx: *const trajbell, dt: float) -> float;
1316     fn a_trajbell_vel(ctx: *const trajbell, dt: float) -> float;
1317     fn a_trajbell_acc(ctx: *const trajbell, dt: float) -> float;
1318     fn a_trajbell_jer(ctx: *const trajbell, dt: float) -> float;
1319 }
1320
1321 impl trajbell {
1322     /// initialize for bell-shaped velocity trajectory
1323     #[inline(always)]
1324     pub fn new() -> Self {
1325         Self::default()
1326     }
1327     /// generate for bell-shaped velocity trajectory
1328     #[allow(clippy::too_many_arguments)]
1329     #[inline(always)]
1330     pub fn gen(
1331         &mut self,
1332         jm: float,
1333         am: float,
1334         vm: float,
1335         p0: float,
1336         p1: float,
1337         v0: float,
1338         v1: float,
1339     ) -> float {
1340         unsafe { a_trajbell_gen(self, jm, am, vm, p0, p1, v0, v1) }
1341     }
1342     /// calculate position for bell-shaped velocity trajectory
1343     #[inline(always)]
1344     pub fn pos(&mut self, dt: float) -> float {
1345         unsafe { a_trajbell_pos(self, dt) }
1346     }
1347     /// calculate velocity for bell-shaped velocity trajectory
1348     #[inline(always)]
1349     pub fn vel(&mut self, dt: float) -> float {
1350         unsafe { a_trajbell_vel(self, dt) }
1351     }
1352     /// calculate acceleration for bell-shaped velocity trajectory
1353     #[inline(always)]
1354     pub fn acc(&mut self, dt: float) -> float {
1355         unsafe { a_trajbell_acc(self, dt) }
1356     }
1357     /// calculate jerk for bell-shaped velocity trajectory
1358     #[inline(always)]
1359     pub fn jer(&mut self, dt: float) -> float {
1360         unsafe { a_trajbell_jer(self, dt) }
1361     }
1362 }
1363
1364 #[test]
1365 fn trajbell() {
1366     extern crate std;
1367     let dt = 0.5;
1368     {
1369         let mut a = crate::trajbell::new();
1370         std::print!("{} ", a.gen(3.0, 2.0, 3.0, 0.0, 10.0, 0.0, 0.0));
1371         std::println!(
1372             "[{}, {}, {}, {}]",
1373             a.pos(dt),
1374             a.vel(dt),
1375             a.acc(dt),
1376             a.jer(dt)
1377         );
1378     }
1379 }
1380
1381 /// cubic polynomial trajectory
1382 #[repr(C)]
1383 pub struct trajpoly3 {
1384     /// coefficients of position
1385     pub p: [float; 4],
1386     /// coefficients of velocity
1387     pub v: [float; 3],
1388     /// coefficients of acceleration
1389     pub a: [float; 2],
1390 }
1391
1392 extern "C" {
1393     fn a_trajpoly3_gen(ctx: *mut trajpoly3, ts: float, p0: float, p1: float, v0: float, v1: float);
1394     fn a_trajpoly3_pos(ctx: *const trajpoly3, dt: float) -> float;
1395     fn a_trajpoly3_vel(ctx: *const trajpoly3, dt: float) -> float;
1396     fn a_trajpoly3_acc(ctx: *const trajpoly3, dt: float) -> float;
1397 }
1398
1399 impl trajpoly3 {
1400     /// initialize for cubic polynomial trajectory
1401     #[inline(always)]
1402     pub fn new(ts: float, p0: float, p1: float, v0: float, v1: float) -> Self {
1403         let mut ctx: Self = Self {
1404             p: [0.0; 4],
1405             v: [0.0; 3],
1406             a: [0.0; 2],
1407         };
1408         unsafe { a_trajpoly3_gen(&mut ctx, ts, p0, p1, v0, v1) };
1409         ctx
1410     }
1411     /// generate for cubic polynomial trajectory
1412     #[inline(always)]
1413     pub fn gen(&mut self, ts: float, p0: float, p1: float, v0: float, v1: float) -> &mut Self {
1414         unsafe { a_trajpoly3_gen(self, ts, p0, p1, v0, v1) };
1415         self
1416     }
1417     /// calculate position for cubic polynomial trajectory
1418     #[inline(always)]
1419     pub fn pos(&mut self, dt: float) -> float {
1420         unsafe { a_trajpoly3_pos(self, dt) }
1421     }
1422     /// calculate velocity for cubic polynomial trajectory
1423     #[inline(always)]
1424     pub fn vel(&mut self, dt: float) -> float {
1425         unsafe { a_trajpoly3_vel(self, dt) }
1426     }
1427     /// calculate acceleration for cubic polynomial trajectory
1428     #[inline(always)]
1429     pub fn acc(&mut self, dt: float) -> float {
1430         unsafe { a_trajpoly3_acc(self, dt) }
1431     }
1432 }
1433
1434 #[test]
1435 fn trajpoly3() {
1436     extern crate std;
1437     let dt = 0.5;
1438     {
1439         let mut a = crate::trajpoly3::new(1.0, 0.0, 1.0, 0.0, 1.0);
1440         std::println!("[{}, {}, {}]", a.pos(dt), a.vel(dt), a.acc(dt));
1441     }
1442 }
1443
1444 /// quintic polynomial trajectory
1445 #[repr(C)]
1446 pub struct trajpoly5 {
1447     /// coefficients of position
1448     pub p: [float; 6],
1449     /// coefficients of velocity
1450     pub v: [float; 5],
1451     /// coefficients of acceleration
1452     pub a: [float; 4],
1453 }
1454
1455 extern "C" {
1456     fn a_trajpoly5_gen(
1457         ctx: *mut trajpoly5,
1458         ts: float,
1459         p0: float,
1460         p1: float,
1461         v0: float,
1462         v1: float,
1463         a0: float,
1464         a1: float,
1465     );
1466     fn a_trajpoly5_pos(ctx: *const trajpoly5, dt: float) -> float;
1467     fn a_trajpoly5_vel(ctx: *const trajpoly5, dt: float) -> float;
1468     fn a_trajpoly5_acc(ctx: *const trajpoly5, dt: float) -> float;
1469 }
1470
1471 impl trajpoly5 {
1472     /// initialize for quintic polynomial trajectory
1473     #[allow(clippy::too_many_arguments)]
1474     #[inline(always)]
1475     pub fn new(
1476         ts: float,
1477         p0: float,
1478         p1: float,
1479         v0: float,
1480         v1: float,
1481         a0: float,
1482         a1: float,
1483     ) -> Self {
1484         let mut ctx: Self = Self {
1485             p: [0.0; 6],
1486             v: [0.0; 5],
1487             a: [0.0; 4],
1488         };
1489         unsafe { a_trajpoly5_gen(&mut ctx, ts, p0, p1, v0, v1, a0, a1) };
1490         ctx
1491     }
1492     /// generate for quintic polynomial trajectory
1493     #[allow(clippy::too_many_arguments)]
1494     #[inline(always)]
1495     pub fn gen(
1496         &mut self,
1497         ts: float,
1498         p0: float,
1499         p1: float,
1500         v0: float,
1501         v1: float,
1502         a0: float,
1503         a1: float,
1504     ) -> &mut Self {
1505         unsafe { a_trajpoly5_gen(self, ts, p0, p1, v0, v1, a0, a1) };
1506         self
1507     }
1508     /// calculate position for quintic polynomial trajectory
1509     #[inline(always)]
1510     pub fn pos(&mut self, dt: float) -> float {
1511         unsafe { a_trajpoly5_pos(self, dt) }
1512     }
1513     /// calculate velocity for quintic polynomial trajectory
1514     #[inline(always)]
1515     pub fn vel(&mut self, dt: float) -> float {
1516         unsafe { a_trajpoly5_vel(self, dt) }
1517     }
1518     /// calculate acceleration for quintic polynomial trajectory
1519     #[inline(always)]
1520     pub fn acc(&mut self, dt: float) -> float {
1521         unsafe { a_trajpoly5_acc(self, dt) }
1522     }
1523 }
1524
1525 #[test]
1526 fn trajpoly5() {
1527     extern crate std;
1528     let dt = 0.5;
1529     {
1530         let mut a = crate::trajpoly5::new(1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0);
1531         std::println!("[{}, {}, {}]", a.pos(dt), a.vel(dt), a.acc(dt));
1532     }
1533 }
1534
1535 /// hepta polynomial trajectory
1536 #[repr(C)]
1537 pub struct trajpoly7 {
1538     /// coefficients of position
1539     pub p: [float; 8],
1540     /// coefficients of velocity
1541     pub v: [float; 7],
1542     /// coefficients of acceleration
1543     pub a: [float; 6],
1544     /// coefficients of jerk
1545     pub j: [float; 5],
1546 }
1547
1548 extern "C" {
1549     fn a_trajpoly7_gen(
1550         ctx: *mut trajpoly7,
1551         ts: float,
1552         p0: float,
1553         p1: float,
1554         v0: float,
1555         v1: float,
1556         a0: float,
1557         a1: float,
1558         j0: float,
1559         j1: float,
1560     );
1561     fn a_trajpoly7_pos(ctx: *const trajpoly7, dt: float) -> float;
1562     fn a_trajpoly7_vel(ctx: *const trajpoly7, dt: float) -> float;
1563     fn a_trajpoly7_acc(ctx: *const trajpoly7, dt: float) -> float;
1564     fn a_trajpoly7_jer(ctx: *const trajpoly7, dt: float) -> float;
1565 }
1566
1567 impl trajpoly7 {
1568     /// initialize for hepta polynomial trajectory
1569     #[allow(clippy::too_many_arguments)]
1570     #[inline(always)]
1571     pub fn new(
1572         ts: float,
1573         p0: float,
1574         p1: float,
1575         v0: float,
1576         v1: float,
1577         a0: float,
1578         a1: float,
1579         j0: float,
1580         j1: float,
1581     ) -> Self {
1582         let mut ctx: Self = Self {
1583             p: [0.0; 8],
1584             v: [0.0; 7],
1585             a: [0.0; 6],
1586             j: [0.0; 5],
1587         };
1588         unsafe { a_trajpoly7_gen(&mut ctx, ts, p0, p1, v0, v1, a0, a1, j0, j1) };
1589         ctx
1590     }
1591     /// generate for hepta polynomial trajectory
1592     #[allow(clippy::too_many_arguments)]
1593     #[inline(always)]
1594     pub fn gen(
1595         &mut self,
1596         ts: float,
1597         p0: float,
1598         p1: float,
1599         v0: float,
1600         v1: float,
1601         a0: float,
1602         a1: float,
1603         j0: float,
1604         j1: float,
1605     ) -> &mut Self {
1606         unsafe { a_trajpoly7_gen(self, ts, p0, p1, v0, v1, a0, a1, j0, j1) };
1607         self
1608     }
1609     /// calculate position for hepta polynomial trajectory
1610     #[inline(always)]
1611     pub fn pos(&mut self, dt: float) -> float {
1612         unsafe { a_trajpoly7_pos(self, dt) }
1613     }
1614     /// calculate velocity for hepta polynomial trajectory
1615     #[inline(always)]
1616     pub fn vel(&mut self, dt: float) -> float {
1617         unsafe { a_trajpoly7_vel(self, dt) }
1618     }
1619     /// calculate acceleration for hepta polynomial trajectory
1620     #[inline(always)]
1621     pub fn acc(&mut self, dt: float) -> float {
1622         unsafe { a_trajpoly7_acc(self, dt) }
1623     }
1624     /// calculate jerk for hepta polynomial trajectory
1625     #[inline(always)]
1626     pub fn jer(&mut self, dt: float) -> float {
1627         unsafe { a_trajpoly7_jer(self, dt) }
1628     }
1629 }
1630
1631 #[test]
1632 fn trajpoly7() {
1633     extern crate std;
1634     let dt = 0.5;
1635     {
1636         let mut a = crate::trajpoly7::new(1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0);
1637         std::println!(
1638             "[{}, {}, {}, {}]",
1639             a.pos(dt),
1640             a.vel(dt),
1641             a.acc(dt),
1642             a.jer(dt)
1643         );
1644     }
1645 }
1646
1647 /// trapezoidal velocity trajectory
1648 #[repr(C)]
1649 pub struct trajtrap {
1650     /// total duration
1651     pub t: float,
1652     /// initial position
1653     pub p0: float,
1654     /// final position
1655     pub p1: float,
1656     /// initial velocity
1657     pub v0: float,
1658     /// final velocity
1659     pub v1: float,
1660     /// constant velocity
1661     pub vc: float,
1662     /// time before constant velocity
1663     pub ta: float,
1664     /// time after constant velocity
1665     pub td: float,
1666     /// position before constant velocity
1667     pub pa: float,
1668     /// position after constant velocity
1669     pub pd: float,
1670     /// acceleration before constant velocity
1671     pub ac: float,
1672     /// acceleration after constant velocity
1673     pub de: float,
1674 }
1675
1676 impl Default for trajtrap {
1677     #[inline(always)]
1678     fn default() -> Self {
1679         Self {
1680             t: 0.0,
1681             p0: 0.0,
1682             p1: 0.0,
1683             v0: 0.0,
1684             v1: 0.0,
1685             vc: 0.0,
1686             ta: 0.0,
1687             td: 0.0,
1688             pa: 0.0,
1689             pd: 0.0,
1690             ac: 0.0,
1691             de: 0.0,
1692         }
1693     }
1694 }
1695
1696 extern "C" {
1697     fn a_trajtrap_gen(
1698         ctx: *mut trajtrap,
1699         vm: float,
1700         ac: float,
1701         de: float,
1702         p0: float,
1703         p1: float,
1704         v0: float,
1705         v1: float,
1706     ) -> float;
1707     fn a_trajtrap_pos(ctx: *const trajtrap, dt: float) -> float;
1708     fn a_trajtrap_vel(ctx: *const trajtrap, dt: float) -> float;
1709     fn a_trajtrap_acc(ctx: *const trajtrap, dt: float) -> float;
1710 }
1711
1712 impl trajtrap {
1713     /// initialize for trapezoidal velocity trajectory
1714     #[inline(always)]
1715     pub fn new() -> Self {
1716         Self::default()
1717     }
1718     /// generate for trapezoidal velocity trajectory
1719     #[allow(clippy::too_many_arguments)]
1720     #[inline(always)]
1721     pub fn gen(
1722         &mut self,
1723         vm: float,
1724         ac: float,
1725         de: float,
1726         p0: float,
1727         p1: float,
1728         v0: float,
1729         v1: float,
1730     ) -> float {
1731         unsafe { a_trajtrap_gen(self, vm, ac, de, p0, p1, v0, v1) }
1732     }
1733     /// calculate position for trapezoidal velocity trajectory
1734     #[inline(always)]
1735     pub fn pos(&mut self, dt: float) -> float {
1736         unsafe { a_trajtrap_pos(self, dt) }
1737     }
1738     /// calculate velocity for trapezoidal velocity trajectory
1739     #[inline(always)]
1740     pub fn vel(&mut self, dt: float) -> float {
1741         unsafe { a_trajtrap_vel(self, dt) }
1742     }
1743     /// calculate acceleration for trapezoidal velocity trajectory
1744     #[inline(always)]
1745     pub fn acc(&mut self, dt: float) -> float {
1746         unsafe { a_trajtrap_acc(self, dt) }
1747     }
1748 }
1749
1750 #[test]
1751 fn trajtrap() {
1752     extern crate std;
1753     let dt = 0.5;
1754     {
1755         let mut a = crate::trajtrap::new();
1756         std::print!("{} ", a.gen(2.0, 2.0, -2.0, 0.0, 2.0, 0.0, 0.0));
1757         std::println!("[{}, {}, {}]", a.pos(dt), a.vel(dt), a.acc(dt));
1758     }
1759 }
1760
1761 /// version
1762 ///
1763 /// ## Examples
1764 ///
1765 /// ```
1766 /// println!(
1767 ///     "version {}.{}.{}+{}",
1768 ///     liba::version::major(),
1769 ///     liba::version::minor(),
1770 ///     liba::version::patch(),
1771 ///     liba::version::tweak()
1772 /// );
1773 /// ```
1774 #[repr(C)]
1775 pub struct version {
1776     /// major number
1777     pub major: uint,
1778     /// minor number
1779     pub minor: uint,
1780     /// third number
1781     pub third: uint,
1782     /// extra number
1783     pub extra: uint,
1784 }
1785
1786 extern "C" {
1787     static a_version_major: uint;
1788     static a_version_minor: uint;
1789     static a_version_patch: uint;
1790     static a_version_tweak: u32;
1791     fn a_version_check(major: uint, minor: uint, patch: uint) -> int;
1792     fn a_version_cmp(lhs: *const version, rhs: *const version) -> int;
1793     fn a_version_lt(lhs: *const version, rhs: *const version) -> bool;
1794     fn a_version_gt(lhs: *const version, rhs: *const version) -> bool;
1795     fn a_version_le(lhs: *const version, rhs: *const version) -> bool;
1796     fn a_version_ge(lhs: *const version, rhs: *const version) -> bool;
1797     fn a_version_eq(lhs: *const version, rhs: *const version) -> bool;
1798 }
1799
1800 impl version {
1801     /// initialize for version
1802     #[inline(always)]
1803     pub fn new(major: uint, minor: uint, third: uint) -> Self {
1804         Self {
1805             major,
1806             minor,
1807             third,
1808             extra: 0,
1809         }
1810     }
1811     /// algorithm library version check
1812     #[inline(always)]
1813     pub fn check(major: uint, minor: uint, patch: uint) -> int {
1814         unsafe { a_version_check(major, minor, patch) }
1815     }
1816     /// algorithm library version major
1817     #[inline(always)]
1818     pub fn major() -> uint {
1819         unsafe { a_version_major }
1820     }
1821     /// algorithm library version minor
1822     #[inline(always)]
1823     pub fn minor() -> uint {
1824         unsafe { a_version_minor }
1825     }
1826     /// algorithm library version patch
1827     #[inline(always)]
1828     pub fn patch() -> uint {
1829         unsafe { a_version_patch }
1830     }
1831     /// algorithm library version tweak
1832     #[inline(always)]
1833     pub fn tweak() -> u32 {
1834         unsafe { a_version_tweak }
1835     }
1836 }
1837
1838 impl PartialOrd for version {
1839     #[inline(always)]
1840     fn partial_cmp(&self, that: &version) -> Option<core::cmp::Ordering> {
1841         let ok: int = unsafe { a_version_cmp(self, that) };
1842         if ok > 0 {
1843             return Some(core::cmp::Ordering::Greater);
1844         }
1845         if ok < 0 {
1846             return Some(core::cmp::Ordering::Less);
1847         }
1848         Some(core::cmp::Ordering::Equal)
1849     }
1850     #[inline(always)]
1851     fn lt(&self, that: &version) -> bool {
1852         unsafe { a_version_lt(self, that) }
1853     }
1854     #[inline(always)]
1855     fn gt(&self, that: &version) -> bool {
1856         unsafe { a_version_gt(self, that) }
1857     }
1858     #[inline(always)]
1859     fn le(&self, that: &version) -> bool {
1860         unsafe { a_version_le(self, that) }
1861     }
1862     #[inline(always)]
1863     fn ge(&self, that: &version) -> bool {
1864         unsafe { a_version_ge(self, that) }
1865     }
1866 }
1867
1868 impl PartialEq for version {
1869     #[inline(always)]
1870     fn eq(&self, that: &version) -> bool {
1871         unsafe { a_version_eq(self, that) }
1872     }
1873 }
1874
1875 #[test]
1876 fn version() {
1877     extern crate std;
1878     impl core::fmt::Debug for version {
1879         fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
1880             f.debug_struct("version")
1881                 .field("major", &self.major)
1882                 .field("minor", &self.minor)
1883                 .field("third", &self.third)
1884                 .field("extra", &self.extra)
1885                 .finish()
1886         }
1887     }
1888     std::println!("{}", crate::version::check(0, 0, 0));
1889     std::println!("major {}", crate::version::major());
1890     std::println!("minor {}", crate::version::minor());
1891     std::println!("patch {}", crate::version::patch());
1892     std::println!("tweak {}", crate::version::tweak());
1893     let v000 = version::new(0, 0, 0);
1894     let v001 = version::new(0, 0, 1);
1895     std::println!("{:?}", v001);
1896     assert!(v001 > v000);
1897     assert!(v000 < v001);
1898     assert!(v000 >= v000);
1899     assert!(v000 <= v000);
1900     assert!(v000 == v000);
1901     assert!(v000 != v001);
1902 }