/*******************************************************************************
* 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 <errno.h>
#include "Peripherals.h"

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

#define ILLEGAL_OUTPUT_POINTER				1
#define ILLEGAL_DIODE_OUTPUT_MIN_MAX_PAIR	2
#define ILLEGAL_LED_SETTING_MIN_MAX_PAIR	3
#define CONSTRAINT_VIOLATION				4

#define MAX8614X_LED_DRIVE_CURRENT_FULL_SCALE		\
		(MAX8614X_MAX_LED_DRIVE_CURRENT - MAX8614X_MIN_LED_DRIVE_CURRENT)

// that parameter is moved to header file to enable modification
//#define MAX8614X_AGC_DEFAULT_LED_OUT_RANGE			30
#define MAX8614X_AGC_DEFAULT_CORRECTION_COEFF		50
#define MAX8614X_AGC_DEFAULT_SENSITIVITY_PERCENT	10
#define MAX8614X_AGC_DEFAULT_NUM_SAMPLES_TO_AVG		25

#define MAX8614X_PROX_THRESHOLD_1			10000
#define MAX8614X_PROX_THRESHOLD_2			40000
#define MAX8614X_PROX_DEBOUNCE_SPS			2
#define MAX8614X_DAQ_DEBOUNCE_SPS			20

#define MAX8614X_DEFAULT_DAQ_LED_CURRENT_1	40000
#define MAX8614X_DEFAULT_DAQ_LED_CURRENT_2	40000
#define MAX8614X_DEFAULT_DAQ_LED_CURRENT_3	0
#define MAX8614X_DEFAULT_PROX_LED_CURRENT_1	10000
#define MAX8614X_DEFAULT_PROX_LED_CURRENT_2	0
#define MAX8614X_DEFAULT_PROX_LED_CURRENT_3	0


#define MAX8614X_MIN_LED_DRIVE_CURRENT		0
#define MAX8614X_MAX_LED_DRIVE_CURRENT		60000

#define MAX8614X_MAX_PPG_DIODE_VAL			((1 << 19) - 1)
#define MAX8614X_MIN_PPG_DIODE_VAL			0

#define MAX8614X_DEFAULT_CURRENT1			0x30
#define MAX8614X_DEFAULT_CURRENT2			0
#define MAX8614X_DEFAULT_CURRENT3			0


int MAX8614X::max8614x_update_led_range(
		int new_range, uint8_t led_num,
		union led_range *led_range_settings)
{
	int old_range;
	Registers reg_addr;

	switch (led_num) {
	case LED_1:
		old_range = led_range_settings->led1;
		led_range_settings->led1 = new_range;
		reg_addr = MAX8614X_LED_RANGE1_REG;
		break;
	case LED_2:
		old_range = led_range_settings->led2;
		led_range_settings->led2 = new_range;
		reg_addr = MAX8614X_LED_RANGE1_REG;
		break;
	case LED_3:
		old_range = led_range_settings->led3;
		led_range_settings->led3 = new_range;
		reg_addr = MAX8614X_LED_RANGE1_REG;
		break;
	case LED_4:
		old_range = led_range_settings->led4;
		led_range_settings->led4 = new_range;
		reg_addr = MAX8614X_LED_RANGE2_REG;
		break;
	case LED_5:
		old_range = led_range_settings->led5;
		led_range_settings->led5 = new_range;
		reg_addr = MAX8614X_LED_RANGE2_REG;
		break;
	case LED_6:
		old_range = led_range_settings->led6;
		led_range_settings->led6 = new_range;
		reg_addr = MAX8614X_LED_RANGE2_REG;
		break;

	default:
		return -EINVAL;
	}

	if (old_range == new_range)
		return 0;

	return writeRegister( reg_addr,
			led_range_settings->val[led_num < LED_4 ? 0 : 1]);
}

int MAX8614X::max8614x_update_led_current(
		union led_range *led_range_settings,
		int led_new_val,
		max8614x_led_t led_num)
{
	int ret = 0;
	Registers led_current_reg_addr;
	int led_range;
	uint8_t led_current_reg_val;
	int	led_range_index = led_new_val / 25000;
	const int led_range_steps[] = {
		LED_RANGE_STEP_25uA,
		LED_RANGE_STEP_50uA,
		LED_RANGE_STEP_75uA,
		LED_RANGE_STEP_100uA,
		LED_RANGE_STEP_100uA, /* For led current greater than 100uA */
	};

	switch(led_num) {
	case LED_1:
		led_current_reg_addr = MAX8614X_LED1_PA_REG;
		break;
	case LED_2:
		led_current_reg_addr = MAX8614X_LED2_PA_REG;
		break;
	case LED_3:
		led_current_reg_addr = MAX8614X_LED3_PA_REG;
		break;
	case LED_4:
		led_current_reg_addr = MAX8614X_LED4_PA_REG;
		break;
	case LED_5:
		led_current_reg_addr = MAX8614X_LED5_PA_REG;
		break;
	case LED_6:
		led_current_reg_addr = MAX8614X_LED6_PA_REG;
		break;
	default:
		pr_err("Invalid led number: %d\n", led_num);
		return -EINVAL;
	}

	if (led_new_val < MAX8614X_MIN_LED_DRIVE_CURRENT
				|| led_new_val > MAX8614X_MAX_LED_DRIVE_CURRENT) {
		pr_err("Invalid led value: %d\n", led_new_val);
		return -EINVAL;
	}

	led_current_reg_val = led_new_val / led_range_steps[led_range_index];

	pr_debug("Updating LED%d current to %d. led_rge_idx: %d, reg_val: %.2X",
			led_num, led_new_val, led_range_index, led_current_reg_val);

	ret = writeRegister(led_current_reg_addr, led_current_reg_val);
	if (ret < 0)
		return ret;


	led_range = led_range_index;
	pr_debug("Updating LED%d range to %d.", led_num, led_range);
	if (led_range > 3)
		led_range = 3;
	ret = max8614x_update_led_range( led_range, led_num, led_range_settings);
	if (ret < 0)
		return ret;
	return ret;
}

int32_t agc_adj_calculator(
		int32_t *change_by_percent_of_range,
		int32_t *change_by_percent_of_current_setting,
		int32_t *change_led_by_absolute_count,
		int32_t *set_led_to_absolute_count,
		int32_t target_percent_of_range,
		int32_t correction_coefficient,
		int32_t allowed_error_in_percentage,
		int32_t current_average,
		int32_t number_of_samples_averaged,
		int32_t led_drive_current_value)
{
	int32_t current_percent_of_range = 0;
	int32_t delta = 0;
	int32_t desired_delta = 0;
	int32_t current_power_percent = 0;

	if (change_by_percent_of_range == 0
			|| change_by_percent_of_current_setting == 0
			|| change_led_by_absolute_count == 0
			|| set_led_to_absolute_count == 0)
		return ILLEGAL_OUTPUT_POINTER;

	if (target_percent_of_range > 90 || target_percent_of_range < 10)
		return CONSTRAINT_VIOLATION;

	if (correction_coefficient > 100 || correction_coefficient < 0)
		return CONSTRAINT_VIOLATION;

	if (allowed_error_in_percentage > 100
			|| allowed_error_in_percentage < 0)
		return CONSTRAINT_VIOLATION;

#if ((MAX8614X_MAX_PPG_DIODE_VAL - MAX8614X_MIN_PPG_DIODE_VAL) <= 0 \
			 || (MAX8614X_MAX_PPG_DIODE_VAL < 0) || (MAX8614X_MIN_PPG_DIODE_VAL < 0))
	#error "Illegal diode Min/Max Pair"
#endif

#if ((MAX8614X_MAX_LED_DRIVE_CURRENT - MAX8614X_MIN_LED_DRIVE_CURRENT) <= 0 \
		|| (MAX8614X_MAX_LED_DRIVE_CURRENT < 0) || (MAX8614X_MIN_LED_DRIVE_CURRENT < 0))
	#error "Illegal LED Min/Max current Pair"
#endif

	if (led_drive_current_value > MAX8614X_MAX_LED_DRIVE_CURRENT
			|| led_drive_current_value < MAX8614X_MIN_LED_DRIVE_CURRENT)
		return CONSTRAINT_VIOLATION;

	if (current_average < MAX8614X_MIN_PPG_DIODE_VAL
			|| current_average > MAX8614X_MAX_PPG_DIODE_VAL)
		return CONSTRAINT_VIOLATION;

	current_percent_of_range = 100 *
		(current_average - MAX8614X_MIN_PPG_DIODE_VAL) /
		(MAX8614X_MAX_PPG_DIODE_VAL - MAX8614X_MIN_PPG_DIODE_VAL) ;

	delta = current_percent_of_range - target_percent_of_range;
	delta = delta * correction_coefficient / 100;

	if (delta > -allowed_error_in_percentage
			&& delta < allowed_error_in_percentage) {
		*change_by_percent_of_range = 0;
		*change_by_percent_of_current_setting = 0;
		*change_led_by_absolute_count = 0;
		*set_led_to_absolute_count = led_drive_current_value;
		return 0;
	}

	current_power_percent = 100 *
			(led_drive_current_value - MAX8614X_MIN_LED_DRIVE_CURRENT) /
			(MAX8614X_MAX_LED_DRIVE_CURRENT - MAX8614X_MIN_LED_DRIVE_CURRENT);
	if (delta < 0)
		desired_delta = -delta * (100 - current_power_percent) /
				(100 - current_percent_of_range);

	if (delta > 0)
		desired_delta = -delta * (current_power_percent)
				/ (current_percent_of_range);

	*change_by_percent_of_range = desired_delta;

	*change_led_by_absolute_count =	(desired_delta
			* MAX8614X_LED_DRIVE_CURRENT_FULL_SCALE / 100);
	*change_by_percent_of_current_setting =
			(*change_led_by_absolute_count * 100)
			/ (led_drive_current_value);
	*set_led_to_absolute_count  = led_drive_current_value
			+ *change_led_by_absolute_count;

	//If we are saturated, cut power in half
	if (current_percent_of_range >= 100)
	{
		*change_by_percent_of_range = -100; //Unknown, set fake value
		*change_by_percent_of_current_setting = -50;
		*change_led_by_absolute_count = 0 - (led_drive_current_value / 2);
		*set_led_to_absolute_count  = led_drive_current_value / 2;
	}

	return 0;
}

void MAX8614X::ppg_auto_gain_ctrl(
		struct led_control *led_ctrl,
		uint32_t sample_cnt, int diode_data, max8614x_led_t led_num)
{
	int ret;
	int diode_avg;

	if (led_num > LED_3) /* TODO: why3? */
		return;

	led_ctrl->diode_sum[led_num] += diode_data;
	if (sample_cnt % led_ctrl->agc_min_num_samples == 0) {
		diode_avg = led_ctrl->diode_sum[led_num]
				/ led_ctrl->agc_min_num_samples;
		led_ctrl->diode_sum[led_num] = 0;
	} else
		return;

	ret = agc_adj_calculator(
		&led_ctrl->change_by_percent_of_range[led_num],
		&led_ctrl->change_by_percent_of_current_setting[led_num],
		&led_ctrl->change_led_by_absolute_count[led_num],
		&led_ctrl->led_current[led_num],
		led_ctrl->agc_led_out_percent,
		led_ctrl->agc_corr_coeff,
		led_ctrl->agc_sensitivity_percent,
		diode_avg,
		led_ctrl->agc_min_num_samples,
		led_ctrl->led_current[led_num]);
	if (ret)
		return;

	if (led_ctrl->change_led_by_absolute_count[led_num] == 0)
		return;

	ret = max8614x_update_led_current(&led_ctrl->led_range_settings,
			led_ctrl->led_current[led_num], led_num);
	if (ret < 0)
		pr_err("%s failed", __func__);
	return;
}

void MAX8614X::max8614x_agc_handler(struct led_control *led_ctrl,
		int *samples)
{
	static int ret = -1;

	if (!led_ctrl->agc_is_enabled)
		return;

	led_ctrl->sample_cnt++;
	ret = led_control_sm(led_ctrl,
		samples[DATA_TYPE_PPG1_LEDC1],
		led_ctrl->lpm_is_enabled);

	if (ret == LED_DATA_ACQ) {
		ppg_auto_gain_ctrl(led_ctrl,
				led_ctrl->sample_cnt,
				samples[DATA_TYPE_PPG1_LEDC1],
				LED_1);
		ppg_auto_gain_ctrl(led_ctrl,
				led_ctrl->sample_cnt,
				samples[DATA_TYPE_PPG1_LEDC1],
				LED_2);
//		ppg_auto_gain_ctrl(led_ctrl,
//				led_ctrl->sample_cnt,
//				samples[DATA_TYPE_PPG1_LEDC3],
//				LED_3);
	}

	return;
}

int MAX8614X::led_prox_init(struct led_control *led_ctrl, char lpm)
{
	int ret;
	const RegisterMap low_pm_settings[] = {
		{ 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_INT_ENABLE1_REG, MAX8614X_INT1_EN_DATA_RDY_MASK },
	};

	led_ctrl->led_current[LED_1] = MAX8614X_DEFAULT_PROX_LED_CURRENT_1;
	ret = max8614x_update_led_current(&led_ctrl->led_range_settings,
			led_ctrl->led_current[LED_1], LED_1);

	led_ctrl->led_current[LED_2] = MAX8614X_DEFAULT_PROX_LED_CURRENT_2;
	ret |= max8614x_update_led_current(&led_ctrl->led_range_settings,
			led_ctrl->led_current[LED_2], LED_2);

	led_ctrl->led_current[LED_3] = MAX8614X_DEFAULT_PROX_LED_CURRENT_3;
	ret |= max8614x_update_led_current(&led_ctrl->led_range_settings,
			led_ctrl->led_current[LED_3], LED_3);

	if (lpm)
		ret |= writeBlock(low_pm_settings,
				ARRAY_SIZE(low_pm_settings));
	return ret;
}

int MAX8614X::led_daq_init(struct led_control *led_ctrl, char lpm)
{
	int ret;
	const RegisterMap non_lpm_settings[] = {
		{ 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_INT_ENABLE1_REG, MAX8614X_INT1_EN_DATA_RDY_MASK },
	};

	led_ctrl->led_current[LED_1] = MAX8614X_DEFAULT_DAQ_LED_CURRENT_1;
	ret = max8614x_update_led_current(&led_ctrl->led_range_settings,
			led_ctrl->led_current[LED_1], LED_1);

	led_ctrl->led_current[LED_2] = MAX8614X_DEFAULT_DAQ_LED_CURRENT_2;
	ret |= max8614x_update_led_current(&led_ctrl->led_range_settings,
			led_ctrl->led_current[LED_2], LED_2);

	led_ctrl->led_current[LED_3] = MAX8614X_DEFAULT_DAQ_LED_CURRENT_3;
	ret |= max8614x_update_led_current(&led_ctrl->led_range_settings,
			led_ctrl->led_current[LED_3], LED_3);

	if (lpm)
		ret |= writeBlock(non_lpm_settings,
				ARRAY_SIZE(non_lpm_settings));

	return ret;
}

int MAX8614X::led_control_sm(struct led_control *led_ctrl, int diode_data, char lpm)
{
	int ret = led_ctrl->state;
	int avg = 0;

	led_ctrl->prox_sample_cnt++;
	led_ctrl->prox_sum += diode_data;

	switch (led_ctrl->state) {
	case LED_PROX:
		if (led_ctrl->prox_sample_cnt % MAX8614X_PROX_DEBOUNCE_SPS != 0)
			break;

		avg = led_ctrl->prox_sum / MAX8614X_PROX_DEBOUNCE_SPS;
		if (avg >= MAX8614X_PROX_THRESHOLD_1) {
			led_ctrl->state = LED_DATA_ACQ;
			ret = led_daq_init(led_ctrl, lpm);
			led_ctrl->prox_sample_cnt = 0;
		}
		led_ctrl->prox_sum = 0;
		break;

	case LED_DATA_ACQ:
		if (led_ctrl->prox_sample_cnt % MAX8614X_DAQ_DEBOUNCE_SPS != 0)
			break;

		avg = led_ctrl->prox_sum / MAX8614X_DAQ_DEBOUNCE_SPS;
		if (avg <= MAX8614X_PROX_THRESHOLD_2) {
			led_ctrl->state = LED_PROX;
			ret = led_prox_init(led_ctrl, lpm);
			led_ctrl->prox_sample_cnt = 0;
		}
		led_ctrl->prox_sum = 0;
		break;

	default:
		led_ctrl->state = LED_PROX;
		led_ctrl->prox_sum = 0;
		led_ctrl->prox_sample_cnt = 0;
		return -EINVAL;
	}

	return ret;
}

void MAX8614X::led_control_reset(struct led_control *led_ctrl)
{
	led_ctrl->led_current[LED_1] = led_ctrl->default_current[LED_1];
	led_ctrl->led_current[LED_2] = led_ctrl->default_current[LED_2];
	led_ctrl->led_current[LED_3] = led_ctrl->default_current[LED_3];

	memset(led_ctrl->change_by_percent_of_range, 0,
			sizeof(led_ctrl->change_by_percent_of_range));
	memset(led_ctrl->change_by_percent_of_current_setting, 0,
			sizeof(led_ctrl->change_by_percent_of_range));
	memset(led_ctrl->change_led_by_absolute_count, 0,
			sizeof(led_ctrl->change_by_percent_of_range));
	memset(led_ctrl->diode_sum, 0, sizeof(led_ctrl->diode_sum));

	led_ctrl->agc_is_enabled = 1;
	led_ctrl->prox_sum = 0;
	led_ctrl->prox_sample_cnt = 0;
	led_ctrl->sample_cnt = 0;
	led_ctrl->state = LED_PROX;
}

void MAX8614X::led_control_init(struct led_control *led_ctrl)
{
	memset(led_ctrl, 0, sizeof(struct led_control));

	led_ctrl->default_current[LED_1] = MAX8614X_DEFAULT_CURRENT1;
	led_ctrl->default_current[LED_2] = MAX8614X_DEFAULT_CURRENT2;
	led_ctrl->default_current[LED_3] = MAX8614X_DEFAULT_CURRENT3;
	led_ctrl->agc_led_out_percent = kMax8614xDefaultLedOutRange;
	led_ctrl->agc_corr_coeff = MAX8614X_AGC_DEFAULT_CORRECTION_COEFF;
	led_ctrl->agc_min_num_samples = MAX8614X_AGC_DEFAULT_NUM_SAMPLES_TO_AVG;
	led_ctrl->agc_sensitivity_percent = MAX8614X_AGC_DEFAULT_SENSITIVITY_PERCENT;
}
