/*******************************************************************************
* Copyright (C) 2017 Maxim Integrated Products, Inc., All Rights Reserved.
*
* Permission is hereby granted, free of charge, to any person obtaining a
* copy of this software and associated documentation files (the "Software"),
* to deal in the Software without restriction, including without limitation
* the rights to use, copy, modify, merge, publish, distribute, sublicense,
* and/or sell copies of the Software, and to permit persons to whom the
* Software is furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included
* in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
* OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
* IN NO EVENT SHALL MAXIM INTEGRATED BE LIABLE FOR ANY CLAIM, DAMAGES
* OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
* ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
* OTHER DEALINGS IN THE SOFTWARE.
*
* Except as contained in this notice, the name of Maxim Integrated
* Products, Inc. shall not be used except as stated in the Maxim Integrated
* Products, Inc. Branding Policy.
*
* The mere transfer of this software does not imply any licenses
* of trade secrets, proprietary technology, copyrights, patents,
* trademarks, maskwork rights, or any other form of intellectual
* property whatsoever. Maxim Integrated Products, Inc. retains all
* ownership rights.
*******************************************************************************
*/

#include "MAX8614X.h"
#include "PpgComm.h"
#include "Peripherals.h"

#define ARRAY_SIZE(array)			(sizeof(array)/sizeof(array[0]))

#define MAX8614X_AGC_DEFAULT_LED_OUT_RANGE			50 //AGC Range parameter
#define MAX8614X_AGC_INCREMENT_MID      			15 //AGC Range parameter MID
#define MAX8614X_AGC_INCREMENT_TOP			        15 //AGC Range parameter TOP

MAX8614X::MAX8614X(SPI &spiBus, DigitalOut &cs, PinName pin)
:m_ir(pin), m_spiBus(spiBus), m_cs(cs)
{
	m_cs = 1;
	kMax8614xDefaultLedOutRange = MAX8614X_AGC_DEFAULT_LED_OUT_RANGE;

}

int MAX8614X::readRegister(uint8_t reg, uint8_t *data, int len)
{
	int i = 0;

	m_cs = 0;

	m_spiBus.write(reg);
	m_spiBus.write(0x80);
	for(; i < len; i++) { /*TODO: make len unsigned*/
		data[i] = m_spiBus.write(0x00);
	}

	m_cs = 1;

	return 0; /*TODO: handle error cases*/
}

int MAX8614X::writeRegister(uint8_t reg, const uint8_t data)
{
	m_cs = 0;

	m_spiBus.write(reg);
	m_spiBus.write(0x00);
	m_spiBus.write(data);

	m_cs = 1;

	return 0; /*TODO: handle error cases*/
}

int MAX8614X::writeBlock(const RegisterMap reg_block[], unsigned int size)
{
	unsigned int i;
	int ret = 0;

	for (i = 0; i < size; i++) {
		ret = writeRegister((Registers) reg_block[i].addr,
				reg_block[i].val);
		if (ret < 0) {
			pr_err("writeRegister failed. ret: %d", ret);
			return ret;
		}
	}

	return ret;
}

int MAX8614X::get_part_info(uint8_t *part_id, uint8_t *rev_id)
{
	uint32_t i;
	int ret;
	uint8_t buf[2];
	const uint8_t max8614x_part_ids[] = {
		MAX86140_PART_ID_VAL,
		MAX86141_PART_ID_VAL,
		MAX86142_PART_ID_VAL,
		MAX86143_PART_ID_VAL,
	};

	ret = readRegister(MAX8614X_REV_ID_REG, buf, 2);
	if (ret < 0) {
		pr_err("readRegister failed. ret: %d", ret);
		return ret;
	}

	for (i = 0; i < ARRAY_SIZE(max8614x_part_ids); i++) {
		if (buf[1] == max8614x_part_ids[i]) {
			*rev_id = buf[0];
			*part_id = buf[1];
			return 0;
		}
	}

	// Unsupported part
	return -1;
}

int MAX8614X::get_num_samples_in_fifo()
{
	int fifo_ovf_cnt;
	uint8_t fifo_data[4];
	uint32_t num_samples;
	int ret;

	ret = readRegister(MAX8614X_OVF_CNT_REG, fifo_data, 2);
	if (ret < 0) {
		pr_err("readRegister failed. ret: %d", ret);
		return ret;
	}

	fifo_ovf_cnt = fifo_data[0] & MAX8614X_OVF_CNT_MASK;
	num_samples = fifo_data[1];

	if (num_samples >= MAX8614X_MAX_FIFO_DEPTH) {
		pr_err("# FIFO is Full. OVF: %d num_samples: %lu",
				fifo_ovf_cnt, num_samples);
	}

	return num_samples;
}

int MAX8614X::read_fifo_data(uint8_t *fifo_data, int num_samples)
{
	uint16_t num_bytes = num_samples * MAX8614X_DATA_WORD_SIZE;
	int ret = 0;

//	const struct RegisterMap test_clk_enable[] = {
//		{0xFF, 0x54},
//		{0xFF, 0x4D},
//		{0x81, 0x20},
//		{0xFF, 0x00}
//	};
//
//	const struct RegisterMap test_clk_disable[] = {
//		{0xFF, 0x54},
//		{0xFF, 0x4D},
//		{0x81, 0x00},
//		{0xFF, 0x00}
//	};

//	ret = writeBlock(test_clk_enable, ARRAY_SIZE(test_clk_enable));
//	if (ret < 0) {
//		pr_err("writeBlock failed. ret: %d", ret);
//		return ret;
//	}

	fifo_data[0] = MAX8614X_FIFO_DATA_REG;
	ret = readRegister(MAX8614X_FIFO_DATA_REG, fifo_data, num_bytes);
	if (ret < 0) {
		pr_err("readRegister failed. ret: %d", ret);
		return ret;
	}

//	ret = writeBlock(test_clk_disable, ARRAY_SIZE(test_clk_disable));
//	if (ret < 0) {
//		pr_err("writeBlock failed. ret: %d", ret);
//		return ret;
//	}

	return ret;
}

void MAX8614X::irq()
{
	irq_triggered = true;
}

int MAX8614X::irq_handler()
{
	int ret;
	int_status_t status;
	irq_triggered = false;

	ret = readRegister(MAX8614X_INT_STATUS1_REG, status.val, 2);
	if (ret < 0) {
		pr_err("readRegister failed. ret: %d", ret);
		return -1;
	}
	pr_debug("Status reg: %X %X", status.val[0], status.val[1]);

	if (status.a_full || status.data_rdy) {
		fifo_irq_handler();
	}

	if (status.die_temp_rdy) {
		pr_debug("Pwr_rdy interrupt was triggered.");
	}

	if (status.pwr_rdy) {
		pr_info("Pwr_rdy interrupt was triggered.");
	}

	if (status.die_temp_rdy) {
		read_die_temp();
	}

	if (status.vdd_oor) {
		vdd_oor_cnt++;
		pr_info("VDD Out of range cnt: %d", vdd_oor_cnt);
	}
	return 0;
}

int MAX8614X::fifo_irq_handler()
{
	uint8_t fifo_buf[MAX8614X_MAX_FIFO_DEPTH * MAX8614X_DATA_WORD_SIZE] = {0};
	int ret;
	int num_samples = 0;
	static int last_samples[DATA_TYPE_PPG2_LEDC4];
	fifo_data_t fifo_data;
	uint16_t idx;
	int i;
	static ppg_data_t ppg_data;
	static uint32_t ppg_data_ack = 0;
	const uint32_t ir_green_red_ppg_type = ((1 << DATA_TYPE_PPG1_LEDC1) |
											(1 << DATA_TYPE_PPG1_LEDC2) |
											(1 << DATA_TYPE_PPG1_LEDC3));

	num_samples = get_num_samples_in_fifo();
	if (num_samples <= 0) {
		pr_err("get_num_samples_in_fifo failed. err: %d", num_samples);
		return -1;
	}

	pr_debug("num_samples: %d", num_samples);

	ret = read_fifo_data(fifo_buf, num_samples);
	if (ret < 0) {
		pr_err("read_fifo_data failed. ret: %d", ret);
		return -1;
	}

	for (i = 0; i < num_samples; i++) {
		idx = MAX8614X_DATA_WORD_SIZE * i;
		fifo_data.raw = fifo_buf[idx + 0] << 16
				| fifo_buf[idx + 1] << 8
				| fifo_buf[idx + 2];
		if (fifo_data.type == DATA_TYPE_INVALID_DATA || fifo_data.type == 0) {
			pr_err("Received invalid data. data: %X, i: %d",
					(unsigned int)fifo_data.raw, i);

			continue;
		}
		pr_debug("\ttype: %lu, val: %lu (%lX)",
				fifo_data.type, fifo_data.val, fifo_data.val);

		if (fifo_data.type > DATA_TYPE_PPG2_LEDC3) {
			pr_err("Wrong Data Type: -> Type: %lu, val:%lu, Raw:%lu",
					fifo_data.type, fifo_data.val, fifo_data.raw);
			continue;
		}

		if (fifo_data.type > 0 && fifo_data.type < DATA_TYPE_PPG2_LEDC3) {
			last_samples[fifo_data.type] = fifo_data.val;
		}

		if (fifo_data.type == DATA_TYPE_PPG1_LEDC1) {
			max8614x_agc_handler(&led_ctrl, last_samples);
		}

		if (fifo_data.type == DATA_TYPE_PPG1_LEDC1) {
			ppg_data.ir = fifo_data.val;
		} else if (fifo_data.type == DATA_TYPE_PPG1_LEDC2) {
			ppg_data.red = fifo_data.val;
		} else if (fifo_data.type == DATA_TYPE_PPG1_LEDC3) {
			ppg_data.green = fifo_data.val;
		}

		ppg_data_ack |= 1 << fifo_data.type;

		if ((ppg_data_ack & ir_green_red_ppg_type) != ir_green_red_ppg_type ){
			continue;
		}
		ppg_data_ack = 0;

		if (ppg_queue.size() >= MAX8614X_COMMON_QUEUE_SIZE) {
			pr_err("ppg_queue is full.");
		} else {
			ppg_queue.push(ppg_data);
		}
	}
	return 0;
}

int MAX8614X::get_sensor_report(ppg_sensor_report &sensor_report)
{
	int32_t ret;
	uint32_t irleddata, redleddata, greenleddata;
	//TODO: Implement
	ret = dequeue_from_fifo_queue(&irleddata, &redleddata, &greenleddata);
	if(ret == RTN_NO_ERROR){
		sensor_report.grn = irleddata;
		sensor_report.grn2 = redleddata;
	}else{
		return -1;
	}
	return 0;
}

int MAX8614X::enable_die_temp()
{
	int ret = 0;

	ret = writeRegister(MAX8614X_DIE_TEMP_CFG_REG, MAX8614X_DIE_TEMP_EN);
	if (ret < 0) {
		pr_err("SPI Communication error");
	}

	return ret;
}

int MAX8614X::read_die_temp()
{
	int ret = 0;
	uint8_t buf[2];

	ret = readRegister(MAX8614X_DIE_TEMP_INT_REG, buf, 2);
	if (ret < 0) {
		pr_err("Unable to read die_temp. ret: %d", ret);
		return ret;
	}

	die_temp.frac = (uint8_t)buf[1] & MAX8614X_DIE_TEMP_FRAC_MASK;
	die_temp.tint = (uint8_t)buf[0];

	pr_debug("Die temp: %d - %d, %d", die_temp.val, buf[0], buf[1]);
	return enable_die_temp();
}

int MAX8614X::reset()
{
	int ret = 0;

	ret = writeRegister(MAX8614X_SYSTEM_CTRL_REG,
			MAX8614X_SYSTEM_RESET_MASK);
	if (ret < 0) {
		pr_err("writeRegister failed. ret: %d", ret);
		return ret;
	}

	/* Reset ppg_queue */
	std::queue<ppg_data_t> empty_queue;
	std::swap(ppg_queue, empty_queue);

	led_control_reset(&led_ctrl);

	return ret;
}

int MAX8614X::poweroff()
{
	int ret = 0;

	ret = writeRegister(MAX8614X_SYSTEM_CTRL_REG,
			MAX8614X_SYSTEM_SHDN_MASK);
	if (ret < 0) {
		pr_err("writeRegister failed. ret: %d", ret);
		return ret;
	}

	return ret;
}

int MAX8614X::init()
{
	int ret = RTN_NO_ERROR;
	uint8_t part_id, rev_id;

	ret = get_part_info(&part_id, &rev_id);
	if (ret < 0) {
		pr_err("MAX8614X is not detected. Part_id: 0x%X, Rev_Id: 0x%X",
				part_id, rev_id);
		ret = RTN_ERR_NOT_8614x;
		goto fail;
	}
	pr_info("MAX8614X detected. Part_id: 0x%X, Rev_Id: 0x%X", part_id, rev_id);

	ret = writeRegister(MAX8614X_SYSTEM_CTRL_REG,
			MAX8614X_SYSTEM_RESET_MASK);
	if (ret < 0) {
		pr_err("writeRegister failed. ret: %d", ret);
		goto fail;
	}

	die_temp.frac = 0;
	die_temp.tint = 0;

	led_control_init(&led_ctrl); /*TODO: after porting agc, test */

	irq_triggered = false;
	m_ir.fall(Callback<void()>(this, &MAX8614X::irq));

	return ret;
fail:
	pr_err("Init failed. ret: %d", ret);
	return ret;
}

int MAX8614X::sensor_enable(int enable)
{
	int ret = RTN_NO_ERROR;
	uint8_t led_seq[3];

	RegisterMap_t ppg_init_cfg[] = {
		{ MAX8614X_SYSTEM_CTRL_REG, MAX8614X_SYSTEM_RESET_MASK},
		{ MAX8614X_PPG_CFG1_REG, MAX8614X_PPG_LED_PW_115_2_US_MASK // PPG_LED_PW = 3 (115.2us)
							   | MAX8614X_PPG1_ADC_RGE_32768_MASK // PPG1_ADC_RGE = 3(32768nA)
							   | MAX8614X_PPG2_ADC_RGE_32768_MASK }, // PPG2_ADC_RGE = 3(32768nA)
		{ MAX8614X_PPG_CFG2_REG, MAX8614X_PPG_SR_25_SPS},
		{ MAX8614X_LED_SEQ1_REG, 0x24},
		{ MAX8614X_LED_SEQ2_REG, 0x03},
		{ MAX8614X_LED1_PA_REG, 0x80},
		{ MAX8614X_LED2_PA_REG, 0x80},
		{ MAX8614X_LED3_PA_REG, 0x00},
		{ MAX8614X_LED_RANGE1_REG, MAX8614X_LED1_RGE_25mA_MASK
								 | MAX8614X_LED2_RGE_25mA_MASK
								 | MAX8614X_LED3_RGE_25mA_MASK},
		{ MAX8614X_FIFO_CFG1_REG, MAX8614X_INT1_EN_DATA_RDY_MASK},
								// 24 (ir+r+g) samples in fifo, size is 128/3=42
		{ MAX8614X_FIFO_CFG2_REG, MAX8614X_FLUSH_FIFO_MASK
								| MAX8614X_FIFO_STAT_CLR_MASK
								| MAX8614X_A_FULL_TYPE_MASK // Try 0
								| MAX8614X_FIFO_RO_MASK
								| MAX8614X_FIFO_EN_MASK},
		{ MAX8614X_INT_ENABLE1_REG, MAX8614X_INT1_EN_A_FULL_MASK
								  | MAX8614X_INT1_EN_DIE_TEMP_MASK
								  | MAX8614X_INT1_EN_VDD_OOR_MASK},
	};

	if (enable) {
		/* Read current sequence settings, and check what modes are open */
		led_seq[0] = MAX8614X_LED_SEQ1_REG;
		ret |= readRegister(MAX8614X_LED_SEQ1_REG, led_seq, 3);
		if (ret < 0) {
			pr_err("readRegister failed. ret: %d", ret);
			return ret;
		}

		pr_debug("0-Sequence registers: %X %X %X", led_seq[0], led_seq[1], led_seq[2]);
		ret |= writeBlock(ppg_init_cfg, ARRAY_SIZE(ppg_init_cfg));
		led_seq[0] = MAX8614X_INT_STATUS1_REG;
		ret |= readRegister(MAX8614X_INT_STATUS1_REG, led_seq, 3); // Mert: the len was 3 ?!
		pr_debug("1-Sequence registers: %X %X %X", led_seq[0], led_seq[1], led_seq[2]);
		if (ret < 0) {
			pr_err("readRegister failed. ret: %d", ret);
			return ret;
		}
		agc_enable(1);
		pr_debug("Wrote register settings. ret: %d", ret);
	} else {
		ret = reset();
		if (ret < 0) {
			pr_err("reset failed. ret: %d", ret);
			return ret;
		}

		ret = poweroff();
		if (ret < 0) {
			pr_err("poweroff failed. ret: %d", ret);
			return ret;
		}
	}

	return RTN_NO_ERROR;
}

int MAX8614X::agc_enable(int agc_enable)
{
	int ret = RTN_NO_ERROR;
	led_ctrl.agc_is_enabled = !!agc_enable;
	led_ctrl.lpm_is_enabled = !!agc_enable;
	ret = led_prox_init(&led_ctrl, led_ctrl.lpm_is_enabled);
	return ret;
}

int MAX8614X::dequeue_from_fifo_queue(uint32_t *ir, uint32_t *red, uint32_t *green)
{
	ppg_data_t ppg_data;

	if (irq_triggered) {
		irq_handler();
	}

	if (ppg_queue.size() <= 0) {
		return RTN_ERR_QUEUE_EMPTY;
	}
	ppg_data = ppg_queue.front();
	ppg_queue.pop();

	pr_debug("%lu  %lu  %lu", ppg_data.ir, ppg_data.red, ppg_data.green);

	*ir = ppg_data.ir;
	*red = ppg_data.red;
	*green = ppg_data.green;

	return RTN_NO_ERROR;
}
// total number of registers to print are 57
int MAX8614X::dump_registers(addr_val_pair *reg_vals)
{
	int i, j = 0;
    int ret = 0;
	uint8_t val;
	// read 23 registers
    for (i = 0x00; i <= 0x16; i++, j++) {
        reg_vals[j].addr = i;
        ret |= readRegister(i, &val, 1);
		reg_vals[j].val = val;
    }
    // read 24 registers
    for (i = 0x20; i <= 0x37; i++, j++) {
        reg_vals[j].addr = i;
        ret |= readRegister(i, &val, 1);
		reg_vals[j].val = val;
    }
    // read 3 registers
    for (i = 0x40; i <= 0x42; i++, j++) {
        reg_vals[j].addr = i;
        ret |= readRegister(i, &val, 1);
		reg_vals[j].val = val;
    }
    // read 5 registers
    for (i = 0xF0; i <= 0xF4; i++, j++) {
        reg_vals[j].addr = i;
        ret |= readRegister(i, &val, 1);
		reg_vals[j].val = val;
    }
    // read 2 registers
    for (i = 0xFE; i <= 0xFF; i++, j++) {
        reg_vals[j].addr = i;
        ret |= readRegister(i, &val, 1);
		reg_vals[j].val = val;
    }
	return ret;
}

const char * MAX8614X::get_sensor_part_name()
{
#if 1
	/* For sensor studio */
	return "max86141";
#else
	uint8_t part_id, rev_id;

	get_part_info(&part_id, &rev_id);

	switch (part_id) {
	case MAX86140_PART_ID_VAL:
		return "max86140";
	case MAX86141_PART_ID_VAL:
		return "max86141";
	case MAX86142_PART_ID_VAL:
		return "max86142";
	case MAX86143_PART_ID_VAL:
		return "max86143";
	}
	return "unknown";
#endif
}

const char * MAX8614X::get_sensor_name()
{
	return "ppg";
}

const char * MAX8614X::get_sensor_algo_ver()
{
	return "dummy_algo_ver";
}


//@brief Get status of the sensor: if PROX mode return true
unsigned char MAX8614X::is_sensor_in_daq_mode(){
	return (led_ctrl.state == LED_DATA_ACQ);
}

//@brief set agc range
void MAX8614X::set_agc_range(enum Max8614x_Agc_Range RangeLevel){

	if(RangeLevel == kRangeHigh)
		kMax8614xDefaultLedOutRange = MAX8614X_AGC_DEFAULT_LED_OUT_RANGE + MAX8614X_AGC_INCREMENT_TOP;
	else if(RangeLevel == kRangeMid)
		kMax8614xDefaultLedOutRange = MAX8614X_AGC_DEFAULT_LED_OUT_RANGE + MAX8614X_AGC_INCREMENT_MID;
	else
		kMax8614xDefaultLedOutRange= MAX8614X_AGC_DEFAULT_LED_OUT_RANGE;

//	printf("Set agc range as: %d\r\n", kMax8614xDefaultLedOutRange);
}


/**
* @brief	Get sensor ID.
*
* @returns	Sensor ID number.
*/
unsigned char MAX8614X::get_sensor_id() {

	return( SENSOR_ID_MAX30205 );

}
