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

static void i2c_init(){
  i2c_config_t conf = {
    .mode = I2C_MODE_MASTER,
    .sda_io_num = GPIO_NUM_21,
    .scl_io_num = GPIO_NUM_22,
    .sda_pullup_en = GPIO_PULLUP_ENABLE,
    .scl_pullup_en = GPIO_PULLUP_ENABLE,
    .master.clk_speed = 100000
  };
  i2c_param_config(I2C_NUM_0, &conf);
  i2c_driver_install(I2C_NUM_0, conf.mode, 0,0,0);
}

static void write_data(uint8_t reg, uint8_t data){
  i2c_cmd_handle_t cmd = i2c_cmd_link_create();
  i2c_master_start(cmd);
  i2c_master_write_byte(cmd, (ox68<<1)|0, true);
  i2c_master_write_byte(cmd, reg, true);
  i2c_master_write_byte(cmd, data, true);
  i2c_master_stop(cmd);
  i2c_master_cmd_begin(I@C_NUM_0 ,cmd, pdMS_TO_TICKS(100));
  i2c_cmd_link_delete(cmd);
  }


static void read_data(uint8_t *x, size_t len){
  i2c_cmd_handle_t cmd = i2c_cmd_link_create();
  i2c_master_start(cmd);
  i2c_master_write_byte(cmd, (0x68<<1)|0, true);
  i2c_master_write_byte(cmd, 0x3B, true);
  i2c_master_start(cmd);
  i2c_master_write_byte(cmd, (0x68<<1)|1, true);
  i2c_master_read(cmd, x, len, I2C_MASTER_LAST_NACK);
  i2c_master_stop(cmd);
  i2c_master_cmd_begin(I2C_NUM_0,cmd,pdMS_TO_TICKS(100));
  i2c_cmd_link_delete(cmd);

}

static uint8_t calc_deflection(uint8_t angle){
  E = 2 * 10^5;
  I = 12 * 10^6;


}

void app_main(void){
  i2c_init();
  printf("i2c protocal established\n");
  while(true){
    uint8_t x[6];
    read_data(x, 6);
    for(int i = 0; i <5;i+=2){
      printf("%d ,", (x[i]<<8)|x[i+1]);
    }
    printf("\n");
  }
}