#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_system.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include "driver/gpio.h"
#include "sensors.h"

#define SCL_PIN GPIO_NUM_22  
#define SDA_PIN GPIO_NUM_21 
static const char *TAG = "main";

static void sensorTask(void *pvParameters) {
	ESP_LOGI(TAG,"sensor task started\n");

	initAcc(SCL_PIN,SDA_PIN);
	ESP_LOGI(TAG,"accelerometer and magnetometer started");

	int acc_mag_vals[6];
	while (1) {
		getAccelerometerData(acc_mag_vals);
		ESP_LOGI(TAG,"X Accelerometer: %d, Y Accelerometer: %d, Z Accelerometer: %d\n, X Magnet: %d, Y Magnet: %d, Z Magnet: %d\n",(int16_t)acc_mag_vals[0],(int16_t)acc_mag_vals[1],(int16_t)acc_mag_vals[2], (int16_t)acc_mag_vals[3],(int16_t)acc_mag_vals[4],(int16_t)acc_mag_vals[5]);
		vTaskDelay(1000 / portTICK_RATE_MS);
	}

}

void app_main() {
	ESP_ERROR_CHECK(nvs_flash_init());
	ESP_LOGI("system","system inited");

	xTaskCreate(&sensorTask,	//pvTaskCode
			"sensorTask",//pcName
			4096,//usStackDepth
			NULL,//pvParameters
			4,//uxPriority
			NULL//pxCreatedTask
			);

}