/* This file was automatically generated by CasADi 3.7.0.
 *  It consists of: 
 *   1) content generated by CasADi runtime: not copyrighted
 *   2) template code copied from CasADi source: permissively licensed (MIT-0)
 *   3) user code: owned by the user
 *
 */
#ifdef __cplusplus
extern "C" {
#endif

/* How to prefix internal symbols */
#ifdef CASADI_CODEGEN_PREFIX
  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
#else
  #define CASADI_PREFIX(ID) cfilter_kernel_ ## ID
#endif

#include <math.h>

#ifndef casadi_real
#define casadi_real float
#endif

#ifndef casadi_int
#define casadi_int long int
#endif

/* Add prefix to internal symbols */
#define casadi_copy CASADI_PREFIX(copy)
#define casadi_dot CASADI_PREFIX(dot)
#define casadi_f0 CASADI_PREFIX(f0)
#define casadi_fabs CASADI_PREFIX(fabs)
#define casadi_norm_2 CASADI_PREFIX(norm_2)
#define casadi_s0 CASADI_PREFIX(s0)
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_sign CASADI_PREFIX(sign)
#define casadi_sq CASADI_PREFIX(sq)

/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
    #if defined(STATIC_LINKED)
      #define CASADI_SYMBOL_EXPORT
    #else
      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
    #endif
  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
  #else
    #define CASADI_SYMBOL_EXPORT
  #endif
#endif

void casadi_copy(const casadi_real* x, casadi_int n, casadi_real* y) {
  casadi_int i;
  if (y) {
    if (x) {
      for (i=0; i<n; ++i) *y++ = *x++;
    } else {
      for (i=0; i<n; ++i) *y++ = 0.;
    }
  }
}

casadi_real casadi_dot(casadi_int n, const casadi_real* x, const casadi_real* y) {
  casadi_int i;
  casadi_real r = 0;
  for (i=0; i<n; ++i) r += *x++ * *y++;
  return r;
}

casadi_real casadi_norm_2(casadi_int n, const casadi_real* x) {
  return sqrt(casadi_dot(n, x, x));
}

casadi_real casadi_sq(casadi_real x) { return x*x;}

casadi_real casadi_fabs(casadi_real x) {
/* Pre-c99 compatibility */
#if __STDC_VERSION__ < 199901L
  return x>0 ? x : -x;
#else
  return fabs(x);
#endif
}

casadi_real casadi_sign(casadi_real x) { return x<0 ? -1 : x>0 ? 1 : x;}

static const casadi_int casadi_s0[3] = {7, 1, 1};
static const casadi_int casadi_s1[3] = {3, 1, 1};
static const casadi_int casadi_s2[3] = {1, 1, 1};

/* cfilter:(att[7],gyro[3],accel[3],alpha,dt)->(att_fused[7]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
  casadi_int i;
  casadi_real *rr, w00, *w01=w+1, *w02=w+8, *w03=w+12, w04, w05, *w06=w+17, w07, w08, w09, w10;
  casadi_real w11, *w12=w+25, w13, w14, w15;
  const casadi_real *cs;
    w00 = arg[3] ? arg[3][0] : 0;
    casadi_copy(arg[0], 7, w01);
    casadi_copy(w01, 4, w02);
    casadi_copy(w01+4, 3, w03);
    w04 = 5.0000000000000000e-01;
    for (rr=(&w05), cs=w02+1; cs!=w02+2; cs+=1) *rr++ = *cs;
    casadi_copy(arg[1], 3, w06);
    for (rr=(&w07), cs=w06+0; cs!=w06+1; cs+=1) *rr++ = *cs;
    w05 *= w07;
    w05 = (- w05 );
    for (rr=(&w07), cs=w02+2; cs!=w02+3; cs+=1) *rr++ = *cs;
    for (rr=(&w08), cs=w06+1; cs!=w06+2; cs+=1) *rr++ = *cs;
    w07 *= w08;
    w05 -= w07;
    for (rr=(&w07), cs=w02+3; cs!=w02+4; cs+=1) *rr++ = *cs;
    for (rr=(&w08), cs=w06+2; cs!=w06+3; cs+=1) *rr++ = *cs;
    w07 *= w08;
    w05 -= w07;
    for (rr=(&w07), cs=w02+0; cs!=w02+1; cs+=1) *rr++ = *cs;
    for (rr=(&w08), cs=w06+0; cs!=w06+1; cs+=1) *rr++ = *cs;
    w07 *= w08;
    for (rr=(&w08), cs=w02+2; cs!=w02+3; cs+=1) *rr++ = *cs;
    for (rr=(&w09), cs=w06+2; cs!=w06+3; cs+=1) *rr++ = *cs;
    w08 *= w09;
    w07 += w08;
    for (rr=(&w08), cs=w02+3; cs!=w02+4; cs+=1) *rr++ = *cs;
    for (rr=(&w09), cs=w06+1; cs!=w06+2; cs+=1) *rr++ = *cs;
    w08 *= w09;
    w07 -= w08;
    for (rr=(&w08), cs=w02+0; cs!=w02+1; cs+=1) *rr++ = *cs;
    for (rr=(&w09), cs=w06+1; cs!=w06+2; cs+=1) *rr++ = *cs;
    w08 *= w09;
    for (rr=(&w09), cs=w02+1; cs!=w02+2; cs+=1) *rr++ = *cs;
    for (rr=(&w10), cs=w06+2; cs!=w06+3; cs+=1) *rr++ = *cs;
    w09 *= w10;
    w08 -= w09;
    for (rr=(&w09), cs=w02+3; cs!=w02+4; cs+=1) *rr++ = *cs;
    for (rr=(&w10), cs=w06+0; cs!=w06+1; cs+=1) *rr++ = *cs;
    w09 *= w10;
    w08 += w09;
    for (rr=(&w09), cs=w02+0; cs!=w02+1; cs+=1) *rr++ = *cs;
    for (rr=(&w10), cs=w06+2; cs!=w06+3; cs+=1) *rr++ = *cs;
    w09 *= w10;
    for (rr=(&w10), cs=w02+1; cs!=w02+2; cs+=1) *rr++ = *cs;
    for (rr=(&w11), cs=w06+1; cs!=w06+2; cs+=1) *rr++ = *cs;
    w10 *= w11;
    w09 += w10;
    for (rr=(&w10), cs=w02+2; cs!=w02+3; cs+=1) *rr++ = *cs;
    for (rr=(&w11), cs=w06+0; cs!=w06+1; cs+=1) *rr++ = *cs;
    w10 *= w11;
    w09 -= w10;
    rr=w12;
    *rr++ = w05;
    *rr++ = w07;
    *rr++ = w08;
    *rr++ = w09;
    for (i=0, rr=w12, cs=w12; i<4; ++i) (*rr++)  = (w04*(*cs++));
    w04 = arg[4] ? arg[4][0] : 0;
    for (i=0, rr=w12; i<4; ++i) (*rr++) *= w04;
    for (i=0, rr=w02, cs=w12; i<4; ++i) (*rr++) += (*cs++);
    for (i=0, rr=w02, cs=w02; i<4; ++i) (*rr++)  = (w00*(*cs++));
    w04 = 1.;
    w04 -= w00;
    w00 = 5.0000000000000000e-01;
    casadi_copy(arg[2], 3, w06);
    w05 = casadi_norm_2(3, w06);
    for (i=0, rr=w06; i<3; ++i) (*rr++) /= w05;
    for (rr=(&w05), cs=w06+1; cs!=w06+2; cs+=1) *rr++ = *cs;
    w07 = (- w05 );
    for (rr=(&w08), cs=w06+2; cs!=w06+3; cs+=1) *rr++ = *cs;
    w09 = (- w08 );
    w07  = atan2(w07,w09);
    w00 *= w07;
    w00 = cos( w00 );
    w09 = 5.0000000000000000e-01;
    for (rr=(&w10), cs=w06+0; cs!=w06+1; cs+=1) *rr++ = *cs;
    w05 = casadi_sq( w05 );
    w08 = casadi_sq( w08 );
    w05 += w08;
    w05 = sqrt( w05 );
    w10  = atan2(w10,w05);
    w09 *= w10;
    w09 = cos( w09 );
    w05  = (w00*w09);
    w08 = 5.0000000000000000e-01;
    for (rr=(&w11), cs=w03+2; cs!=w03+3; cs+=1) *rr++ = *cs;
    w08 *= w11;
    w08 = cos( w08 );
    w05 *= w08;
    w13 = 5.0000000000000000e-01;
    w13 *= w07;
    w13 = sin( w13 );
    w07 = 5.0000000000000000e-01;
    w07 *= w10;
    w07 = sin( w07 );
    w10  = (w13*w07);
    w14 = 5.0000000000000000e-01;
    w14 *= w11;
    w14 = sin( w14 );
    w10 *= w14;
    w05 += w10;
    w10  = (w13*w09);
    w10 *= w08;
    w11  = (w00*w07);
    w11 *= w14;
    w10 -= w11;
    w11  = (w00*w07);
    w11 *= w08;
    w15  = (w13*w09);
    w15 *= w14;
    w11 += w15;
    w00 *= w09;
    w00 *= w14;
    w13 *= w07;
    w13 *= w08;
    w00 -= w13;
    rr=w12;
    *rr++ = w05;
    *rr++ = w10;
    *rr++ = w11;
    *rr++ = w00;
    for (i=0, rr=w12, cs=w12; i<4; ++i) (*rr++)  = (w04*(*cs++));
    for (i=0, rr=w02, cs=w12; i<4; ++i) (*rr++) += (*cs++);
    w04 = casadi_norm_2(4, w02);
    for (i=0, rr=w02; i<4; ++i) (*rr++) /= w04;
    casadi_copy(w02, 4, res[0]);
    for (rr=(&w04), cs=w02+0; cs!=w02+1; cs+=1) *rr++ = *cs;
    for (rr=(&w05), cs=w02+1; cs!=w02+2; cs+=1) *rr++ = *cs;
    w04 *= w05;
    for (rr=(&w05), cs=w02+2; cs!=w02+3; cs+=1) *rr++ = *cs;
    for (rr=(&w10), cs=w02+3; cs!=w02+4; cs+=1) *rr++ = *cs;
    w05 *= w10;
    w04 += w05;
    w04 = (2.* w04 );
    w05 = 1.;
    for (rr=(&w10), cs=w02+1; cs!=w02+2; cs+=1) *rr++ = *cs;
    w10 = casadi_sq( w10 );
    for (rr=(&w11), cs=w02+2; cs!=w02+3; cs+=1) *rr++ = *cs;
    w11 = casadi_sq( w11 );
    w10 += w11;
    w10 = (2.* w10 );
    w05 -= w10;
    w04  = atan2(w04,w05);
    if (res[0]) res[0][4] = w04;
    w04 = 1.;
    for (rr=(&w05), cs=w02+0; cs!=w02+1; cs+=1) *rr++ = *cs;
    for (rr=(&w10), cs=w02+2; cs!=w02+3; cs+=1) *rr++ = *cs;
    w05 *= w10;
    for (rr=(&w10), cs=w02+3; cs!=w02+4; cs+=1) *rr++ = *cs;
    for (rr=(&w11), cs=w02+1; cs!=w02+2; cs+=1) *rr++ = *cs;
    w10 *= w11;
    w05 -= w10;
    w05 = (2.* w05 );
    w10 = casadi_fabs( w05 );
    w04  = (w04<=w10);
    w10 = 1.5707963267948966e+00;
    w11 = casadi_sign( w05 );
    w10 *= w11;
    w10  = (w04?w10:0);
    w04 = (! w04 );
    w05 = asin( w05 );
    w04  = (w04?w05:0);
    w10 += w04;
    if (res[0]) res[0][5] = w10;
    for (rr=(&w10), cs=w02+0; cs!=w02+1; cs+=1) *rr++ = *cs;
    for (rr=(&w04), cs=w02+3; cs!=w02+4; cs+=1) *rr++ = *cs;
    w10 *= w04;
    for (rr=(&w04), cs=w02+1; cs!=w02+2; cs+=1) *rr++ = *cs;
    for (rr=(&w05), cs=w02+2; cs!=w02+3; cs+=1) *rr++ = *cs;
    w04 *= w05;
    w10 += w04;
    w10 = (2.* w10 );
    w04 = 1.;
    for (rr=(&w05), cs=w02+2; cs!=w02+3; cs+=1) *rr++ = *cs;
    w05 = casadi_sq( w05 );
    for (rr=(&w11), cs=w02+3; cs!=w02+4; cs+=1) *rr++ = *cs;
    w11 = casadi_sq( w11 );
    w05 += w11;
    w05 = (2.* w05 );
    w04 -= w05;
    w10  = atan2(w10,w04);
    if (res[0]) res[0][6] = w10;
    return 0;
}

CASADI_SYMBOL_EXPORT int cfilter(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
    return casadi_f0(arg, res, iw, w, mem);
}

CASADI_SYMBOL_EXPORT int cfilter_alloc_mem(void) {
    return 0;
}

CASADI_SYMBOL_EXPORT int cfilter_init_mem(int mem) {
    return 0;
}

CASADI_SYMBOL_EXPORT void cfilter_free_mem(int mem) {
}

CASADI_SYMBOL_EXPORT int cfilter_checkout(void) {
    return 0;
}

CASADI_SYMBOL_EXPORT void cfilter_release(int mem) {
}

CASADI_SYMBOL_EXPORT void cfilter_incref(void) {
}

CASADI_SYMBOL_EXPORT void cfilter_decref(void) {
}

CASADI_SYMBOL_EXPORT casadi_int cfilter_n_in(void) { return 5;}

CASADI_SYMBOL_EXPORT casadi_int cfilter_n_out(void) { return 1;}

CASADI_SYMBOL_EXPORT casadi_real cfilter_default_in(casadi_int i) {
    switch (i) {
        default: return 0;
    }
}

CASADI_SYMBOL_EXPORT const char* cfilter_name_in(casadi_int i) {
    switch (i) {
        case 0: return "att";
        case 1: return "gyro";
        case 2: return "accel";
        case 3: return "alpha";
        case 4: return "dt";
        default: return 0;
    }
}

CASADI_SYMBOL_EXPORT const char* cfilter_name_out(casadi_int i) {
    switch (i) {
        case 0: return "att_fused";
        default: return 0;
    }
}

CASADI_SYMBOL_EXPORT const casadi_int* cfilter_sparsity_in(casadi_int i) {
    switch (i) {
        case 0: return casadi_s0;
        case 1: return casadi_s1;
        case 2: return casadi_s1;
        case 3: return casadi_s2;
        case 4: return casadi_s2;
        default: return 0;
    }
}

CASADI_SYMBOL_EXPORT const casadi_int* cfilter_sparsity_out(casadi_int i) {
    switch (i) {
        case 0: return casadi_s0;
        default: return 0;
    }
}

CASADI_SYMBOL_EXPORT int cfilter_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
    if (sz_arg) *sz_arg = 9;
    if (sz_res) *sz_res = 3;
    if (sz_iw) *sz_iw = 0;
    if (sz_w) *sz_w = 32;
    return 0;
}

CASADI_SYMBOL_EXPORT int cfilter_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
    if (sz_arg) *sz_arg = 9*sizeof(const casadi_real*);
    if (sz_res) *sz_res = 3*sizeof(casadi_real*);
    if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
    if (sz_w) *sz_w = 32*sizeof(casadi_real);
    return 0;
}


#ifdef __cplusplus
} /* extern "C" */
#endif
