src/lib.rs

   1 /*!
   2 An algorithm library based on C/C++
   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, x: float) -> float;
1316     fn a_trajbell_vel(ctx: *const trajbell, x: float) -> float;
1317     fn a_trajbell_acc(ctx: *const trajbell, x: float) -> float;
1318     fn a_trajbell_jer(ctx: *const trajbell, x: 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, x: float) -> float {
1345         unsafe { a_trajbell_pos(self, x) }
1346     }
1347     /// calculate velocity for bell-shaped velocity trajectory
1348     #[inline(always)]
1349     pub fn vel(&mut self, x: float) -> float {
1350         unsafe { a_trajbell_vel(self, x) }
1351     }
1352     /// calculate acceleration for bell-shaped velocity trajectory
1353     #[inline(always)]
1354     pub fn acc(&mut self, x: float) -> float {
1355         unsafe { a_trajbell_acc(self, x) }
1356     }
1357     /// calculate jerk for bell-shaped velocity trajectory
1358     #[inline(always)]
1359     pub fn jer(&mut self, x: float) -> float {
1360         unsafe { a_trajbell_jer(self, x) }
1361     }
1362 }
1363
1364 #[test]
1365 fn trajbell() {
1366     extern crate std;
1367     let x = 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!("[{}, {}, {}, {}]", a.pos(x), a.vel(x), a.acc(x), a.jer(x));
1372     }
1373 }
1374
1375 /// cubic polynomial trajectory
1376 #[repr(C)]
1377 pub struct trajpoly3 {
1378     /// coefficients of position
1379     pub p: [float; 4],
1380     /// coefficients of velocity
1381     pub v: [float; 3],
1382     /// coefficients of acceleration
1383     pub a: [float; 2],
1384 }
1385
1386 extern "C" {
1387     fn a_trajpoly3_gen(ctx: *mut trajpoly3, ts: float, p0: float, p1: float, v0: float, v1: float);
1388     fn a_trajpoly3_pos(ctx: *const trajpoly3, x: float) -> float;
1389     fn a_trajpoly3_vel(ctx: *const trajpoly3, x: float) -> float;
1390     fn a_trajpoly3_acc(ctx: *const trajpoly3, x: float) -> float;
1391 }
1392
1393 impl trajpoly3 {
1394     /// initialize for cubic polynomial trajectory
1395     #[inline(always)]
1396     pub fn new(ts: float, p0: float, p1: float, v0: float, v1: float) -> Self {
1397         let mut ctx: Self = Self {
1398             p: [0.0; 4],
1399             v: [0.0; 3],
1400             a: [0.0; 2],
1401         };
1402         unsafe { a_trajpoly3_gen(&mut ctx, ts, p0, p1, v0, v1) };
1403         ctx
1404     }
1405     /// generate for cubic polynomial trajectory
1406     #[inline(always)]
1407     pub fn gen(&mut self, ts: float, p0: float, p1: float, v0: float, v1: float) -> &mut Self {
1408         unsafe { a_trajpoly3_gen(self, ts, p0, p1, v0, v1) };
1409         self
1410     }
1411     /// calculate position for cubic polynomial trajectory
1412     #[inline(always)]
1413     pub fn pos(&mut self, x: float) -> float {
1414         unsafe { a_trajpoly3_pos(self, x) }
1415     }
1416     /// calculate velocity for cubic polynomial trajectory
1417     #[inline(always)]
1418     pub fn vel(&mut self, x: float) -> float {
1419         unsafe { a_trajpoly3_vel(self, x) }
1420     }
1421     /// calculate acceleration for cubic polynomial trajectory
1422     #[inline(always)]
1423     pub fn acc(&mut self, x: float) -> float {
1424         unsafe { a_trajpoly3_acc(self, x) }
1425     }
1426 }
1427
1428 #[test]
1429 fn trajpoly3() {
1430     extern crate std;
1431     let x = 0.5;
1432     {
1433         let mut a = crate::trajpoly3::new(1.0, 0.0, 1.0, 0.0, 1.0);
1434         std::println!("[{}, {}, {}]", a.pos(x), a.vel(x), a.acc(x));
1435     }
1436 }
1437
1438 /// quintic polynomial trajectory
1439 #[repr(C)]
1440 pub struct trajpoly5 {
1441     /// coefficients of position
1442     pub p: [float; 6],
1443     /// coefficients of velocity
1444     pub v: [float; 5],
1445     /// coefficients of acceleration
1446     pub a: [float; 4],
1447 }
1448
1449 extern "C" {
1450     fn a_trajpoly5_gen(
1451         ctx: *mut trajpoly5,
1452         ts: float,
1453         p0: float,
1454         p1: float,
1455         v0: float,
1456         v1: float,
1457         a0: float,
1458         a1: float,
1459     );
1460     fn a_trajpoly5_pos(ctx: *const trajpoly5, x: float) -> float;
1461     fn a_trajpoly5_vel(ctx: *const trajpoly5, x: float) -> float;
1462     fn a_trajpoly5_acc(ctx: *const trajpoly5, x: float) -> float;
1463 }
1464
1465 impl trajpoly5 {
1466     /// initialize for quintic polynomial trajectory
1467     #[allow(clippy::too_many_arguments)]
1468     #[inline(always)]
1469     pub fn new(
1470         ts: float,
1471         p0: float,
1472         p1: float,
1473         v0: float,
1474         v1: float,
1475         a0: float,
1476         a1: float,
1477     ) -> Self {
1478         let mut ctx: Self = Self {
1479             p: [0.0; 6],
1480             v: [0.0; 5],
1481             a: [0.0; 4],
1482         };
1483         unsafe { a_trajpoly5_gen(&mut ctx, ts, p0, p1, v0, v1, a0, a1) };
1484         ctx
1485     }
1486     /// generate for quintic polynomial trajectory
1487     #[allow(clippy::too_many_arguments)]
1488     #[inline(always)]
1489     pub fn gen(
1490         &mut self,
1491         ts: float,
1492         p0: float,
1493         p1: float,
1494         v0: float,
1495         v1: float,
1496         a0: float,
1497         a1: float,
1498     ) -> &mut Self {
1499         unsafe { a_trajpoly5_gen(self, ts, p0, p1, v0, v1, a0, a1) };
1500         self
1501     }
1502     /// calculate position for quintic polynomial trajectory
1503     #[inline(always)]
1504     pub fn pos(&mut self, x: float) -> float {
1505         unsafe { a_trajpoly5_pos(self, x) }
1506     }
1507     /// calculate velocity for quintic polynomial trajectory
1508     #[inline(always)]
1509     pub fn vel(&mut self, x: float) -> float {
1510         unsafe { a_trajpoly5_vel(self, x) }
1511     }
1512     /// calculate acceleration for quintic polynomial trajectory
1513     #[inline(always)]
1514     pub fn acc(&mut self, x: float) -> float {
1515         unsafe { a_trajpoly5_acc(self, x) }
1516     }
1517 }
1518
1519 #[test]
1520 fn trajpoly5() {
1521     extern crate std;
1522     let x = 0.5;
1523     {
1524         let mut a = crate::trajpoly5::new(1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0);
1525         std::println!("[{}, {}, {}]", a.pos(x), a.vel(x), a.acc(x));
1526     }
1527 }
1528
1529 /// hepta polynomial trajectory
1530 #[repr(C)]
1531 pub struct trajpoly7 {
1532     /// coefficients of position
1533     pub p: [float; 8],
1534     /// coefficients of velocity
1535     pub v: [float; 7],
1536     /// coefficients of acceleration
1537     pub a: [float; 6],
1538     /// coefficients of jerk
1539     pub j: [float; 5],
1540 }
1541
1542 extern "C" {
1543     fn a_trajpoly7_gen(
1544         ctx: *mut trajpoly7,
1545         ts: float,
1546         p0: float,
1547         p1: float,
1548         v0: float,
1549         v1: float,
1550         a0: float,
1551         a1: float,
1552         j0: float,
1553         j1: float,
1554     );
1555     fn a_trajpoly7_pos(ctx: *const trajpoly7, x: float) -> float;
1556     fn a_trajpoly7_vel(ctx: *const trajpoly7, x: float) -> float;
1557     fn a_trajpoly7_acc(ctx: *const trajpoly7, x: float) -> float;
1558     fn a_trajpoly7_jer(ctx: *const trajpoly7, x: float) -> float;
1559 }
1560
1561 impl trajpoly7 {
1562     /// initialize for hepta polynomial trajectory
1563     #[allow(clippy::too_many_arguments)]
1564     #[inline(always)]
1565     pub fn new(
1566         ts: float,
1567         p0: float,
1568         p1: float,
1569         v0: float,
1570         v1: float,
1571         a0: float,
1572         a1: float,
1573         j0: float,
1574         j1: float,
1575     ) -> Self {
1576         let mut ctx: Self = Self {
1577             p: [0.0; 8],
1578             v: [0.0; 7],
1579             a: [0.0; 6],
1580             j: [0.0; 5],
1581         };
1582         unsafe { a_trajpoly7_gen(&mut ctx, ts, p0, p1, v0, v1, a0, a1, j0, j1) };
1583         ctx
1584     }
1585     /// generate for hepta polynomial trajectory
1586     #[allow(clippy::too_many_arguments)]
1587     #[inline(always)]
1588     pub fn gen(
1589         &mut self,
1590         ts: float,
1591         p0: float,
1592         p1: float,
1593         v0: float,
1594         v1: float,
1595         a0: float,
1596         a1: float,
1597         j0: float,
1598         j1: float,
1599     ) -> &mut Self {
1600         unsafe { a_trajpoly7_gen(self, ts, p0, p1, v0, v1, a0, a1, j0, j1) };
1601         self
1602     }
1603     /// calculate position for hepta polynomial trajectory
1604     #[inline(always)]
1605     pub fn pos(&mut self, x: float) -> float {
1606         unsafe { a_trajpoly7_pos(self, x) }
1607     }
1608     /// calculate velocity for hepta polynomial trajectory
1609     #[inline(always)]
1610     pub fn vel(&mut self, x: float) -> float {
1611         unsafe { a_trajpoly7_vel(self, x) }
1612     }
1613     /// calculate acceleration for hepta polynomial trajectory
1614     #[inline(always)]
1615     pub fn acc(&mut self, x: float) -> float {
1616         unsafe { a_trajpoly7_acc(self, x) }
1617     }
1618     /// calculate jerk for hepta polynomial trajectory
1619     #[inline(always)]
1620     pub fn jer(&mut self, x: float) -> float {
1621         unsafe { a_trajpoly7_jer(self, x) }
1622     }
1623 }
1624
1625 #[test]
1626 fn trajpoly7() {
1627     extern crate std;
1628     let x = 0.5;
1629     {
1630         let mut a = crate::trajpoly7::new(1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0);
1631         std::println!("[{}, {}, {}, {}]", a.pos(x), a.vel(x), a.acc(x), a.jer(x));
1632     }
1633 }
1634
1635 /// trapezoidal velocity trajectory
1636 #[repr(C)]
1637 pub struct trajtrap {
1638     /// total duration
1639     pub t: float,
1640     /// initial position
1641     pub p0: float,
1642     /// final position
1643     pub p1: float,
1644     /// initial velocity
1645     pub v0: float,
1646     /// final velocity
1647     pub v1: float,
1648     /// constant velocity
1649     pub vc: float,
1650     /// time before constant velocity
1651     pub ta: float,
1652     /// time after constant velocity
1653     pub td: float,
1654     /// position before constant velocity
1655     pub pa: float,
1656     /// position after constant velocity
1657     pub pd: float,
1658     /// acceleration before constant velocity
1659     pub ac: float,
1660     /// acceleration after constant velocity
1661     pub de: float,
1662 }
1663
1664 impl Default for trajtrap {
1665     #[inline(always)]
1666     fn default() -> Self {
1667         Self {
1668             t: 0.0,
1669             p0: 0.0,
1670             p1: 0.0,
1671             v0: 0.0,
1672             v1: 0.0,
1673             vc: 0.0,
1674             ta: 0.0,
1675             td: 0.0,
1676             pa: 0.0,
1677             pd: 0.0,
1678             ac: 0.0,
1679             de: 0.0,
1680         }
1681     }
1682 }
1683
1684 extern "C" {
1685     fn a_trajtrap_gen(
1686         ctx: *mut trajtrap,
1687         vm: float,
1688         ac: float,
1689         de: float,
1690         p0: float,
1691         p1: float,
1692         v0: float,
1693         v1: float,
1694     ) -> float;
1695     fn a_trajtrap_pos(ctx: *const trajtrap, x: float) -> float;
1696     fn a_trajtrap_vel(ctx: *const trajtrap, x: float) -> float;
1697     fn a_trajtrap_acc(ctx: *const trajtrap, x: float) -> float;
1698 }
1699
1700 impl trajtrap {
1701     /// initialize for trapezoidal velocity trajectory
1702     #[inline(always)]
1703     pub fn new() -> Self {
1704         Self::default()
1705     }
1706     /// generate for trapezoidal velocity trajectory
1707     #[allow(clippy::too_many_arguments)]
1708     #[inline(always)]
1709     pub fn gen(
1710         &mut self,
1711         vm: float,
1712         ac: float,
1713         de: float,
1714         p0: float,
1715         p1: float,
1716         v0: float,
1717         v1: float,
1718     ) -> float {
1719         unsafe { a_trajtrap_gen(self, vm, ac, de, p0, p1, v0, v1) }
1720     }
1721     /// calculate position for trapezoidal velocity trajectory
1722     #[inline(always)]
1723     pub fn pos(&mut self, x: float) -> float {
1724         unsafe { a_trajtrap_pos(self, x) }
1725     }
1726     /// calculate velocity for trapezoidal velocity trajectory
1727     #[inline(always)]
1728     pub fn vel(&mut self, x: float) -> float {
1729         unsafe { a_trajtrap_vel(self, x) }
1730     }
1731     /// calculate acceleration for trapezoidal velocity trajectory
1732     #[inline(always)]
1733     pub fn acc(&mut self, x: float) -> float {
1734         unsafe { a_trajtrap_acc(self, x) }
1735     }
1736 }
1737
1738 #[test]
1739 fn trajtrap() {
1740     extern crate std;
1741     let x = 0.5;
1742     {
1743         let mut a = crate::trajtrap::new();
1744         std::print!("{} ", a.gen(2.0, 2.0, -2.0, 0.0, 2.0, 0.0, 0.0));
1745         std::println!("[{}, {}, {}]", a.pos(x), a.vel(x), a.acc(x));
1746     }
1747 }
1748
1749 /// version
1750 ///
1751 /// ## Examples
1752 ///
1753 /// ```
1754 /// println!(
1755 ///     "version {}.{}.{}+{}",
1756 ///     liba::version::major(),
1757 ///     liba::version::minor(),
1758 ///     liba::version::patch(),
1759 ///     liba::version::tweak()
1760 /// );
1761 /// ```
1762 #[repr(C)]
1763 pub struct version {
1764     /// major number
1765     pub major: uint,
1766     /// minor number
1767     pub minor: uint,
1768     /// third number
1769     pub third: uint,
1770     /// extra number
1771     pub extra: uint,
1772     /// alphabet
1773     pub alpha: [u8; 4],
1774 }
1775
1776 extern "C" {
1777     static a_version_major: uint;
1778     static a_version_minor: uint;
1779     static a_version_patch: uint;
1780     static a_version_tweak: u32;
1781     fn a_version_set_alpha(ctx: *mut version, alpha: *const u8);
1782     fn a_version_alpha(ctx: *const version, alpha: &mut [u8; 5]);
1783     fn a_version_parse(ctx: *mut version, ver: *const u8) -> uint;
1784     fn a_version_tostr(ctx: *const version, p: *mut u8, n: usize) -> int;
1785     fn a_version_cmp(ctx: *const version, rhs: *const version) -> int;
1786     fn a_version_lt(ctx: *const version, rhs: *const version) -> bool;
1787     fn a_version_gt(ctx: *const version, rhs: *const version) -> bool;
1788     fn a_version_le(ctx: *const version, rhs: *const version) -> bool;
1789     fn a_version_ge(ctx: *const version, rhs: *const version) -> bool;
1790     fn a_version_eq(ctx: *const version, rhs: *const version) -> bool;
1791     fn a_version_check(major: uint, minor: uint, patch: uint) -> int;
1792 }
1793
1794 impl version {
1795     /// initialize for version
1796     #[inline(always)]
1797     pub fn new(major: uint, minor: uint, third: uint) -> Self {
1798         Self {
1799             major,
1800             minor,
1801             third,
1802             extra: 0,
1803             alpha: [b'.', 0, 0, 0],
1804         }
1805     }
1806     /// set alphabet for version
1807     #[inline(always)]
1808     pub fn set_alpha(&mut self, alpha: &[u8]) {
1809         unsafe { a_version_set_alpha(self, alpha.as_ptr()) }
1810     }
1811     /// get alphabet for version
1812     #[inline(always)]
1813     pub fn alpha(&self, alpha: &mut [u8; 5]) {
1814         unsafe { a_version_alpha(self, alpha) }
1815     }
1816     /// parse version string to version
1817     #[inline(always)]
1818     pub fn parse(&mut self, ver: &str) -> uint {
1819         unsafe { a_version_parse(self, ver.as_ptr()) }
1820     }
1821     /// convert version to string
1822     #[inline(always)]
1823     pub fn tostr(&mut self, ver: &mut [u8]) -> int {
1824         unsafe { a_version_tostr(self, ver.as_mut_ptr(), ver.len()) }
1825     }
1826     /// algorithm library version check
1827     #[inline(always)]
1828     pub fn check(major: uint, minor: uint, patch: uint) -> int {
1829         unsafe { a_version_check(major, minor, patch) }
1830     }
1831     /// algorithm library version major
1832     #[inline(always)]
1833     pub fn major() -> uint {
1834         unsafe { a_version_major }
1835     }
1836     /// algorithm library version minor
1837     #[inline(always)]
1838     pub fn minor() -> uint {
1839         unsafe { a_version_minor }
1840     }
1841     /// algorithm library version patch
1842     #[inline(always)]
1843     pub fn patch() -> uint {
1844         unsafe { a_version_patch }
1845     }
1846     /// algorithm library version tweak
1847     #[inline(always)]
1848     pub fn tweak() -> u32 {
1849         unsafe { a_version_tweak }
1850     }
1851 }
1852
1853 impl PartialOrd for version {
1854     #[inline(always)]
1855     fn partial_cmp(&self, that: &version) -> Option<core::cmp::Ordering> {
1856         let ok: int = unsafe { a_version_cmp(self, that) };
1857         if ok > 0 {
1858             return Some(core::cmp::Ordering::Greater);
1859         }
1860         if ok < 0 {
1861             return Some(core::cmp::Ordering::Less);
1862         }
1863         Some(core::cmp::Ordering::Equal)
1864     }
1865     #[inline(always)]
1866     fn lt(&self, that: &version) -> bool {
1867         unsafe { a_version_lt(self, that) }
1868     }
1869     #[inline(always)]
1870     fn gt(&self, that: &version) -> bool {
1871         unsafe { a_version_gt(self, that) }
1872     }
1873     #[inline(always)]
1874     fn le(&self, that: &version) -> bool {
1875         unsafe { a_version_le(self, that) }
1876     }
1877     #[inline(always)]
1878     fn ge(&self, that: &version) -> bool {
1879         unsafe { a_version_ge(self, that) }
1880     }
1881 }
1882
1883 impl PartialEq for version {
1884     #[inline(always)]
1885     fn eq(&self, that: &version) -> bool {
1886         unsafe { a_version_eq(self, that) }
1887     }
1888 }
1889
1890 #[test]
1891 fn version() {
1892     extern crate std;
1893     impl core::fmt::Debug for version {
1894         fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
1895             let mut alpha = [0u8; 5];
1896             self.alpha(&mut alpha);
1897             f.debug_struct("version")
1898                 .field("major", &self.major)
1899                 .field("minor", &self.minor)
1900                 .field("third", &self.third)
1901                 .field("extra", &self.extra)
1902                 .field("alpha", &std::str::from_utf8(&alpha).unwrap())
1903                 .finish()
1904         }
1905     }
1906     std::println!("{}", crate::version::check(0, 0, 0));
1907     std::println!("major {}", crate::version::major());
1908     std::println!("minor {}", crate::version::minor());
1909     std::println!("patch {}", crate::version::patch());
1910     std::println!("tweak {}", crate::version::tweak());
1911     let v000 = version::new(0, 0, 0);
1912     let mut v001 = version::new(0, 0, 0);
1913     let mut ver = [0u8; 48];
1914     v001.parse("0.0.1-a.1");
1915     v001.tostr(&mut ver);
1916     std::println!("{:?}", v001);
1917     std::println!("{}", std::str::from_utf8(&ver).unwrap());
1918     assert!(v001 > v000);
1919     assert!(v000 < v001);
1920     assert!(v000 >= v000);
1921     assert!(v000 <= v000);
1922     assert!(v000 == v000);
1923     assert!(v000 != v001);
1924 }