#include "AFE4301V2.h"
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include "driver/spi_master.h"

#include "driver/ledc.h"
#include "driver/periph_ctrl.h"

#include <cstring>
#include "sdkconfig.h"
#include <iostream>

const unsigned char ADC_DATA_RESULT 		= 0x00;
const unsigned char ADC_CONTROL_REGISTER 	= 0x01;
const unsigned char MISC1_REGISTER 			= 0x02;
const unsigned char MISC2_REGISTER 			= 0x03;
const unsigned char DEVICE_CONTROL_1 		= 0x09;
const unsigned char ISW_MATRIX	 			= 0x0A;
const unsigned char VSW_MATRIX 				= 0x0B;
const unsigned char IQ_MODE_ENABLE 			= 0x0C;
const unsigned char WEIGHT_SCALE_CONTROL 	= 0x0D;
const unsigned char BCM_DAC_FREQ 			= 0x0E;
const unsigned char DEVICE_CONTROL_2 		= 0x0F;
const unsigned char ADC_CONTROL_REGISTER_2 	= 0x10;
const unsigned char MISC3_REGISTER 			= 0x1A;

#define SPI_TEST 0

namespace sts {
namespace driver {

	AFE4300V2::AFE4300V2(int reset, int rdy, int mosi, int miso, int sclk, int cs)
		: PIN_RESET(gpio_num_t(reset))
		, PIN_DRDY(gpio_num_t(rdy))
		, PIN_MOSI(gpio_num_t(mosi))
		, PIN_MISO(gpio_num_t(miso))
		, PIN_SCLK(gpio_num_t(sclk))
		, PIN_CS(gpio_num_t(cs))
		, gain_(1)
		, weightscale_(1)
		, weightoffset(1)
	{
		PIN_CLOCK = GPIO_NUM_4;

		gpio_set_direction(PIN_RESET,	GPIO_MODE_OUTPUT);
		gpio_set_direction(PIN_DRDY, 	GPIO_MODE_INPUT);
		gpio_set_direction(PIN_CS, 		GPIO_MODE_OUTPUT);

	//	gpio_set_direction(PIN_MISO, GPIO_MODE_INPUT);
	//	gpio_set_pull_mode(PIN_MISO, GPIO_PULLUP_PULLDOWN);

		//gpio_set_level(PIN_CS, 0);
		m_spi = new SPI(mosi, miso, sclk, cs, 1000*1000*1);

		gpio_set_level(PIN_CS, false);
		resetAFE4300();
		initClock();

		init();
		initWeightScale();
	}


	AFE4300V2::AFE4300V2(AFE4300Setting setting)
		: gain_(1), weightscale_(1), weightoffset(1)
	{
		PIN_RESET 	= gpio_num_t(setting.reset_pin);
		PIN_DRDY 	= gpio_num_t(setting.ready_pin);
		PIN_MOSI 	= gpio_num_t(setting.mosi_pin);
		PIN_MISO 	= gpio_num_t(setting.miso_pin);
		PIN_SCLK 	= gpio_num_t(setting.sclk_pin);
		PIN_CS 		= gpio_num_t(setting.cs_pin);
		PIN_CLOCK 	= gpio_num_t(setting.clock_pin);

		gpio_set_direction(PIN_DRDY, 	GPIO_MODE_INPUT);
		gpio_set_direction(PIN_RESET,	GPIO_MODE_OUTPUT);
		gpio_set_direction(PIN_CS, 		GPIO_MODE_OUTPUT);

		m_spi = new SPI(PIN_MOSI, PIN_MISO, PIN_SCLK, PIN_CS, setting.sclk_freq_hz);

		//gpio_set_level(PIN_CS, false);
		initClock();
		resetAFE4300();

		init();
		initWeightScale();
	}


	void AFE4300V2::resetAFE4300() const
	{
		gpio_set_level(PIN_RESET, 0);
		delayMicroseconds(100);
		gpio_set_level(PIN_RESET, 1);
	}


	void AFE4300V2::init() const
	{
		writeRegister(ADC_CONTROL_REGISTER,		0x4950);
		writeRegister(MISC1_REGISTER,			0x0000);
		writeRegister(MISC2_REGISTER,			0xFFFF);
		writeRegister(DEVICE_CONTROL_1,			0x0004); //?? 0x96 ~ 0110 0000
		writeRegister(ISW_MATRIX,				0x0000);
		writeRegister(VSW_MATRIX,				0x0000);
		writeRegister(IQ_MODE_ENABLE,			0x0000);
		writeRegister(WEIGHT_SCALE_CONTROL,		0x0000);
		writeRegister(BCM_DAC_FREQ,				0x00FA);
		writeRegister(DEVICE_CONTROL_2,			0x0000);
		writeRegister(ADC_CONTROL_REGISTER_2,	0x0011);
		writeRegister(MISC3_REGISTER,			0x00C0);
	}


	void AFE4300V2::initWeightScale() const
	{
		writeRegister(ADC_CONTROL_REGISTER,		0x4150); // ?? Differential measurement mode, 32 SPS
		writeRegister(DEVICE_CONTROL_1,			0x4001); //Power up weigh scale signal chain
		writeRegister(ADC_CONTROL_REGISTER_2,	0x0000); //ADC selects output of weigh scale
		writeRegister(WEIGHT_SCALE_CONTROL,		0x003F); //Gain = 1 DAC Offset = -1
		writeRegister(BCM_DAC_FREQ,				0x0001); //?? Frequency = default
		writeRegister(DEVICE_CONTROL_2,			0x1000); //Power up weigh scale signal chain
		writeRegister(IQ_MODE_ENABLE,			0x0000); //Disable IQ mode
		writeRegister(ISW_MATRIX,				0x0000); //Channels IOUTP1 and IOUTN0
		writeRegister(VSW_MATRIX,				0x0000); //Channels VSENSEP1 and VSENSEN0
	}


	/**
	* @brief  Initializes the BCM Module
	*/
	void AFE4300V2::initBCM() const
	{
		writeRegister(ADC_CONTROL_REGISTER,		0x4120); //Differential measurement mode, 32 SPS
		writeRegister(DEVICE_CONTROL_1,			0x0006); //Power up BCM signal chain
		writeRegister(ISW_MATRIX,				0x0804); //Channels IOUTP1 and IOUTN0
		writeRegister(VSW_MATRIX,				0x0804); //Channels VSENSEP1 and VSENSEN0
		writeRegister(ADC_CONTROL_REGISTER_2,	0x0063); //ADC selects output of BCM-I output
		writeRegister(WEIGHT_SCALE_CONTROL,		0x0000); //Gain = 1 DAC Offset = 0
	}


	int AFE4300V2::read() const
	{
		return readRegister(ADC_DATA_RESULT);
	}


	void AFE4300V2::writeRegister(unsigned char address, unsigned int data) const
	{
		unsigned char firstByte = (unsigned char)(data >> 8);
		unsigned char secondByte = (unsigned char)data;

	#if SPI_TEST == 0
		uint8_t buffer[3] = {address, firstByte, secondByte};
		m_spi->transfer(buffer, 3);
	#endif

	#if SPI_TEST == 1
		m_spi->transferByte(address, 	Transfer::CONTINUE);
		m_spi->transferByte(firstByte, 	Transfer::CONTINUE);
		m_spi->transferByte(secondByte, Transfer::LAST);
	#endif
	}


	int AFE4300V2::readRegister(unsigned char address) const
	{
		int spiReceive = 0;
		unsigned char spiReceiveFirst = 0;
		unsigned char spiReceiveSecond = 0;

		address = address & 0x1F;
		address = address | 0x20;

	#if SPI_TEST == 0
		uint8_t buffer[3] = {address, 0x00, 0x00};
		m_spi->transfer(buffer, 3);
		spiReceiveFirst = buffer[1];
		spiReceiveSecond = buffer[2];
	#endif

	#if SPI_TEST == 1
		m_spi->transferByte(address, Transfer::CONTINUE);
		spiReceiveFirst  = m_spi->transferByte(0x00, Transfer::CONTINUE);
		spiReceiveSecond = m_spi->transferByte(0x00, Transfer::LAST);
	#endif

		spiReceive = (spiReceiveFirst << 8);
		spiReceive |= spiReceiveSecond;

		return spiReceive;
	}


	void AFE4300V2::initClock() const
	{
		periph_module_enable(PERIPH_LEDC_MODULE);

		int bit_width = 1; 			// 1 - 20 bits
		int divider = 10240;  		// Q10.8 fixed point number, 0x100 — 0x3FFFF
		int duty_cycle = 1 << (bit_width - 1);

		float freq_hz = ((uint64_t) LEDC_APB_CLK_HZ << 8) / (float) divider / (1 << bit_width);
		printf("frequency: %f Hz\n", freq_hz);

		ledc_timer_set(LEDC_HIGH_SPEED_MODE, LEDC_TIMER_0, divider, bit_width, LEDC_APB_CLK);
		ledc_timer_rst(LEDC_HIGH_SPEED_MODE, LEDC_TIMER_0);
		ledc_timer_resume(LEDC_HIGH_SPEED_MODE, LEDC_TIMER_0);

		ledc_channel_config_t channel_config = {0};
		channel_config.channel    = LEDC_CHANNEL_0;
		channel_config.duty       = duty_cycle;
		channel_config.gpio_num   = PIN_CLOCK;
		channel_config.speed_mode = LEDC_HIGH_SPEED_MODE;
		channel_config.timer_sel  = LEDC_TIMER_0;

		ledc_channel_config(&channel_config);
	}


	void AFE4300V2::setGain(uint8_t gain)
	{
		gain_ = gain;
	}


	int AFE4300V2::getWeightScale() const
	{
		return weightscale_;
	}


	void AFE4300V2::setWeightScale(int scale)
	{
		weightscale_ = scale;
	}


	int AFE4300V2::getWeightOffset() const
	{
		return weightoffset;
	}


	void AFE4300V2::setWeightOffset(int offset)
	{
		weightoffset = offset;
	}


	void AFE4300V2::tare()
	{

	}


	uint32_t AFE4300V2::micros() const
	{
		uint32_t ccount;
		asm volatile ( "rsr %0, ccount" : "=a" (ccount) );
		return ccount;
	}


	void AFE4300V2::delayMicroseconds(uint32_t us) const
	{
		if(us) {
			uint32_t m = micros();
			while( (micros() - m ) < us ){
				__asm__ __volatile__("nop;");
			}
		}
	}

} // end namespace driver
} // end namespace sts