rkepler 0.4.0

/*
Copyright (c) 2022 Peter Hristozov / Петър Христозов

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

//! # References
//! Lainey V., Dehant V., Pätzold M.: 2007, First numerical ephemerides of
//! the Martian moons, A&A 465, 1075-1084
//! # Link
//! <https://ftp.imcce.fr/pub/ephem/satel/martian/>

use crate::datetime::mjd;
use crate::datetime::mjd::J2000;
use crate::math::angle;
use crate::math::angle::norm_dblpi;
use crate::math::angle::DBLPI;
use crate::math::matrix::mat_r::*;
use crate::math::vector::vec_p;
use crate::math::vector::vec_pv;
use crate::pos_eph::equ_orbit::OrbElem;
use crate::BodyId;

const EL1AA: [f64; 33] = [
    0.00006269161880,
    0.00000000226877,
    0.00000000226877,
    0.00000000036560,
    0.00000000036560,
    0.00000000038928,
    0.00000000038928,
    0.00000000017647,
    0.00000000017647,
    0.00000000010473,
    0.00000000010473,
    0.00000000004570,
    0.00000000004570,
    0.00000000003102,
    0.00000000003102,
    0.00000000002648,
    0.00000000002648,
    0.00000000000906,
    0.00000000000906,
    0.00000000000782,
    0.00000000000782,
    0.00000000000768,
    0.00000000000768,
    0.00000000000652,
    0.00000000000652,
    0.00000000000831,
    0.00000000000831,
    0.00000000000664,
    0.00000000000664,
    0.00000000000428,
    0.00000000000428,
    0.00000000000362,
    0.00000000000362,
];
const EL1AP: [f64; 33] = [
    0.00000000,
    -0.23137652,
    0.23137652,
    0.31903311,
    -0.31903311,
    -0.15425102,
    0.15425102,
    0.46275305,
    -0.46275305,
    -0.13411243,
    0.13411243,
    0.11568826,
    -0.11568826,
    -0.84211324,
    0.84211324,
    0.10397809,
    -0.10397809,
    0.15952911,
    -0.15952911,
    0.10875357,
    -0.10875357,
    0.29864398,
    -0.29864398,
    -0.18884134,
    0.18884134,
    0.15951656,
    -0.15951656,
    0.84383237,
    -0.84383237,
    -0.15939353,
    0.15939353,
    -0.10628011,
    0.10628011,
];

const EL1AF: [f64; 33] = [
    0.000000000000,
    1.269694941721,
    -1.269694972385,
    0.683991069326,
    -0.683991048830,
    1.769154106463,
    -1.769154016711,
    -0.751833721919,
    0.751833534661,
    0.582878090440,
    -0.582877938274,
    -2.055123065092,
    2.055123107938,
    -1.167907572776,
    1.167907593259,
    -1.085910468998,
    1.085910738159,
    -1.123139908698,
    1.123139843925,
    2.223276867087,
    -2.223277592714,
    0.692161581064,
    -0.692161667059,
    0.067382573233,
    -0.067382536015,
    1.368231513583,
    -1.368231474439,
    0.260153915930,
    -0.260153924005,
    -0.059038385412,
    0.059038528330,
    0.103874462974,
    -0.103874462966,
];

const EL1LA: [f64; 57] = [
    2.09129739264173,
    0.00005016848130,
    0.00005016848130,
    0.00003350264970,
    0.00003350264903,
    0.00002827839093,
    0.00002827839093,
    0.00001578862428,
    0.00001578862427,
    0.00001504422160,
    0.00001504422160,
    0.00000817856317,
    0.00000817856334,
    0.00001022163922,
    0.00001022163922,
    0.00000651695172,
    0.00000651695172,
    0.00000506658494,
    0.00000506658494,
    0.00000374173438,
    0.00000374173438,
    0.00000258935939,
    0.00000258935939,
    0.00000112514742,
    0.00000112514742,
    0.00000076176630,
    0.00000076176630,
    0.00000049765123,
    0.00000049765123,
    0.00000068419668,
    0.00000068419668,
    0.00000056243429,
    0.00000056243429,
    0.00000039367057,
    0.00000039367057,
    0.00000052032286,
    0.00000052032286,
    0.00000035833947,
    0.00000035833947,
    0.00000030110114,
    0.00000030110114,
    0.00000019649769,
    0.00000019649645,
    0.00000024936539,
    0.00000024936539,
    0.00000024024140,
    0.00000024024140,
    0.00000017471331,
    0.00000017471331,
    0.00000017930767,
    0.00000017930768,
    0.00000013268776,
    0.00000013268776,
    0.00000012696364,
    0.00000012696364,
    0.00000012547154,
    0.00000012547155,
];

const EL1LP: [f64; 57] = [
    0.00000000,
    686.99369592,
    -686.99369241,
    -4077.46840757,
    4077.46821917,
    -343.48537930,
    343.48537949,
    826.22438660,
    -826.22438999,
    0.23137650,
    -0.23137650,
    2026.08615872,
    -2026.08614258,
    -0.31903309,
    0.31903309,
    228.99331848,
    -228.99331849,
    0.46275300,
    -0.46275300,
    0.15425100,
    -0.15425100,
    -242.61956612,
    242.61956592,
    -171.74597329,
    171.74597310,
    0.13411241,
    -0.13411241,
    -375.12500185,
    375.12500278,
    -413.26245099,
    413.26245103,
    179.29817450,
    -179.29817450,
    -1.02723078,
    1.02723078,
    -0.11568825,
    0.11568825,
    0.84211307,
    -0.84211307,
    -4.78104120,
    4.78104120,
    -8341.85985545,
    8341.72443362,
    0.15952910,
    -0.15952910,
    -0.10397808,
    0.10397808,
    -137.39750245,
    137.39750251,
    1035.39217901,
    -1035.39219300,
    0.15951654,
    -0.15951654,
    -0.18884132,
    0.18884132,
    -513.09805470,
    513.09805450,
];

const EL1LF: [f64; 57] = [
    -0.000000000014,
    -0.949015738908,
    0.949016158884,
    -0.380534526596,
    0.380533948496,
    0.994821588644,
    -0.994821497820,
    -2.870499589128,
    2.870499308383,
    -2.834185436330,
    2.834185466290,
    0.919444544403,
    -0.919444319278,
    0.895559796633,
    -0.895559859111,
    2.848996739890,
    -2.848996748767,
    -2.328774139340,
    2.328774139340,
    2.912659020395,
    -2.912659547077,
    2.215327142355,
    -2.215327333708,
    -0.377627161385,
    0.377626798556,
    -2.161248941800,
    2.161248590720,
    -1.231836210236,
    1.231836585135,
    -2.488083095032,
    2.488083105051,
    1.576141879515,
    -1.576141881804,
    0.137856626591,
    -0.137856591483,
    -2.626756099135,
    2.626756111094,
    2.759579592331,
    -2.759579510413,
    0.277512180092,
    -0.277511887506,
    -0.093530900566,
    0.093432654525,
    0.430292851332,
    -0.430292825369,
    2.695745393985,
    -2.695745301579,
    2.099637656810,
    -2.099637491253,
    -0.693740926999,
    0.693740188061,
    -0.220489234489,
    0.220488707822,
    1.654303228826,
    -1.654303208682,
    1.546722735399,
    -1.546722718402,
];

const EL1ZA: [f64; 28] = [
    0.01514110912521,
    0.00038496208674,
    0.00006903413242,
    0.00004946101994,
    0.00003671320788,
    0.00003267983782,
    0.00000875048305,
    0.00000617093881,
    0.00000695700594,
    0.00000573408896,
    0.00000343157786,
    0.00000403740951,
    0.00000211529622,
    0.00000190482762,
    0.00000084613232,
    0.00000086960947,
    0.00000063298682,
    0.00000061303178,
    0.00000057372171,
    0.00000081810883,
    0.00000074341453,
    0.00000054416478,
    0.00000050445150,
    0.00000042845991,
    0.00000042249373,
    0.00000035819498,
    0.00000038088812,
    0.00000040557392,
];

const EL1ZP: [f64; 28] = [
    827.21507332,
    0.31891012,
    587.39458533,
    -826.16172371,
    0.13409069,
    -0.84297139,
    0.15948580,
    1.02595676,
    0.10396503,
    316.64130873,
    0.18879824,
    -0.29875184,
    4054.54269012,
    -0.10629376,
    0.00000000,
    -0.12164857,
    686.47877752,
    375.32150909,
    -4052.07162766,
    0.23131182,
    216.74351242,
    -0.23144126,
    -0.13798985,
    -0.31920648,
    -0.18154621,
    242.67861994,
    1035.98671884,
    2043.26880788,
];
const EL1ZF: [f64; 28] = [
    1.404382124885,
    2.088385523034,
    -2.387807946031,
    0.498957209475,
    0.821521022172,
    -2.905108919700,
    2.772375719698,
    2.839303414769,
    0.318450317043,
    1.410329121503,
    1.336796551701,
    -2.429538561192,
    0.254706736621,
    1.508224230072,
    2.321836603108,
    0.034626075514,
    -3.084529992972,
    -1.119230636263,
    0.780393743699,
    -3.018597072641,
    -1.071855877472,
    2.691716763490,
    1.001733960626,
    -3.071035704337,
    -2.137527057619,
    -1.219306169725,
    2.558639707373,
    -0.990686281624,
];

const EL1ZETAA: [f64; 29] = [
    0.00940860518312,
    0.00007892894454,
    0.00005699538102,
    0.00002474369483,
    0.00002062715397,
    0.00000594557416,
    0.00000392019020,
    0.00000216779197,
    0.00000276976375,
    0.00000181353917,
    0.00000108342918,
    0.00000106665701,
    0.00000079098533,
    0.00000088808851,
    0.00000055207589,
    0.00000043978860,
    0.00000045836091,
    0.00000035331311,
    0.00000035029885,
    0.00000038288517,
    0.00000027112473,
    0.00000019917454,
    0.00000020368977,
    0.00000017267796,
    0.00000015744728,
    0.00000016985677,
    0.00000010150045,
    0.00000008259149,
    0.00000008504923,
];
const EL1ZETAP: [f64; 29] = [
    -826.20642714,
    0.00000000,
    -686.99749665,
    343.48630483,
    827.20268346,
    228.99362064,
    687.00755782,
    -343.48244903,
    -0.84297139,
    0.15942429,
    0.31891012,
    171.74558971,
    242.62779510,
    -4.75353965,
    1.02595676,
    0.13409069,
    -228.99359936,
    -374.99999997,
    4073.32789493,
    0.51266007,
    587.77857598,
    -242.62375723,
    179.29612278,
    0.23144134,
    -0.29875184,
    137.41607584,
    0.18879824,
    -0.31890996,
    -1028.57142835,
];
const EL1ZETAF: [f64; 29] = [
    2.058107128488,
    -2.386965958787,
    -3.034328817803,
    -1.725904319907,
    -0.169761953966,
    2.067112903892,
    1.700689207004,
    -2.706125953253,
    -1.342900415409,
    2.117885747855,
    0.517605536523,
    -0.418936524968,
    -3.042493138839,
    0.757075199434,
    0.660891079606,
    -0.763516173091,
    -0.439185212306,
    -1.680429911951,
    2.685649467156,
    -2.930008429370,
    2.556761413073,
    -1.660815588764,
    0.749769089433,
    -2.317979704428,
    -1.539326818938,
    -2.877315237045,
    0.374238084921,
    1.374678754422,
    1.140160118874,
];

const EL2AA: [f64; 50] = [
    0.00015681340867,
    0.00000000269918,
    0.00000000269918,
    0.00000000035476,
    0.00000000035476,
    0.00000000018689,
    0.00000000018689,
    0.00000000011687,
    0.00000000011687,
    0.00000000008485,
    0.00000000008485,
    0.00000000002657,
    0.00000000002657,
    0.00000000003523,
    0.00000000003523,
    0.00000000002117,
    0.00000000002117,
    0.00000000001407,
    0.00000000001407,
    0.00000000001275,
    0.00000000001275,
    0.00000000000838,
    0.00000000000838,
    0.00000000000486,
    0.00000000000486,
    0.00000000000476,
    0.00000000000476,
    0.00000000000487,
    0.00000000000487,
    0.00000000000514,
    0.00000000000514,
    0.00000000000334,
    0.00000000000334,
    0.00000000000344,
    0.00000000000344,
    0.00000000000310,
    0.00000000000310,
    0.00000000000268,
    0.00000000000268,
    0.00000000000168,
    0.00000000000168,
    0.00000000000146,
    0.00000000000146,
    0.00000000000149,
    0.00000000000149,
    0.00000000000132,
    0.00000000000132,
    0.00000000000102,
    0.00000000000102,
    0.00000000000109,
];

const EL2AP: [f64; 50] = [
    0.00000000,
    2.73847176,
    -2.73847176,
    0.63238251,
    -0.63238251,
    1.82564784,
    -1.82564784,
    -0.63296515,
    0.63296515,
    5.47694352,
    -5.47694352,
    0.63354887,
    -0.63354887,
    0.63122038,
    -0.63122038,
    0.63180092,
    -0.63180092,
    2.73884829,
    -2.73884829,
    -2.73809533,
    2.73809533,
    1.36923588,
    -1.36923588,
    -0.63118038,
    0.63118038,
    0.63064094,
    -0.63064094,
    -0.42669999,
    0.42669999,
    0.63413367,
    -0.63413367,
    2.34192368,
    -2.34192368,
    2.34247408,
    -2.34247408,
    0.63236243,
    -0.63236243,
    -1.26252066,
    1.26252066,
    0.63240258,
    -0.63240258,
    -1.82581518,
    1.82581518,
    2.34219905,
    -2.34219905,
    -1.82548053,
    1.82548053,
    0.63294504,
    -0.63294504,
    2.72759910,
];

const EL2AF: [f64; 50] = [
    0.000000000000,
    -0.136381775523,
    0.136381777698,
    -2.850380153005,
    2.850380089315,
    -1.926604205693,
    1.926604195461,
    0.367583823079,
    -0.367583824486,
    1.615243161942,
    -1.615243160951,
    2.114704903813,
    -2.114704908419,
    0.939338838730,
    -0.939338816854,
    -2.333690484620,
    2.333690544274,
    2.089322500989,
    -2.089322282950,
    -0.779557024035,
    0.779557327338,
    2.361851648242,
    -2.361851691087,
    -2.770026814512,
    2.770026952645,
    -1.549662975963,
    1.549663021137,
    3.024403974280,
    -3.024403881041,
    -1.686885659588,
    1.686885282544,
    3.057688429345,
    -3.057688457348,
    -0.829253165285,
    0.829253093148,
    -1.954686031809,
    1.954686103625,
    -2.168321203999,
    2.168321557125,
    2.516818643150,
    -2.516818534200,
    -0.299782666734,
    0.299782747411,
    2.143143410577,
    -2.143143430348,
    1.011403786810,
    -1.011403745572,
    0.538228537741,
    -0.538228903445,
    -2.653997343428,
];

const EL2LA: [f64; 30] = [
    1.91675212352556,
    0.00248303166019,
    0.00248303165949,
    0.00020234269782,
    0.00020234269782,
    0.00010431675642,
    0.00010431675643,
    0.00008185668873,
    0.00008185668873,
    0.00002410335710,
    0.00002410335710,
    0.00001497066023,
    0.00001497066034,
    0.00000568514429,
    0.00000568514429,
    0.00000417530493,
    0.00000417530493,
    0.00000438325450,
    0.00000438325450,
    0.00000415808821,
    0.00000415808821,
    0.00000322379379,
    0.00000322379379,
    0.00000396334370,
    0.00000396334370,
    0.00000390506421,
    0.00000311780183,
    0.00000390506421,
    0.00000311780183,
    0.00000165754846,
];
const EL2LP: [f64; 30] = [
    0.00000000,
    -19925.25479395,
    19925.25482649,
    -686.99361389,
    686.99361415,
    -343.48525280,
    343.48525251,
    2.73847175,
    -2.73847175,
    -228.99419233,
    228.99419233,
    -337.66341441,
    337.66341418,
    5.47694261,
    -5.47694261,
    1.82564754,
    -1.82564754,
    10149.06972719,
    -10149.06971679,
    171.76869151,
    -171.76869151,
    226.39849071,
    -226.39849071,
    -0.63238250,
    0.63238250,
    -711.69825336,
    -663.85314762,
    711.69825335,
    663.85314763,
    0.00000000,
];
const EL2LF: [f64; 30] = [
    -3.141592653540,
    -2.488175621789,
    2.488175634463,
    0.946296042215,
    -0.946295960274,
    1.001646985816,
    -1.001647348657,
    1.433490072350,
    -1.433489965321,
    -2.852007788402,
    2.852007788273,
    3.136008703144,
    -3.136009000145,
    -3.100668654449,
    3.100668633924,
    -0.366003419529,
    0.366003444013,
    -2.738313647435,
    2.738313658144,
    0.461419338359,
    -0.461419338359,
    0.675422268593,
    -0.675422268593,
    1.281544455534,
    -1.281544458794,
    1.797966683376,
    -0.222101391798,
    -1.797966684284,
    0.222101394466,
    -3.141592653523,
];

const EL2ZA: [f64; 17] = [
    0.00027441315346,
    0.00006015912711,
    0.00003534984782,
    0.00003614585349,
    0.00002577157903,
    0.00000680199783,
    0.00000445297914,
    0.00000223848292,
    0.00000092341709,
    0.00000147874757,
    0.00000145772004,
    0.00000134703031,
    0.00000075381791,
    0.00000087162297,
    0.00000101564888,
    0.00000050909893,
    0.00000065370775,
];
const EL2ZP: [f64; 17] = [
    19956.74479178,
    1.26244078,
    0.00000000,
    -19926.60456897,
    2.34219912,
    -1.26709782,
    4.09222912,
    -1.26943918,
    0.00000000,
    26995.13970336,
    16059.93520224,
    0.86409248,
    0.42132977,
    349.50296239,
    16.18594275,
    -1.27178920,
    -1.26244078,
];
const EL2ZF: [f64; 17] = [
    2.198649419514,
    -1.916548788202,
    2.345230773944,
    1.463816210370,
    1.362467588064,
    0.933679289332,
    -3.130753296830,
    -1.549133813261,
    0.599042274749,
    -0.546542453837,
    -1.198239348674,
    -2.050414656408,
    1.516439412109,
    3.100618466711,
    -1.127752616215,
    2.250768124831,
    -2.860933968311,
];

const EL2ZETAA: [f64; 28] = [
    0.01562693319959,
    0.00775101287565,
    0.00013218186311,
    0.00003833652719,
    0.00002660211842,
    0.00002882438535,
    0.00002713213911,
    0.00000929632495,
    0.00000446096029,
    0.00000490415603,
    0.00000236688115,
    0.00000205755285,
    0.00000236575014,
    0.00000122881444,
    0.00000121518946,
    0.00000096028570,
    0.00000117447177,
    0.00000105844105,
    0.00000238314749,
    0.00000076563038,
    0.00000040758243,
    0.00000049623409,
    0.00000030030779,
    0.00000026134164,
    0.00000028919452,
    0.00000022137882,
    0.00000024499503,
    0.00000016524166,
];
const EL2ZETAP: [f64; 28] = [
    -19916.19973129,
    0.00000000,
    343.48730623,
    -686.99215577,
    687.01233091,
    228.99391436,
    19892.53158003,
    337.66474671,
    -343.48211159,
    171.74656701,
    -664.09364734,
    226.39163557,
    711.55812043,
    -337.66025169,
    349.52237555,
    -10037.28889631,
    -228.99413227,
    664.12627842,
    37351.49200304,
    137.38646249,
    170.59316073,
    0.63120042,
    -226.50126587,
    -705.59895012,
    231.14050852,
    -171.66458272,
    0.63238270,
    692.28265851,
];
const EL2ZETAF: [f64; 28] = [
    2.981506933511,
    -2.386504179132,
    -1.725684412813,
    -3.035061242067,
    1.701551338710,
    2.067662003165,
    -1.477858084701,
    2.318788380092,
    -2.669604598165,
    -0.415374756507,
    -0.786669759362,
    -0.162822349334,
    -2.670796098338,
    -0.729022305245,
    -2.721699324043,
    1.941975361869,
    -0.437940530420,
    1.821048365911,
    0.758713545461,
    -2.990293668575,
    -2.517169198799,
    -0.535940350960,
    1.724062868741,
    -1.853437577537,
    0.865916190901,
    2.259603352363,
    1.108725025984,
    1.680391963791,
];
const L: [f64; 2] = [19.7020556283139, 4.9770138896523];
const ACC: [f64; 2] = [1.657852042683113E-10, -2.331793571572226E-14];
const MU: [f64; 2] = [3.2271557149889E-07, 3.2271556750209E-07];

///
/// # Arguments
/// # Returns
///
pub fn get_orb_elem(mjd_tdb: f64, body: &BodyId) -> OrbElem {
    let dt = mjd_tdb + 6491.5 - mjd::J2000;
    match body {
        BodyId::PHOBOS => OrbElem {
            mu: MU[0],
            mjd0: mjd_tdb,
            a: calc_cos_ser(dt, &EL1AA, &EL1AF, &EL1AP),
            l: norm_dblpi((L[0] + ACC[0] * dt) * dt + calc_cos_ser(dt, &EL1LA, &EL1LF, &EL1LP)),
            k: calc_cos_ser(dt, &EL1ZA, &EL1ZF, &EL1ZP),
            h: calc_sin_ser(dt, &EL1ZA, &EL1ZF, &EL1ZP),
            q: calc_cos_ser(dt, &EL1ZETAA, &EL1ZETAF, &EL1ZETAP),
            p: calc_sin_ser(dt, &EL1ZETAA, &EL1ZETAF, &EL1ZETAP),
        },
        BodyId::DEIMOS => OrbElem {
            mu: MU[1],
            mjd0: mjd_tdb,
            a: calc_cos_ser(dt, &EL2AA, &EL2AF, &EL2AP),
            l: norm_dblpi((L[1] + ACC[1] * dt) * dt + calc_cos_ser(dt, &EL2LA, &EL2LF, &EL2LP)),
            k: calc_cos_ser(dt, &EL2ZA, &EL2ZF, &EL2ZP),
            h: calc_sin_ser(dt, &EL2ZA, &EL2ZF, &EL2ZP),
            q: calc_cos_ser(dt, &EL2ZETAA, &EL2ZETAF, &EL2ZETAP),
            p: calc_sin_ser(dt, &EL2ZETAA, &EL2ZETAF, &EL2ZETAP),
        },
        _ => OrbElem {
            mu: f64::NAN,
            mjd0: f64::NAN,
            a: f64::NAN,
            l: f64::NAN,
            k: f64::NAN,
            h: f64::NAN,
            q: f64::NAN,
            p: f64::NAN,
        },
    }
}

///
/// # Arguments
/// # Returns
///
pub fn get_icrs_p(mjd_tdb: f64, body: &BodyId) -> vec_p::VecP {
    let orb_el = get_orb_elem(mjd_tdb, body);
    let rot = to_icrs(mjd_tdb);
    let out = orb_el.get_p();
    mul_p(&rot, &out)
}

///
/// # Arguments
/// # Returns
///
pub fn get_icrs_pv(mjd_tdb: f64, body: &BodyId) -> vec_pv::VecPv {
    let orb_el = get_orb_elem(mjd_tdb, body);
    let rot = to_icrs(mjd_tdb);
    let out = orb_el.get_pv();
    mul_pv(&rot, &out)
}

fn to_icrs(mjd_tdb: f64) -> MatR {
    let dt = (mjd_tdb - J2000) / mjd::DJCENT;
    let ome = (47.68143 - 0.1061 * dt) * angle::DEGRAD;
    let ainc = (37.1135 + 0.0609 * dt) * angle::DEGRAD;
    mul(&rot_z(-ome), &rot_x(-ainc))
}

fn calc_cos_ser(dt: f64, ampl: &[f64], phase: &[f64], frec: &[f64]) -> f64 {
    let mut out = 0.0;
    for j in 0..ampl.len() {
        if frec[j] == 0.0 {
            out += ampl[j] * phase[j].cos()
        } else {
            out += ampl[j] * (phase[j] + DBLPI * dt / frec[j]).cos();
        }
    }
    out
}

fn calc_sin_ser(dt: f64, ampl: &[f64], phase: &[f64], frec: &[f64]) -> f64 {
    let mut out = 0.0;
    for j in 0..ampl.len() {
        if frec[j] == 0.0 {
            out += ampl[j] * phase[j].sin()
        } else {
            out += ampl[j] * (phase[j] + DBLPI * dt / frec[j]).sin();
        }
    }
    out
}