/***********************************************************************************************************************
PicoMite MMBasic

MATHS.c

<COPYRIGHT HOLDERS>  Geoff Graham, Peter Mather
Copyright (c) 2021, <COPYRIGHT HOLDERS> All rights reserved. 
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 
1.	Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2.	Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer
    in the documentation and/or other materials provided with the distribution.
3.	The name MMBasic be used when referring to the interpreter in any documentation and promotional material and the original copyright message be displayed 
    on the console at startup (additional copyright messages may be added).
4.	All advertising materials mentioning features or use of this software must display the following acknowledgement: This product includes software developed 
    by the <copyright holder>.
5.	Neither the name of the <copyright holder> nor the names of its contributors may be used to endorse or promote products derived from this software 
    without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY <COPYRIGHT HOLDERS> AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDERS> BE LIABLE FOR ANY DIRECT, 
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 

************************************************************************************************************************/
/**
* @file MATHS.c
* @author Geoff Graham, Peter Mather
* @brief Source for MATHS MMBasic commands and functions
*/
/**
 * @cond
 * The following section will be excluded from the documentation.
 */

#include "MMBasic_Includes.h"
#include "Hardware_Includes.h"
#include <math.h>
#include <complex.h>
#ifdef rp2350
#include "pico/rand.h"
#endif
#define CBC 1
#define CTR 1
#define ECB 1
#include "aes.h"
MMFLOAT PI;
typedef MMFLOAT complex cplx;
typedef float complex fcplx;
void cmd_FFT(unsigned char *pp);
const MMFLOAT chitable[51][15]={
		{0.995,0.99,0.975,0.95,0.9,0.5,0.2,0.1,0.05,0.025,0.02,0.01,0.005,0.002,0.001},
		{0.0000397,0.000157,0.000982,0.00393,0.0158,0.455,1.642,2.706,3.841,5.024,5.412,6.635,7.879,9.550,10.828},
		{0.0100,0.020,0.051,0.103,0.211,1.386,3.219,4.605,5.991,7.378,7.824,9.210,10.597,12.429,13.816},
		{0.072,0.115,0.216,0.352,0.584,2.366,4.642,6.251,7.815,9.348,9.837,11.345,12.838,14.796,16.266},
		{0.207,0.297,0.484,0.711,1.064,3.357,5.989,7.779,9.488,11.143,11.668,13.277,14.860,16.924,18.467},
		{0.412,0.554,0.831,1.145,1.610,4.351,7.289,9.236,11.070,12.833,13.388,15.086,16.750,18.907,20.515},
		{0.676,0.872,1.237,1.635,2.204,5.348,8.558,10.645,12.592,14.449,15.033,16.812,18.548,20.791,22.458},
		{0.989,1.239,1.690,2.167,2.833,6.346,9.803,12.017,14.067,16.013,16.622,18.475,20.278,22.601,24.322},
		{1.344,1.646,2.180,2.733,3.490,7.344,11.030,13.362,15.507,17.535,18.168,20.090,21.955,24.352,26.124},
		{1.735,2.088,2.700,3.325,4.168,8.343,12.242,14.684,16.919,19.023,19.679,21.666,23.589,26.056,27.877},
		{2.156,2.558,3.247,3.940,4.865,9.342,13.442,15.987,18.307,20.483,21.161,23.209,25.188,27.722,29.588},
		{2.603,3.053,3.816,4.575,5.578,10.341,14.631,17.275,19.675,21.920,22.618,24.725,26.757,29.354,31.264},
		{3.074,3.571,4.404,5.226,6.304,11.340,15.812,18.549,21.026,23.337,24.054,26.217,28.300,30.957,32.909},
		{3.565,4.107,5.009,5.892,7.042,12.340,16.985,19.812,22.362,24.736,25.472,27.688,29.819,32.535,34.528},
		{4.075,4.660,5.629,6.571,7.790,13.339,18.151,21.064,23.685,26.119,26.873,29.141,31.319,34.091,36.123},
		{4.601,5.229,6.262,7.261,8.547,14.339,19.311,22.307,24.996,27.488,28.259,30.578,32.801,35.628,37.697},
		{5.142,5.812,6.908,7.962,9.312,15.338,20.465,23.542,26.296,28.845,29.633,32.000,34.267,37.146,39.252},
		{5.697,6.408,7.564,8.672,10.085,16.338,21.615,24.769,27.587,30.191,30.995,33.409,35.718,38.648,40.790},
		{6.265,7.015,8.231,9.390,10.865,17.338,22.760,25.989,28.869,31.526,32.346,34.805,37.156,40.136,42.312},
		{6.844,7.633,8.907,10.117,11.651,18.338,23.900,27.204,30.144,32.852,33.687,36.191,38.582,41.610,43.820},
		{7.434,8.260,9.591,10.851,12.443,19.337,25.038,28.412,31.410,34.170,35.020,37.566,39.997,43.072,45.315},
		{8.034,8.897,10.283,11.591,13.240,20.337,26.171,29.615,32.671,35.479,36.343,38.932,41.401,44.522,46.797},
		{8.643,9.542,10.982,12.338,14.041,21.337,27.301,30.813,33.924,36.781,37.659,40.289,42.796,45.962,48.268},
		{9.260,10.196,11.689,13.091,14.848,22.337,28.429,32.007,35.172,38.076,38.968,41.638,44.181,47.391,49.728},
		{9.886,10.856,12.401,13.848,15.659,23.337,29.553,33.196,36.415,39.364,40.270,42.980,45.559,48.812,51.179},
		{10.520,11.524,13.120,14.611,16.473,24.337,30.675,34.382,37.652,40.646,41.566,44.314,46.928,50.223,52.620},
		{11.160,12.198,13.844,15.379,17.292,25.336,31.795,35.563,38.885,41.923,42.856,45.642,48.290,51.627,54.052},
		{11.808,12.879,14.573,16.151,18.114,26.336,32.912,36.741,40.113,43.195,44.140,46.963,49.645,53.023,55.476},
		{12.461,13.565,15.308,16.928,18.939,27.336,34.027,37.916,41.337,44.461,45.419,48.278,50.993,54.411,56.892},
		{13.121,14.256,16.047,17.708,19.768,28.336,35.139,39.087,42.557,45.722,46.693,49.588,52.336,55.792,58.301},
		{13.787,14.953,16.791,18.493,20.599,29.336,36.250,40.256,43.773,46.979,47.962,50.892,53.672,57.167,59.703},
		{14.458,15.655,17.539,19.281,21.434,30.336,37.359,41.422,44.985,48.232,49.226,52.191,55.003,58.536,61.098},
		{15.134,16.362,18.291,20.072,22.271,31.336,38.466,42.585,46.194,49.480,50.487,53.486,56.328,59.899,62.487},
		{15.815,17.074,19.047,20.867,23.110,32.336,39.572,43.745,47.400,50.725,51.743,54.776,57.648,61.256,63.870},
		{16.501,17.789,19.806,21.664,23.952,33.336,40.676,44.903,48.602,51.966,52.995,56.061,58.964,62.608,65.247},
		{17.192,18.509,20.569,22.465,24.797,34.336,41.778,46.059,49.802,53.203,54.244,57.342,60.275,63.955,66.619},
		{17.887,19.233,21.336,23.269,25.643,35.336,42.879,47.212,50.998,54.437,55.489,58.619,61.581,65.296,67.985},
		{18.586,19.960,22.106,24.075,26.492,36.336,43.978,48.363,52.192,55.668,56.730,59.892,62.883,66.633,69.346},
		{19.289,20.691,22.878,24.884,27.343,37.335,45.076,49.513,53.384,56.896,57.969,61.162,64.181,67.966,70.703},
		{19.996,21.426,23.654,25.695,28.196,38.335,46.173,50.660,54.572,58.120,59.204,62.428,65.476,69.294,72.055},
		{20.707,22.164,24.433,26.509,29.051,39.335,47.269,51.805,55.758,59.342,60.436,63.691,66.766,70.618,73.402},
		{21.421,22.906,25.215,27.326,29.907,40.335,48.363,52.949,56.942,60.561,61.665,64.950,68.053,71.938,74.745},
		{22.138,23.650,25.999,28.144,30.765,41.335,49.456,54.090,58.124,61.777,62.892,66.206,69.336,73.254,76.084},
		{22.859,24.398,26.785,28.965,31.625,42.335,50.548,55.230,59.304,62.990,64.116,67.459,70.616,74.566,77.419},
		{23.584,25.148,27.575,29.787,32.487,43.335,51.639,56.369,60.481,64.201,65.337,68.710,71.893,75.874,78.750},
		{24.311,25.901,28.366,30.612,33.350,44.335,52.729,57.505,61.656,65.410,66.555,69.957,73.166,77.179,80.077},
		{25.041,26.657,29.160,31.439,34.215,45.335,53.818,58.641,62.830,66.617,67.771,71.201,74.437,78.481,81.400},
		{25.775,27.416,29.956,32.268,35.081,46.335,54.906,59.774,64.001,67.821,68.985,72.443,75.704,79.780,82.720},
		{26.511,28.177,30.755,33.098,35.949,47.335,55.993,60.907,65.171,69.023,70.197,73.683,76.969,81.075,84.037},
		{27.249,28.941,31.555,33.930,36.818,48.335,57.079,62.038,66.339,70.222,71.406,74.919,78.231,82.367,85.351},
		{27.991,29.707,32.357,34.764,37.689,49.335,58.164,63.167,67.505,71.420,72.613,76.154,79.490,83.657,86.661}
};
MMFLOAT q[4]={1,0,0,0};
MMFLOAT eInt[3]={0,0,0};
/* An implementation of the MT19937 Algorithm for the Mersenne Twister
 * by Evan Sultanik.  Based upon the pseudocode in: M. Matsumoto and
 * T. Nishimura, "Mersenne Twister: A 623-dimensionally
 * equidistributed uniform pseudorandom number generator," ACM
 * Transactions on Modeling and Computer Simulation Vol. 8, No. 1,
 * January pp.3-30 1998.
 *
 * http://www.sultanik.com/Mersenne_twister
 */
struct tagMTRand *g_myrand=NULL;
#define UPPER_MASK		0x80000000
#define LOWER_MASK		0x7fffffff
#define TEMPERING_MASK_B	0x9d2c5680
#define TEMPERING_MASK_C	0xefc60000
s_PIDchan PIDchannels[MAXPID+1]={0};

inline static void m_seedRand(MTRand* rand, unsigned long seed) {
  /* set initial seeds to mt[STATE_VECTOR_LENGTH] using the generator
   * from Line 25 of Table 1 in: Donald Knuth, "The Art of Computer
   * Programming," Vol. 2 (2nd Ed.) pp.102.
   */
  rand->mt[0] = seed & 0xffffffff;
  for(rand->index=1; rand->index<STATE_VECTOR_LENGTH; rand->index++) {
    rand->mt[rand->index] = (6069 * rand->mt[rand->index-1]) & 0xffffffff;
  }
}

/**
* Creates a new random number generator from a given seed.
*/
void seedRand(unsigned long seed) {
  m_seedRand(g_myrand, seed);
//  return rand;
}

/**
 * Generates a pseudo-randomly generated long.
 */
unsigned long genRandLong(MTRand* rand) {

  unsigned long y;
  static unsigned long mag[2] = {0x0, 0x9908b0df}; /* mag[x] = x * 0x9908b0df for x = 0,1 */
  if(rand->index >= STATE_VECTOR_LENGTH || rand->index < 0) {
    /* generate STATE_VECTOR_LENGTH words at a time */
    int kk;
    if(rand->index >= STATE_VECTOR_LENGTH+1 || rand->index < 0) {
      m_seedRand(rand, 4357);
    }
    for(kk=0; kk<STATE_VECTOR_LENGTH-STATE_VECTOR_M; kk++) {
      y = (rand->mt[kk] & UPPER_MASK) | (rand->mt[kk+1] & LOWER_MASK);
      rand->mt[kk] = rand->mt[kk+STATE_VECTOR_M] ^ (y >> 1) ^ mag[y & 0x1];
    }
    for(; kk<STATE_VECTOR_LENGTH-1; kk++) {
      y = (rand->mt[kk] & UPPER_MASK) | (rand->mt[kk+1] & LOWER_MASK);
      rand->mt[kk] = rand->mt[kk+(STATE_VECTOR_M-STATE_VECTOR_LENGTH)] ^ (y >> 1) ^ mag[y & 0x1];
    }
    y = (rand->mt[STATE_VECTOR_LENGTH-1] & UPPER_MASK) | (rand->mt[0] & LOWER_MASK);
    rand->mt[STATE_VECTOR_LENGTH-1] = rand->mt[STATE_VECTOR_M-1] ^ (y >> 1) ^ mag[y & 0x1];
    rand->index = 0;
  }
  y = rand->mt[rand->index++];
  y ^= (y >> 11);
  y ^= (y << 7) & TEMPERING_MASK_B;
  y ^= (y << 15) & TEMPERING_MASK_C;
  y ^= (y >> 18);
  return y;
}

unsigned char b64_chr[] = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";

unsigned int b64_int(unsigned int ch) {

	// ASCII to base64_int
	// 65-90  Upper Case  >>  0-25
	// 97-122 Lower Case  >>  26-51
	// 48-57  Numbers     >>  52-61
	// 43     Plus (+)    >>  62
	// 47     Slash (/)   >>  63
	// 61     Equal (=)   >>  64~
	if (ch==43)
	return 62;
	if (ch==47)
	return 63;
	if (ch==61)
	return 64;
	if ((ch>47) && (ch<58))
	return ch + 4;
	if ((ch>64) && (ch<91))
	return ch - 'A';
	if ((ch>96) && (ch<123))
	return (ch - 'a') + 26;
	return 0;
}

unsigned int b64e_size(unsigned int in_size) {

	// size equals 4*floor((1/3)*(in_size+2));
	int i, j = 0;
	for (i=0;i<in_size;i++) {
		if (i % 3 == 0)
		j += 1;
	}
	return (4*j);
}

unsigned int b64d_size(unsigned int in_size) {

	return ((3*in_size)/4);
}

unsigned int b64_encode(const unsigned char* in, unsigned int in_len, unsigned char* out) {

	unsigned int i=0, j=0, k=0, s[3];
	
	for (i=0;i<in_len;i++) {
		s[j++]=*(in+i);
		if (j==3) {
			out[k+0] = b64_chr[ (s[0]&255)>>2 ];
			out[k+1] = b64_chr[ ((s[0]&0x03)<<4)+((s[1]&0xF0)>>4) ];
			out[k+2] = b64_chr[ ((s[1]&0x0F)<<2)+((s[2]&0xC0)>>6) ];
			out[k+3] = b64_chr[ s[2]&0x3F ];
			j=0; k+=4;
		}
	}
	
	if (j) {
		if (j==1)
			s[1] = 0;
		out[k+0] = b64_chr[ (s[0]&255)>>2 ];
		out[k+1] = b64_chr[ ((s[0]&0x03)<<4)+((s[1]&0xF0)>>4) ];
		if (j==2)
			out[k+2] = b64_chr[ ((s[1]&0x0F)<<2) ];
		else
			out[k+2] = '=';
		out[k+3] = '=';
		k+=4;
	}

	out[k] = '\0';
	
	return k;
}

unsigned int b64_decode(const unsigned char* in, unsigned int in_len, unsigned char* out) {

	unsigned int i=0, j=0, k=0, s[4];
	
	for (i=0;i<in_len;i++) {
		s[j++]=b64_int(*(in+i));
		if (j==4) {
			out[k+0] = ((s[0]&255)<<2)+((s[1]&0x30)>>4);
			if (s[2]!=64) {
				out[k+1] = ((s[1]&0x0F)<<4)+((s[2]&0x3C)>>2);
				if ((s[3]!=64)) {
					out[k+2] = ((s[2]&0x03)<<6)+(s[3]); k+=3;
				} else {
					k+=2;
				}
			} else {
				k+=1;
			}
			j=0;
		}
	}
	
	return k;
}

/**
 * Generates a pseudo-randomly generated double in the range [0..1].
 */
MMFLOAT genRand(MTRand* rand) {
  return((MMFLOAT)genRandLong(rand) / (unsigned long)0xffffffff);
}

MMFLOAT determinant(MMFLOAT **matrix,int size);
void transpose(MMFLOAT **matrix,MMFLOAT **matrix_cofactor,MMFLOAT **newmatrix, int size);
void cofactor(MMFLOAT **matrix,MMFLOAT **newmatrix,int size);
static void floatshellsort(MMFLOAT a[],  int n) {
    long h, l, j;
    MMFLOAT k;
    for (h = n; h /= 2;) {
        for (l = h; l < n; l++) {
            k = a[l];
            for (j = l; j >= h &&  k < a[j - h]; j -= h) {
                a[j] = a[j - h];
            }
            a[j] = k;
        }
    }
}

static MMFLOAT* alloc1df (int n)
{
//    int i;
    MMFLOAT* array;
    if ((array = (MMFLOAT*) GetMemory(n * sizeof(MMFLOAT))) == NULL) {
        error("Unable to allocate memory for 1D float array...\n");
        exit(0);
    }

//    for (i = 0; i < n; i++) {
//        array[i] = 0.0;
//    }

    return array;
}

static MMFLOAT** alloc2df (int m, int n)
{
    int i;
    MMFLOAT** array;
    if ((array = (MMFLOAT **) GetMemory(m * sizeof(MMFLOAT*))) == NULL) {
        error("Unable to allocate memory for 2D float array...\n");
        exit(0);
    }

    for (i = 0; i < m; i++) {
        array[i] = alloc1df(n);
    }

    return array;
}

static void dealloc2df (MMFLOAT** array, int m, int n)
{
    int i;
    for (i = 0; i < m; i++) {
        FreeMemorySafe((void **)&array[i]);
    }

    FreeMemorySafe((void **)&array);
}

void Q_Mult(MMFLOAT *q1, MMFLOAT *q2, MMFLOAT *n){
    MMFLOAT a1=q1[0],a2=q2[0],b1=q1[1],b2=q2[1],c1=q1[2],c2=q2[2],d1=q1[3],d2=q2[3];
    n[0]=a1*a2-b1*b2-c1*c2-d1*d2;
    n[1]=a1*b2+b1*a2+c1*d2-d1*c2;
    n[2]=a1*c2-b1*d2+c1*a2+d1*b2;
    n[3]=a1*d2+b1*c2-c1*b2+d1*a2;
    n[4]=q1[4]*q2[4];
}

void Q_Invert(MMFLOAT *q, MMFLOAT *n){
    n[0]=q[0];
    n[1]=-q[1];
    n[2]=-q[2];
    n[3]=-q[3];
    n[4]=q[4];
}
static uint8_t reverse8(uint8_t in)
{
  uint8_t x = in;
  x = (((x & 0xAA) >> 1) | ((x & 0x55) << 1));
  x = (((x & 0xCC) >> 2) | ((x & 0x33) << 2));
  x =          ((x >> 4) | (x << 4));
  return x;
}


static uint16_t reverse16(uint16_t in)
{
  uint16_t x = in;
  x = (((x & 0XAAAA) >> 1) | ((x & 0X5555) << 1));
  x = (((x & 0xCCCC) >> 2) | ((x & 0X3333) << 2));
  x = (((x & 0xF0F0) >> 4) | ((x & 0X0F0F) << 4));
  x = (( x >> 8) | (x << 8));
  return x;
}


static uint16_t reverse12(uint16_t in)
{
  return reverse16(in) >> 4;
}


static uint32_t reverse32(uint32_t in)
{
  uint32_t x = in;
  x = (((x & 0xAAAAAAAA) >> 1)  | ((x & 0x55555555) << 1));
  x = (((x & 0xCCCCCCCC) >> 2)  | ((x & 0x33333333) << 2));
  x = (((x & 0xF0F0F0F0) >> 4)  | ((x & 0x0F0F0F0F) << 4));
  x = (((x & 0xFF00FF00) >> 8)  | ((x & 0x00FF00FF) << 8));
  x = (x >> 16) | (x << 16);
  return x;
}


static uint64_t reverse64(uint64_t in)
{
  uint64_t x = in;
  x = (((x & 0xAAAAAAAAAAAAAAAA) >> 1)  | ((x & 0x5555555555555555) << 1));
  x = (((x & 0xCCCCCCCCCCCCCCCC) >> 2)  | ((x & 0x3333333333333333) << 2));
  x = (((x & 0xF0F0F0F0F0F0F0F0) >> 4)  | ((x & 0x0F0F0F0F0F0F0F0F) << 4));
  x = (((x & 0xFF00FF00FF00FF00) >> 8)  | ((x & 0x00FF00FF00FF00FF) << 8));
  x = (((x & 0xFFFF0000FFFF0000) >> 16) | ((x & 0x0000FFFF0000FFFF) << 16));
  x = (x >> 32) | (x << 32);
  return x;
}


///////////////////////////////////////////////////////////////////////////////////
/*MIT License

Copyright (c) 2021-2024 Rob Tillaart

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.*/
// CRC POLYNOME = x8 + x5 + x4 + 1 = 1001 1000 = 0x8C
uint8_t crc8(uint8_t *array, uint16_t length, const uint8_t polynome, 
             const uint8_t startmask, const uint8_t endmask, 
             const uint8_t reverseIn, const uint8_t reverseOut)
{
  uint8_t crc = startmask;
  while (length--) 
  {
    if ((length & 0xFF) == 0) routinechecks();  // RTOS
    uint8_t data = *array++;
    if (reverseIn) data = reverse8(data);
    crc ^= data;
    for (uint8_t i = 8; i; i--) 
    {
      if (crc & 0x80)
      {
        crc <<= 1;
        crc ^= polynome;
      }
      else
      {
        crc <<= 1;
      }
    }
  }
  crc ^= endmask;
  if (reverseOut) crc = reverse8(crc);
  return crc;
}


// CRC POLYNOME = x12 + x3 + x2 + 1 =  0000 1000 0000 1101 = 0x80D
uint16_t crc12(const uint8_t *array, uint16_t length, const uint16_t polynome,
               const uint16_t startmask, const uint16_t endmask, 
               const uint8_t reverseIn, const uint8_t reverseOut)
{
  uint16_t crc = startmask;
  while (length--) 
  {
    if ((length & 0xFF) == 0) routinechecks();  // RTOS
    uint8_t data = *array++;
    if (reverseIn) data = reverse8(data);

    crc ^= ((uint16_t)data) << 4;
    for (uint8_t i = 8; i; i--) 
    {
      if (crc & (1 << 11) )
      {
        crc <<= 1;
        crc ^= polynome;
      }
      else
      {
        crc <<= 1;
      }
    }
  }

  if (reverseOut) crc = reverse12(crc);
  crc ^= endmask;
  return crc;
}


// CRC POLYNOME = x15 + 1 =  1000 0000 0000 0001 = 0x8001
uint16_t crc16(const uint8_t *array, uint16_t length, const uint16_t polynome,
               const uint16_t startmask, const uint16_t endmask, 
               const uint8_t reverseIn, const uint8_t reverseOut)
{
  uint16_t crc = startmask;
  while (length--) 
  {
    if ((length & 0xFF) == 0) routinechecks();  // RTOS
    uint8_t data = *array++;
    if (reverseIn) data = reverse8(data);
    crc ^= ((uint16_t)data) << 8;
    for (uint8_t i = 8; i; i--) 
    {
      if (crc & (1 << 15))
      {
        crc <<= 1;
        crc ^= polynome;
      }
      else
      {
        crc <<= 1;
      }
    }
  }
  if (reverseOut) crc = reverse16(crc);
  crc ^= endmask;
  return crc;
}


// CRC-CCITT POLYNOME = x13 + X5 + 1 =  0001 0000 0010 0001 = 0x1021
uint16_t crc16_CCITT(uint8_t *array, uint16_t length)
{
  return crc16(array, length, 0x1021, 0xFFFF,0,0,0);
}

// CRC-32 POLYNOME =  x32 + ..... + 1
uint32_t crc32(const uint8_t *array, uint16_t length, const uint32_t polynome, 
               const uint32_t startmask, const uint32_t endmask, 
               const uint8_t reverseIn, const uint8_t reverseOut)
{
  uint32_t crc = startmask;
  while (length--) 
  {
    if ((length & 0xFF) == 0) routinechecks();  // RTOS
    uint8_t data = *array++;
    if (reverseIn) data = reverse8(data);
    crc ^= ((uint32_t) data) << 24;
    for (uint8_t i = 8; i; i--) 
    {
      if (crc & (1UL << 31))
      {
        crc <<= 1;
        crc ^= polynome;
      }
      else
      {
        crc <<= 1;
      }
    }
  }
  crc ^= endmask;
  if (reverseOut) crc = reverse32(crc);
  return crc;
}


// CRC-CCITT POLYNOME =  x64 + ..... + 1
// CRC_ECMA64 = 0x42F0E1EBA9EA3693
uint64_t crc64(const uint8_t *array, uint16_t length, const uint64_t polynome, 
               const uint64_t startmask, const uint64_t endmask, 
               const uint8_t reverseIn, const uint8_t reverseOut)
{
  uint64_t crc = startmask;
  while (length--) 
  {
    if ((length & 0xFF) == 0) routinechecks();  // RTOS
    uint8_t data = *array++;
    if (reverseIn) data = reverse8(data);
    crc ^= ((uint64_t) data) << 56;
    for (uint8_t i = 8; i; i--) 
    {
      if (crc & (1ULL << 63))
      {
        crc <<= 1;
        crc ^= polynome;
      }
      else
      {
        crc <<= 1;
      }
    }
  }
  crc ^= endmask;
  if (reverseOut) crc = reverse64(crc);
  return crc;
}
#ifdef rp2350
int parseintegerarray(unsigned char *tp, int64_t **a1int, int argno, int dimensions, int *dims, bool ConstantNotAllowed){
#else
int parseintegerarray(unsigned char *tp, int64_t **a1int, int argno, int dimensions, short *dims, bool ConstantNotAllowed){
#endif
	void *ptr1 = NULL;
	int i,j;
	ptr1 = findvar(tp, V_FIND | V_EMPTY_OK | V_NOFIND_ERR);
	if((g_vartbl[g_VarIndex].type & T_CONST) && ConstantNotAllowed) error("Cannot change a constant");
	if(dims==NULL)dims=g_vartbl[g_VarIndex].dims;
	if(g_vartbl[g_VarIndex].type & T_INT) {
#ifdef rp2350
		memcpy(dims,g_vartbl[g_VarIndex].dims, MAXDIM * sizeof(int));
#else
		memcpy(dims,g_vartbl[g_VarIndex].dims, MAXDIM * sizeof(short));
#endif
		*a1int = (int64_t *)ptr1;
		if((uint32_t)ptr1!=(uint32_t)g_vartbl[g_VarIndex].val.s)error("Syntax");
	} else error("Argument % must be an integer array",argno);
	int card=1;
	if(dimensions==1 && (dims[0]<=0 || dims[1]>0))error("Argument % must be a 1D integer point array",argno);
	if(dimensions==2 && (dims[0]<=0 || dims[1]<=0 || dims[2]>0))error("Argument % must be a 2D integer point array",argno);
	for(i=0;i<MAXDIM;i++){
		j=(dims[i] - g_OptionBase + 1);
		if(j)card *= j;
	}
	return card;
}
#ifdef rp2350
int parsestringarray(unsigned char *tp, unsigned char **a1str, int argno, int dimensions, int *dims, bool ConstantNotAllowed, unsigned char *length){
#else
int parsestringarray(unsigned char *tp, unsigned char **a1str, int argno, int dimensions, short *dims, bool ConstantNotAllowed, unsigned char *length){
#endif
	void *ptr1 = NULL;
	int i,j;
	ptr1 = findvar(tp, V_FIND | V_EMPTY_OK | V_NOFIND_ERR);
	if((g_vartbl[g_VarIndex].type & T_CONST) && ConstantNotAllowed) error("Cannot change a constant");
	if(dims==NULL)dims=g_vartbl[g_VarIndex].dims;
	if(g_vartbl[g_VarIndex].type & T_STR) {
#ifdef rp2350
		memcpy(dims,g_vartbl[g_VarIndex].dims, MAXDIM * sizeof(int));
#else
		memcpy(dims,g_vartbl[g_VarIndex].dims, MAXDIM * sizeof(short));
#endif
		*length=g_vartbl[g_VarIndex].size;
		*a1str = (unsigned char *)ptr1;
		if((uint32_t)ptr1!=(uint32_t)g_vartbl[g_VarIndex].val.s)error("Syntax");
	} else error("Argument % must be a string array",argno);
	int card=1;
	if(dimensions==1 && (dims[0]<=0 || dims[1]>0))error("Argument % must be a 1D string array",argno);
	if(dimensions==2 && (dims[0]<=0 || dims[1]<=0 || dims[2]>0))error("Argument % must be a 2D string array",argno);
	for(i=0;i<MAXDIM;i++){
		j=(dims[i] - g_OptionBase + 1);
		if(j)card *= j;
	}
	return card;
}

#ifdef rp2350
int parsenumberarray(unsigned char *tp, MMFLOAT **a1float, int64_t **a1int, int argno, int dimensions, int *dims, bool ConstantNotAllowed){
#else
int parsenumberarray(unsigned char *tp, MMFLOAT **a1float, int64_t **a1int, int argno, short dimensions, short *dims, bool ConstantNotAllowed){
#endif
	void *ptr1 = NULL;
	int i,j;
	ptr1 = findvar(tp, V_FIND | V_EMPTY_OK | V_NOFIND_ERR);
	if((g_vartbl[g_VarIndex].type & T_CONST) && ConstantNotAllowed) error("Cannot change a constant");
	if(dims==NULL)dims=g_vartbl[g_VarIndex].dims;
	if(g_vartbl[g_VarIndex].type & (T_INT | T_NBR)) {
#ifdef rp2350
		memcpy(dims,g_vartbl[g_VarIndex].dims, MAXDIM * sizeof(int));
#else
		memcpy(dims,g_vartbl[g_VarIndex].dims, MAXDIM * sizeof(short));
#endif
		if(g_vartbl[g_VarIndex].type & T_NBR) *a1float = (MMFLOAT *)ptr1;
		else *a1int=(int64_t *)ptr1;
		if((uint32_t)ptr1!=(uint32_t)g_vartbl[g_VarIndex].val.s)error("Syntax");
	} else error("Argument % must be a numerical array",argno);
	int card=1;
	if(dimensions==1 && (dims[0]<=0 || dims[1]>0))error("Argument % must be a 1D numerical array",argno);
	if(dimensions==2 && (dims[0]<=0 || dims[1]<=0 || dims[2]>0))error("Argument % must be a 2D numerical array",argno);
	for(i=0;i<MAXDIM;i++){
		j=(dims[i] - g_OptionBase + 1);
		if(j)card *= j;
	}
	return card;
}
#ifdef rp2350
int parsefloatrarray(unsigned char *tp, MMFLOAT **a1float, int argno, int dimensions, int *dims, bool ConstantNotAllowed){
#else
int parsefloatrarray(unsigned char *tp, MMFLOAT **a1float, int argno, int dimensions, short *dims, bool ConstantNotAllowed){
#endif
	void *ptr1 = NULL;
	int i,j;
	ptr1 = findvar(tp, V_FIND | V_EMPTY_OK | V_NOFIND_ERR);
	if((g_vartbl[g_VarIndex].type & T_CONST) && ConstantNotAllowed) error("Cannot change a constant");
	if(dims==NULL)dims=g_vartbl[g_VarIndex].dims;
	if(g_vartbl[g_VarIndex].type & T_NBR) {
#ifdef rp2350
		memcpy(dims,g_vartbl[g_VarIndex].dims, MAXDIM * sizeof(int));
#else
		memcpy(dims,g_vartbl[g_VarIndex].dims, MAXDIM * sizeof(short));
#endif
		*a1float = (MMFLOAT *)ptr1;
		if((uint32_t)ptr1!=(uint32_t)g_vartbl[g_VarIndex].val.s)error("Syntax");
	} else error("Argument % must be a floating point array",argno);
	int card=1;
	if(dimensions==1 && (dims[0]<=0 || dims[1]>0))error("Argument % must be a 1D floating point array",argno);
	if(dimensions==2 && (dims[0]<=0 || dims[1]<=0 || dims[2]>0))error("Argument % must be a 2D floating point array",argno);
	for(i=0;i<MAXDIM;i++){
		j=(dims[i] - g_OptionBase + 1);
		if(j)card *= j;
	}
	return card;
}
int parsearrays(unsigned char *tp, MMFLOAT **a1float, MMFLOAT **a2float,MMFLOAT **a3float, int64_t **a1int, int64_t **a2int, int64_t **a3int){
	int card1,card2,card3;
	getargs(&tp, 5,(unsigned char *)",");
	if(!(argc == 5)) error("Argument count");
	card1=parsenumberarray(argv[0],a1float,a1int,1,0, NULL, false);
	card2=parsenumberarray(argv[2],a2float,a2int,2,0, NULL, false);
	card3=parsenumberarray(argv[4],a3float,a3int,3,0, NULL, true);
	if(!(card1==card2 && card2==card3))error("Array size mismatch");
	if(!((*a3float==NULL && *a2float==NULL && *a1float==NULL) || (*a3int==NULL && *a2int==NULL && *a1int==NULL)))error("Arrays must be all integer or all floating point");
	return card1;
}
int parseany(unsigned char *tp, MMFLOAT **a1float, int64_t **a1int, unsigned char ** a1str, int *length, bool stringarray){
	void *ptr1 = findvar(tp, V_FIND | V_EMPTY_OK | V_NOFIND_ERR);
	int arraylength;
	if(g_vartbl[g_VarIndex].type & T_NBR) {
		if(g_vartbl[g_VarIndex].dims[1] != 0) error("Invalid variable");
		if(g_vartbl[g_VarIndex].dims[0] <= 0) {		// Not an array
			error("Argument 1 must be a numerical array");
		}
		arraylength=g_vartbl[g_VarIndex].dims[0] - g_OptionBase + 1;
		if(*length==0)*length=arraylength;
		if(*length>arraylength)error("Array size");
		*a1float = (MMFLOAT *)ptr1;
		if((uint32_t)ptr1!=(uint32_t)g_vartbl[g_VarIndex].val.s)error("Syntax");
	} else if(ptr1 && g_vartbl[g_VarIndex].type & T_INT) {
		if(g_vartbl[g_VarIndex].dims[1] != 0) error("Invalid variable");
		if(g_vartbl[g_VarIndex].dims[0] <= 0) {		// Not an array
			error("Argument 1 must be a numerical array");
		}
		arraylength=g_vartbl[g_VarIndex].dims[0] - g_OptionBase + 1;
		if(*length==0)*length=arraylength;
		if(*length>arraylength)error("Array size");
		*a1int = (int64_t *)ptr1;
		if((uint32_t)ptr1!=(uint32_t)g_vartbl[g_VarIndex].val.s)error("Syntax");
	} else if(ptr1 && g_vartbl[g_VarIndex].type & T_STR && !stringarray) {
		*a1str=(unsigned char *)ptr1;
		if(*length==0)*length=**a1str;
		if(**a1str<*length)error("String size");
	} else if(ptr1 && g_vartbl[g_VarIndex].type & T_STR && stringarray) {
		if(g_vartbl[g_VarIndex].dims[1] != 0) error("Invalid variable");
		if(g_vartbl[g_VarIndex].dims[0] <= 0) {		// Not an array
			error("Argument 1 must be a string array");
		}
		arraylength=g_vartbl[g_VarIndex].dims[0] - g_OptionBase + 1;
		if(*length==0)*length=arraylength;
		if(*length>arraylength)error("Array size");
		*a1str=(unsigned char *)ptr1;
		if((uint32_t)ptr1!=(uint32_t)g_vartbl[g_VarIndex].val.s)error("Syntax");
		*length=g_vartbl[g_VarIndex].size;
		return arraylength;
	} else error("Syntax");
	return *length;
}
unsigned char * parseAES(uint8_t *p, int ivadd, uint8_t *keyx, uint8_t *ivx, int64_t **outint, unsigned char **outstr, MMFLOAT **outfloat, int *card2){
	int64_t *a1int=NULL, *a2int=NULL, *a3int=NULL, *a4int=NULL;
	unsigned char *a1str=NULL, *a2str=NULL,*a3str=NULL,*a4str=NULL;
	MMFLOAT *a1float=NULL, *a2float=NULL, *a3float=NULL, *a4float=NULL;
	int card1, card3;
	getargs(&p,7,(unsigned char *)",");
	if(ivx==NULL){
		if(argc!=5)error("Syntax");
	} else {
		if(argc<5)error("Syntax");
	}
	*outstr=NULL;
	*outint=NULL;
	int length=0;
	card1= parseany(argv[0], &a1float, &a1int, &a1str, &length, false);
	if(card1!=16)error("Key must be 16 elements long");
	length=0;
	*card2= parseany(argv[2], &a2float, &a2int, &a2str, &length, false);
	if(*card2 % 16)error("input must be multiple of 16 elements long");
//	if(card2 >256)error("input must be <= 256 elements long");
	unsigned char *inx=(unsigned char *)GetTempMemory(*card2+16);
	length=0;
	card3= parseany(argv[4], &a3float, &a3int, &a3str, &length, false);
	if(card3!=*card2 + ivadd && a3str==NULL)error("Array size mismatch");
	if(argc==7){
		length=0;
		card1= parseany(argv[6], &a4float, &a4int, &a4str, &length, false);
		if(card1!=16)error("Initialisation vector must be 16 elements long");
		if(a4int!=NULL){
			for(int i=0;i<16;i++){
				if(a4int[i]<0 || a4int[i]>255)error("Key number out of bounds 0-255");
				ivx[i]=a4int[i];
			}
		} else if (a4float!=NULL){
			for(int i=0;i<16;i++){
				if(a4float[i]<0 || a4float[i]>255)error("Key number out of bounds 0-255");
				ivx[i]=a4float[i];
			}
		} else if(a4str!=NULL){
			for(int i=0;i<16;i++){
				ivx[i]=a4str[i+1];
			}
		}
	}

	if(a1int!=NULL){
		for(int i=0;i<16;i++){
			if(a1int[i]<0 || a1int[i]>255)error("Key number out of bounds 0-255");
			keyx[i]=a1int[i];
		}
	} else if (a1float!=NULL){
		for(int i=0;i<16;i++){
			if(a1float[i]<0 || a1float[i]>255)error("Key number out of bounds 0-255");
			keyx[i]=a1float[i];
		}
	} else if(a1str!=NULL){
		for(int i=0;i<16;i++){
			keyx[i]=a1str[i+1];
		}
	}
	if(a2int!=NULL){
		for(int i=0;i<*card2;i++){
			if(a2int[i]<0 || a2int[i]>255)error("input number out of bounds 0-255");
			inx[i]=a2int[i];
		}
	} else if (a2float!=NULL){
		for(int i=0;i<*card2;i++){
			if(a2float[i]<0 || a2float[i]>255)error("input number out of bounds 0-255");
			inx[i]=a2float[i];
		}
	} else if(a2str!=NULL){
		for(int i=0;i<*card2;i++){
			inx[i]=a2str[i+1];
		}
	}
	if(a3int!=NULL){
		*outint=a3int;
	} else if (a3float!=NULL){
		*outfloat=a3float;
	} else if(a3str!=NULL){
		*outstr=a3str;
	}
	return inx;
}
unsigned char * parseB64(uint8_t *p,int64_t **outint, unsigned char **outstr, MMFLOAT **outfloat, int *card1, int *card2){
	int64_t *a1int=NULL, *a3int=NULL;
	unsigned char *a1str=NULL,*a3str=NULL;
	MMFLOAT *a1float=NULL, *a3float=NULL;
	getargs(&p,3,(unsigned char *)",");
	if(argc!=3)error("Syntax");
	*outstr=NULL;
	*outint=NULL;
	int length=0;
	*card1= parseany(argv[0], &a1float, &a1int, &a1str, &length, false);
	length=0;
	*card2= parseany(argv[2], &a3float, &a3int, &a3str, &length, false);
	unsigned char *keyx=(unsigned char *)GetTempMemory(b64e_size(*card1)+1);
	if(a1int!=NULL){
		for(int i=0;i<*card1;i++){
			if(a1int[i]<0 || a1int[i]>255)error("Key number out of bounds 0-255");
			keyx[i]=a1int[i];
		}
	} else if (a1float!=NULL){
		for(int i=0;i<*card1;i++){
			if(a1float[i]<0 || a1float[i]>255)error("Key number out of bounds 0-255");
			keyx[i]=a1float[i];
		}
	} else if(a1str!=NULL){
		for(int i=0;i<*card1;i++){
			keyx[i]=a1str[i+1];
		}
	}
	if(a3int!=NULL){
		*outint=a3int;
	} else if (a3float!=NULL){
		*outfloat=a3float;
	} else if(a3str!=NULL){
		*outstr=a3str;
	}
	return keyx;
}
void returnAES(int64_t *outint, MMFLOAT *outflt, uint8_t *outstr, uint8_t *inx, uint8_t *iv, int card){
	if(outint!=NULL){
		if(iv){
			for(int i=0;i<16;i++){
				outint[i]=iv[i];
			}
			for(int i=16;i<card+16;i++){
				outint[i]=inx[i-16];
			}
		}  else {
			for(int i=0;i<card;i++){
				outint[i]=inx[i];
			}
		}
	} else if(outflt!=NULL){
		if(iv){
			for(int i=0;i<16;i++){
				outflt[i]=iv[i];
			}
			for(int i=16;i<card+16;i++){
				outflt[i]=inx[i-16];
			}
		}  else {
			for(int i=0;i<card;i++){
				outflt[i]=inx[i];
			}
		}
	} else if(outstr!=NULL){
		if(iv){
			if(card+16>=256)error("Too many elements for string output");
			memcpy(&outstr[1],iv,16);
			memcpy(&outstr[17],inx,card);
			*outstr=card+16;
		} else {
			if(card>=256)error("Too many elements for string output");
			memcpy(&outstr[1],inx,card);
			*outstr=card;
		}
	}
}
MMFLOAT farr2d(MMFLOAT *arr,int d1, int a, int b){
	arr+=d1*b+a;
	return *arr;
}
int64_t iarr2d(int64_t *arr,int d1, int a, int b){
	arr+=d1*b+a;
	return *arr;
}
MMFLOAT PIDController_Update(PIDController *pid, MMFLOAT setpoint, MMFLOAT measurement) {

	/*
	* Error signal
	*/
    MMFLOAT error = setpoint - measurement;


	/*
	* Proportional
	*/
    MMFLOAT proportional = pid->Kp * error;
	/*
	* Integral
	*/
    pid->integrator = pid->integrator + 0.5 * pid->Ki * pid->T * (error + pid->prevError);
	/* Anti-wind-up via integrator clamping */
    if (pid->integrator > pid->limMaxInt) {

        pid->integrator = pid->limMaxInt;

    } else if (pid->integrator < pid->limMinInt) {

        pid->integrator = pid->limMinInt;

    }


	/*
	* Derivative (band-limited differentiator)
	*/
		
    pid->differentiator = -(2.0 * pid->Kd * (measurement - pid->prevMeasurement)	/* Note: derivative on measurement, therefore minus sign in front of equation! */
                        + (2.0 * pid->tau - pid->T) * pid->differentiator)
                        / (2.0 * pid->tau + pid->T);


	/*
	* Compute output and apply limits
	*/
    pid->out = proportional + pid->integrator + pid->differentiator;

    if (pid->out > pid->limMax) {

        pid->out = pid->limMax;

    } else if (pid->out < pid->limMin) {

        pid->out = pid->limMin;

    }

	/* Store error and measurement for later use */
    pid->prevError       = error;
    pid->prevMeasurement = measurement;

	/* Return controller output */
    return pid->out;

}
/*  @endcond */
uint8_t getrnd(void){
#ifdef rp2350
	return get_rand_32() & 0xFF;
#else
	return rand() & 0xFF;
#endif
}
void cmd_math(void){
	unsigned char *tp;
    int t = T_NBR;
    MMFLOAT f;
    long long int i64;
    unsigned char *s;
	#ifdef rp2350
	int dims[MAXDIM]={0};
	#else
	short dims[MAXDIM]={0};
	#endif

	skipspace(cmdline);
	if(toupper(*cmdline)=='S'){

		tp = checkstring(cmdline, (unsigned char *)"SET");
		if(tp) {
			array_set(tp);
			return;
		}

		tp = checkstring(cmdline, (unsigned char *)"SCALE");
		if(tp) {
			int i,card1=1, card2=1;
			MMFLOAT *a1float=NULL,*a2float=NULL, scale;
			int64_t *a1int=NULL, *a2int=NULL;
			getargs(&tp, 5,(unsigned char *)",");
			if(!(argc == 5)) error("Argument count");
			card1=parsenumberarray(argv[0],&a1float,&a1int,1,0, dims, false);
		    evaluate(argv[2], &f, &i64, &s, &t, false);
		    if(t & T_STR) error("Syntax");
		    scale=getnumber(argv[2]);
			card2=parsenumberarray(argv[4],&a2float,&a2int,3,0, dims, true);
			if(card1 != card2)error("Size mismatch");
			if(scale!=1.0){
				if(a2float!=NULL && a1float!=NULL){
					for(i=0; i< card1;i++)*a2float++ = ((t & T_INT) ? (MMFLOAT)i64 : f) * (*a1float++);
				} else if(a2float!=NULL && a1float==NULL){
					for(i=0; i< card1;i++)(*a2float++) = ((t & T_INT) ? (MMFLOAT)i64 : f) * ((MMFLOAT)*a1int++);
				} else if(a2float==NULL && a1float!=NULL){
					for(i=0; i< card1;i++)(*a2int++) = FloatToInt64(((t & T_INT) ? i64 : FloatToInt64(f)) * (*a1float++));
				} else {
					for(i=0; i< card1;i++)(*a2int++) = ((t & T_INT) ? i64 : FloatToInt64(f)) * (*a1int++);
				}
			} else {
				if(a2float!=NULL && a1float!=NULL){
					for(i=0; i< card1;i++)*a2float++ = *a1float++;
				} else if(a2float!=NULL && a1float==NULL){
					for(i=0; i< card1;i++)(*a2float++) = ((MMFLOAT)*a1int++);
				} else if(a2float==NULL && a1float!=NULL){
					for(i=0; i< card1;i++)(*a2int++) = FloatToInt64(*a1float++);
				} else {
					for(i=0; i< card1;i++)*a2int++ = *a1int++;
				}
			}
			return;
		}
		tp = checkstring(cmdline, (unsigned char *)"SHIFT");
		if(tp) {
			int i, card1=1, card2=1;
			int64_t *a1int=NULL, *a2int=NULL;
			getargs(&tp, 7,(unsigned char *)",");
			if(!(argc == 5 || argc==7)) error("Argument count");
			card1=parseintegerarray(argv[0],&a1int,1,0, dims, false);
		    evaluate(argv[2], &f, &i64, &s, &t, false);
		    int shift=getint(argv[2], -63,63);
			card2=parseintegerarray(argv[4],&a2int,3,0, dims, true);
			if(card1 != card2)error("Size mismatch");
				if(shift>0)for(i=0; i< card1;i++)*a2int++ = (((uint64_t)*a1int++)<<shift);
				else {
					if(argc==7 && checkstring(argv[6],(unsigned char *)"U")){
						for(i=0; i< card1;i++)*a2int++ = ((uint64_t)(*a1int++)>>(-shift));
					} else {
						for(i=0; i< card1;i++)*a2int++ = ((*a1int++)>>(-shift));
					}
				}
			return;
		}

		tp = checkstring(cmdline, (unsigned char *)"SLICE");
		if(tp) {
			array_slice(tp);
			return;
		}
		tp = checkstring(cmdline, (unsigned char *)"SENSORFUSION");
		if(tp) {
			cmd_SensorFusion((char *)tp);
			return;
		}
	} else if(toupper(*cmdline)=='C') {
		unsigned char *tp1=NULL;
		tp = checkstring(cmdline, (unsigned char *)"C_ADD");
		if(tp) {
			MMFLOAT *a1float=NULL,*a2float=NULL,*a3float=NULL;
			int64_t *a1int=NULL,*a2int=NULL,*a3int=NULL;
			int card=parsearrays(tp, &a1float, &a2float, &a3float, &a1int, &a2int, &a3int);
			if(a1float){
				while(card--){
					*a3float++ = *a1float++ + *a2float++;
				}
			} else {
				while(card--){
					*a3int++ = *a1int++ + *a2int++;
				}
			}
			return;
		}
		tp = checkstring(cmdline, (unsigned char *)"C_MUL");
		tp1 = checkstring(cmdline, (unsigned char *)"C_MULT");
		if(tp || tp1) {
			if(tp1)tp=tp1;
			MMFLOAT *a1float=NULL,*a2float=NULL,*a3float=NULL;
			int64_t *a1int=NULL,*a2int=NULL,*a3int=NULL;
			int card=parsearrays(tp, &a1float, &a2float, &a3float, &a1int, &a2int, &a3int);
			if(a1float){
				while(card--){
					*a3float++ = *a1float++ * *a2float++;
				}
			} else {
				while(card--){
					*a3int++ = *a1int++ * *a2int++;
				}
			}
			return;
		}
		tp = checkstring(cmdline, (unsigned char *)"C_AND");
		if(tp) {
			MMFLOAT *a1float=NULL,*a2float=NULL,*a3float=NULL;
			int64_t *a1int=NULL,*a2int=NULL,*a3int=NULL;
			int card=parsearrays(tp, &a1float, &a2float, &a3float, &a1int, &a2int, &a3int);
			if(a1float){
				while(card--){
					*a3float++ = (MMFLOAT)((int64_t)*a1float++ & (int64_t)*a2float++);
				}
			} else {
				while(card--){
					*a3int++ = *a1int++ & *a2int++;
				}
			}
			return;
		}
		tp = checkstring(cmdline, (unsigned char *)"C_XOR");
		if(tp) {
			MMFLOAT *a1float=NULL,*a2float=NULL,*a3float=NULL;
			int64_t *a1int=NULL,*a2int=NULL,*a3int=NULL;
			int card=parsearrays(tp, &a1float, &a2float, &a3float, &a1int, &a2int, &a3int);
			if(a1float){
				while(card--){
					*a3float++ = (MMFLOAT)((int64_t)*a1float++ ^ (int64_t)*a2float++);
				}
			} else {
				while(card--){
					*a3int++ = *a1int++ & *a2int++;
				}
			}
			return;
		}
		tp = checkstring(cmdline, (unsigned char *)"C_OR");
		if(tp) {
			MMFLOAT *a1float=NULL,*a2float=NULL,*a3float=NULL;
			int64_t *a1int=NULL,*a2int=NULL,*a3int=NULL;
			int card=parsearrays(tp, &a1float, &a2float, &a3float, &a1int, &a2int, &a3int);
			if(a1float){
				while(card--){
					*a3float++ = (MMFLOAT)((int64_t)*a1float++ | (int64_t)*a2float++);
				}
			} else {
				while(card--){
					*a3int++ = *a1int++ & *a2int++;
				}
			}
			return;
		}
		tp = checkstring(cmdline, (unsigned char *)"C_SUB");
		if(tp) {
			MMFLOAT *a1float=NULL,*a2float=NULL,*a3float=NULL;
			int64_t *a1int=NULL,*a2int=NULL,*a3int=NULL;
			int card=parsearrays(tp, &a1float, &a2float, &a3float, &a1int, &a2int, &a3int);
			if(a1float){
				while(card--){
					*a3float++ = *a1float++ - *a2float++;
				}
			} else {
				while(card--){
					*a3int++ = *a1int++ - *a2int++;
				}
			}
			return;
		}
		tp = checkstring(cmdline, (unsigned char *)"C_DIV");
		if(tp) {
			MMFLOAT *a1float=NULL,*a2float=NULL,*a3float=NULL;
			int64_t *a1int=NULL,*a2int=NULL,*a3int=NULL;
			int card=parsearrays(tp, &a1float, &a2float, &a3float, &a1int, &a2int, &a3int);
			if(a1float){
				while(card--){
					*a3float++ = *a1float++ / *a2float++;
				}
			} else {
				while(card--){
					*a3int++ = *a1int++ / *a2int++;
				}
			}
			return;
		}
	} else if(toupper(*cmdline)=='V') {
		tp = checkstring(cmdline, (unsigned char *)"V_MULT");
		if(tp) {
			int i,j, numcols=0, numrows=0;
			MMFLOAT *a1float=NULL,*a2float=NULL,*a2sfloat=NULL,*a3float=NULL;
			getargs(&tp, 5,(unsigned char *)",");
			if(!(argc == 5)) error("Argument count");
			parsefloatrarray(argv[0],&a1float,1,2,dims,false);
			numcols=dims[0] - g_OptionBase;
			numrows=dims[1] - g_OptionBase;
			parsefloatrarray(argv[2],&a2float,1,1,dims,false);
			if((dims[0] - g_OptionBase) != numcols)error("Array size mismatch");
			parsefloatrarray(argv[4],&a3float,1,1,dims,true);
			if((dims[0] - g_OptionBase) != numrows)error("Array size mismatch");
			if(a3float==a1float || a3float==a2float)error("Destination array same as source");
			a2sfloat=a2float;
			numcols++;
			numrows++;
			for(i=0;i<numrows;i++){
				a2float=a2sfloat;
				*a3float=0.0;
				for(j=0;j<numcols;j++){
					*a3float= *a3float + ((*a1float++) * (*a2float++));
				}
				a3float++;
			}
			return;
		}

		tp = checkstring(cmdline, (unsigned char *)"V_ROTATE");
		if(tp) {
	    // xorigin!, yorigin!,angle!,xin!(), yin!(),xout(1), yout!()
			getargs(&tp, 13,(unsigned char *)",");
			if(!(argc == 13)) error("Argument count");
			MMFLOAT xorigin=getnumber(argv[0]);
			MMFLOAT yorigin=getnumber(argv[2]);
			MMFLOAT angle=getnumber(argv[4])/optionangle;
			MMFLOAT *a1float=NULL, *xfout=NULL, *yfout=NULL, cangle=cos(angle), sangle=sin(angle),x,y;
			int64_t *a1int=NULL, *xiout=NULL, *yiout=NULL;
			int numpoints=parsenumberarray(argv[6],&a1float,&a1int,4,1,dims, false);
			MMFLOAT *xin=GetTempMemory(numpoints * sizeof(MMFLOAT));
			for(int i=0;i<numpoints;i++)xin[i]=(a1float!=NULL ? a1float[i]-xorigin : (MMFLOAT)a1int[i]-xorigin);
			a1float=NULL;
			a1int=NULL;
			if(parsenumberarray(argv[8],&a1float,&a1int,5,1,dims, false)!=numpoints)error("Array size mismatch");
			MMFLOAT *yin=GetTempMemory(numpoints * sizeof(MMFLOAT));
			for(int i=0;i<numpoints;i++)yin[i]=(a1float!=NULL ? a1float[i]-yorigin : (MMFLOAT)a1int[i]-yorigin);
			a1float=NULL;
			a1int=NULL;
			if(parsenumberarray(argv[10],&xfout,&xiout,6,1,dims, false)!=numpoints)error("Array size mismatch");
			if(parsenumberarray(argv[12],&yfout,&yiout,7,1,dims, false)!=numpoints)error("Array size mismatch");
			for(int i=0;i<numpoints;i++){
				x= xin[i] * cangle - yin[i] * sangle + xorigin;
				y= yin[i] * cangle + xin[i] * sangle + yorigin;
				if(xfout)xfout[i]=x;
				else xiout[i]=round(x);
				if(yfout)yfout[i]=y;
				else yiout[i]=round(y);
			}
			return;
		}
		tp = checkstring(cmdline, (unsigned char *)"V_NORMALISE");
		if(tp) {
			int j, numrows=0, card2;
			MMFLOAT *a1float=NULL,*a1sfloat=NULL,*a2float=NULL,mag=0.0;
			getargs(&tp, 3,(unsigned char *)",");
			if(!(argc == 3)) error("Argument count");
			numrows=parsefloatrarray(argv[0],&a1float,1,1, dims, false);
			a1sfloat=a1float;
			card2=parsefloatrarray(argv[2],&a2float,2,1, dims, true);
			if(numrows!=card2)error("Array size mismatch");
			for(j=0;j<numrows;j++){
				mag+= (*a1sfloat) * (*a1sfloat);
				a1sfloat++;
			}
			mag= sqrt(mag);
			for(j=0;j<numrows;j++){
				*a2float++ = (*a1float++)/mag;
			}
			return;
		}

		tp = checkstring(cmdline, (unsigned char *)"V_CROSS");
		if(tp) {
			int j, numcols=0;
			MMFLOAT *a1float=NULL,*a2float=NULL,*a3float=NULL;
			MMFLOAT a[3],b[3];
			getargs(&tp, 5,(unsigned char *)",");
			if(!(argc == 5)) error("Argument count");
			numcols=parsefloatrarray(argv[0],&a1float,1,1, dims, false);
			if(numcols!=3)error("Argument 1 must be a 3 element floating point array");
			numcols=parsefloatrarray(argv[2],&a2float,2,1, dims, false);
			if(numcols!=3)error("Argument 2 must be a 3 element floating point array");
			numcols=parsefloatrarray(argv[4],&a3float,3,1, dims, true);
			if(numcols!=3)error("Argument 3 must be a 3 element floating point array");
			for(j=0;j<numcols;j++){
				a[j]=*a1float++;
				b[j]=*a2float++;
			}
			*a3float++ = a[1]*b[2] - a[2]*b[1];
			*a3float++ = a[2]*b[0] - a[0]*b[2];
			*a3float = a[0]*b[1] - a[1]*b[0];
			return;
		}
		tp = checkstring(cmdline, (unsigned char *)"V_PRINT");
		if(tp) {
			int j, numcols=0;
			MMFLOAT *a1float=NULL;
			int64_t *a1int=NULL;
			getargs(&tp, 3,(unsigned char *)",");
			if(!(argc == 1 || argc==3)) error("Argument count");
			numcols=parsenumberarray(argv[0],&a1float,&a1int,1,1, dims, false);
			if(a1float!=NULL){
				if(argc==3)error("Trying to print a float in HEX");
				PFlt(*a1float++);
				for(j=1;j<numcols;j++)PFltComma(*a1float++);
				PRet();
			} else {
				if(argc==3){
					if(checkstring(argv[2],(unsigned char *)"HEX")){
						PIntH(*a1int++);
						for(j=1;j<numcols;j++)PIntHC(*a1int++);
						PRet();
					} else error("Syntax");
				} else {
					PInt(*a1int++);
					for(j=1;j<numcols;j++)PIntComma(*a1int++);
					PRet();
				}
			}
			return;
		}
	} else if(toupper(*cmdline)=='M') {
		tp = checkstring(cmdline, (unsigned char *)"M_INVERSE");
		if(tp){
			int i, j, n, numcols=0, numrows=0;
			MMFLOAT *a1float=NULL, *a2float=NULL,det;
			getargs(&tp, 3,(unsigned char *)",");
			if(!(argc == 3)) error("Argument count");
			parsefloatrarray(argv[0], &a1float, 1,2,dims, false);
			numcols=dims[0] - g_OptionBase;
			numrows=dims[1] - g_OptionBase;
			parsefloatrarray(argv[2], &a2float, 2,2,dims, true);
			if(dims[0] - g_OptionBase != numcols || dims[1] - g_OptionBase!= numrows)error("Array size mismatch");
			if(numcols!=numrows)error("Array must be square");
			if(a1float==a2float)error("Same array specified for input and output");
			n=numrows+1;
			MMFLOAT **matrix=alloc2df(n,n);
			for(i=0;i<n;i++){ //load the matrix
				for(j=0;j<n;j++){
					matrix[j][i]=*a1float++;
				}
			}
			det=determinant(matrix,n);
			if(det==0.0){
				dealloc2df(matrix,numcols,numrows);
				error("Determinant of array is zero");
			}
			MMFLOAT **matrix1=alloc2df(n,n);
			cofactor(matrix, matrix1, n);
			for(i=0;i<n;i++){ //load the matrix
				for(j=0;j<n;j++){
					*a2float++=matrix1[j][i];
				}
			}
			dealloc2df(matrix,numcols,numrows);
			dealloc2df(matrix1,numcols,numrows);

			return;
		}
		tp = checkstring(cmdline, (unsigned char *)"M_TRANSPOSE");
		if(tp) {
			int i,j, numcols1=0, numrows1=0, numcols2=0, numrows2=0;
			MMFLOAT *a1float=NULL,*a2float=NULL;
			getargs(&tp, 3,(unsigned char *)",");
			if(!(argc == 3)) error("Argument count");
			parsefloatrarray(argv[0], &a1float, 1,2,dims, false);
			numcols1=numrows2=dims[0] - g_OptionBase;
			numrows1=numcols2=dims[1] - g_OptionBase;
			parsefloatrarray(argv[2], &a2float,2,2,dims, true);
			if(numcols2 !=dims[0] - g_OptionBase)error("Array size mismatch");
			if(numrows2 !=dims[1] - g_OptionBase)error("Array size mismatch");
			numcols1++;
			numrows1++;
			numcols2++;
			numrows2++;
			MMFLOAT **matrix1=alloc2df(numcols1,numrows1);
			MMFLOAT **matrix2=alloc2df(numcols2,numrows2);
			for(i=0;i<numrows1;i++){
				for(j=0;j<numcols1;j++){
					matrix1[j][i]=*a1float++;
				}
			}
			for(i=0;i<numrows1;i++){
				for(j=0;j<numcols1;j++){
					matrix2[i][j]=matrix1[j][i];
				}
			}
			for(i=0;i<numrows2;i++){
				for(j=0;j<numcols2;j++){
					*a2float++=matrix2[j][i];
				}
			}
			dealloc2df(matrix1,numcols1,numrows1);
			dealloc2df(matrix2,numcols2,numrows2);
			return;
		}

		tp = checkstring(cmdline, (unsigned char *)"M_MULT");
		if(tp) {
			int i,j, k, numcols1=0, numrows1=0, numcols2=0, numrows2=0, numcols3=0, numrows3=0;
			MMFLOAT *a1float=NULL,*a2float=NULL,*a3float=NULL;
			getargs(&tp, 5,(unsigned char *)",");
			if(!(argc == 5)) error("Argument count");
			parsefloatrarray(argv[0], &a1float, 1, 2, dims, false);
			numcols1=numrows2=dims[0] - g_OptionBase + 1;
			numrows1=dims[1] - g_OptionBase + 1;
			parsefloatrarray(argv[2], &a2float, 2, 2, dims, false);
			numcols2=dims[0] - g_OptionBase + 1;
			numrows2=dims[1] - g_OptionBase + 1;
			if(numrows2!=numcols1)error("Input array size mismatch");
			parsefloatrarray(argv[4], &a3float, 3, 2, dims, true);
			numcols3=dims[0] - g_OptionBase + 1;
			numrows3=dims[1] - g_OptionBase + 1;
			if(numcols3!=numcols2 || numrows3!=numrows1)error("Output array size mismatch");
			if(a3float==a1float || a3float==a2float)error("Destination array same as source");
//			MMFLOAT **matrix1=alloc2df(numcols1,numrows1);
//			MMFLOAT **matrix2=alloc2df(numcols2,numrows2);
/*			s=a1float;
			for(i=0;i<numrows1;i++){ //load the first matrix
				for(j=0;j<numcols1;j++){
					matrix1[j][i]=*a1float++;
				}
			}
			a1float=s;
			s=a2float;
			for(i=0;i<numrows2;i++){ //load the second matrix
				for(j=0;j<numcols2;j++){
					matrix2[j][i]=*a2float++;
				}
			}
			a2float=s;*/
	// Now calculate the dot products
			for(i=0;i<numrows3;i++){
				for(j=0;j<numcols3;j++){
					*a3float=0.0;
					for(k=0;k<numcols1;k++){
	//					PFlt(farr2d(a1float,numcols1,k,i));PFltComma(matrix1[k][i]);PFltComma(farr2d(a2float,numcols2,j,k));PFltComma(matrix2[j][k]);PRet();
						*a3float+=farr2d(a1float,numcols1,k,i)*farr2d(a2float,numcols2,j,k);
	//					*a3float+= matrix1[k][i] * matrix2[j][k];
					}
					a3float++;
				}
			}

//			dealloc2df(matrix1,numcols1,numrows1);
//			dealloc2df(matrix2,numcols2,numrows2);
			return;
		}
		tp = checkstring(cmdline, (unsigned char *)"M_PRINT");
		if(tp) {
			int i,j, numcols=0, numrows=0;
			MMFLOAT *a1float=NULL;
			int64_t *a1int=NULL;
			// need three arrays with same cardinality, second array must be 2 dimensional
			getargs(&tp, 1,(unsigned char *)",");
			if(!(argc == 1)) error("Argument count");
			parsenumberarray(argv[0],&a1float,&a1int,1,2,dims, false);
			numcols=dims[0]+1-g_OptionBase;
			numrows=dims[1]+1-g_OptionBase;
//			MMFLOAT **matrix=alloc2df(numcols,numrows);
//			int64_t **imatrix= (int64_t **)matrix;
			if(a1float!=NULL){
/*				for(i=0;i<numrows;i++){
					for(j=0;j<numcols;j++){
						matrix[j][i]=*a1float++;
					}
				}*/
				for(i=0;i<numrows;i++){
					PFlt(farr2d(a1float,numcols,0,i));
//					PFlt(matrix[0][i]);
					for(j=1;j<numcols;j++){
						PFltComma(farr2d(a1float,numcols,j,i));
//						PFltComma(matrix[j][i]);
					}
					PRet();
				}
			} else {
/*				for(i=0;i<numrows;i++){
					for(j=0;j<numcols;j++){
						imatrix[j][i]=*a1int++;
					}
				}*/
				for(i=0;i<numrows;i++){
					PInt(iarr2d(a1int,numcols,0,i));
//					PInt(imatrix[0][i]);
					for(j=1;j<numcols;j++){
						PIntComma(iarr2d(a1int,numcols,j,i));
//						PIntComma(imatrix[j][i]);
					}
					PRet();
				}
			}
//			dealloc2df(matrix,numcols,numrows);
			return;
		}
	} else if(toupper(*cmdline)=='Q') {

		tp = checkstring(cmdline, (unsigned char *)"Q_INVERT");
		if(tp) {
			int card;
			MMFLOAT *q=NULL,*n=NULL;
			getargs(&tp, 3,(unsigned char *)",");
			if(!(argc == 3)) error("Argument count");
			card=parsefloatrarray(argv[0],&q,1,1, dims, false);
			if(card!=5)error("Argument 1 must be a 5 element floating point array");
			card=parsefloatrarray(argv[2],&n,2,1, dims, true);
			if(card!=5)error("Argument 2 must be a 5 element floating point array");
			Q_Invert(q, n);
			return;
		}

		tp = checkstring(cmdline, (unsigned char *)"Q_VECTOR");
		if(tp) {
			int card;
			MMFLOAT *q=NULL;
			MMFLOAT mag=0.0;
			getargs(&tp, 7,(unsigned char *)",");
			if(!(argc == 7)) error("Argument count");
			MMFLOAT x=getnumber(argv[0]);
			MMFLOAT y=getnumber(argv[2]);
			MMFLOAT z=getnumber(argv[4]);
			card=parsefloatrarray(argv[6],&q,4,1, dims, true);
			if(card!=5)error("Argument 4 must be a 5 element floating point array");
			mag=sqrt(x*x + y*y + z*z) ;//calculate the magnitude
			q[0]=0.0; //create a normalised vector
			q[1]=x/mag;
			q[2]=y/mag;
			q[3]=z/mag;
			q[4]=mag;
			return;
		}

		tp = checkstring(cmdline, (unsigned char *)"Q_EULER");
		if(tp) {
			int card;
			MMFLOAT *q=NULL;
			getargs(&tp, 7,(unsigned char *)",");
			if(!(argc == 7)) error("Argument count");
			MMFLOAT yaw=-getnumber(argv[0])/optionangle;
			MMFLOAT pitch=getnumber(argv[2])/optionangle;
			MMFLOAT roll=getnumber(argv[4])/optionangle;
			card=parsefloatrarray(argv[6],&q,4,1, dims, true);
			if(card!=5)error("Argument 4 must be a 5 element floating point array");
			MMFLOAT s1=sin(pitch/2);
			MMFLOAT c1=cos(pitch/2);
			MMFLOAT s2=sin(yaw/2);
			MMFLOAT c2=cos(yaw/2);
			MMFLOAT s3=sin(roll/2);
			MMFLOAT c3=cos(roll/2);
			q[1] = s1 * c2 * c3 - c1 * s2 * s3;
			q[2] = c1 * s2 * c3 + s1 * c2 * s3;
			q[3] = c1 * c2 * s3 - s1 * s2 * c3;
			q[0] = c1 * c2 * c3 + s1 * s2 * s3;
			q[4]=1.0;
			return;
		}

		tp = checkstring(cmdline, (unsigned char *)"Q_CREATE");
		if(tp) {
			int card;
			MMFLOAT *q=NULL;
			MMFLOAT mag=0.0;
			getargs(&tp, 9,(unsigned char *)",");
			if(!(argc == 9)) error("Argument count");
			MMFLOAT theta=getnumber(argv[0]);
			MMFLOAT x=getnumber(argv[2]);
			MMFLOAT y=getnumber(argv[4]);
			MMFLOAT z=getnumber(argv[6]);
			card=parsefloatrarray(argv[8],&q,5,1, dims, true);
			if(card!=5)error("Argument 4 must be a 5 element floating point array");
			MMFLOAT sineterm= sin(theta/2.0/optionangle);
			q[0]=cos(theta/2.0);
			q[1]=x* sineterm;
			q[2]=y* sineterm;
			q[3]=z* sineterm;
			mag=sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]) ;//calculate the magnitude
			q[0]=q[0]/mag; //create a normalised quaternion
			q[1]=q[1]/mag;
			q[2]=q[2]/mag;
			q[3]=q[3]/mag;
			q[4]=1.0;
			return;
		}

		tp = checkstring(cmdline, (unsigned char *)"Q_MULT");
		if(tp) {
			MMFLOAT *q1=NULL,*q2=NULL,*n=NULL;
			int card;
			getargs(&tp, 5,(unsigned char *)",");
			if(!(argc == 5)) error("Argument count");
			card=parsefloatrarray(argv[0],&q1,1,1, dims, false);
			if(card!=5)error("Argument 1 must be a 5 element floating point array");
			card=parsefloatrarray(argv[2],&q2,2,1, dims, false);
			if(card!=5)error("Argument 2 must be a 5 element floating point array");
			card=parsefloatrarray(argv[4],&n,31,1, dims, true);
			if(card!=5)error("Argument 3 must be a 5 element floating point array");
			Q_Mult(q1, q2, n);
			return;
		}

		tp = checkstring(cmdline, (unsigned char *)"Q_ROTATE");
		if(tp) {
			int card;
			MMFLOAT *q1=NULL,*v1=NULL,*n=NULL;
			MMFLOAT temp[5], qtemp[5];
			getargs(&tp, 5,(unsigned char *)",");
			if(!(argc == 5)) error("Argument count");
			card=parsefloatrarray(argv[0],&q1,1,1, dims, false);
			if(card!=5)error("Argument 1 must be a 5 element floating point array");
			card=parsefloatrarray(argv[2],&v1,2,1, dims, false);
			if(card!=5)error("Argument 2 must be a 5 element floating point array");
			card=parsefloatrarray(argv[4],&n,31,1, dims, true);
			if(card!=5)error("Argument 3 must be a 5 element floating point array");
			Q_Mult(q1, v1, temp);
			Q_Invert(q1, qtemp);
			Q_Mult(temp, qtemp, n);
			return;
		}
	} else {
		tp = checkstring(cmdline, (unsigned char *)"AES128");
		if(tp) {
			struct AES_ctx ctx;
 			int64_t *outint=NULL;
			unsigned char *outstr=NULL;
			MMFLOAT *outflt=NULL;
			unsigned char keyx[16];
			unsigned char * p;
			int card;
//unsigned char * parseAES(uint8_t *p, int ivadd, uint8_t *keyx, uint8_t *ivx, int64_t **outint, unsigned char **outstr, MMFLOAT **outfloat, int *card2){
			if((p=checkstring(tp, (unsigned char *)"ENCRYPT CBC"))){
				uint8_t iv[16];
				for(int i=0;i<16;i++)iv[i]=getrnd();
				uint8_t *inx= parseAES(p, 16, &keyx[0], &iv[0], &outint, &outstr, &outflt, &card);
				AES_init_ctx_iv(&ctx, keyx, iv);
				AES_CBC_encrypt_buffer(&ctx, inx, card);
				returnAES(outint, outflt, outstr, inx, iv, card);
				return;
			} else if((p=checkstring(tp, (unsigned char *)"DECRYPT CBC"))){
//   				uint8_t iv[16]16  = { 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f };
				uint8_t *inx = parseAES(p, -16, &keyx[0], NULL, &outint, &outstr, &outflt, &card);
				AES_init_ctx_iv(&ctx, keyx, inx);
				AES_CBC_decrypt_buffer(&ctx, &inx[16], card-16);
				returnAES(outint, outflt, outstr, &inx[16], NULL, card-16);
				return;
			} else if((p=checkstring(tp, (unsigned char *)"ENCRYPT ECB"))){
				struct AES_ctx ctxcopy;
				uint8_t *inx = parseAES(p, 0, &keyx[0], NULL, &outint, &outstr, &outflt, &card);
				AES_init_ctx(&ctxcopy, keyx);
				for(int i=0;i<card;i+=16){
					memcpy(&ctx,&ctxcopy,sizeof(ctx));
					AES_ECB_encrypt(&ctx, &inx[i]);
				}
				returnAES(outint, outflt, outstr, inx, NULL, card);
				return;
			} else if((p=checkstring(tp, (unsigned char *)"DECRYPT ECB"))){
				struct AES_ctx ctxcopy;
				uint8_t *inx = parseAES(p, 0, &keyx[0], NULL, &outint, &outstr, &outflt, &card);
				AES_init_ctx(&ctxcopy, keyx);
				for(int i=0;i<card;i+=16){
					memcpy(&ctx,&ctxcopy,sizeof(ctx));
					AES_ECB_decrypt(&ctx, &inx[i]);
				}
				returnAES(outint, outflt, outstr, inx, NULL, card);
				return;
			} else if((p=checkstring(tp, (unsigned char *)"ENCRYPT CTR"))){
				uint8_t iv[16];
				for(int i=0;i<16;i++)iv[i]=getrnd();
				uint8_t *inx= parseAES(p, 16, &keyx[0], &iv[0], &outint, &outstr, &outflt, &card);
				AES_init_ctx_iv(&ctx, keyx, iv);
				AES_CTR_xcrypt_buffer(&ctx, inx, card);
				returnAES(outint, outflt, outstr, inx, iv, card);
				return;
			} else if((p=checkstring(tp, (unsigned char *)"DECRYPT CTR"))){
				uint8_t *inx = parseAES(p, -16, &keyx[0], NULL, &outint, &outstr, &outflt, &card);
				AES_init_ctx_iv(&ctx, keyx, inx);
				AES_CTR_xcrypt_buffer(&ctx, &inx[16], card-16);
				returnAES(outint, outflt, outstr, &inx[16], NULL, card-16);
				return;
			} else error("Syntax");
		}
		tp = checkstring(cmdline, (unsigned char *)"PID");
		if(tp) {
			unsigned char * pi;
			if((pi=checkstring(tp, (unsigned char *)"START"))){
				int channel = getint(pi,1,MAXPID);
				if(PIDchannels[channel].interrupt==NULL)error("Channel not initialised");
				PIDchannels[channel].timenext=time_us_64() + (PIDchannels[channel].PIDparams->T * 1000);
				PIDchannels[channel].active=true;
				InterruptUsed=true;
			} else if((pi=checkstring(tp, (unsigned char *)"STOP"))){
				int channel = getint(pi,1,MAXPID);
				if(PIDchannels[channel].interrupt==NULL)error("Channel not initialised");
				PIDchannels[channel].active=false;
				memset(&PIDchannels[channel],0,sizeof(s_PIDchan));
			} else if((pi=checkstring(tp, (unsigned char *)"INIT"))){
				getargs(&pi,5,(unsigned char *)",");
				if(argc!=5)error("Syntax");
				MMFLOAT *q1=NULL;
				int channel = getint(argv[0],1,MAXPID);
				int card=parsefloatrarray(argv[2],&q1,2,1, NULL, true);
				PIDchannels[channel].PIDparams=(PIDController *)q1;
				if(card!=14)error("Argument 2 must be a 14 element floating point array");
				if(PIDchannels[channel].PIDparams->T < 0.001)error("Invalid update rate");
				PIDchannels[channel].interrupt=GetIntAddress(argv[4]);	
			} else error("Syntax");
			return;
		}
		tp = checkstring(cmdline, (unsigned char *)"ADD");
		if(tp) {
			array_add(tp);
			return;
		}
		tp = checkstring(cmdline, (unsigned char *)"POWER");
		if(tp) {
			int i,card1=1, card2=1;
			MMFLOAT *a1float=NULL,*a2float=NULL, scale;
			int64_t *a1int=NULL, *a2int=NULL;
			getargs(&tp, 5,(unsigned char *)",");
			if(!(argc == 5)) error("Argument count");
			card1=parsenumberarray(argv[0], &a1float, &a1int, 1, 0,dims, false);
		    evaluate(argv[2], &f, &i64, &s, &t, false);
		    if(t & T_STR) error("Syntax");
		    scale=getnumber(argv[2]);
			card2=parsenumberarray(argv[4], &a2float, &a2int, 3, 0,dims, true);
			if(card1 != card2)error("Array size mismatch");
			if(scale!=1.0){
				if(a2float!=NULL && a1float!=NULL){
					for(i=0; i< card1;i++)*a2float++ = pow(*a1float++,(t & T_INT) ? (MMFLOAT)i64 : f);
				} else if(a2float!=NULL && a1float==NULL){
					for(i=0; i< card1;i++)(*a2float++) = pow((MMFLOAT)*a1int++,((t & T_INT) ? (MMFLOAT)i64 : f));
				} else if(a2float==NULL && a1float!=NULL){
					for(i=0; i< card1;i++)(*a2int++) = FloatToInt64(pow(*a1float++,((t & T_INT) ? i64 : FloatToInt64(f))));
				} else {
					for(i=0; i< card1;i++)(*a2int++) = FloatToInt64(pow(*a1int++,(t & T_INT) ? i64 : FloatToInt64(f)));
				}
			} else {
				if(a2float!=NULL && a1float!=NULL){
					for(i=0; i< card1;i++)*a2float++ = *a1float++;
				} else if(a2float!=NULL && a1float==NULL){
					for(i=0; i< card1;i++)(*a2float++) = ((MMFLOAT)*a1int++);
				} else if(a2float==NULL && a1float!=NULL){
					for(i=0; i< card1;i++)(*a2int++) = FloatToInt64(*a1float++);
				} else {
					for(i=0; i< card1;i++)*a2int++ = *a1int++;
				}
			}
			return;
		}

		tp = checkstring(cmdline, (unsigned char *)"WINDOW");
		if(tp) {
			int i,card1=1, card2=1;
			MMFLOAT *a1float=NULL,*a2float=NULL, outmin,outmax, inmin=1.5e+308 , inmax=-1.5e308;
			int64_t *a1int=NULL, *a2int=NULL;
			getargs(&tp, 11,(unsigned char *)",");
			if(!(argc == 7 || argc==11)) error("Argument count");
			card1=parsenumberarray(argv[0], &a1float, &a1int, 1, 0,dims, false);
		    outmin=getnumber(argv[2]);
			outmax=getnumber(argv[4]);
			card2=parsenumberarray(argv[6], &a2float, &a2int, 4, 0,dims, true);
			if(card1 != card2)error("Size mismatch");
			for(i=0; i< card1;i++){
				if(a1float!=NULL){ //find min and max if in is a float
					if(a1float[i]<inmin)inmin=a1float[i];
					if(a1float[i]>inmax)inmax=a1float[i];
				} else {
					if(a1int[i]<inmin)inmin=(MMFLOAT)a1int[i];
					if(a1int[i]>inmax)inmax=(MMFLOAT)a1int[i];
				}
			}
			if(argc==11){
				void *ptr1 = findvar(argv[8], V_FIND);
				if(!(g_vartbl[g_VarIndex].type & (T_NBR | T_INT))) error("Invalid variable");
				if(g_vartbl[g_VarIndex].type==T_INT)*(long long int *)ptr1=(long long int)inmin;
				else *(MMFLOAT *)ptr1=inmin;
				void *ptr2 = findvar(argv[10], V_FIND);
				if(!(g_vartbl[g_VarIndex].type & (T_NBR | T_INT))) error("Invalid variable");
				if(g_vartbl[g_VarIndex].type==T_INT)*(long long int *)ptr2=(long long int)inmax;
				else *(MMFLOAT *)ptr2=inmax;
			}
			if(a2float!=NULL && a1float!=NULL){ //in and out are floats
				for(i=0; i< card1;i++)a2float[i] = ((a1float[i]-inmin)/(inmax-inmin))*(outmax-outmin)+outmin;
			} else if(a2float==NULL && a1float!=NULL){ //in is a float and out is an integer
				for(i=0; i< card1;i++)a2int[i] =(long long int)(((a1float[i]-inmin)/(inmax-inmin))*(outmax-outmin)+outmin);
			} else if(a2float!=NULL && a1float==NULL){ //in is an integer and out is a float
				for(i=0; i< card1;i++)a2float[i] =((((MMFLOAT)a1int[i]-inmin)/(inmax-inmin))*(outmax-outmin)+outmin);
			} else {  // in and out are integers
				for(i=0; i< card1;i++)a2int[i] =(long long int)((((MMFLOAT)a1int[i]-inmin)/(inmax-inmin))*(outmax-outmin)+outmin);
			}
			return;
		}


		tp = checkstring(cmdline, (unsigned char *)"RANDOMIZE");
		if(tp) {
			int i;
			getargs(&tp,1,(unsigned char *)",");
			if(argc==1)i = getinteger(argv[0]);
			else i=time_us_32();
			if(i < 0) error("Number out of bounds");
			if(g_myrand==NULL)g_myrand=(struct tagMTRand *)GetMemory(sizeof(struct tagMTRand));
			seedRand(i);
			return;
		}
		tp = checkstring(cmdline, (unsigned char *)"INTERPOLATE");
		if(tp) {
			int i,card1, card2, card3;
			MMFLOAT *a1float=NULL,*a2float=NULL, *a3float=NULL, scale, tmp1, tmp2, tmp3;
			int64_t *a1int=NULL, *a2int=NULL, *a3int=NULL;
			getargs(&tp, 7,(unsigned char *)",");
			if(!(argc == 7)) error("Argument count");
			card1=parsenumberarray(argv[0], &a1float, &a1int, 1, 0, dims, false);
		    evaluate(argv[4], &f, &i64, &s, &t, false);
		    if(t & T_STR) error("Syntax");
		    scale=getnumber(argv[4]);
			card2=parsenumberarray(argv[2], &a2float, &a2int, 3, 0, dims, false);
			card3=parsenumberarray(argv[6], &a3float, &a3int, 4, 0, dims, true);
			if((card1 != card2) || (card1!=card3))error("Size mismatch");
			if(a3int!=NULL){
				if((a1int==a2int) || (a1int==a3int) || (a2int==a3int))error("Arrays must be different");
				for(i=0; i< card1;i++){
					if(a1int!=NULL)tmp1=(MMFLOAT)*a1int++;
					else tmp1=*a1float++;
					if(a2int!=NULL)tmp2=(MMFLOAT)*a2int++;
					else tmp2=*a2float++;
					tmp3=(tmp2-tmp1)*scale + tmp1;
					*a3int++=FloatToInt64(tmp3);
				}
			} else {
				if((a1float==a2float) || (a1float==a3float) || (a2float==a3float))error("Arrays must be different");
				for(i=0; i< card1;i++){
					if(a1int!=NULL)tmp1=(MMFLOAT)*a1int++;
					else tmp1=*a1float++;
					if(a2int!=NULL)tmp2=(MMFLOAT)*a2int++;
					else tmp2=*a2float++;
					tmp3=(tmp2-tmp1)*scale + tmp1;
					*a3float++=tmp3;
				}
			}
			return;
		}
		tp = checkstring(cmdline, (unsigned char *)"INSERT");
		if(tp) {
			array_insert(tp);
			return;
		}
		tp = checkstring(cmdline, (unsigned char *)"FFT");
		if(tp) {
			cmd_FFT(tp);
			return;
		}
	}

	error("Syntax");
}
/* 
 * @cond
 * The following section will be excluded from the documentation.
 */

fcplx getComplex(unsigned char *in){
	int64_t i=getinteger(in);
	fcplx x;
	memcpy(&x, &i, 8);
	return x;
}
void retComplex(fcplx in){
	iret=0;
	memcpy(&iret, &in, 8);
	targ=T_INT;
}
/*  @endcond */

void fun_math(void){
	unsigned char *tp, *tp1;
#ifdef rp2350
	int dims[MAXDIM]={0};
#else
	short dims[MAXDIM]={0};
#endif
	skipspace(ep);
	if(toupper(*ep)=='P'){
		tp = checkstring(ep, (unsigned char *)"PID");
		if(tp){
			getargs(&tp, 5,(unsigned char *)",");
			if(argc != 5)error("Syntax");
			int channel=getint(argv[0],1,MAXPID);
			MMFLOAT setpoint=getnumber(argv[2]);
			MMFLOAT measurement=getnumber(argv[4]);
			if(PIDchannels[channel].interrupt==NULL)error("Channel not configured");
			if(!PIDchannels[channel].active)error("Channel not active");
			fret=PIDController_Update(PIDchannels[channel].PIDparams, setpoint, measurement);
			targ=T_NBR;
			return;
		}
	}
	if(toupper(*ep)=='A'){
		tp = checkstring(ep, (unsigned char *)"ATAN3");
		if(tp) {
			MMFLOAT y,x,z;
			getargs(&tp, 3,(unsigned char *)",");
			if(argc != 3)error("Syntax");
			y=getnumber(argv[0]);
			x=getnumber(argv[2]);
			z=atan2(y,x);
			if (z < 0.0) z = z + M_TWOPI;
			fret=z;
	 		fret *=optionangle;
			targ = T_NBR;
			return;
		}
	} else if(toupper(*ep)=='C') {
		if(ep[1]=='_'){
			if((tp=checkstring(&ep[2],(unsigned char *)"REAL"))){
				fret=(MMFLOAT)crealf(getComplex(tp));
				targ=T_NBR;
			} else if((tp=checkstring(&ep[2],(unsigned char *)"IMAG"))){
				fret=(MMFLOAT)cimagf(getComplex(tp));
				targ=T_NBR;
			} else if((tp=checkstring(&ep[2],(unsigned char *)"MOD"))){
				MMFLOAT a=(MMFLOAT)crealf(getComplex(tp));
				MMFLOAT b=(MMFLOAT)cimagf(getComplex(tp));
				a*=a;
				b*=b;
				b+=a;
				fret=sqrt(b);
				targ=T_NBR;
			} else if((tp=checkstring(&ep[2],(unsigned char *)"PHASE"))){
				MMFLOAT a=(MMFLOAT)crealf(getComplex(tp));
				MMFLOAT b=(MMFLOAT)cimagf(getComplex(tp));
				fret=atan2(b,a);
				if(useoptionangle)fret*=optionangle;
				targ=T_NBR;
			} else if((tp=checkstring(&ep[2],(unsigned char *)"CARG"))){
				fret=(MMFLOAT)cargf(getComplex(tp));
				targ=T_NBR;
			} else if((tp=checkstring(&ep[2],(unsigned char *)"ADD"))){
				getargs(&tp,3,(unsigned char *)",");
				fcplx x=getComplex(argv[0])+getComplex(argv[2]);
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"MUL"))){
				getargs(&tp,3,(unsigned char *)",");
				fcplx x=getComplex(argv[0])*getComplex(argv[2]);
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"SUB"))){
				getargs(&tp,3,(unsigned char *)",");
				fcplx x=getComplex(argv[0])-getComplex(argv[2]);
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"DIV"))){
				getargs(&tp,3,(unsigned char *)",");
				fcplx x=getComplex(argv[0])/getComplex(argv[2]);
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"POW"))){
				getargs(&tp,3,(unsigned char *)",");
				fcplx x=cpowf(getComplex(argv[0]),getComplex(argv[2]));
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"CONJ"))){
				fcplx x=conjf(getComplex(tp));
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"ACOS"))){
				fcplx x=cacosf(getComplex(tp));
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"ASIN"))){
				fcplx x=casinf(getComplex(tp));
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"ATAN"))){
				fcplx x=catanf(getComplex(tp));
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"SIN"))){
				fcplx x=csinf(getComplex(tp));
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"COS"))){
				fcplx x=ccosf(getComplex(tp));
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"TAN"))){
				fcplx x=ctanf(getComplex(tp));
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"SINH"))){
				fcplx x=csinhf(getComplex(tp));
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"COSH"))){
				fcplx x=ccoshf(getComplex(tp));
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"TANH"))){
				fcplx x=ctanhf(getComplex(tp));
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"ASINH"))){
				fcplx x=casinhf(getComplex(tp));
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"ACOSH"))){
				fcplx x=cacoshf(getComplex(tp));
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"ATANH"))){
				fcplx x=catanhf(getComplex(tp));
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"EXP"))){
				fcplx x=cexpf(getComplex(tp));
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"LOG"))){
				fcplx x=clogf(getComplex(tp));
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"ABS"))){
				fcplx x=cabsf(getComplex(tp));
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"SQRT"))){
				fcplx x=csqrtf(getComplex(tp));
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"PROJ"))){
				fcplx x=cprojf(getComplex(tp));
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"CPLX"))){
				getargs(&tp,3,(unsigned char *)",");
				fcplx x=(float)(getnumber(argv[0]))+(float)(getnumber(argv[2]))*I;
				retComplex(x);
			} else if((tp=checkstring(&ep[2],(unsigned char *)"POLAR"))){
				getargs(&tp,3,(unsigned char *)",");
				MMFLOAT r=getnumber(argv[0]);
				MMFLOAT theta=getnumber(argv[2])/optionangle;
				MMFLOAT stheta=sin(theta)*r;
				MMFLOAT ctheta=cos(theta)*r;
				fcplx x=(float)(ctheta)+(float)(stheta)*I;
				retComplex(x);
			} else error("Syntax");
			return;
		}
		tp = checkstring(ep, (unsigned char *)"CRC8");
		if(tp) {
		    int i;
		    MMFLOAT *a1float=NULL;
		    int64_t *a1int=NULL;
			getargs(&tp,13,(unsigned char *)",");
			if(argc<1)error("Syntax");
			uint8_t polynome=CRC8_DEFAULT_POLYNOME;
			uint8_t startmask=0;
			uint8_t endmask=0;
			uint8_t reverseIn=false;
			uint8_t reverseOut=false;
			unsigned char *a1str=NULL;
			int length=0;
			if(argc>1 && *argv[2])length=getint(argv[2],1,65535);
			length=parseany(argv[0], &a1float, &a1int, &a1str, &length, false);
			uint8_t *array=GetTempMemory(length);
			if(argc>3 && *argv[4])polynome=getint(argv[4],0,255);
			if(argc>5 && *argv[6])startmask=getint(argv[6],0,255);
			if(argc>7 && *argv[8])endmask=getint(argv[8],0,255);
			if(argc>9 && *argv[10])reverseIn=getint(argv[10],0,1);
			if(argc==13 && *argv[12])reverseOut=getint(argv[10],0,1);
			for(i=0;i<length;i++){
				if(a1float){
					if(a1float[i]>255)error("Variable > 255");
					else array[i]=(uint8_t)a1float[i];
				} else if(a1int){
					if(a1int[i]>255)error("Variable > 255");
					else array[i]=(uint8_t)a1int[i];
				} else 	memcpy(array,&a1str[1],length);
			}
			iret=crc8(array, length, polynome, startmask, endmask, reverseIn, reverseOut);
			targ=T_INT;
			return;
		}
		tp = checkstring(ep, (unsigned char *)"CRC12");
		if(tp) {
		    int i;
		    MMFLOAT *a1float=NULL;
		    int64_t *a1int=NULL;
			getargs(&tp,13,(unsigned char *)",");
			if(argc<1)error("Syntax");
			uint16_t polynome=CRC12_DEFAULT_POLYNOME;
			uint16_t startmask=0;
			uint16_t endmask=0;
			uint8_t reverseIn=false;
			uint8_t reverseOut=false;
			unsigned char *a1str=NULL;
			int length=0;
			if(argc>1 && *argv[2])length=getint(argv[2],1,65535);
			length=parseany(argv[0], &a1float, &a1int, &a1str, &length, false);
			uint8_t *array=GetTempMemory(length);
			if(argc>3 && *argv[4])polynome=getint(argv[4],0,4095);
			if(argc>5 && *argv[6])startmask=getint(argv[6],0,4095);
			if(argc>7 && *argv[8])endmask=getint(argv[8],0,4095);
			if(argc>9 && *argv[10])reverseIn=getint(argv[10],0,1);
			if(argc==13 && *argv[12])reverseOut=getint(argv[10],0,1);
			for(i=0;i<length;i++){
				if(a1float){
					if(a1float[i]>255)error("Variable > 255");
					else array[i]=(uint8_t)a1float[i];
				} else if(a1int){
					if(a1int[i]>255)error("Variable > 255");
					else array[i]=(uint8_t)a1int[i];
				} else 	memcpy(array,&a1str[1],length);
			}
			iret=crc12(array, length, polynome, startmask, endmask, reverseIn, reverseOut);
			targ=T_INT;
			return;
		}
		tp = checkstring(ep, (unsigned char *)"CRC16");
		if(tp) {
		    int i;
		    MMFLOAT *a1float=NULL;
		    int64_t *a1int=NULL;
			getargs(&tp,13,(unsigned char *)",");
			if(argc<1)error("Syntax");
			uint16_t polynome=CRC16_DEFAULT_POLYNOME;
			uint16_t startmask=0;
			uint16_t endmask=0;
			uint8_t reverseIn=false;
			uint8_t reverseOut=false;
			unsigned char *a1str=NULL;
			int length=0;
			if(argc>1 && *argv[2])length=getint(argv[2],1,65535);
			length=parseany(argv[0], &a1float, &a1int, &a1str, &length, false);
			uint8_t *array=GetTempMemory(length);
			if(argc>3 && *argv[4])polynome=getint(argv[4],0,65535);
			if(argc>5 && *argv[6])startmask=getint(argv[6],0,65535);
			if(argc>7 && *argv[8])endmask=getint(argv[8],0,65535);
			if(argc>9 && *argv[10])reverseIn=getint(argv[10],0,1);
			if(argc==13 && *argv[12])reverseOut=getint(argv[10],0,1);
			for(i=0;i<length;i++){
				if(a1float){
					if(a1float[i]>255)error("Variable > 255");
					else array[i]=(uint8_t)a1float[i];
				} else if(a1int){
					if(a1int[i]>255)error("Variable > 255");
					else array[i]=(uint8_t)a1int[i];
				} else 	memcpy(array,&a1str[1],length);
			}
			iret=crc16(array, length, polynome, startmask, endmask, reverseIn, reverseOut);
			targ=T_INT;
			return;
		}
		tp = checkstring(ep, (unsigned char *)"CRC32");
		if(tp) {
		    int i;
		    MMFLOAT *a1float=NULL;
		    int64_t *a1int=NULL;
			getargs(&tp,13,(unsigned char *)",");
			if(argc<1)error("Syntax");
			uint32_t polynome=CRC32_DEFAULT_POLYNOME;
			uint32_t startmask=0;
			uint32_t endmask=0;
			uint8_t reverseIn=false;
			uint8_t reverseOut=false;
			unsigned char *a1str=NULL;
			int length=0;
			if(argc>1 && *argv[2])length=getint(argv[2],1,65535);
			length=parseany(argv[0], &a1float, &a1int, &a1str, &length, false);
			uint8_t *array=GetTempMemory(length);
			if(argc>3 && *argv[4])polynome=getint(argv[4],0,0xFFFFFFFF);
			if(argc>5 && *argv[6])startmask=getint(argv[6],0,0xFFFFFFFF);
			if(argc>7 && *argv[8])endmask=getint(argv[8],0,0xFFFFFFFF);
			if(argc>9 && *argv[10])reverseIn=getint(argv[10],0,1);
			if(argc==13 && *argv[12])reverseOut=getint(argv[10],0,1);
			for(i=0;i<length;i++){
				if(a1float){
					if(a1float[i]>255)error("Variable > 255");
					else array[i]=(uint8_t)a1float[i];
				} else if(a1int){
					if(a1int[i]>255)error("Variable > 255");
					else array[i]=(uint8_t)a1int[i];
				} else 	memcpy(array,&a1str[1],length);
			}
			iret=crc32(array, length, polynome, startmask, endmask, reverseIn, reverseOut);
			targ=T_INT;
			return;
		}
		tp = checkstring(ep, (unsigned char *)"COSH");
		if(tp) {
			getargs(&tp, 1,(unsigned char *)",");
			if(!(argc == 1)) error("Argument count");
			fret=cosh(getnumber(argv[0]));
			targ=T_NBR;
			return;
		}
		tp = checkstring(ep, (unsigned char *)"CROSSING");
		if(tp){
		    MMFLOAT *a1float=NULL;
		    int64_t *a1int=NULL;
			int arraylength=0;
			MMFLOAT crossing=0.0;
			int direction=1;
			int found=-1;
			getargs(&tp,5,(unsigned char *)",");
			if(argc<1)error("Syntax");
			if(argc>=3 && *argv[2])crossing = getnumber(argv[2]);
			if(argc==5) direction=getint(argv[4],-1,1);
			if(direction==0)error ("Valid are -1 and 1");
			arraylength=parsenumberarray(argv[0],&a1float,&a1int,1,1,dims, false);
			for(int i=0;i<arraylength-3;i++){
				if(a1float){
					if(a1float[i]<crossing && a1float[i+2]>crossing && (a1float[i+1]>=a1float[i] && a1float[i+1]<=a1float[i+2]) && direction==1){
						found=i+1;
						break;
					}
					if(a1float[i]>crossing && a1float[i+2]<crossing && (a1float[i+1]<=a1float[i] && a1float[i+1]>=a1float[i+2]) && direction==-1){
						found=i+1;
						break;
					}
				} else {
					if(a1int[i]<crossing && a1int[i+2]>crossing && (a1int[i+1]>=a1int[i] && a1int[i+1]<=a1int[i+2]) && direction==1){
						found=i+1;
						break;
					}
					if(a1int[i]>crossing && a1int[i+2]<crossing && (a1int[i+1]<=a1int[i] && a1int[i+1]>=a1int[i+2]) && direction==-1){
						found=i+1;
						break;
					}
				}
			}
			if(found==-1){ //try a slower moving slope
				for(int i=0;i<arraylength-5;i++){
					if(a1float){
						if(a1float[i+1]<=crossing && a1float[i+3]>=crossing && (a1float[i+2]>=a1float[i+1] && a1float[i+2]<=a1float[i+3]) && direction==1){
							if(a1float[i]<a1float[i+2] && a1float[i+4]>a1float[i+2]){
								found=i+2;
								break;
							}
						}
						if(a1float[i+1]>=crossing && a1float[i+3]<=crossing && (a1float[i+2]<=a1float[i+1] && a1float[i+2]>=a1float[i+3]) && direction==-1){
							if(a1float[i]>a1float[i+2] && a1float[i+4]<a1float[i+2]){
								found=i+2;
								break;
							}
						}
					} else {
						if(a1int[i+1]<=crossing && a1int[i+3]>=crossing && (a1int[i+2]>=a1int[i+1] && a1int[i+2]<=a1int[i+3]) && direction==1){
							if(a1int[i]<a1int[i+2] && a1int[i+4]>a1int[i+2]){
								found=i+2;
								break;
							}
						}
						if(a1int[i+1]>=crossing && a1int[i+3]<=crossing && (a1int[i+2]<=a1int[i+1] && a1int[i+2]>=a1int[i+3]) && direction==-1){
							if(a1int[i]>a1int[i+2] && a1int[i+4]<a1int[i+2]){
								found=i+2;
								break;
							}
						}
					}
				}
			}
			targ=T_INT;
			iret=found;
			return;
		}
		tp = checkstring(ep, (unsigned char *)"CORREL");
		if(tp) {
		    int i,card1=1, card2=1;
		    MMFLOAT *a1float=NULL, *a2float=NULL, mean1=0, mean2=0;
		    MMFLOAT *a3float=NULL, *a4float=NULL;
		    MMFLOAT axb=0, a2=0, b2=0;
		    int64_t *a1int=NULL, *a2int=NULL;
		    getargs(&tp, 3,(unsigned char *)",");
		    if(!(argc == 3)) error("Argument count");
			card1=parsenumberarray(argv[0],&a1float,&a1int,1,0,dims, false);
			card2=parsenumberarray(argv[2],&a2float,&a2int,2,0,dims, false);
			if(card1!=card2)error("Array size mismatch");
			a3float=GetTempMemory(card1*sizeof(MMFLOAT));
			a4float=GetTempMemory(card1*sizeof(MMFLOAT));
			if(a1float!=NULL){
				for(i=0; i< card1;i++)a3float[i] = (*a1float++);
			} else {
				for(i=0; i< card1;i++)a3float[i] = (MMFLOAT)(*a1int++);
			}
			if(a2float!=NULL){
				for(i=0; i< card1;i++)a4float[i] = (*a2float++);
			} else {
				for(i=0; i< card1;i++)a4float[i] = (MMFLOAT)(*a2int++);
			}
			for(i=0;i<card1;i++){
				mean1+=a3float[i];
				mean2+=a4float[i];
			}
			mean1/=card1;
			mean2/=card1;
			for(i=0;i<card1;i++){
				a3float[i]-=mean1;
				a2+=(a3float[i]*a3float[i]);
				a4float[i]-=mean2;
				b2+=(a4float[i]*a4float[i]);
				axb+=(a3float[i]*a4float[i]);
			}
			targ=T_NBR;
			fret=axb/sqrt(a2*b2);
			return;
		}
	tp = (checkstring(ep, (unsigned char *)"CHI_P"));
	tp1 = (checkstring(ep, (unsigned char *)"CHI"));
	if(tp || tp1) {
			int chi_p=1;
			if(tp1){
				tp=tp1;
				chi_p=0;
			}
			int i,j, df, numcols=0, numrows=0;
			MMFLOAT *a1float=NULL,*rows=NULL, *cols=NULL, chi=0, prob, chi_prob;
			MMFLOAT total=0.0;
			int64_t *a1int=NULL;
			{
				getargs(&tp, 1,(unsigned char *)",");
				if(!(argc == 1)) error("Argument count");
				parsenumberarray(argv[0],&a1float,&a1int,1,2,dims, false);
				numcols=dims[0];
				numrows=dims[1];
				df=numcols*numrows;
				numcols+=(1-g_OptionBase);
				numrows+=(1-g_OptionBase);
				MMFLOAT **observed=alloc2df(numcols,numrows);
				MMFLOAT **expected=alloc2df(numcols,numrows);
				rows=alloc1df(numrows);
				cols=alloc1df(numcols);
				if(a1float!=NULL){
					for(i=0;i<numrows;i++){
						for(j=0;j<numcols;j++){
							observed[j][i]=*a1float++;
							total+=observed[j][i];
							rows[i]+=observed[j][i];
						}
					}
				} else {
					for(i=0;i<numrows;i++){
						for(j=0;j<numcols;j++){
							observed[j][i]=(MMFLOAT)(*a1int++);
							total+=observed[j][i];
							rows[i]+=observed[j][i];
						}
					}
				}
				for(j=0;j<numcols;j++){
					for(i=0;i<numrows;i++){
						cols[j]+=observed[j][i];
					}
				}
				for(i=0;i<numrows;i++){
					for(j=0;j<numcols;j++){
						expected[j][i]=cols[j]*rows[i]/total;
						expected[j][i]=((observed[j][i]-expected[j][i]) * (observed[j][i]-expected[j][i]) / expected[j][i]);
						chi+=expected[j][i];
					}
				}
				prob=chitable[df][7];
				if(chi>prob){
					i=7;
					while(i<15 && chi>=chitable[df][i])i++;
					chi_prob=chitable[0][i-1];
				} else {
					i=7;
					while(i>=0 && chi<=chitable[df][i])i--;
					chi_prob=chitable[0][i+1];
				}
				dealloc2df(observed,numcols,numrows);
				dealloc2df(expected,numcols,numrows);
				FreeMemorySafe((void **)&rows);
				FreeMemorySafe((void **)&cols);
				targ=T_NBR;
				fret=(chi_p ? chi_prob*100 : chi);
				return;
			}
		}

	} else if(toupper(*ep)=='D') {

		tp = checkstring(ep, (unsigned char *)"DOTPRODUCT");
		if(tp) {
			int i;
			int card1,card2;
			MMFLOAT *a1float=NULL, *a2float=NULL;
			// need two arrays with same cardinality
			getargs(&tp, 3,(unsigned char *)",");
			if(!(argc == 3)) error("Argument count");
			card1=parsefloatrarray(argv[0],&a1float,1,1,dims, false);
			card2=parsefloatrarray(argv[2],&a2float,2,1,dims, false);
			if(card1!=card2)error("Array size mismatch");
			fret=0;
			for(i=0;i<card1;i++){
				fret = fret + ((*a1float++) * (*a2float++));
			}
			targ = T_NBR;
			return;
		}
	} else if(toupper(*ep)=='L') {
		tp = checkstring(ep, (unsigned char *)"LOG10");
		if(tp) {
			getargs(&tp, 1,(unsigned char *)",");
			if(!(argc == 1)) error("Argument count");
			MMFLOAT f=getnumber(argv[0]);
			if(f == 0) error("Divide by zero");
			if(f < 0) error("Negative argument");
			fret=log10(f);
			targ=T_NBR;
			return;
		}

	} else if(toupper(*ep)=='M') {
		tp = checkstring(ep, (unsigned char *)"M_DETERMINANT");
		if(tp){
			int i, j, n, numcols=0, numrows=0;
			MMFLOAT *a1float=NULL;
			getargs(&tp, 1,(unsigned char *)",");
			if(!(argc == 1)) error("Argument count");
			parsefloatrarray(argv[0],&a1float,1,2,dims, false);
			numcols=dims[0]+1-g_OptionBase;
			numrows=dims[1]+1-g_OptionBase;
			if(numcols!=numrows)error("Array must be square");
			n=numrows;
			MMFLOAT **matrix=alloc2df(n,n);
			for(i=0;i<n;i++){ //load the matrix
				for(j=0;j<n;j++){
					matrix[j][i]=*a1float++;
				}
			}
			fret=determinant(matrix,n);
			dealloc2df(matrix,numcols,numrows);
			targ=T_NBR;

			return;
		}

		tp = checkstring(ep, (unsigned char *)"MAX");
		if(tp) {
			int i,card1=1;
			MMFLOAT *a1float=NULL, max=-3.0e+38;
			int64_t *a1int=NULL;
			long long int *temp=NULL;
			getargs(&tp, 3,(unsigned char *)",");
//			if(!(argc == 1)) error("Argument count");
			card1=parsenumberarray(argv[0],&a1float,&a1int,1,0,dims, false);
			if(argc==3){
				if(dims[1] > 0) {		// Not an array
					error("Argument 1 must be a 1D numerical array");
				}
				temp = findvar(argv[2], V_FIND);
				if(!(g_vartbl[g_VarIndex].type & T_INT)) error("Invalid variable");
			}

			if(a1float!=NULL){
				for(i=0; i< card1;i++){
					if((*a1float)>max){
						max=(*a1float);
						if(temp!=NULL){
							*temp=i+g_OptionBase;
						}
					}
					a1float++;
				}
			} else {
				for(i=0; i< card1;i++){
					if(((MMFLOAT)(*a1int))>max){
						max=(MMFLOAT)(*a1int);
						if(temp!=NULL){
							*temp=i+g_OptionBase;
						}
					}
					a1int++;
				}
			}
			targ=T_NBR;
			fret=max;
			return;
		}
		tp = checkstring(ep, (unsigned char *)"MIN");
		if(tp) {
			int i,card1=1;
			MMFLOAT *a1float=NULL, min=3.0e+38;
			int64_t *a1int=NULL;
			long long int *temp=NULL;
			getargs(&tp, 3,(unsigned char *)",");
//			if(!(argc == 1)) error("Argument count");
			card1=parsenumberarray(argv[0],&a1float,&a1int,1,0,dims, false);
			if(argc==3){
				if(dims[1] > 0) {		// Not an array
					error("Argument 1 must be a 1D numerical array");
				}
				temp = findvar(argv[2], V_FIND);
				if(!(g_vartbl[g_VarIndex].type & T_INT)) error("Invalid variable");
			}
			if(a1float!=NULL){
				for(i=0; i< card1;i++){
					if((*a1float)<min){
						min=(*a1float);
						if(temp!=NULL){
							*temp=i+g_OptionBase;
						}
					}
					a1float++;
				}
			} else {
				for(i=0; i< card1;i++){
					if(((MMFLOAT)(*a1int))<min){
						min=(MMFLOAT)(*a1int);
						if(temp!=NULL){
							*temp=i+g_OptionBase;
						}
					}
					a1int++;
				}
			}
			targ=T_NBR;
			fret=min;
			return;
		}
		tp = checkstring(ep, (unsigned char *)"MAGNITUDE");
		if(tp) {
			int i;
			int numcols=0;
			MMFLOAT *a1float=NULL;
			MMFLOAT mag=0.0;
			getargs(&tp, 1,(unsigned char *)",");
			if(!(argc == 1)) error("Argument count");
			numcols=parsefloatrarray(argv[0],&a1float,1,0,dims, false);
			for(i=0;i<numcols;i++){
				mag = mag + ((*a1float) * (*a1float));
				a1float++;
			}
			fret=sqrt(mag);
			targ = T_NBR;
			return;
		}

		tp = checkstring(ep, (unsigned char *)"MEAN");
		if(tp) {
			int i,card1=1;
			MMFLOAT *a1float=NULL, mean=0;
			int64_t *a1int=NULL;
			getargs(&tp, 1,(unsigned char *)",");
			if(!(argc == 1)) error("Argument count");
			card1=parsenumberarray(argv[0],&a1float,&a1int,1,0,dims, false);
			if(a1float!=NULL){
				for(i=0; i< card1;i++)mean+= (*a1float++);
			} else {
				for(i=0; i< card1;i++)mean+= (MMFLOAT)(*a1int++);
			}
			targ=T_NBR;
			fret=mean/(MMFLOAT)card1;
			return;
		}

		tp = checkstring(ep, (unsigned char *)"MEDIAN");
		if(tp) {
			int i,card1, card2=1;
			MMFLOAT *a1float=NULL, *a2float=NULL;
			int64_t *a2int=NULL;
			getargs(&tp, 1,(unsigned char *)",");
			if(!(argc == 1)) error("Argument count");
			card2=parsenumberarray(argv[0],&a2float,&a2int,1,0,dims,false);
			card1=card2;
			card2=(card2-1)/2;
			a1float=GetTempMemory(card1*sizeof(MMFLOAT));
			if(a2float!=NULL){
				for(i=0; i< card1;i++)a1float[i] = (*a2float++);
			} else {
				for(i=0; i< card1;i++)a1float[i] = (MMFLOAT)(*a2int++);
			}
			floatshellsort(a1float,  card1);
			targ=T_NBR;
			if(card1 & 1)fret=a1float[card2];
			else fret=(a1float[card2]+a1float[card2+1])/2.0;
			return;
		}
	} else if(toupper(*ep)=='S') {

		tp = checkstring(ep, (unsigned char *)"SINH");
		if(tp) {
			getargs(&tp, 1,(unsigned char *)",");
			if(!(argc == 1)) error("Argument count");
			fret=sinh(getnumber(argv[0]));
			targ=T_NBR;
			return;
		}

		tp = checkstring(ep, (unsigned char *)"SD");
		if(tp) {
			int i,card1=1;
			MMFLOAT *a2float=NULL, *a1float=NULL, mean=0, var=0, deviation;
			int64_t *a2int=NULL, *a1int=NULL;
			getargs(&tp, 1,(unsigned char *)",");
			if(!(argc == 1)) error("Argument count");
			card1=parsenumberarray(argv[0],&a1float,&a1int,1,0,dims, false);
			if(a1float!=NULL){
				a2float=a1float;
				for(i=0; i< card1;i++)mean+= (*a2float++);
			} else {
				a2int=a1int;
				for(i=0; i< card1;i++)mean+= (MMFLOAT)(*a2int++);
			}
			mean=mean/(MMFLOAT)card1;
			if(a1float!=NULL){
				for(i=0; i< card1;i++){
					deviation = (*a1float++) - mean;
					var += deviation * deviation;
				}
			} else {
				for(i=0; i< card1;i++){
					deviation = (MMFLOAT)(*a1int++) - mean;
					var += deviation * deviation;
				}
			}
			targ=T_NBR;
			fret=sqrt(var/(card1-1));
			return;
		}

		tp = checkstring(ep, (unsigned char *)"SUM");
		if(tp) {
			int i,card1=1;
			MMFLOAT *a1float=NULL, sum=0;
			int64_t *a1int=NULL;
			getargs(&tp, 1,(unsigned char *)",");
			if(!(argc == 1)) error("Argument count");
			card1=parsenumberarray(argv[0],&a1float,&a1int,1,0,dims, false);
			if(a1float!=NULL){
				for(i=0; i< card1;i++)sum+= (*a1float++);
			} else {
				for(i=0; i< card1;i++)sum+= (MMFLOAT)(*a1int++);
			}
			targ=T_NBR;
			fret=sum;
			return;
		}
	} else if(toupper(*ep)=='T') {

		tp = checkstring(ep, (unsigned char *)"TANH");
		if(tp) {
			getargs(&tp, 1,(unsigned char *)",");
			if(!(argc == 1)) error("Argument count");
			fret=tanh(getnumber(argv[0]));
			targ=T_NBR;
			return;
		}
	} else if(toupper(*ep)=='R') {
		tp = checkstring(ep, (unsigned char *)"RAND");
		if(tp) {
			if(g_myrand==NULL){
				g_myrand=(struct tagMTRand *)GetMemory(sizeof(struct tagMTRand));
				seedRand(time_us_32());
			}
			fret=genRand(g_myrand);
			targ = T_NBR;
			return;
		}
	} else {
		tp = checkstring(ep, (unsigned char *)"BASE64");
		if(tp) {
 			int64_t *outint=NULL;
			unsigned char *outstr=NULL;
			MMFLOAT *outflt=NULL;
			unsigned char * p;
			int card, card2;
			if((p=checkstring(tp, (unsigned char *)"ENCODE"))){
				unsigned char *inx=parseB64(p, &outint, &outstr, &outflt, &card, &card2);
				if(!outstr && card2 < b64e_size(card)) error("Output array too small");
				if(outstr && b64e_size(card) > 255) error("Output exceeds string size");
				unsigned char *out =(unsigned char *)GetTempMemory(b64e_size(card+1));
				int size = b64_encode(inx, card, out);
				returnAES(outint, outflt, outstr, out, NULL, size);
				iret=size;
				targ=T_INT;
				return;
			} else if((p=checkstring(tp, (unsigned char *)"DECODE"))){
				unsigned char *inx=parseB64(p, &outint, &outstr, &outflt, &card, &card2);
				if(!outstr && card2 < b64d_size(card)) error("Output array too small");
				if(outstr && b64d_size(card) > 255) error("Output exceeds string size");
				unsigned char *out =(unsigned char *)GetTempMemory(b64e_size(card+1));
				int size = b64_decode(inx,card,out);
				returnAES(outint, outflt, outstr, out, NULL, size);
				iret=size;
				targ=T_INT;
				return;
			} else error("Syntax");

		}
	}
	error("Syntax");
}

static size_t reverse_bits(size_t val, int width) {
	size_t result = 0;
	for (int i = 0; i < width; i++, val >>= 1)
		result = (result << 1) | (val & 1U);
	return result;
}
/* 
 * @cond
 * The following section will be excluded from the documentation.
 */

bool Fft_transformRadix2(double complex vec[], size_t n, bool inverse) {
	// Length variables
	int levels = 0;  // Compute levels = floor(log2(n))
	for (size_t temp = n; temp > 1U; temp >>= 1)
		levels++;
	if ((size_t)1U << levels != n)
		return false;  // n is not a power of 2

	// Trigonometric tables
	if (SIZE_MAX / sizeof(double complex) < n / 2)
		return false;
	double complex *exptable = GetMemory((n / 2) * sizeof(double complex));
	if (exptable == NULL)
		return false;
	for (size_t i = 0; i < n / 2; i++)
		exptable[i] = cexp((inverse ? 2 : -2) * M_PI * i / n * I);

	// Bit-reversed addressing permutation
	for (size_t i = 0; i < n; i++) {
		size_t j = reverse_bits(i, levels);
		if (j > i) {
			double complex temp = vec[i];
			vec[i] = vec[j];
			vec[j] = temp;
		}
	}

	// Cooley-Tukey decimation-in-time radix-2 FFT
	for (size_t size = 2; size <= n; size *= 2) {
		size_t halfsize = size / 2;
		size_t tablestep = n / size;
		for (size_t i = 0; i < n; i += size) {
			for (size_t j = i, k = 0; j < i + halfsize; j++, k += tablestep) {
				size_t l = j + halfsize;
				double complex temp = vec[l] * exptable[k];
				vec[l] = vec[j] - temp;
				vec[j] += temp;
			}
		}
		if (size == n)  // Prevent overflow in 'size *= 2'
			break;
	}

	FreeMemory((void *)exptable);
	return true;
}
/*  @endcond */


void cmd_FFT(unsigned char *pp){
    unsigned char *tp;
	PI = atan2(1, 1) * 4;
#ifdef rp2350
	int dims[MAXDIM]={0};
#else
	short dims[MAXDIM]={0};
#endif
    cplx *a1cplx=NULL, *a2cplx=NULL;
    MMFLOAT *a3float=NULL, *a4float=NULL, *a5float;
    int i, card1,card2, powerof2=0;
	tp = checkstring(pp, (unsigned char *)"MAGNITUDE");
	if(tp) {
		getargs(&tp,3,(unsigned char *)",");
		card1=parsefloatrarray(argv[0],&a3float,1,1,dims, false);
		card2=parsefloatrarray(argv[2],&a4float,2,1,dims, true);
	    if(card1 !=card2)error("Array size mismatch");
	    for(i=1;i<65536;i*=2){
	    	if(card1==i)powerof2=1;
	    }
	    if(!powerof2)error("array size must be a power of 2");
        a1cplx=(cplx *)GetTempMemory((card1)*16);
        a5float=(MMFLOAT *)a1cplx;
        for(i=0;i<card1;i++){a5float[i*2]=a3float[i];a5float[i*2+1]=0;}
        Fft_transformRadix2(a1cplx, card1, 0);
//	    fft((MMFLOAT *)a1cplx,size+1);
	    for(i=0;i<card1;i++)a4float[i]=cabs(a1cplx[i]);
		return;
	}
	tp = checkstring(pp, (unsigned char *)"PHASE");
	if(tp) {
		getargs(&tp,3,(unsigned char *)",");
		card1=parsefloatrarray(argv[0],&a3float,1,1,dims, false);
		card2=parsefloatrarray(argv[2],&a4float,2,1,dims, true);
	    if(card1 !=card2)error("Array size mismatch");
	    for(i=1;i<65536;i*=2){
	    	if(card1==i)powerof2=1;
	    }
	    if(!powerof2)error("array size must be a power of 2");
        a1cplx=(cplx *)GetTempMemory((card1)*16);
        a5float=(MMFLOAT *)a1cplx;
        for(i=0;i<card1;i++){a5float[i*2]=a3float[i];a5float[i*2+1]=0;}
        Fft_transformRadix2(a1cplx, card1, 0);
//	    fft((MMFLOAT *)a1cplx,size+1);
	    for(i=0;i<card1;i++)a4float[i]=carg(a1cplx[i]);
		return;
	}
	tp = checkstring(pp, (unsigned char *)"INVERSE");
	if(tp) {
		getargs(&tp,3,(unsigned char *)",");
		card1=parsefloatrarray(argv[0],&a4float,1,2,dims, false);
		int size=dims[1] - g_OptionBase +1;
		a1cplx=(cplx *)a4float;
		card2=parsefloatrarray(argv[2],&a3float,2,1,dims, true);
	    if(card2 !=size)error("Array size mismatch");
	    for(i=1;i<65536;i*=2){
	    	if(card2==i)powerof2=1;
	    }
	    if(!powerof2)error("array size must be a power of 2");
        a2cplx=(cplx *)GetTempMemory((card2)*16);
	    memcpy(a2cplx,a1cplx,card2*16);
	    for(i=0;i<card2;i++)a2cplx[i]=conj(a2cplx[i]);
        Fft_transformRadix2(a2cplx, card2, 0);
//	    fft((MMFLOAT *)a2cplx,size+1);
	    for(i=0;i<card2;i++)a2cplx[i]=conj(a2cplx[i])/(cplx)(card2);
	    for(i=0;i<card2;i++)a3float[i]=creal(a2cplx[i]);
	    return;
	}
	getargs(&pp,3,(unsigned char *)",");
	card1=parsefloatrarray(argv[0],&a3float,1,1,dims, false);
	card2=parsefloatrarray(argv[2],&a4float,2,2,dims, true);
    a2cplx = (cplx *)a4float;
    if((dims[1] - g_OptionBase + 1) !=card1)error("Array size mismatch");
    for(i=1;i<65536;i*=2){
    	if(card1==i)powerof2=1;
    }
    if(!powerof2)error("array size must be a power of 2");
    for(i=0;i<card1;i++){a4float[i*2]=a3float[i];a4float[i*2+1]=0;}
//    fft((MMFLOAT *)a2cplx,size+1);
    Fft_transformRadix2(a2cplx, card1, 0);
}
void cmd_SensorFusion(char *passcmdline){
    unsigned char *p;
    if((p = checkstring((unsigned char *)passcmdline, (unsigned char *)"MADGWICK")) != NULL) {
    getargs(&p, 25,(unsigned char *)",");
    if(argc < 23) error("Incorrect number of parameters");
        MMFLOAT t;
        MMFLOAT *pitch, *yaw, *roll;
        MMFLOAT ax; MMFLOAT ay; MMFLOAT az; MMFLOAT gx; MMFLOAT gy; MMFLOAT gz; MMFLOAT mx; MMFLOAT my; MMFLOAT mz; MMFLOAT beta;
        ax=getnumber(argv[0]);
        ay=getnumber(argv[2]);
        az=getnumber(argv[4]);
        gx=getnumber(argv[6]);
        gy=getnumber(argv[8]);
        gz=getnumber(argv[10]);
        mx=getnumber(argv[12]);
        my=getnumber(argv[14]);
        mz=getnumber(argv[16]);
        pitch = findvar(argv[18], V_FIND);
        if(!(g_vartbl[g_VarIndex].type & T_NBR)) error("Invalid variable");
        roll = findvar(argv[20], V_FIND);
        if(!(g_vartbl[g_VarIndex].type & T_NBR)) error("Invalid variable");
        yaw = findvar(argv[22], V_FIND);
        if(!(g_vartbl[g_VarIndex].type & T_NBR)) error("Invalid variable");
        beta = 0.5;
        if(argc == 25) beta=getnumber(argv[24]);
        t=(MMFLOAT)AHRSTimer/1000.0;
        if(t>1.0)t=1.0;
        AHRSTimer=0;
        MadgwickQuaternionUpdate(ax, ay, az, gx, gy, gz, mx, my, mz, beta, t, pitch, yaw, roll);
        return;
    }
    if((p = checkstring((unsigned char *)passcmdline, (unsigned char *)"MAHONY")) != NULL) {
    getargs(&p, 27,(unsigned char *)",");
    if(argc < 23) error("Incorrect number of parameters");
        MMFLOAT t;
        MMFLOAT *pitch, *yaw, *roll;
        MMFLOAT Kp, Ki;
        MMFLOAT ax; MMFLOAT ay; MMFLOAT az; MMFLOAT gx; MMFLOAT gy; MMFLOAT gz; MMFLOAT mx; MMFLOAT my; MMFLOAT mz;
        ax=getnumber(argv[0]);
        ay=getnumber(argv[2]);
        az=getnumber(argv[4]);
        gx=getnumber(argv[6]);
        gy=getnumber(argv[8]);
        gz=getnumber(argv[10]);
        mx=getnumber(argv[12]);
        my=getnumber(argv[14]);
        mz=getnumber(argv[16]);
        pitch = findvar(argv[18], V_FIND);
        if(!(g_vartbl[g_VarIndex].type & T_NBR)) error("Invalid variable");
        roll = findvar(argv[20], V_FIND);
        if(!(g_vartbl[g_VarIndex].type & T_NBR)) error("Invalid variable");
        yaw = findvar(argv[22], V_FIND);
        if(!(g_vartbl[g_VarIndex].type & T_NBR)) error("Invalid variable");
        Kp=10.0 ; Ki=0.0;
        if(argc >= 25)Kp=getnumber(argv[24]);
        if(argc == 27)Ki=getnumber(argv[26]);
        t=(MMFLOAT)AHRSTimer/1000.0;
        if(t>1.0)t=1.0;
        AHRSTimer=0;
        MahonyQuaternionUpdate(ax, ay, az, gx, gy, gz, mx, my, mz, Ki, Kp, t, yaw, pitch, roll) ;
        return;
    }
    error("Invalid command");
}
/* 
 * @cond
 * The following section will be excluded from the documentation.
 */

void MadgwickQuaternionUpdate(MMFLOAT ax, MMFLOAT ay, MMFLOAT az, MMFLOAT gx, MMFLOAT gy, MMFLOAT gz, MMFLOAT mx, MMFLOAT my, MMFLOAT mz, MMFLOAT beta, MMFLOAT deltat, MMFLOAT *pitch, MMFLOAT *yaw, MMFLOAT *roll)
        {
            MMFLOAT q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
            MMFLOAT norm;
            MMFLOAT hx, hy, _2bx, _2bz;
            MMFLOAT s1, s2, s3, s4;
            MMFLOAT qDot1, qDot2, qDot3, qDot4;

            // Auxiliary variables to avoid repeated arithmetic
            MMFLOAT _2q1mx;
            MMFLOAT _2q1my;
            MMFLOAT _2q1mz;
            MMFLOAT _2q2mx;
            MMFLOAT _4bx;
            MMFLOAT _4bz;
            MMFLOAT _2q1 = 2.0 * q1;
            MMFLOAT _2q2 = 2.0 * q2;
            MMFLOAT _2q3 = 2.0 * q3;
            MMFLOAT _2q4 = 2.0 * q4;
            MMFLOAT _2q1q3 = 2.0 * q1 * q3;
            MMFLOAT _2q3q4 = 2.0 * q3 * q4;
            MMFLOAT q1q1 = q1 * q1;
            MMFLOAT q1q2 = q1 * q2;
            MMFLOAT q1q3 = q1 * q3;
            MMFLOAT q1q4 = q1 * q4;
            MMFLOAT q2q2 = q2 * q2;
            MMFLOAT q2q3 = q2 * q3;
            MMFLOAT q2q4 = q2 * q4;
            MMFLOAT q3q3 = q3 * q3;
            MMFLOAT q3q4 = q3 * q4;
            MMFLOAT q4q4 = q4 * q4;

            // Normalise accelerometer measurement
            norm = sqrt(ax * ax + ay * ay + az * az);
            if (norm == 0.0) return; // handle NaN
            norm = 1.0/norm;
            ax *= norm;
            ay *= norm;
            az *= norm;

            // Normalise magnetometer measurement
            norm = sqrt(mx * mx + my * my + mz * mz);
            if (norm == 0.0) return; // handle NaN
            norm = 1.0/norm;
            mx *= norm;
            my *= norm;
            mz *= norm;

            // Reference direction of Earth's magnetic field
            _2q1mx = 2.0 * q1 * mx;
            _2q1my = 2.0 * q1 * my;
            _2q1mz = 2.0 * q1 * mz;
            _2q2mx = 2.0 * q2 * mx;
            hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
            hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
            _2bx = sqrt(hx * hx + hy * hy);
            _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
            _4bx = 2.0 * _2bx;
            _4bz = 2.0 * _2bz;

            // Gradient decent algorithm corrective step
            s1 = -_2q3 * (2.0 * q2q4 - _2q1q3 - ax) + _2q2 * (2.0 * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz);
            s2 = _2q4 * (2.0 * q2q4 - _2q1q3 - ax) + _2q1 * (2.0 * q1q2 + _2q3q4 - ay) - 4.0 * q2 * (1.0 - 2.0 * q2q2 - 2.0 * q3q3 - az) + _2bz * q4 * (_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz);
            s3 = -_2q1 * (2.0 * q2q4 - _2q1q3 - ax) + _2q4 * (2.0 * q1q2 + _2q3q4 - ay) - 4.0 * q3 * (1.0 - 2.0 * q2q2 - 2.0 * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz);
            s4 = _2q2 * (2.0 * q2q4 - _2q1q3 - ax) + _2q3 * (2.0 * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz);
            norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
            norm = 1.0/norm;
            s1 *= norm;
            s2 *= norm;
            s3 *= norm;
            s4 *= norm;

            // Compute rate of change of quaternion
            qDot1 = 0.5 * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
            qDot2 = 0.5 * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
            qDot3 = 0.5 * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
            qDot4 = 0.5 * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;

            // Integrate to yield quaternion
            q1 += qDot1 * deltat;
            q2 += qDot2 * deltat;
            q3 += qDot3 * deltat;
            q4 += qDot4 * deltat;
            norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
            norm = 1.0/norm;
            q[0] = q1 * norm;
            q[1] = q2 * norm;
            q[2] = q3 * norm;
            q[3] = q4 * norm;

            MMFLOAT ysqr = q3 * q3;


            // roll (x-axis rotation)
            MMFLOAT t0 = +2.0 * (q1 * q2 + q3 * q4);
            MMFLOAT t1 = +1.0 - 2.0 * (q2 * q2 + ysqr);
            *roll = atan2(t0, t1);

            // pitch (y-axis rotation)
            MMFLOAT t2 = +2.0 * (q1 * q3 - q4 * q2);
            t2 = t2 > 1.0 ? 1.0 : t2;
            t2 = t2 < -1.0 ? -1.0 : t2;
            *pitch = asin(t2);

            // yaw (z-axis rotation)
            MMFLOAT t3 = +2.0 * (q1 * q4 + q2 *q3);
            MMFLOAT t4 = +1.0 - 2.0 * (ysqr + q4 * q4);
            *yaw = atan2(t3, t4);

}
void MahonyQuaternionUpdate(MMFLOAT ax, MMFLOAT ay, MMFLOAT az, MMFLOAT gx, MMFLOAT gy, MMFLOAT gz, MMFLOAT mx, MMFLOAT my, MMFLOAT mz, MMFLOAT Ki, MMFLOAT Kp, MMFLOAT deltat, MMFLOAT *yaw, MMFLOAT *pitch, MMFLOAT *roll)        {
            MMFLOAT q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
            MMFLOAT norm;
            MMFLOAT hx, hy, bx, bz;
            MMFLOAT vx, vy, vz, wx, wy, wz;
            MMFLOAT ex, ey, ez;
            MMFLOAT pa, pb, pc;

            // Auxiliary variables to avoid repeated arithmetic
            MMFLOAT q1q1 = q1 * q1;
            MMFLOAT q1q2 = q1 * q2;
            MMFLOAT q1q3 = q1 * q3;
            MMFLOAT q1q4 = q1 * q4;
            MMFLOAT q2q2 = q2 * q2;
            MMFLOAT q2q3 = q2 * q3;
            MMFLOAT q2q4 = q2 * q4;
            MMFLOAT q3q3 = q3 * q3;
            MMFLOAT q3q4 = q3 * q4;
            MMFLOAT q4q4 = q4 * q4;

            // Normalise accelerometer measurement
            norm = sqrt(ax * ax + ay * ay + az * az);
            if (norm == 0.0) return; // handle NaN
            norm = 1.0 / norm;        // use reciprocal for division
            ax *= norm;
            ay *= norm;
            az *= norm;

            // Normalise magnetometer measurement
            norm = sqrt(mx * mx + my * my + mz * mz);
            if (norm == 0.0) return; // handle NaN
            norm = 1.0 / norm;        // use reciprocal for division
            mx *= norm;
            my *= norm;
            mz *= norm;

            // Reference direction of Earth's magnetic field
            hx = 2.0 * mx * (0.5 - q3q3 - q4q4) + 2.0 * my * (q2q3 - q1q4) + 2.0 * mz * (q2q4 + q1q3);
            hy = 2.0 * mx * (q2q3 + q1q4) + 2.0 * my * (0.5 - q2q2 - q4q4) + 2.0 * mz * (q3q4 - q1q2);
            bx = sqrt((hx * hx) + (hy * hy));
            bz = 2.0 * mx * (q2q4 - q1q3) + 2.0 * my * (q3q4 + q1q2) + 2.0 * mz * (0.5 - q2q2 - q3q3);

            // Estimated direction of gravity and magnetic field
            vx = 2.0 * (q2q4 - q1q3);
            vy = 2.0 * (q1q2 + q3q4);
            vz = q1q1 - q2q2 - q3q3 + q4q4;
            wx = 2.0 * bx * (0.5 - q3q3 - q4q4) + 2.0 * bz * (q2q4 - q1q3);
            wy = 2.0 * bx * (q2q3 - q1q4) + 2.0 * bz * (q1q2 + q3q4);
            wz = 2.0 * bx * (q1q3 + q2q4) + 2.0 * bz * (0.5 - q2q2 - q3q3);

            // Error is cross product between estimated direction and measured direction of gravity
            ex = (ay * vz - az * vy) + (my * wz - mz * wy);
            ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
            ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
            if (Ki > 0.0)
            {
                eInt[0] += ex;      // accumulate integral error
                eInt[1] += ey;
                eInt[2] += ez;
            }
            else
            {
                eInt[0] = 0.0;     // prevent integral wind up
                eInt[1] = 0.0;
                eInt[2] = 0.0;
            }

            // Apply feedback terms
            gx = gx + Kp * ex + Ki * eInt[0];
            gy = gy + Kp * ey + Ki * eInt[1];
            gz = gz + Kp * ez + Ki * eInt[2];

            // Integrate rate of change of quaternion
            pa = q2;
            pb = q3;
            pc = q4;
            q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5 * deltat);
            q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5 * deltat);
            q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5 * deltat);
            q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5 * deltat);

            // Normalise quaternion
            norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
            norm = 1.0 / norm;
            q[0] = q1 * norm;
            q[1] = q2 * norm;
            q[2] = q3 * norm;
            q[3] = q4 * norm;
            MMFLOAT ysqr = q3 * q3;


            // roll (x-axis rotation)
            MMFLOAT t0 = +2.0 * (q1 * q2 + q3 * q4);
            MMFLOAT t1 = +1.0 - 2.0 * (q2 * q2 + ysqr);
            *roll = atan2(t0, t1);

            // pitch (y-axis rotation)
            MMFLOAT t2 = +2.0 * (q1 * q3 - q4 * q2);
            t2 = t2 > 1.0 ? 1.0 : t2;
            t2 = t2 < -1.0 ? -1.0 : t2;
            *pitch = asin(t2);

            // yaw (z-axis rotation)
            MMFLOAT t3 = +2.0 * (q1 * q4 + q2 *q3);
            MMFLOAT t4 = +1.0 - 2.0 * (ysqr + q4 * q4);
            *yaw = atan2(t3, t4);
        }

/*Finding transpose of cofactor of matrix*/
void transpose(MMFLOAT **matrix,MMFLOAT **matrix_cofactor,MMFLOAT **newmatrix, int size)
{
    int i,j;
    MMFLOAT d;
	MMFLOAT **m_transpose=alloc2df(size,size);
	MMFLOAT **m_inverse=alloc2df(size,size);

    for (i=0;i<size;i++)
    {
        for (j=0;j<size;j++)
        {
            m_transpose[i][j]=matrix_cofactor[j][i];
        }
    }
    d=determinant(matrix,size);
    for (i=0;i<size;i++)
    {
        for (j=0;j<size;j++)
        {
            m_inverse[i][j]=m_transpose[i][j] / d;
        }
    }

    for (i=0;i<size;i++)
    {
        for (j=0;j<size;j++)
        {
            newmatrix[i][j]=m_inverse[i][j];
        }
    }
	dealloc2df(m_transpose,size,size);
	dealloc2df(m_inverse,size,size);
}
/*calculate cofactor of matrix*/
void cofactor(MMFLOAT **matrix,MMFLOAT **newmatrix,int size)
{
	MMFLOAT **m_cofactor=alloc2df(size,size);
	MMFLOAT **matrix_cofactor=alloc2df(size,size);
    int p,q,m,n,i,j;
    for (q=0;q<size;q++)
    {
        for (p=0;p<size;p++)
        {
            m=0;
            n=0;
            for (i=0;i<size;i++)
            {
                for (j=0;j<size;j++)
                {
                    if (i != q && j != p)
                    {
                       m_cofactor[m][n]=matrix[i][j];
                       if (n<(size-2))
                          n++;
                       else
                       {
                           n=0;
                           m++;
                       }
                    }
                }
            }
            matrix_cofactor[q][p]=pow(-1,q + p) * determinant(m_cofactor,size-1);
        }
    }
    transpose(matrix, matrix_cofactor, newmatrix, size);
	dealloc2df(m_cofactor,size,size);
	dealloc2df(matrix_cofactor,size,size);

}
/*For calculating Determinant of the Matrix . this function is recursive*/
MMFLOAT determinant(MMFLOAT **matrix,int size)
{
   MMFLOAT s=1,det=0;
   MMFLOAT **m_minor=alloc2df(size,size);
   int i,j,m,n,c;
   if (size==1)
   {
       return (matrix[0][0]);
   }
   else
   {
       det=0;
       for (c=0;c<size;c++)
       {
           m=0;
           n=0;
           for (i=0;i<size;i++)
           {
               for (j=0;j<size;j++)
               {
                   m_minor[i][j]=0;
                   if (i != 0 && j != c)
                   {
                      m_minor[m][n]=matrix[i][j];
                      if (n<(size-2))
                         n++;
                      else
                      {
                          n=0;
                          m++;
                      }
                   }
               }
           }
           det=det + s * (matrix[0][c] * determinant(m_minor,size-1));
           s=-1 * s;
       }
   }
   dealloc2df(m_minor,size,size);
   return (det);
}
/*  @endcond */