#include <stdio.h>
#include "esp_log.h"
#include "driver/i2c.h"
#include "gw_grayscale_sensor.h"

static const char *TAG = "gw gray i2c example";

#define I2C_MASTER_SCL_IO           22      /*!< GPIO number used for I2C master clock */
#define I2C_MASTER_SDA_IO           21      /*!< GPIO number used for I2C master data  */
#define I2C_MASTER_NUM              0                          /*!< I2C master i2c port number, the number of i2c peripheral interfaces available will depend on the chip */
#define I2C_MASTER_FREQ_HZ          100000                     /*!< I2C master clock frequency */
#define I2C_MASTER_TX_BUF_DISABLE   0                          /*!< I2C master doesn't need buffer */
#define I2C_MASTER_RX_BUF_DISABLE   0                          /*!< I2C master doesn't need buffer */
#define I2C_MASTER_TIMEOUT_MS       0

static uint8_t gw_gray_addr;

static esp_err_t gw_gray_write_cmd(uint8_t cmd)
{
	esp_err_t err;
	err = i2c_master_write_to_device(I2C_MASTER_NUM, gw_gray_addr,
	                                 &cmd, 1, pdMS_TO_TICKS(I2C_MASTER_TIMEOUT_MS));
	return err;
}

static esp_err_t gw_gray_read_byte(uint8_t *byte_read)
{
	esp_err_t err;
	err = i2c_master_read_from_device(I2C_MASTER_NUM, gw_gray_addr,
	                                  byte_read, 1, pdMS_TO_TICKS(I2C_MASTER_TIMEOUT_MS));
	return err;
}

static esp_err_t gw_gray_read(uint8_t *read_arr, size_t read_size)
{
	esp_err_t err;
	err = i2c_master_read_from_device(I2C_MASTER_NUM, gw_gray_addr,
	                                  read_arr, read_size, pdMS_TO_TICKS(I2C_MASTER_TIMEOUT_MS));
	return err;
}

static esp_err_t gw_gray_write(uint8_t *write_arr, size_t write_size)
{
	esp_err_t err;
	err = i2c_master_write_to_device(I2C_MASTER_NUM, gw_gray_addr,
	                                 write_arr, write_size, pdMS_TO_TICKS(I2C_MASTER_TIMEOUT_MS));
	return err;
}

static void i2c_scan_generic(void)
{
	printf("i2c generic scan: \n");
	for (uint8_t i = 1; i < 127; i++)
	{
		int ret;
		i2c_cmd_handle_t cmd = i2c_cmd_link_create();
		i2c_master_start(cmd);
		i2c_master_write_byte(cmd, (i << 1) | I2C_MASTER_WRITE, 1);
		i2c_master_stop(cmd);
		ret = i2c_master_cmd_begin(I2C_NUM_0, cmd, pdMS_TO_TICKS(100));
		i2c_cmd_link_delete(cmd);

		if (ret == ESP_OK) {
			printf("Found device at: 0x%2x\n", i);
		}
		vTaskDelay(pdMS_TO_TICKS(10));
	}
}

static uint8_t i2c_scan_gw_gray(void)
{
	printf("i2c gw gray ping scan: \n");
	uint8_t i2c_cmd = GW_GRAY_PING;
	uint8_t ping_rsp;
	uint8_t gw_gray_device_addr = 0;
	for (uint8_t i = 1; i < 127; i++)
	{
		int ret;
		ping_rsp = 0;
		ret = i2c_master_write_read_device(I2C_MASTER_NUM, i, &i2c_cmd, 1,
		                             &ping_rsp, 1, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
		if (ret != ESP_FAIL && ret != ESP_OK) {
			printf("%d, %s\n", i, esp_err_to_name(ret));
		}
		if (ping_rsp == 0x66) {
			printf("Found gw gray device at: 0x%2x\n", i);
			gw_gray_device_addr = i;
		} else if (ping_rsp != 0) {
			printf("Device at: 0x%2x respond :%x\n", i, ping_rsp);
		}
		vTaskDelay(pdMS_TO_TICKS(2));
	}

	return gw_gray_device_addr;
}

static esp_err_t i2c_master_init(void)
{
	int i2c_master_port = I2C_MASTER_NUM;

	i2c_config_t conf = {
			.mode = I2C_MODE_MASTER,
			.sda_io_num = I2C_MASTER_SDA_IO,
			.scl_io_num = I2C_MASTER_SCL_IO,
			.sda_pullup_en = GPIO_PULLUP_ENABLE,
			.scl_pullup_en = GPIO_PULLUP_ENABLE,
			.master.clk_speed = I2C_MASTER_FREQ_HZ,
	};

	i2c_param_config(i2c_master_port, &conf);

	return i2c_driver_install(i2c_master_port, conf.mode, I2C_MASTER_RX_BUF_DISABLE, I2C_MASTER_TX_BUF_DISABLE, 0);
}


void app_main(void)
{
	int err;
	uint8_t ping_rsp = 0;
	uint8_t i2c_cmd;
	uint8_t data = 0;
	uint8_t data_arr[8];
	uint8_t data_arr2[8];
	i2c_cmd_handle_t cmd;

	gw_gray_addr = GW_GRAY_ADDR_DEF;

	ESP_ERROR_CHECK(i2c_master_init());
	ESP_LOGI(TAG, "I2C initialized successfully");

	vTaskDelay(pdMS_TO_TICKS(1000));


	if(!gw_gray_write_cmd(GW_GRAY_PING)){
		ESP_ERROR_CHECK_WITHOUT_ABORT(gw_gray_read_byte(&data));
	}

	if (data == GW_GRAY_PING_OK) {
		ESP_LOGI(TAG, "[1.0] PING OK");
	} else {
		ESP_LOGI(TAG, "[1.1.0] PING FAILED, SEARCH FOR GW_GRAY DEVICE");
		ESP_LOGI(TAG, "[1.1.2] SCAN BEGIN");
		i2c_scan_generic();
		gw_gray_addr = i2c_scan_gw_gray();
		ESP_LOGI(TAG, "[1.1.3] SCAN END");
		if (gw_gray_addr == 0) {
			ESP_LOGI(TAG, "[1.1.4] NO GW GRAY DEVICE FOUND, RET");
			goto endless_while;
		}
	}

	/* PING 通为止 */
	ESP_LOGI(TAG, "[2.0] PING BEGIN");
	i2c_cmd = GW_GRAY_PING;
	while (ping_rsp != GW_GRAY_PING_OK) {
		err = i2c_master_write_read_device(I2C_MASTER_NUM, gw_gray_addr, &i2c_cmd, 1,
		                             &ping_rsp, 1, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
		if (err) {
			ESP_LOGI(TAG, "%s", esp_err_to_name(err));
		}
		ESP_LOGI(TAG, "[2.1] ping_rsp from %X : %X", gw_gray_addr, ping_rsp);
		vTaskDelay(pdMS_TO_TICKS(500));
	}
	ESP_LOGI(TAG, "[2.2] PING OK");

	err = gw_gray_write_cmd(GW_GRAY_FIRMWARE);
	if (err) {
		ESP_LOGI(TAG, "[4.1] READ FIRMWARE ERROR %s", esp_err_to_name(err));
	} else {
		err = gw_gray_read_byte(&data);
		if (err) {
			ESP_LOGI(TAG, "[4.2] READ FIRMWARE ERROR %s", esp_err_to_name(err));
		} else {
			ESP_LOGI(TAG, "[4.0] GW GRAY FIRMWARE VERSION: %d.%d", data >> 4, data & 0x0f);
		}
	}

	goto endless_while;
//	ESP_LOGI(TAG, "[3.0] ADDR CHANGE BEGIN");
//	data = 0x33;
//	cmd = i2c_cmd_link_create();
//	i2c_master_start(cmd);
//	i2c_master_write_byte(cmd, (gw_gray_addr << 1) | I2C_MASTER_WRITE, 1);
//	i2c_master_write_byte(cmd, GW_GRAY_CHANGE_ADDR, 1);
//	i2c_master_write(cmd, &data, 1, 0);
//	i2c_master_stop(cmd);
//	err = i2c_master_cmd_begin(I2C_NUM_0, cmd, pdMS_TO_TICKS(100));
//	if (err) {
//		ESP_LOGI(TAG, "%s", esp_err_to_name(err));
//	}
//	i2c_cmd_link_delete(cmd);
//
//	vTaskDelay(pdMS_TO_TICKS(100));
//	err = gw_gray_write_cmd(GW_GRAY_REBOOT);
//	if (err) {
//		ESP_LOGI(TAG, "%s", esp_err_to_name(err));
//	}
//
//	vTaskDelay(pdMS_TO_TICKS(2));
//	gw_gray_addr = i2c_scan_gw_gray();
//
//	if (gw_gray_addr == 0x19) {
//		ESP_LOGI(TAG, "[3.1] BROADCAST BEGIN");
//		gw_gray_addr = 0x00;
//		gw_gray_write((uint8_t *)GW_GRAY_BROADCAST_RESET, 8);
//		vTaskDelay(pdTICKS_TO_MS(2));
//		gw_gray_addr = i2c_scan_gw_gray();
//	} else {
//		ESP_LOGI(TAG, "[3.0.1] ADDR CHANGE ERROR");
//	}

	ESP_ERROR_CHECK(gw_gray_read_byte(&data));
	ESP_LOGI(TAG, "data: %2x", data);

	// read 2x digital data
	ESP_ERROR_CHECK(gw_gray_write_cmd(GW_GRAY_DIGITAL_MODE));
	ESP_ERROR_CHECK(gw_gray_read_byte(&data));
	ESP_LOGI(TAG, "[0] data 0: %2x", data);

	vTaskDelay(pdMS_TO_TICKS(1));
	ESP_ERROR_CHECK(gw_gray_read_byte(&data));
	ESP_LOGI(TAG, "[1] data 1: %2x", data);

	// ping test
	ESP_ERROR_CHECK(gw_gray_write_cmd(GW_GRAY_PING));
	ESP_ERROR_CHECK(gw_gray_read_byte(&data));
	ESP_LOGI(TAG, "[2.1] ping : %2x", data);

	ESP_ERROR_CHECK(gw_gray_read_byte(&data));
	ESP_LOGI(TAG, "[2.2] ping : %2x", data);

	ESP_ERROR_CHECK(gw_gray_write_cmd(GW_GRAY_PING));
	vTaskDelay(pdMS_TO_TICKS(2));
	ESP_ERROR_CHECK(gw_gray_read_byte(&data));
	ESP_LOGI(TAG, "[2.3] ping with delay : %2x", data);

	// read digital data
	ESP_ERROR_CHECK(gw_gray_write_cmd(GW_GRAY_DIGITAL_MODE));
	ESP_ERROR_CHECK(gw_gray_read_byte(&data));
	ESP_LOGI(TAG, "[3] data : %2x", data);

	ESP_ERROR_CHECK(gw_gray_write_cmd(GW_GRAY_PING));
	vTaskDelay(pdMS_TO_TICKS(1));
	ESP_LOGI(TAG, "[2.1] ping : %2x", data);

	// change calibartion test
	ESP_ERROR_CHECK(gw_gray_write_cmd(GW_GRAY_CALIBRATION_BLACK));
	ESP_ERROR_CHECK(gw_gray_read(data_arr, 8));
	printf("[4.1] calib black data_arr ");
	for (int i = 0; i < 8; ++i) {
		printf("%d ", data_arr[i]);
		data_arr[i] += 2;
	}
	printf("\n");

	ESP_ERROR_CHECK(gw_gray_write_cmd(GW_GRAY_CALIBRATION_WHITE));
	ESP_ERROR_CHECK(gw_gray_read(data_arr2, 8));
	printf("[4.2] calib white data_arr2 ");
	for (int i = 0; i < 8; ++i) {
		printf("%d ", data_arr2[i]);
		data_arr2[i] += 2;
	}
	printf("\n");

	cmd = i2c_cmd_link_create();
	i2c_master_start(cmd);
	i2c_master_write_byte(cmd, (gw_gray_addr << 1) | I2C_MASTER_WRITE, 1);
	i2c_master_write_byte(cmd, GW_GRAY_CALIBRATION_BLACK, 1);
	i2c_master_write(cmd, data_arr, 8, 1);
	i2c_master_stop(cmd);
	err = i2c_master_cmd_begin(I2C_NUM_0, cmd, pdMS_TO_TICKS(100));
	i2c_cmd_link_delete(cmd);

	cmd = i2c_cmd_link_create();
	i2c_master_start(cmd);
	i2c_master_write_byte(cmd, (gw_gray_addr << 1) | I2C_MASTER_WRITE, 1);
	i2c_master_write_byte(cmd, GW_GRAY_CALIBRATION_WHITE, 1);
	i2c_master_write(cmd, data_arr2, 8, 1);
	i2c_master_stop(cmd);
	err = i2c_master_cmd_begin(I2C_NUM_0, cmd, pdMS_TO_TICKS(100));
	i2c_cmd_link_delete(cmd);

	ESP_ERROR_CHECK(gw_gray_write_cmd(GW_GRAY_CALIBRATION_BLACK));
	ESP_ERROR_CHECK(gw_gray_read(data_arr, 8));
	printf("[4.1] data_arr ");
	for (int i = 0; i < 8; ++i) {
		printf("%d ", data_arr[i]);
		data_arr[i] += 2;
	}
	printf("\n");

	ESP_ERROR_CHECK(gw_gray_write_cmd(GW_GRAY_CALIBRATION_WHITE));
	ESP_ERROR_CHECK(gw_gray_read(data_arr2, 8));
	printf("[4.2] data_arr2 ");
	for (int i = 0; i < 8; ++i) {
		printf("%d ", data_arr2[i]);
		data_arr2[i] += 2;
	}
	printf("\n");


	ping_rsp = 0;
	while (ping_rsp == 0) {
		i2c_master_write_read_device(I2C_MASTER_NUM, gw_gray_addr, &i2c_cmd, 1,
		                             &ping_rsp, 1, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
		ESP_LOGI(TAG, "ping_rsp 0x4d = %X", ping_rsp);
		vTaskDelay(pdMS_TO_TICKS(500));
	}
	ESP_LOGI(TAG, "PING OK");


endless_while:
	while (1) {
		vTaskDelay(portMAX_DELAY);
	}
}