#include <stdio.h>
#include <stdlib.h>
#include "stm32f4xx.h"
#define ARM_MATH_CM4
#include "arm_math.h"


uint32_t random32bit=0;
float avg_temp = 0;
uint32_t prev = 0;

volatile uint32_t msTicks = 0;                      /* counts 1ms timeTicks       */

// SysTick Handler (Interrupt Service Routine for the System Tick interrupt)
void SysTick_Handler(void){
  msTicks++;
}

// initialize the system tick
void InitSystick(void){
	SystemCoreClockUpdate();                      /* Get Core Clock Frequency   */
  if (SysTick_Config(SystemCoreClock / 1000)) { /* SysTick 1 msec interrupts  */
    while (1);                                  /* Capture error              */
  }
}

void linear_least_squares(float32_t * Y_f32, float32_t * X_f32,
	float32_t * W_f32, uint32_t n, uint32_t m, uint32_t p)
{
	float32_t XT_f32[16];
	float32_t XMXT_f32[4];
	float32_t XMXTI_f32[4];
	float32_t YMXT_f32[2];
	//float32_t W_f32[2];

	arm_matrix_instance_f32 X;
	arm_matrix_instance_f32 XT;
  arm_matrix_instance_f32 XMXT;
	arm_matrix_instance_f32 XMXTI;
	arm_matrix_instance_f32 Y;
	arm_matrix_instance_f32 YMXT;
	arm_matrix_instance_f32 W;

	arm_mat_init_f32(&X,m,n,X_f32);
	arm_mat_init_f32(&XT,n,m,XT_f32);
	arm_mat_init_f32(&XMXT,m,m,XMXT_f32);
	arm_mat_init_f32(&XMXTI,m,m,XMXTI_f32);
	arm_mat_init_f32(&YMXT,p,m,YMXT_f32);
	arm_mat_init_f32(&W,p,m,W_f32);

	arm_mat_trans_f32(&X,&XT);
	arm_mat_mult_f32(&X,&XT,&XMXT);
	arm_mat_inverse_f32(&XMXT,&XMXTI);


	arm_mat_init_f32(&Y,p,n,Y_f32);
	arm_mat_mult_f32(&Y,&XT,&YMXT);
	arm_mat_mult_f32(&YMXT,&XMXTI,&W);


}

int32_t main(void){

	float32_t W[2];

	float32_t X[16] = {
		0.8351, 0.0594, 0.3187, 0.8979, 0.2461, 0.5484, 0.7548, 0.2654,
		0.4823, 0.4603, 0.9102, 0.6512,0.2340, 0.3928, 0.3926, 0.3469};

	float32_t Y[8] = {
		1.7207, 0.2564, 1.0798, 2.0602, 0.2290, 0.7769, 1.3231,0.3144};

	int m = 2;
	int n = 8;
	int p = 1;

	linear_least_squares(Y,X,W,8,2,1);

	printf("%f\n ",W[0]);
	printf("%f\n ",W[1]);

}