/*
 * Copyright (c) 2018 Samsung Electronics Co., Ltd All Rights Reserved
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an AS IS BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <peripheral_io.h>

#include "log.h"
#include "resource/resource_lidar_v3.h"

/* http://static.garmin.com/pumac/LIDAR_Lite_v3_Operation_Manual_and_Technical_Specifications.pdf */

#define I2C_BUS_DEF (1)
#define LLV3_I2C_ADDR_DEF (0x62)

// Control Registers
#define LLV3_ACQ_CMD (0x00)
#define LLV3_STATUS (0x01)
#define LLV3_SIG_CNT_VAL (0x02)
#define LLV3_ACQ_CONFIG (0x04)
#define LLV3_DISTANCE (0x8f)
#define LLv3_DISTANCE_HIGH (0x0f)
#define LLv3_DISTANCE_LOW (0x10)
#define LLV3_REF_CNT_VAL (0x12)
#define LLV3_THRESH_BYPASS (0x1c)

// ACQ commands
#define CMD_RESET (0x00)
#define CMD_WITHOUT_BIAS_CORRECTION (0x03)
#define CMD_WITH_BIAS_CORRECTION (0x04)

#define BUSY_TIMEMOUT_CNT (9999)

static peripheral_i2c_h g_sensor_h;
static llv3_mode_e g_mode;

static int __lidar_v3_mode_set(llv3_mode_e mode)
{
	unsigned char sig_cnt_max = 0x80;
	unsigned char acq_conf_reg = 0x08;
	unsigned char ref_cnt_max = 0x05;
	unsigned char threadhold = 0x00;
	int ret = 0;

	switch (mode) {
	case LLV3_MODE_SRHS:
		sig_cnt_max = 0x1d;
		ref_cnt_max = 0x03;
		break;
	case LLV3_MODE_DR_HSSR:
		acq_conf_reg = 0x00;
		ref_cnt_max = 0x03;
		break;
	case LLV3_MODE_MAXRANGE:
		sig_cnt_max = 0xff;
		break;
	case LLV3_MODE_HIGH_SENSE:
		threadhold = 0x80;
		break;
	case LLV3_MODE_LOW_SENSE:
		threadhold = 0xb0;
		break;
	case LLV3_MODE_DEFAULT:
	default:
		break;
	}

	ret = peripheral_i2c_write_register_byte(g_sensor_h, LLV3_SIG_CNT_VAL, sig_cnt_max);
	retv_if(ret, -1);

	ret = peripheral_i2c_write_register_byte(g_sensor_h, LLV3_ACQ_CONFIG, acq_conf_reg);
	retv_if(ret, -1);

	ret = peripheral_i2c_write_register_byte(g_sensor_h, LLV3_REF_CNT_VAL, ref_cnt_max);
	retv_if(ret, -1);

	ret = peripheral_i2c_write_register_byte(g_sensor_h, LLV3_THRESH_BYPASS, threadhold);
	retv_if(ret, -1);

	return 0;
}

static int __lidar_v3_i2c_open(int i2c_bus)
{
	int ret = 0;

	if (g_sensor_h)
		return 0;

	ret = peripheral_i2c_open(i2c_bus, LLV3_I2C_ADDR_DEF, &g_sensor_h);
	retv_if(ret, -1);

	ret = __lidar_v3_mode_set(g_mode);
	if (ret) {
		_E("failed to set mode : %d", g_mode);
		peripheral_i2c_close(g_sensor_h);
		g_sensor_h = NULL;
		return -1;
	}
	return 0;
}

static int __wait_busy_flag(void)
{
	int cnt = 0;
	int busy = 0;

	do {
		unsigned char state = 0;
		peripheral_i2c_read_register_byte(g_sensor_h, LLV3_STATUS, &state);
		busy = state & 0x01;
		cnt++;
	} while (busy && (cnt <= BUSY_TIMEMOUT_CNT));

	if (cnt > BUSY_TIMEMOUT_CNT) {
		_E("TIMEOUT : The lidar sensor is busy now");
		return -1;
	}

	return 0;
}

void resource_close_lidar_v3(void)
{
	g_mode = LLV3_MODE_DEFAULT;

	if (!g_sensor_h)
		return;

	peripheral_i2c_close(g_sensor_h);
	g_sensor_h = NULL;
}

int resource_lidar_v3_mode_set(llv3_mode_e mode)
{
	g_mode = mode;
	if (!g_sensor_h)
		return 0;

	return __lidar_v3_mode_set(mode);
}

int resource_read_lidar_v3(unsigned int *out_value)
{
	int ret = 0;
	unsigned char val_h = 0;
	unsigned char val_l = 0;

	retv_if(!out_value, -1);

	ret = __lidar_v3_i2c_open(I2C_BUS_DEF);
	retv_if(ret, -1);

	ret = peripheral_i2c_write_register_byte(g_sensor_h, LLV3_ACQ_CMD, CMD_WITH_BIAS_CORRECTION);
	retv_if(ret, -1);

	ret = __wait_busy_flag();
	retv_if(ret, -1);

	ret = peripheral_i2c_read_register_byte(g_sensor_h, LLv3_DISTANCE_HIGH, &val_h);
	retv_if(ret, -1);

	ret = peripheral_i2c_read_register_byte(g_sensor_h, LLv3_DISTANCE_LOW, &val_l);
	retv_if(ret, -1);

	*out_value = ((val_h << 8) | val_l);

	return 0;
}