/*
 * app.c
 *
 *  Created on: Apr 3, 2021
 *      Author: Think
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "app.h"
#include "main.h"
#include "LSM303DLHC.h"

#include "COMPASS_LEDS.h"

#define BLINK_RATE_MS 500
#define PRINT_RATE_MS 100

uint32_t ts_blink = 0, ts_print = 0;
int16_t x = 0, y = 0, z = 0;
uint8_t id[3] = {0};

//eCompasse declaration
extern I2C_HandleTypeDef hi2c1;
LSM303 eComp;
COMPASS_LEDS compLeds;

void setup(void)
{
	COMPASS_LEDS_Init(&compLeds);

	if(!LSM303_Init(&eComp, &hi2c1))
		printf("Failed to init eCompass\r\n");

	if(!LSM303_EnableTemperatureSensor(&eComp, true))
		printf("Failed to enable temp sensor\r\n");

	if(!LSM303_ApplyConfig(&eComp))
		printf("Failed to apply config\r\n");

	if(!LSM303_GetDeviceID(&eComp, id))
		printf("Failed to retrieve id\r\n");
	else
		printf("LSM303 ID : %#X,%#X,%#X\r\n", id[0], id[1], id[2]);
}

void loop(void)
{
	if(HAL_GetTick() - ts_print > PRINT_RATE_MS)
	{
		//Lets read the temperature :
		float temperature = 0;

		if(!LSM303_GetTemperature(&eComp, &temperature, NULL))
			printf("Failed to get temperature\r\n");
		else
			printf("Temp is : %.3f\r\n", temperature);

		if(!LSM303_GetMagneticFieldData(&eComp, &x, &y, &z))
			printf("Failed to get magnetic data\r\n");
		else
			printf("x : %d, y : %d, z : %d\r\n", x, y, z);

		ts_print = HAL_GetTick();
	}

	if(x > 0 && x > abs(y) - 50)
		COMPASS_LEDS_Light(&compLeds, NORTH);
	if(x < 0 && abs(x) > abs(y) - 50)
		COMPASS_LEDS_Light(&compLeds, SOUTH);
	if(y > 0 && y > abs(x) - 50)
		COMPASS_LEDS_Light(&compLeds, EAST);
	if(y < 0 && abs(y) > abs(x) - 50)
		COMPASS_LEDS_Light(&compLeds, WEST);
}