#include <driver/i2c.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>

#define SDA_PIN 21
#define SCL_PIN 22
#define I2C_ADDRESS 0x68 // I2C address of MPU6050

#define MPU6050_ACCEL_XOUT_H 0x3B
#define MPU6050_PWR_MGMT_1   0x6B

/*
   The following registers contain the primary data we are interested in
   0x3B MPU6050_ACCEL_XOUT_H
   0x3C MPU6050_ACCEL_XOUT_L
   0x3D MPU6050_ACCEL_YOUT_H
   0x3E MPU6050_ACCEL_YOUT_L
   0x3F MPU6050_ACCEL_ZOUT_H
   0x50 MPU6050_ACCEL_ZOUT_L
   0x41 MPU6050_TEMP_OUT_H
   0x42 MPU6050_TEMP_OUT_L
   0x43 MPU6050_GYRO_XOUT_H
   0x44 MPU6050_GYRO_XOUT_L
   0x45 MPU6050_GYRO_YOUT_H
   0x46 MPU6050_GYRO_YOUT_L
   0x47 MPU6050_GYRO_ZOUT_H
   0x48 MPU6050_GYRO_ZOUT_L
*/


esp_err_t i2c_master_init(void)
{
  i2c_config_t i2c_config = {
    .mode = I2C_MODE_MASTER,
    .sda_io_num = SDA_PIN,
    .scl_io_num = SCL_PIN,
    .sda_pullup_en = GPIO_PULLUP_ENABLE,
    .scl_pullup_en = GPIO_PULLUP_ENABLE,
    .master.clk_speed = 100000
  };
  i2c_param_config(I2C_NUM_0, &i2c_config);
  i2c_driver_install(I2C_NUM_0, I2C_MODE_MASTER, 0, 0, 0);
  return ESP_OK;
}

esp_err_t mpu6050_init(void)
{
  i2c_cmd_handle_t cmd;
  vTaskDelay(200 / portTICK_PERIOD_MS);
  cmd = i2c_cmd_link_create();
  i2c_master_start(cmd);
  i2c_master_write_byte(cmd, (I2C_ADDRESS << 1) | I2C_MASTER_WRITE, 1);
  i2c_master_write_byte(cmd, MPU6050_ACCEL_XOUT_H, 1);
  i2c_master_stop(cmd);
  i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_PERIOD_MS);
  i2c_cmd_link_delete(cmd);
  cmd = i2c_cmd_link_create();
  i2c_master_start(cmd);
  i2c_master_write_byte(cmd, (I2C_ADDRESS << 1) | I2C_MASTER_WRITE, 1);
  i2c_master_write_byte(cmd, MPU6050_PWR_MGMT_1, 1);
  i2c_master_write_byte(cmd, 0, 1);
  i2c_master_stop(cmd);
  i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_PERIOD_MS);
  i2c_cmd_link_delete(cmd);
  return ESP_OK;
}

esp_err_t read_mpu6050(void) {

  uint8_t data[14];
  uint8_t accel_x;
  uint8_t accel_y;
  uint8_t accel_z;
  i2c_cmd_handle_t cmd;
  // Tell the MPU6050 to position the internal register pointer to register
  // MPU6050_ACCEL_XOUT_H.
  cmd = i2c_cmd_link_create();
  i2c_master_start(cmd);
  i2c_master_write_byte(cmd, (I2C_ADDRESS << 1) | I2C_MASTER_WRITE, 1);
  i2c_master_write_byte(cmd, MPU6050_ACCEL_XOUT_H, 1);
  i2c_master_stop(cmd);
  i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_PERIOD_MS);
  i2c_cmd_link_delete(cmd);

  cmd = i2c_cmd_link_create();
  i2c_master_start(cmd);
  i2c_master_write_byte(cmd, (I2C_ADDRESS << 1) | I2C_MASTER_READ, 1);

  i2c_master_read_byte(cmd, data,   0);
  i2c_master_read_byte(cmd, data + 1, 0);
  i2c_master_read_byte(cmd, data + 2, 0);
  i2c_master_read_byte(cmd, data + 3, 0);
  i2c_master_read_byte(cmd, data + 4, 0);
  i2c_master_read_byte(cmd, data + 5, 1);

  //i2c_master_read(cmd, data, sizeof(data), 1);
  i2c_master_stop(cmd);
  i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_PERIOD_MS);
  i2c_cmd_link_delete(cmd);

  accel_x = (data[0] << 8) | data[1];
  accel_y = (data[2] << 8) | data[3];
  accel_z = (data[4] << 8) | data[5];
  printf("Accel X: %3d, Y: %3d, Z: %3d\n", accel_x, accel_y, accel_z);

  vTaskDelay(500 / portTICK_PERIOD_MS);
  return ESP_OK;
}


void app_main()
{

  i2c_master_init();
  mpu6050_init();
  while (1)
  {
    read_mpu6050();
  }

}