#include "freertos/projdefs.h"
#include <math.h>
#include <stdint.h>
#include <stdio.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <esp_err.h>
#include <esp_log.h>
#include <mpu6050.h>
#include <esp_timer.h>
#include <math.h>

#define ADDR 0x68
#define DT 0.01f

static const char *TAG = "MPU";

typedef struct {
	float tangazh_angle; // Наклон вперед/назад
	float kren_angle; // Наклон влево/вправо
	float acl_bas[3];
	float gyr_bas[3];
} dop_value;

dop_value dop = {0};

static void calibrate(mpu6050_dev_t dev) {
	const int kol_vo = 500;
	float acl_sum[3] = {0};
	float rot_sum[3] = {0};
	
	int a[3] = {0};
    long double s_a[3] = {0};
	for(short i = 0; i < kol_vo; i++) {
		mpu6050_acceleration_t acl;
		mpu6050_rotation_t rot;
		
		if(mpu6050_get_motion(&dev, &acl, &rot) == ESP_OK) {
			acl_sum[0] += acl.x;
			acl_sum[1] += acl.y;
			acl_sum[2] += acl.z;
			a[0] = acl.x * 100;
			s_a[0] += a[0];
			a[1] = acl.y * 100;
			s_a[1] += a[1];
			a[2] = acl.z * 100;
			s_a[2] += a[2];
			
			rot_sum[0] += rot.x;
			rot_sum[1] += rot.y;
			rot_sum[2] += rot.z;
		}
		vTaskDelay(pdMS_TO_TICKS(10));	
	}
	
	for(uint8_t i = 0; i < 3; i++) {
		dop.acl_bas[i] = acl_sum[i] / kol_vo - s_a[i] / (kol_vo * 100);
		dop.gyr_bas[i] = rot_sum[i] / kol_vo;
 	}
	
}

static void func(float gy, float acl_tangazh, float acl_kren) {
	const float alpha = 0.97;
	dop.tangazh_angle = alpha * (dop.tangazh_angle + gy * DT) + (1 - alpha) * acl_tangazh;
	//dop.kren_angle = alpha * (dop.kren_angle + gx * DT) + (1 - alpha) * acl_kren;
}

static void compl_filter_update(mpu6050_acceleration_t accel, mpu6050_rotation_t gyro) {
	float ax = accel.x - dop.acl_bas[0];
	float ay = accel.y - dop.acl_bas[1];
	float az = accel.z - dop.acl_bas[2];
	
	
	float gx = gyro.x - dop.gyr_bas[0];
	float gy = gyro.y - dop.gyr_bas[1];
	// float gz = gyro->z - dop.gyr_bas[2];
	
	//float acl_tangazh = atan(ax/sqrt(ay * ay + az * az));
	float acl_tangazh = M_PI / 2 - acos(otrezok(ax, 1, -1));
	float acl_kren = atan(ay/sqrt(ax * ax + az * az));
	 
	post_gy = gy * DT;
	
	func(gy, acl_tangazh, acl_kren);

}

void app_main() {
  ESP_ERROR_CHECK(i2cdev_init());
    
  mpu6050_dev_t dev = { 0 };

  ESP_ERROR_CHECK(mpu6050_init_desc(&dev, ADDR, 0, 5, 4));

  while (1)
  {
      esp_err_t res = i2c_dev_probe(&dev.i2c_dev, I2C_DEV_WRITE);
      if (res == ESP_OK)
      {
          ESP_LOGI(TAG, "Found MPU60x0 device");
          break;
      }
      ESP_LOGE(TAG, "MPU60x0 not found");
      vTaskDelay(pdMS_TO_TICKS(1000));
  }
  
  mpu6050_set_full_scale_accel_range(&dev, MPU6050_ACCEL_RANGE_16);
  mpu6050_set_full_scale_gyro_range(&dev, MPU6050_GYRO_RANGE_2000);

  ESP_ERROR_CHECK(mpu6050_init(&dev));

  calibrate(dev);
  mpu6050_acceleration_t accel = { 0 };
  mpu6050_rotation_t gyro = { 0 };
  compl_filter_update(accel, gyro);

  while (true) {
    ESP_ERROR_CHECK(mpu6050_get_motion(&dev, &accel, &gyro));
		compl_filter_update(accel, gyro);

    printf("%.3f \n", dop.tangazh_angle);

    vTaskDelay(1000 / portTICK_PERIOD_MS);
  }
}