typedef struct { int quot, rem; } div_t;

typedef struct { long quot, rem; } ldiv_t;

typedef struct { long long quot, rem; } lldiv_t;



typedef unsigned long size_t;

typedef unsigned int wchar_t;



 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 


 


 
 
 

 
 
 

 
 
 


 
 
 
 
 
 
 
      int       abs(int _val); 
      long      labs(long _val);
      long long llabs(long long _val);
     int       atoi(const char *_st);
     long      atol(const char *_st);
     long long atoll(const char *_st);
     int       ltoa(long val, char *buffer);
          static __inline double    atof(const char *_st);

     long      strtol(const char *_st, char **_endptr, int _base);
     unsigned long strtoul(const char *_st, char **_endptr,
    					  int _base);
     long long strtoll(const char *_st, char **_endptr, int _base);
     unsigned long long strtoull(const char *_st, char **_endptr,
					     int _base);
     double    strtod(const char *_st, char **_endptr);
     long double strtold(const char *_st, char **_endptr);
    
     int    rand(void);
     void   srand(unsigned _seed);
    
     void  *calloc(size_t _num, size_t _size)
               ;
     void  *malloc(size_t _size)
               ;
     void  *realloc(void *_ptr, size_t _size)
               ;
     void   free(void *_ptr);
     void  *memalign(size_t _aln, size_t _size)
               ;
    
     void   abort(void); 
     int    atexit(void (*_func)(void));
     void  *bsearch(const void *_key, const void *_base,
    				   size_t _nmemb, size_t _size, 
    			           int (*compar)(const void *,const void *));
     void   qsort(void *_base, size_t _nmemb, size_t _size, 
    			         int (*_compar)(const void *, const void *));
     void   exit(int _status);
    
     div_t  div(int _numer, int _denom);
     ldiv_t ldiv(long _numer, long _denom);
     lldiv_t lldiv(long long _numer, long long _denom);

     char  *getenv(const char *_string);
     int    system(const char *_name);

     int    mblen(const char *, size_t);
     size_t mbstowcs(wchar_t *, const char *, size_t);
     int    mbtowc(wchar_t *, const char *, size_t);

     size_t wcstombs(char *, const wchar_t *, size_t);
     int    wctomb(char *, wchar_t);





static __inline double atof(const char *_st) 
{
  return strtod(_st, (char **)0); 
}





 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 

 long          far_strtol (const far char *st, far char **endptr, 
				   int base);
 unsigned long far_strtoul(const far char *st, far char **endptr, 
				   int base);
 double        far_strtod (const far char *st, far char **endptr);
 long double   far_strtold (const far char *st, far char **endptr);
 long long     far_strtoll(const far char *st, far char **endptr, 
		 						    int base);
 unsigned long long far_strtoull(const far char *st, 
                                                   far char **endptr, int base);
 int           far_atoi(const far char *st);
 long          far_atol(const far char *st);
static __inline       double        far_atof(const far char *st);
 long long     far_atoll(const far char *st);

 far void     *far_calloc  (unsigned long num, unsigned long size);
 far void     *far_malloc  (unsigned long size);
 far void     *far_realloc (far void *ptr, unsigned long size);
 void          far_free    (far void *ptr);

 far void     *far_bsearch(const far void *key, 
                                       const far void *base, 
			               size_t nmemb, size_t size, 
			               int (*compar)(const far void *,
                                                     const far void *));
 void          far_qsort  (far void *base, 
                                       size_t nmemb, size_t size,
			               int (*compar)(const far void *, 
                                                     const far void *));

 int	   free_memory(void);
 int           max_free(void);
 void         *chkheap();

static __inline double far_atof(const far char *st) 
{ 
   return far_strtod(st, 0);
}


 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 

 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 





typedef char *va_list;



 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 





 
 
 



 
 
 

typedef struct {
      int fd;                     
      unsigned char* buf;         
      unsigned char* pos;         
      unsigned char* bufend;      
      unsigned char* buff_stop;   
      unsigned int   flags;       
} FILE;

typedef long fpos_t;

 
 
 
 
 
 



 
 
 








 


 
 
 

extern  FILE _ftable[10];
extern  char _tmpnams[10][16];

 
 
 
 
 
 
extern  int     remove(const char *_file);
extern  int     rename(const char *_old, const char *_new);
extern  FILE   *tmpfile(void);
extern  char   *tmpnam(char *_s);

 
 
 
extern  int     fclose(FILE *_fp); 
extern  FILE   *fopen(const char *_fname, const char *_mode);
extern  FILE   *freopen(const char *_fname, const char *_mode,
			            register FILE *_fp);
extern  void    setbuf(register FILE *_fp, char *_buf);
extern  int     setvbuf(register FILE *_fp, register char *_buf, 
			            register int _type, register size_t _size);
extern  int     fflush(register FILE *_fp); 

 
 
 
extern  int fprintf(FILE *_fp, const char *_format, ...)
               ;
extern  int fscanf(FILE *_fp, const char *_fmt, ...)
               ;
extern  int printf(const char *_format, ...)
               ;
extern  int scanf(const char *_fmt, ...)
               ;
extern  int sprintf(char *_string, const char *_format, ...)
               ;
extern  int snprintf(char *_string, size_t _n, 
				 const char *_format, ...)
               ;
extern  int sscanf(const char *_str, const char *_fmt, ...)
               ;
extern  int vfprintf(FILE *_fp, const char *_format, va_list _ap)
               ;
extern  int vfscanf(FILE *_fp, const char *_fmt, va_list _ap)
               ;
extern  int vprintf(const char *_format, va_list _ap)
               ;
extern  int vscanf(const char *_format, va_list _ap)
               ;
extern  int vsprintf(char *_string, const char *_format,
				 va_list _ap)
               ;
extern  int vsnprintf(char *_string, size_t _n, 
				  const char *_format, va_list _ap)
               ;
extern  int vsscanf(const char *_str, const char *_fmt, va_list _ap)
               ;

 
 
 
extern  int     fgetc(register FILE *_fp);
extern  char   *fgets(char *_ptr, register int _size,
				  register FILE *_fp);
extern  int     fputc(int _c, register FILE *_fp);
extern  int     fputs(const char *_ptr, register FILE *_fp);
extern  int     getc(FILE *_p);
extern  int     getchar(void);
extern  char   *gets(char *_ptr); 
extern  int     putc(int _x, FILE *_fp);
extern  int     putchar(int _x);
extern  int     puts(const char *_ptr); 
extern  int     ungetc(int _c, register FILE *_fp);

 
 
 
extern  size_t  fread(void *_ptr, size_t _size, size_t _count,
				  FILE *_fp);
extern  size_t  fwrite(const void *_ptr, size_t _size,
				   size_t _count, register FILE *_fp); 

 
 
 
extern  int     fgetpos(FILE *_fp, fpos_t *_pos);
extern  int     fseek(register FILE *_fp, long _offset,
				  int _ptrname);
extern  int     fsetpos(FILE *_fp, const fpos_t *_pos);
extern  long    ftell(FILE *_fp);
extern  void    rewind(register FILE *_fp); 

 
 
 
extern  void    clearerr(FILE *_fp);
extern  int     feof(FILE *_fp);
extern  int     ferror(FILE *_fp);
extern  void    perror(const char *_s);








 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 

 
typedef char            int8_t;
typedef unsigned char  uint8_t;

 

    typedef          short  int16_t;
    typedef unsigned short uint16_t;
    typedef          long   int32_t;
    typedef unsigned long  uint32_t;


    typedef          long long  int64_t;
    typedef unsigned long long uint64_t;

 

    typedef  int16_t  int_least8_t;
    typedef uint16_t uint_least8_t;

    typedef  int16_t  int_least16_t;
    typedef uint16_t uint_least16_t;
    typedef  int32_t  int_least32_t;
    typedef uint32_t uint_least32_t;


    typedef  int64_t  int_least64_t;
    typedef uint64_t uint_least64_t;

 

    typedef  int16_t  int_fast8_t;
    typedef uint16_t uint_fast8_t;
    typedef  int16_t  int_fast16_t;
    typedef uint16_t uint_fast16_t;

    typedef  int32_t  int_fast32_t;
    typedef uint32_t uint_fast32_t;


    typedef  int64_t  int_fast64_t;
    typedef uint64_t uint_fast64_t;

 
    typedef          long intptr_t;
    typedef unsigned long uintptr_t;

 
    typedef          long long intmax_t;
    typedef unsigned long long uintmax_t;





 

 
















 







 







 




 



 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
























































 


  

  





		 


		 

typedef long _Int32t;
typedef unsigned long _Uint32t;

typedef  long  _Ptrdifft;

typedef  unsigned long _Sizet;







		 


		 
 
 



 


		 
 

		 





		 







		 


         

		 




		 

		 



typedef long long _Longlong;
typedef unsigned long long _ULonglong;


		 



       typedef unsigned int _Wchart;
       typedef unsigned int _Wintt;

		 

		 


		 
typedef  va_list _Va_list;

		 


void _Atexit(void (*)(void));


		 

typedef char _Sysch_t;

		 

		 


		 

		 



		 






 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 




 void _nop();

extern  void (  *_lock)();
extern  void (*_unlock)();

 void _register_lock  (void (  *lock)());
 void _register_unlock(void (*unlock)());








 





		 

typedef unsigned int _Bool;

		 






 



 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 




typedef long ptrdiff_t;







 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 



 




 size_t  strlen(const char *_string);

 char *strcpy(char *_dest, const char *_src);
 char *strncpy(char *_to, const char *_from, size_t _n);
 char *strcat(char *_string1, const char *_string2);
 char *strncat(char *_to, const char *_from, size_t _n);
 char *strchr(const char *_string, int _c);
 char *strrchr(const char *_string, int _c);

 int  strcmp(const char *_string1, const char *_string2);
 int  strncmp(const char *_string1, const char *_string2, size_t _n);

 int     strcoll(const char *_string1, const char *_string2);
 size_t  strxfrm(char *_to, const char *_from, size_t _n);
 char   *strpbrk(const char *_string, const char *_chs);
 size_t  strspn(const char *_string, const char *_chs);
 size_t  strcspn(const char *_string, const char *_chs);
 char   *strstr(const char *_string1, const char *_string2);
 char   *strtok(char *_str1, const char *_str2);
 char   *strerror(int _errno);

 void   *memmove(void *_s1, const void *_s2, size_t _n);

 int     memcmp(const void *_cs, const void *_ct, size_t _n);
 void   *memchr(const void *_cs, int _c, size_t _n);

   void   *memset(void *_mem, int _ch, size_t _n);


size_t    far_strlen(const far char *s);
char     *strcpy_nf(char *s1, const char far *s2);
char far *strcpy_fn(char far *s1, const char *s2);
char far *strcpy_ff(char far *s1, const char far *s2);
far char *far_strncpy(far char *s1, far const char *s2, size_t n);
far char *far_strcat(far char *s1, const far char *s2);
far char *far_strncat(far char *s1, const far char *s2, size_t n);
far char *far_strchr(const far char *s, int c);
far char *far_strrchr(const far char *s, int c);
int       far_strcmp(const far char *s1, const far char *s2);
int       far_strncmp(const far char *s1, const far char *s2, size_t n);
int       far_strcoll(const far char *s1, const far char *s2);
size_t    far_strxfrm(far char *s1, const far char *s2, size_t n);
far char *far_strpbrk(const far char *s1, const far char *s2);
size_t    far_strspn(const far char *s1, const far char *s2);
size_t    far_strcspn(const far char *s1, const far char *s2);
far char *far_strstr(const far char *s1, const far char *s2);
far char *far_strtok(far char *s1, const far char *s2);
far char *far_strerror(int errno);
void far *far_memmove(far void *s1, far const void *s2, size_t n);
void     *__memcpy_nf (void *_s1, far const void *_s2, size_t _n);
void far *__memcpy_fn (far void *_s1, const void *_s2, size_t _n);
void far *__memcpy_ff (far void *_s1, far const void *_s2, size_t _n);
int       far_memcmp(const far void *s1, const far void *s2, size_t n);
far void *far_memchr(const far void *s, int c, size_t n);
far void *far_memset(far void *s, int c, size_t n);
far void *far_memlcpy(far void *to, const far void *from, unsigned long n);
far void *far_memlmove(far void *to, const far void *from, unsigned long n);












 



typedef enum
{
    ESTOP,               
    UAV_COMMAND,         
    MOTOR_STATUS,        
    IMU_DATA,            
    CONTROL_PARAMS,      
    C28_RADIO,           
    MOTOR_TEST,          
    CONTROL_DATA,        
    CPDELTA,             
    RADIO_REQUEST,       
    UAV_FLAG,            
    NAV_STATE,           
    UAV_STATE,           
    UAV_STATUS,          
    CONTROL_ODOMETRY,    
    C28_HEARTBEAT,       
    NAV_STATUS,          
    LOGDUMP_REQ,         
    LOGDUMP_RES,         
    TIMESYNC_PACKET,     
    EMERGENCY_LOG,       
    DATA_REQUESTS,       
    UTILITY_CMD,         
    NUM_MESSAGE_TYPES    

} message_type_t;





typedef enum _motor_status_t
{
	MOTOR_DISABLE,
	MOTOR_ENABLE,
	MOTOR_AWAITING_RESET
} motor_status_t;

typedef enum _controller_status_t {
    VEHICLE_DISABLE,
	VEHICLE_IDLE,
	VEHICLE_ENABLE,
    VEHICLE_DEBUG_MODE
} controller_status_t;

typedef int(*lcm_encode_func_t)(void*,int,int,const void*,int);






































typedef int                 int16;
typedef long                int32;
typedef long long           int64;
typedef unsigned int        Uint16;
typedef unsigned long       Uint32;
typedef unsigned long long  Uint64;
typedef float               float32;
typedef long double         float64;






float32 isqrt(float32 X);
void sincos(float32 radian, float32* PtrSin, float32* PtrCos);










    typedef float32 float64_t;

    typedef float32 float32_t;






 
typedef struct _matrix_t {
	uint16_t rows;  
	uint16_t cols;  
	float64_t *values; 
} matrix_t;















 
uint32_t matrix_init(matrix_t *dest, uint16_t rows, uint16_t cols);






 
void matrix_destroy(matrix_t *dest);

matrix_t* matrix_clone(matrix_t **dest, const matrix_t *src);







 
void matrix_copy(matrix_t *dest, const matrix_t *src);







 
void matrix_set_all(matrix_t *dest, float64_t val);






 
void matrix_identity(matrix_t *dest);






 
void matrix_diag(matrix_t *dest, float64_t val);







 
void matrix_print(FILE *dest, const matrix_t *src);








 
void matrix_print_diff(FILE *dest, const matrix_t *A, const matrix_t *B);










 
float64_t* matrix_get(matrix_t *src, uint16_t row, uint16_t col);










 
const float64_t* matrix_const_get(const matrix_t *src, uint16_t row, uint16_t col);








 
void matrix_submatrix(matrix_t *dest, const matrix_t *src, uint16_t startRow, uint16_t startCol,
                                                           uint16_t numRows,  uint16_t numCols);






 
uint32_t matrix_size(const matrix_t *src);








 
void matrix_set_col(matrix_t *dest, uint16_t col, const float64_t *src);








 
void matrix_set_row(matrix_t *dest, uint16_t row, const float64_t *src);







 
void matrix_set(matrix_t *dest, const float64_t* src);







 

void matrix_set_2D(matrix_t *dest, const float32_t* src);

void matrix_scale(matrix_t *dest, float64_t scalar);








 
void matrix_scale_row(matrix_t *dest, uint16_t idx, float64_t scalar);








 
void matrix_add(matrix_t *dest, const matrix_t *a, const matrix_t *b);








 
void matrix_subtract(matrix_t *dest, const matrix_t *a, const matrix_t *b);







 
matrix_t* matrix_negate(matrix_t *dest, const matrix_t *a);


















 
void matrix_mult(matrix_t *dest, const matrix_t *a, const matrix_t *b);

void matrix_submatrix_mult( matrix_t * dest , const matrix_t * a, const matrix_t * b,
        uint16_t arow, uint16_t acol, uint16_t brow, uint16_t bcol, uint16_t destrow, uint16_t destcol,
        uint16_t a_rows, uint16_t a_cols, uint16_t b_rows, uint16_t b_cols);








 
void matrix_transpose(matrix_t *dest, const matrix_t *src);











 
void matrix_inverse(matrix_t *dest, const matrix_t *src);







 
float64_t matrix_magnitude(const matrix_t *src);








 
void matrix_normalize(matrix_t *dest, const matrix_t *src);








 
uint16_t matrix_equal(const matrix_t *a, const matrix_t *b);








 
void matrix_cross_product(matrix_t *dest, const matrix_t *a, const matrix_t *b);

float64_t matrix_dot_product( const matrix_t *a, const matrix_t *b );











 
float64_t matrix_determinant(const matrix_t *src);

_Bool matrix_symmetric(const matrix_t * src );

void gauss_jordan(matrix_t *a, matrix_t *b);
    
    
    
    
    
    

float64_t matrix_trace(const matrix_t * src );

float64_t matrix_2norm(const matrix_t *src );

 



 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 



 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 



 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 

 
 
 
 
 

 
 
 



 
 
 
         double modf(double x, double *y); 
         double far_modf(double x, far double *y); 
         double asin(double x);
         double acos(double x);
         double atan(double x);
         double atan2(double y, double x);
__inline double ceil(double x)  { double y; return (modf(x, &y) > 0 ? y+1:y); }
         double cos(double x);
         double cosh(double x);
         double exp(double x);
         double fabs(double x);
__inline double floor(double x) { double y; return (modf(x, &y) < 0 ? y-1:y); }
         double fmod(double x, double y);
         double frexp(double x, int *exp);
         double far_frexp(double x, far int *exp);
         double ldexp(double x, int exp);
         double log(double x);
         double log10(double x);
         double pow(double x, double y);
         double sin(double x);
         double sinh(double x);
         double tan(double x);
         double tanh(double x);
         double sqrt(double x);


 
 
 
 
 
 


 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 




 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 






 

extern  void _abort_msg(const char *);






 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 



 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 



 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 











 


 








 






















































 

	#pragma CODE_SECTION(matrix_get, "ramfuncs");
	#pragma CODE_SECTION(matrix_const_get, "ramfuncs");
	#pragma CODE_SECTION(matrix_add, "ramfuncs");
	#pragma CODE_SECTION(matrix_subtract, "ramfuncs");
	#pragma CODE_SECTION(matrix_negate, "ramfuncs");
	#pragma CODE_SECTION(matrix_mult, "ramfuncs");
	#pragma CODE_SECTION(matrix_submatrix_mult, "ramfuncs");
	#pragma CODE_SECTION(matrix_inverse, "ramfuncs");
	#pragma CODE_SECTION(matrix_magnitude, "ramfuncs");
	#pragma CODE_SECTION(matrix_determinant, "ramfuncs");



 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 








static float64_t matrix_det_helper(const matrix_t *src, uint16_t idx1, uint16_t idx2, uint16_t idx3, uint16_t idx4);

static void matrix_entry_swap( float64_t * one, float64_t * two)
{
    float64_t twocopy = *two;
    *two = *one;
    *one = twocopy;
}

uint32_t matrix_init(matrix_t *dest, uint16_t rows, uint16_t cols)
{
    do{ if(!(rows > 0 && cols > 0)) { do{}while(1); } else do{}while(0); }while(0);
    dest->rows = rows;
    dest->cols = cols;
    dest->values = (float64_t*) calloc(rows*cols, sizeof(float64_t));
    if(!dest->values) return 1;
    else return 0;
}

matrix_t* matrix_clone(matrix_t **dest, const matrix_t *src)
{
    do{ if(!(*dest == 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(src->rows > 0 && src->cols > 0)) { do{}while(1); } else do{}while(0); }while(0);
    *dest = malloc(sizeof(matrix_t));
    if(*dest == 0)
        return 0;
    (*dest)->rows = src->rows;
    (*dest)->cols = src->cols;
    (*dest)->values = (float64_t*) malloc(sizeof(float64_t)*src->rows*src->cols);
    if((*dest)->values == 0) return 0;

    matrix_copy(*dest, src);

    return *dest;
}

void matrix_destroy(matrix_t *dest)
{
    do{ if(!(dest->rows > 0 && dest->cols > 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->values != 0)) { do{}while(1); } else do{}while(0); }while(0);
    free((void*)dest->values);
    dest->values=0;
    dest->rows=0;
    dest->cols=0;
}

void matrix_copy(matrix_t *dest, const matrix_t *src)
{
    do{ if(!(dest != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(src != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->rows == src->rows)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->cols == src->cols)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->values != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(src->values != 0)) { do{}while(1); } else do{}while(0); }while(0);

    memcpy((void*)dest->values, (const void*)src->values,
            sizeof(src->values[0])*matrix_size(src));
}

void matrix_set_all(matrix_t *dest, float64_t val)
{
    do{ if(!(dest != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->values != 0)) { do{}while(1); } else do{}while(0); }while(0);
    

    uint32_t i;
    uint32_t size = matrix_size(dest);
    for(i=0; i<size; i++)
        dest->values[i] = val;
}

void matrix_identity(matrix_t *dest)
{
    matrix_diag(dest, 1);
}

void matrix_diag(matrix_t *dest, float64_t val)
{
    do{ if(!(dest != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->values != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->cols == dest->rows)) { do{}while(1); } else do{}while(0); }while(0);

    matrix_set_all(dest, 0);
    uint16_t i;
    uint16_t rows = dest->rows;
    for(i=0; i<rows; i++)
        *matrix_get(dest, i, i) = val;
}

void matrix_print(FILE *dest, const matrix_t *src)
{
    do{ if(!(src != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(src->values != 0)) { do{}while(1); } else do{}while(0); }while(0);

    uint16_t i,j;
    uint16_t src_rows = src->rows, src_cols = src->cols;
    for(i=0; i<src_rows; i++)
    {
        for(j=0; j<src_cols; j++)
        {
            if(j==0)
                fprintf(dest, "\t" "%+7.3e ", *matrix_const_get(src, i, j));
            else
            {
                
                fprintf(dest, "%+7.3e ", *matrix_const_get(src, i, j));
            }
        }
        fprintf(dest, "\n");
    }
    fflush(dest);
}

void matrix_print_diff(FILE *dest, const matrix_t *A, const matrix_t *B)
{
    do{ if(!(A != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(B != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(A->values != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(B->values != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(B->rows == A->rows)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(B->cols == A->cols)) { do{}while(1); } else do{}while(0); }while(0);

    uint16_t i,j;
    uint16_t src_rows = A->rows, src_cols = A->cols;
    for(i=0; i<src_rows; i++)
    {
        for(j=0; j<src_cols; j++)
        {
            if(j==0)
                if(*matrix_const_get(A, i, j) == *matrix_const_get(B, i, j))
                    fprintf(dest, "\t" "-------------");
                else
                {
                    fprintf(dest, "\t%-5.3lf/", *matrix_const_get(A, i, j));
                    fprintf(dest, "\t%-5.3lf", *matrix_const_get(B, i, j));
                }
            else
            {
                
                if(*matrix_const_get(A, i, j) == *matrix_const_get(B, i, j))
                    fprintf(dest, "-------------");
                else
                {
                    fprintf(dest, "%-5.3lf/", *matrix_const_get(A, i, j));
                    fprintf(dest, "%-5.3lf", *matrix_const_get(B, i, j));
                }
            }
        }
        fprintf(dest, "\n");
    }
    fflush(dest);
}

float64_t* matrix_get(matrix_t *src, uint16_t row, uint16_t col)
{
    do{ if(!(src != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(src->values != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(row < src->rows)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(col < src->cols)) { do{}while(1); } else do{}while(0); }while(0);
    return &src->values[src->cols*row + col];
}

const float64_t* matrix_const_get(const matrix_t *src, uint16_t row, uint16_t col)
{
    do{ if(!(src != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(src->values != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(row < src->rows)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(col < src->cols)) { do{}while(1); } else do{}while(0); }while(0);
    return &src->values[src->cols*row + col];
}

void matrix_submatrix(matrix_t *dest, const matrix_t *src,
        uint16_t startRow, uint16_t startCol,
        uint16_t numRows,  uint16_t numCols)
{
    do{ if(!(dest != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(src != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->rows == numRows)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->cols == numCols)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(startRow + numRows <= src->rows)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(startCol + numCols <= src->cols)) { do{}while(1); } else do{}while(0); }while(0);

    uint16_t i, j;
    for(i = 0; i < numRows; i++)
    {
        for(j = 0; j < numCols; j++)
        {
            *matrix_get(dest, i, j) = *matrix_const_get(src, i + startRow,
                                                             j + startCol);
        }
    }
}

void matrix_set_col(matrix_t *dest, uint16_t col, const float64_t *src)
{
    do{ if(!(dest != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(src != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->cols > col)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->values != 0)) { do{}while(1); } else do{}while(0); }while(0);

    uint16_t i;
    uint16_t dest_rows = dest->rows;
    for(i=0; i<dest_rows; i++)
        *matrix_get(dest, i, col) = src[i];
}

void matrix_set_row(matrix_t *dest, uint16_t row, const float64_t *src)
{
    do{ if(!(dest != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(src != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->rows > row)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->values != 0)) { do{}while(1); } else do{}while(0); }while(0);

    memcpy((void*)matrix_get(dest, row, 0), (const void*)src, sizeof(src[0])*dest->cols);
}

void matrix_set(matrix_t *dest, const float64_t* src)
{
    do{ if(!(dest != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(src != 0)) { do{}while(1); } else do{}while(0); }while(0);

    memcpy((void*)matrix_get(dest, 0, 0), (const void*)src, sizeof(src[0])*dest->rows*dest->cols);
}

void matrix_set_2D(matrix_t *dest, const float32_t * src)
{
    do{ if(!(dest != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(src != 0)) { do{}while(1); } else do{}while(0); }while(0);

    uint16_t i, j;
    uint16_t dest_rows = dest->rows;
    uint16_t dest_cols = dest->cols;
    for(i=0;i<dest_rows;i++)
    {
        for(j=0;j<dest_cols;j++)
        {
            *matrix_get(dest, i, j) = (float64_t ) (src[i*dest->cols+j]);
        }
    }
    
}

void matrix_scale(matrix_t *dest, float64_t scalar)
{
    do{ if(!(dest != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->values != 0)) { do{}while(1); } else do{}while(0); }while(0);

    uint16_t i;
    uint16_t size = matrix_size(dest);
    for(i=0; i<size; i++)
    {
        dest->values[i] *= scalar;
    }
}

void matrix_scale_row(matrix_t *dest, uint16_t idx, float64_t scalar)
{
    do{ if(!(dest != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->values != 0)) { do{}while(1); } else do{}while(0); }while(0);

    uint16_t i;
    uint16_t dest_cols = dest->cols;
    for(i=idx*dest_cols; i<(idx+1)*dest_cols; i++)
    {
        dest->values[i] *= scalar;
    }
}

void matrix_add(matrix_t *dest, const matrix_t *a, const matrix_t *b)
{
    do{ if(!(dest != 0 && a != 0 && b != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->values != 0 && a->values != 0 && b->values != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(a->rows == b->rows && a->cols == b->cols && a->cols == dest->cols && a->rows == dest->rows)) { do{}while(1); } else do{}while(0); }while(0);

    uint16_t i;
    uint16_t size = matrix_size(dest);
    for(i=0; i<size; i++)
    {
        dest->values[i] = a->values[i] + b->values[i];
    }
}

void matrix_subtract(matrix_t *dest, const matrix_t *a, const matrix_t *b)
{
    do{ if(!(dest != 0 && a != 0 && b != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->values != 0 && a->values != 0 && b->values != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(a->rows == b->rows)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(a->cols == b->cols)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(a->rows == dest->rows)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(a->cols == dest->cols)) { do{}while(1); } else do{}while(0); }while(0);

    uint16_t i;
    uint16_t size = matrix_size(dest);
    for(i=0; i<size; i++)
    {
        dest->values[i] = a->values[i] - b->values[i];
    }
}

matrix_t* matrix_negate(matrix_t *dest, const matrix_t *a)
{
    do{ if(!(dest != 0 && a != 0)) { do{}while(1); } else do{}while(0); }while(0);
    uint16_t i = 0;
    uint16_t size = matrix_size(dest);
    for(i=0; i<size; i++)
    {
        dest->values[i] = -a->values[i];
    }
    return dest;
}

void matrix_mult(matrix_t *dest, const matrix_t *a, const matrix_t *b)
{
    do{ if(!(dest != 0 && a != 0 && b != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->values != 0 && a->values != 0 && b->values != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(a->cols == b->rows)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->cols == b->cols)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->rows == a->rows)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest != a)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest != b)) { do{}while(1); } else do{}while(0); }while(0);

    uint16_t j,k, i;
    uint16_t dest_rows = dest->rows;
    uint16_t dest_cols = dest->cols;
    uint16_t a_cols = a->cols;
    float64_t mult_buf;
    for(i=0; i<dest_rows; i++) {
        for(j=0; j<dest_cols; j++) {
            mult_buf = 0;
            for(k=0; k<a_cols; k++) {
                 mult_buf += (*matrix_const_get(a, i, k))*(*matrix_const_get(b, k, j));
            }
            *matrix_get(dest, i, j) = mult_buf;
        }
    }
}

void matrix_submatrix_mult(
        matrix_t * dest ,
        const matrix_t * a,
        const matrix_t * b,
        uint16_t arow,
        uint16_t acol,
        uint16_t brow,
        uint16_t bcol,
        uint16_t destrow,
        uint16_t destcol,
        uint16_t a_rows,
        uint16_t a_cols,
        uint16_t b_rows,
        uint16_t b_cols)
{
    do{ if(!(a_cols == b_rows)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(destrow + a_rows <= dest->rows)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(destcol + b_cols <= dest->cols)) { do{}while(1); } else do{}while(0); }while(0);

    do{ if(!(arow + a_rows <= a->rows)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(brow + b_rows <= b->rows)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(acol + a_cols <= a->cols)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(acol + a_cols <= a->cols)) { do{}while(1); } else do{}while(0); }while(0);

    uint16_t i,j,k = 0;
    for(i=0; i<a_rows; i++){
        for(j=0; j<b_cols; j++){

            float64_t element = 0.0;
            for(k=0; k<a_cols; k++){
                element += (*matrix_const_get(a, arow + i, acol + k) ) *
                    (*matrix_const_get(b, brow + k, bcol + j) );
            }
            *matrix_get(dest, destrow + i, destcol + j) = element;
        }
    }

}


void matrix_transpose(matrix_t *dest, const matrix_t *src)
{
    do{ if(!(dest != 0 && src != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->values != 0 && src->values != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->cols == src->rows && dest->rows == src->cols)) { do{}while(1); } else do{}while(0); }while(0);

    uint16_t i,j;
    for(i=0; i<src->rows; i++)
        for(j=0; j<src->cols; j++)
            *matrix_get(dest, j, i) = *matrix_const_get(src, i, j);
}

uint32_t matrix_size(const matrix_t *src)
{
    do{ if(!(src != 0)) { do{}while(1); } else do{}while(0); }while(0);
    return (uint32_t) src->cols* (uint32_t) src->rows;
}

void matrix_inverse(matrix_t *dest, const matrix_t *src)
{
    do{ if(!(dest != 0 && src != 0 && dest != src)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->values != 0 && src->values != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->rows == src->rows && dest->cols == src->cols)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(dest->cols == src->rows && dest->rows == src->cols)) { do{}while(1); } else do{}while(0); }while(0);

    if ( (src->rows == 2) && (src->cols ==2) && (dest->rows == 2) && (dest->cols == 2) )
    {
        if ( matrix_determinant(src) >= 1e-26 ) {
            printf("det: %e", matrix_determinant(src));
        }

        float64_t det = (*matrix_const_get(src,0,0))*(*matrix_const_get(src,1,1)) - (*matrix_const_get(src,0,1))*(*matrix_const_get(src,1,0));
        dest->values[dest->cols*0 + 0] =  (*matrix_const_get(src,1,1));
        dest->values[dest->cols*0 + 1] = -(*matrix_const_get(src,0,1));
        dest->values[dest->cols*1 + 0] = -(*matrix_const_get(src,1,0));
        dest->values[dest->cols*1 + 1] =  (*matrix_const_get(src,0,0));
        matrix_scale(dest,1.0/det);
    }
    else if ( (src->rows == 3) && (src->cols == 3))
    {
        do{ if(!(matrix_determinant(src) >= 1e-26)) { do{}while(1); } else do{}while(0); }while(0);
        dest->values[0] = matrix_det_helper(src, 4, 5, 7, 8);
        dest->values[1] = matrix_det_helper(src, 2, 1, 8, 7);
        dest->values[2] = matrix_det_helper(src, 1, 2, 4, 5);

        dest->values[3] = matrix_det_helper(src, 5, 3, 8, 6);
        dest->values[4] = matrix_det_helper(src, 0, 2, 6, 8);
        dest->values[5] = matrix_det_helper(src, 2, 0, 5, 3);

        dest->values[6] = matrix_det_helper(src, 3, 4, 6, 7);
        dest->values[7] = matrix_det_helper(src, 1, 0, 7, 6);
        dest->values[8] = matrix_det_helper(src, 0, 1, 3, 4);

        matrix_scale(dest, 1.0/matrix_determinant(src));
    }
    else {
        matrix_t srccopy, identity;

        matrix_init(&srccopy, src->rows, src->cols);
        matrix_init(&identity, src->rows, src->rows);

        matrix_copy(&srccopy, src);
        matrix_identity(&identity);

        matrix_copy(&srccopy, src);
        gauss_jordan(&srccopy, &identity);
        matrix_copy(dest, &identity);

        matrix_destroy(&srccopy);
        matrix_destroy(&identity);
    }
}

float64_t matrix_magnitude(const matrix_t *src)
{
	do{ if(!(src != 0)) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(src->cols == 1 || src->rows == 1)) { do{}while(1); } else do{}while(0); }while(0);
    float64_t res=0;
    uint16_t i;
    for(i=0; i<matrix_size(src); i++)
    {
        res += src->values[i] * src->values[i];
    }
    return sqrt(res);
}

void matrix_normalize(matrix_t *dest, const matrix_t *src)
{
    do{ if(!(src->cols == 1 || src->rows == 1)) { do{}while(1); } else do{}while(0); }while(0);
    float64_t factor = 1.0 / matrix_magnitude(src);
    uint16_t i;
    uint16_t size = matrix_size(dest);
    for(i=0; i<size; i++)
        dest->values[i] *= factor;
}

uint16_t matrix_equal(const matrix_t *a, const matrix_t *b)
{
    const float64_t epsilon = 1e-10;
    if(a->rows != b->rows
            || a->cols != b->cols)
        return 0;

    uint16_t i;
    uint16_t size = matrix_size(a);
    for(i=0; i<size; i++)
        if(fabs(a->values[i] - b->values[i]) > epsilon)
            return 0;
    return 1;
}

void matrix_cross_product(matrix_t *dest, const matrix_t *a, const matrix_t *b)
{
    do{ if(!((a->cols == 1 && b->cols == 1) || (a->rows == 1 && b->rows == 1))) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(matrix_size(a) == matrix_size(b))) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(a->cols == dest->cols && a->rows == b->rows)) { do{}while(1); } else do{}while(0); }while(0);

     
    do{ if(!(matrix_size(a) == 3)) { do{}while(1); } else do{}while(0); }while(0);

    dest->values[0] = a->values[1]*b->values[2] - a->values[2] *b->values[1];
    dest->values[1] = a->values[2]*b->values[0] - a->values[0] *b->values[2];
    dest->values[2] = a->values[0]*b->values[1] - a->values[1] *b->values[0];
}

float64_t matrix_determinant(const matrix_t *src)
{
    if(src->rows == 2 && src->cols == 2)
    {
        return *matrix_const_get(src, 0, 0) * *matrix_const_get(src, 1, 1)
            - *matrix_const_get(src, 0, 1)* *matrix_const_get(src, 1, 0);
    }
    else if(src->rows == 3 && src->cols == 3)
    {
        return *matrix_const_get(src, 0, 0) * matrix_det_helper(src, 4, 5, 7, 8)
         - *matrix_const_get(src, 0, 1) * matrix_det_helper(src, 3, 5, 6, 8)
         + *matrix_const_get(src, 0, 2) * matrix_det_helper(src, 3, 4, 6, 7);
    }
    else
    {
         
        do{ if(!(0)) { do{}while(1); } else do{}while(0); }while(0);
        return 0;
    }

}

static float64_t matrix_det_helper(const matrix_t *src, uint16_t idx1, uint16_t idx2, uint16_t idx3, uint16_t idx4)
{
    return src->values[idx1]*src->values[idx4] - src->values[idx2]*src->values[idx3];
}

float64_t matrix_dot_product( const matrix_t *a, const matrix_t *b )
{
    do{ if(!((a->cols == 1 && b->cols == 1) || (a->rows == 1 && b->rows == 1))) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(matrix_size(a) == matrix_size(b))) { do{}while(1); } else do{}while(0); }while(0);
    do{ if(!(a->cols == b->cols && a->rows == b->rows)) { do{}while(1); } else do{}while(0); }while(0);

    float64_t res=0.0;
    uint16_t i;
    uint16_t size = matrix_size(a);
    for( i=0; i<size; i++ )
        res += a->values[i]*b->values[i];

    return res;
}

void gauss_jordan(matrix_t * a, matrix_t *b)
{
    int16_t i = 0;
    for( i=0; i<a->rows; i++)
    {
        uint16_t max_row = i, j;
        

        for(j=i+1; j<a->rows; j++)
            if(fabs(*matrix_get(a,j,i)) > fabs(*matrix_get(a,max_row,i)))
                max_row = j;

        
        if(max_row != i)
        {
            for(j=0; j<a->cols; j++)
                matrix_entry_swap( matrix_get(a,max_row, j), matrix_get(a,i,j));
            for(j=0; j<b->cols; j++)
                matrix_entry_swap( matrix_get(b,max_row, j), matrix_get(b,i,j));
        }

        
        

        
        for(j=i+1; j<a->rows; j++)
        {
            float64_t scale = (*matrix_get(a,j,i))/(*matrix_get(a,i,i));
            uint16_t k;
            for(k=i; k<a->cols; k++)
                *matrix_get(a,j,k) -= *matrix_get(a,i,k)*scale;
            for(k=0; k<b->cols; k++)
                *matrix_get(b,j,k) -= *matrix_get(b,i,k)*scale;
        }

        
        
        
    }

    for(i=a->rows-1; i>=0; i--)
    {
        float64_t scale = *matrix_get(a,i,i);
        int16_t j, k;
        for(j=0; j<i; j++)
        {
            float64_t row_scale  = *matrix_get(a,j,i)/scale;
            for(k=a->cols-1; k>i-1; k--)
                *matrix_get(a,j,k) -= *matrix_get(a,i,k)*row_scale;
            for(k=b->cols-1; k>=0; k--)
                *matrix_get(b,j,k) -= *matrix_get(b,i,k)*row_scale;
        }

        
        for(j=0; j<a->cols; j++)
            *matrix_get(a,i,j) /= scale;
        for(j=0; j<b->cols; j++)
            *matrix_get(b,i,j) /= scale;
    }
}

float64_t matrix_trace(const matrix_t * src ) 
{
    do{ if(!(src->rows == src->cols)) { do{}while(1); } else do{}while(0); }while(0);

    float64_t ret = 0;

    uint16_t i ;
    for (i=0; i<src->rows; i++) {
        ret += *matrix_const_get(src, i, i);
    }
    return ret;
}

_Bool matrix_symmetric(const matrix_t * src ) 
{
    uint16_t i, j;

    if (src->rows != src->cols) {
        return 0;
    }

    for (i=0; i<src->rows; i++) {
        for (j=0; j<src->cols; j++) {
            if (fabs(*matrix_const_get(src, i, j) - *matrix_const_get(src, j, i))
                    >= fabs(*matrix_const_get(src,i,j) * 0.01))
            {

                if ( fabs (*matrix_const_get(src, i , j) >= 1e-26) &&
                        fabs(*matrix_const_get(src, j, i) >= 1e-26))
                {
                    printf("i: %d, j: %d, el1: %e, el2: %e\n", i, j,
                        *matrix_const_get(src, i, j), *matrix_const_get(src,j,i));

                    do { fprintf((&_ftable[1]), "%s = \n[\n", "*src"); matrix_print((&_ftable[1]), & *src); fprintf((&_ftable[1]), "]\n"); } while(0);
                    return 0;
                }
            }
        }
    }

    return 1;
}

float64_t matrix_2norm(const matrix_t *src )
{
    float64_t temp = 0;

    uint16_t i;
    for(i=0; i<matrix_size(src); i++)
    {
        temp += src->values[i] * src->values[i];
    }
    return sqrt(temp);
}