#include "MAX30003.h"

#define WREG 0x00
#define RREG 0x01

extern SPI_HandleTypeDef hspi2;
max30003_str max30003_struct;
unsigned int heartRate;
unsigned int RRinterval;
signed long ecgdata;
uint8_t ecg_flag,Ptag,Etag;


void max30003_ready(void)
{
	max30003_struct.ready = true;
}


void max30003_Init_hard(void)
{
	__HAL_RCC_GPIOA_CLK_ENABLE();
	__HAL_RCC_GPIOB_CLK_ENABLE();
	__HAL_RCC_GPIOC_CLK_ENABLE();
	GPIO_InitTypeDef GPIO_InitStructure = { 0 };
    //Пин питания
    GPIO_InitStructure.Pin 			= MAX_POWER_PIN;
    GPIO_InitStructure.Mode 		= GPIO_MODE_OUTPUT_PP;
    GPIO_InitStructure.Pull 		= GPIO_NOPULL;
    GPIO_InitStructure.Speed 		= GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(MAX_POWER_GPIO_PORT, &GPIO_InitStructure);
    //HAL_GPIO_WritePin(ECG_POWER_GPIO_PORT, ECG_POWER_PIN, GPIO_PIN_SET);

    //Configure GPIO pin IN
    GPIO_InitStructure.Pin = MAX_IRQ_PIN;
    GPIO_InitStructure.Mode = GPIO_MODE_INPUT;
    GPIO_InitStructure.Pull = GPIO_NOPULL;
    HAL_GPIO_Init(MAX_IRQ_GPIO_PORT, &GPIO_InitStructure);
}


//концепция такая на вход
void max30003_Control(max30003_str* MAX)
{
	if ((MAX->mode_set == true) && ((MAX->mode_now == false) || MAX->error != HAL_OK))
	{
		//предварительно всё отключили
		__HAL_RCC_SPI2_CLK_DISABLE();
		HAL_GPIO_DeInit(MAX_CS_GPIO_PORT, 			MAX_CS_PIN);
		HAL_GPIO_DeInit(MAX_SPI_SCK_GPIO_PORT,  	MAX_SPI_SCK_PIN);
		HAL_GPIO_DeInit(MAX_SPI_MISO_GPIO_PORT, 	MAX_SPI_MISO_PIN);
		HAL_GPIO_DeInit(MAX_SPI_MOSI_GPIO_PORT, 	MAX_SPI_MOSI_PIN);
		HAL_GPIO_WritePin(MAX_POWER_GPIO_PORT, 		MAX_POWER_PIN, GPIO_PIN_SET); //отключили питание
		if (HAL_SPI_DeInit(&hspi2) != HAL_OK)
		{
			Error_Handler();
		}
		osDelay(100);
		//процесс включения запущен
		GPIO_InitTypeDef GPIO_InitStructure = { 0 };
			/* SPI2 clock enable */
		__HAL_RCC_SPI2_CLK_ENABLE(); //Зависит от того какой SPI используется
		//На всякий включаем все порты
		__HAL_RCC_GPIOA_CLK_ENABLE();
		__HAL_RCC_GPIOB_CLK_ENABLE();
		__HAL_RCC_GPIOC_CLK_ENABLE();

	    //SPI main pin config
	    GPIO_InitStructure.Pin 			= MAX_SPI_SCK_PIN;
	    GPIO_InitStructure.Mode 		= GPIO_MODE_AF_PP;
	    GPIO_InitStructure.Pull 		= GPIO_PULLUP;
	    GPIO_InitStructure.Speed 		= GPIO_SPEED_FREQ_VERY_HIGH;
	    GPIO_InitStructure.Alternate 	= GPIO_AF5_SPI2;
	    HAL_GPIO_Init(MAX_SPI_SCK_GPIO_PORT, &GPIO_InitStructure);

		GPIO_InitStructure.Pin 			= MAX_SPI_MISO_PIN;
	    GPIO_InitStructure.Mode 		= GPIO_MODE_AF_PP;
	    GPIO_InitStructure.Pull 		= GPIO_PULLUP;
	    GPIO_InitStructure.Speed 		= GPIO_SPEED_FREQ_VERY_HIGH;
	    GPIO_InitStructure.Alternate 	= GPIO_AF5_SPI2;
	    HAL_GPIO_Init(MAX_SPI_MISO_GPIO_PORT, &GPIO_InitStructure);

		GPIO_InitStructure.Pin 			= MAX_SPI_MOSI_PIN;
	    GPIO_InitStructure.Mode 		= GPIO_MODE_AF_PP;
	    GPIO_InitStructure.Pull 		= GPIO_PULLUP;
	    GPIO_InitStructure.Speed 		= GPIO_SPEED_FREQ_VERY_HIGH;
	    GPIO_InitStructure.Alternate 	= GPIO_AF5_SPI2;
	    HAL_GPIO_Init(MAX_SPI_MOSI_GPIO_PORT, &GPIO_InitStructure);

	    //Configure GPIO pins
	    GPIO_InitStructure.Pin 			= MAX_CS_PIN;
	    GPIO_InitStructure.Mode 		= GPIO_MODE_OUTPUT_PP;
	    GPIO_InitStructure.Pull 		= GPIO_NOPULL;
	    GPIO_InitStructure.Speed 		= GPIO_SPEED_FREQ_LOW;
	    HAL_GPIO_Init(MAX_CS_GPIO_PORT, &GPIO_InitStructure);

	    HAL_GPIO_WritePin(MAX_POWER_GPIO_PORT, MAX_POWER_PIN, GPIO_PIN_SET);//отключили питание
		osDelay(1000);
	    HAL_GPIO_WritePin(MAX_POWER_GPIO_PORT, MAX_POWER_PIN, GPIO_PIN_RESET);//включили питание

		hspi2.Instance = MAX_SPIx;
		hspi2.Init.Mode = SPI_MODE_MASTER;
		hspi2.Init.Direction = SPI_DIRECTION_2LINES;
		hspi2.Init.DataSize = SPI_DATASIZE_8BIT;
		hspi2.Init.CLKPolarity = SPI_POLARITY_LOW;
		hspi2.Init.CLKPhase = SPI_PHASE_1EDGE;
		hspi2.Init.NSS = SPI_NSS_SOFT;
		hspi2.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8;
		hspi2.Init.FirstBit = SPI_FIRSTBIT_MSB;
		hspi2.Init.TIMode = SPI_TIMODE_DISABLE;
		hspi2.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
		hspi2.Init.CRCPolynomial = 7;
		if (HAL_SPI_Init(&hspi2) != HAL_OK)
		{
			Error_Handler();
		}

		osDelay(100);
		max30003Begin();
		MAX->mode_now = true;
	}
	else if((MAX->mode_set == false) && (MAX->mode_now == true))
	{
		/* Peripheral clock disable */
		__HAL_RCC_SPI2_CLK_DISABLE();
		HAL_GPIO_DeInit(MAX_CS_GPIO_PORT, 			MAX_CS_PIN);
		HAL_GPIO_DeInit(MAX_SPI_SCK_GPIO_PORT,  	MAX_SPI_SCK_PIN);
		HAL_GPIO_DeInit(MAX_SPI_MISO_GPIO_PORT, 	MAX_SPI_MISO_PIN);
		HAL_GPIO_DeInit(MAX_SPI_MOSI_GPIO_PORT, 	MAX_SPI_MOSI_PIN);
		HAL_GPIO_WritePin(MAX_POWER_GPIO_PORT, 		MAX_POWER_PIN, GPIO_PIN_SET); //отключили питание
		if (HAL_SPI_DeInit(&hspi2) != HAL_OK)
		{
			Error_Handler();
		}
		MAX->mode_now = false;
	}
	else if(MAX->mode_set == false)
	{
		HAL_GPIO_WritePin(MAX_POWER_GPIO_PORT, MAX_POWER_PIN, GPIO_PIN_SET); //отключили питание
	}
}






void SPI_Send_byteMAX(uint8_t data)
{
	HAL_SPI_Transmit(&hspi2, &data, 1, 1);
}

uint8_t SPI_Receive_byteMAX(uint8_t data)
{
	delay_us(1);
	uint8_t data_out = 0;
	HAL_SPI_Receive(&hspi2,&data_out, 1, 1);
	return data_out;
}






void max30003RegWrite (uint8_t WRITE_ADDRESS, unsigned long data)
{
    // now combine the register address and the command into one byte:
    //uint8_t dataToSend = (WRITE_ADDRESS<<1) | WREG;
	uint8_t dataToSend = (WRITE_ADDRESS<<1);
     // take the chip select low to select the device:
    MAX_CS(0);

    delay_ms(1);
    SPI_Send_byteMAX((dataToSend));
    SPI_Send_byteMAX((data>>16) & 0xFF);
    SPI_Send_byteMAX((data>>8) & 0xFF);
    SPI_Send_byteMAX((data) & 0xFF);
    //delay_ms(2);
    // take the chip select high to de-select:
    MAX_CS(1);
}


void max30003SwReset(void)
{
    max30003RegWrite(SW_RST,0x000000);
    delay_ms(100);
}

void max30003Synch(void)
{
    max30003RegWrite(SYNCH,0x000000);
}

void max30003RegRead(uint8_t Reg_address, uint8_t * buff)
{
    uint8_t spiTxBuff;

    MAX_CS(0);

    spiTxBuff = (Reg_address<<1 ) | RREG;
    SPI_Send_byteMAX(spiTxBuff); //Send register location
    for ( int i = 0; i < 3; i++)
    {
       buff[i] = SPI_Receive_byteMAX(0xFF);
    }
    MAX_CS(1);
}

uint32_t max30003RegRead_hex(uint8_t Reg_address)
{
    uint8_t spiTxBuff;
    uint32_t data = 0;
    MAX_CS(0);

    spiTxBuff = (Reg_address<<1 ) | RREG;
    delay_us(1);
    SPI_Send_byteMAX(spiTxBuff); //Send register location
   	data |= ((uint32_t) SPI_Receive_byteMAX( 0xFF) << 16); // read top 8 bits data
   	data |= ((uint32_t) SPI_Receive_byteMAX( 0xFF) << 8); // read middle 8 bits  data
   	data |=  (uint32_t) SPI_Receive_byteMAX( 0xFF); // read bottom 8 bits data
    MAX_CS(1);
    return data;
}

bool max30003ReadInfo(void)
{
    uint8_t spiTxBuff;
    uint8_t readBuff[4] ;

    MAX_CS(0);

    spiTxBuff = (INFO << 1 ) | RREG;
    SPI_Send_byteMAX(spiTxBuff); //Send register location

    for ( int i = 0; i < 3; i++)
    {
       readBuff[i] = SPI_Receive_byteMAX(0xff);
    }

    MAX_CS(1);

    if((readBuff[0]&0xf0) == 0x50 ){

    	//print_USB_DEBUG("max30003 is ready");
    	//print_USB_DEBUG("Rev ID:  %x",(readBuff[0]&0xf0));
    	//print_usart(USART2,(readBuff[0]&0xf0));

      return true;
    }else{

    	//print_USB_DEBUG("max30003 read info error\n");
    	//print_USB_DEBUG("Rev ID:   %x %x %x %x\n",readBuff[3],readBuff[2],readBuff[1],readBuff[0]);
      return false;
    }

    return false;
}

void max30003ReadData(int num_samples, uint8_t * readBuffer)
{
    uint8_t spiTxBuff;
    MAX_CS(0);

    spiTxBuff = (ECG_FIFO_BURST<<1 ) | RREG;
    SPI_Send_byteMAX(spiTxBuff); //Send register location

    for ( int i = 0; i < num_samples*3; ++i)
    {
      readBuffer[i] = SPI_Receive_byteMAX(0x00);
    }

    MAX_CS(1);
}

void max30003Begin()
{
    max30003SwReset();
    osDelay(10);
    //при опорной частоте 32кГц как у нас возможно 2 частоты съема данных 200Гц и 500Гц
    //max30003RegWrite(CNFG_GEN, 0x280007);//32кГц опорная частота 200Гц ЭКГ | (d3-d2)Rbias 50MR b00 |(d1-d0)ECGN ECGP b11 присоединены к VMID через резистор
    max30003RegWrite(CNFG_GEN, 0x281513);//32кГц опорная частота 200Гц ЭКГ | (d3-d2)Rbias 50MR b00 |(d1-d0)ECGN ECGP b11 присоединены к VMID через резистор//fixme поменял всё ок
    osDelay(10);
    //Настройка калибровочных сигналов по идеи они не нужны
    max30003RegWrite(CNFG_CAL, 0x000000);// 0x700000
    osDelay(10);
    //Подключение калибровочных сигналов к входам |(d23) полярность сигнала ЭКГ |(d21-d20) Отключить от входов ацп
    max30003RegWrite(CNFG_EMUX,0x000000);//всё калибровочные сигналы отключены

    osDelay(10);
    //Настройка ЭКГ. При частоте 32000 возможны несколько вариантов:
    //-если мы в регистре FMSTR (d21-d20) регистра CNFG_GEN выставили b01 нам доступны 3 частоты 500 250 125:
    //они выставляются CNFG_ECG (d23-d22) b00-b10
    //-если выставили в регистре FMSTR (d21-d20) регистра CNFG_GEN выставили b10 нам доступна 1 частота 200Гц её и выставим:
    //CNFG_ECG (d23-d22) b10
    //коэф усиления (d17-d16) b00-b11
    //hp filter 0.5hz d14 b1-enabled
    //lp filter (d13-d12) b00 bypass b01-11 40 100 150 но если 200Гц то только 40Гц
    //max30003RegWrite(CNFG_ECG, 0x835000);  // 0x835000 200hz 0.5hf 40lf gain 160
    max30003RegWrite(CNFG_ECG, 0x805000);  // 0x835000 200hz 0.5hf 40lf gain 160
    osDelay(10);
    //fixme новое чтоб считал чсс
    max30003RegWrite(CNFG_RTOR1,0xB6C600);//вроде не нужно 0x3fc600//fixme поменял СТАРЫЙ ВАРИАНТ
    //max30003RegWrite(CNFG_RTOR1,0x3FC600);//вроде не нужно 0x3fc600//fixme поменял как было АВТОМАТ

    osDelay(10);
    //max30003RegWrite(EN_INT,0xF00201);//новое настройка прерываний
    max30003RegWrite(EN_INT,0x000201);//новое настройка прерываний//fixme поменял всё ок
    osDelay(10);
    max30003RegWrite(EN_INT2,0x000201);//новое настройка прерываний
    osDelay(10);
    //max30003RegWrite(MNGR_INT,0xF80101);//новое настройка прерываний
    max30003RegWrite(MNGR_INT,0x080104);//новое настройка прерываний
    osDelay(10);
    //max30003RegWrite(MNGR_INT,0x080104);//новое настройка прерываний
    osDelay(10);

    max30003Synch();
    osDelay(10);
}



void max30003BeginRtorMode()
{
    max30003SwReset();
    delay_ms(100);
    //max30003RegWrite(CNFG_GEN, 0x080004);
    max30003RegWrite(CNFG_GEN, 0x180004);//выставил частоту 32000Гц
    delay_ms(100);
    max30003RegWrite(CNFG_CAL, 0x720000);  // 0x700000
    delay_ms(100);
    //max30003RegWrite(CNFG_EMUX,0x0B0000);
    max30003RegWrite(CNFG_EMUX,0x050000);
    delay_ms(100);
    max30003RegWrite(CNFG_ECG, 0x805000);  // d23 - d22 : 10 for 250sps , 00:500 sps
    delay_ms(100);
    max30003RegWrite(CNFG_RTOR1,0x3fc600);
    delay_ms(100);
    max30003RegWrite(EN_INT, 0x000401);
    delay_ms(100);
    max30003Synch();
    delay_ms(100);
}

//not tested
void max30003SetsamplingRate(uint16_t samplingRate)
{
    uint8_t regBuff[4] = {0};
    max30003RegRead(CNFG_ECG, regBuff);

    switch(samplingRate){
        case SAMPLINGRATE_128:
                regBuff[0] = (regBuff[0] | 0x80 );
            break;

        case SAMPLINGRATE_256:
                regBuff[0] = (regBuff[0] | 0x40 );
            break;

        case SAMPLINGRATE_512:
                regBuff[0] = (regBuff[0] | 0x00 );
            break;

        default :
        	//print_usart(USART2,"Wrong samplingRate, please choose between 128, 256 or 512");
            break;
    }

    unsigned long cnfgEcg;
    memcpy(&cnfgEcg, regBuff, 4);

    //print_usart(USART2," cnfg ECG %x",cnfgEcg);
    max30003RegWrite(CNFG_ECG, (cnfgEcg >> 8));
}

int32_t  getEcgSamples_int(void)
{
    uint8_t regReadBuff[4];
    max30003RegRead(ECG_FIFO, regReadBuff);
    //FIXME тут неверно считаются данные
    uint32_t data0 = (uint32_t) (regReadBuff[0]);
    data0 = data0 <<24;
    uint32_t data1 = (uint32_t) (regReadBuff[1]);
    data1 = data1 <<16;
    uint32_t data2 = (uint32_t) (regReadBuff[2]);
    data2 = data2 >>6;
    data2 = data2 & 0x03;
    data2 = data2 << 14;

    uint32_t data = (uint32_t) (data0 | data1 | data2);
    //unsigned long data = (unsigned long) (data0 | data1);
    int32_t ecg = (int32_t) (data);
    return ecg;
}


void getHRandRR(void)
{
    uint8_t regReadBuff[4];
    max30003RegRead(RTOR, regReadBuff);

    unsigned long RTOR_msb = (unsigned long) (regReadBuff[0]);
    unsigned char RTOR_lsb = (unsigned char) (regReadBuff[1]);
    unsigned long rtor = (RTOR_msb<<8 | RTOR_lsb);
    rtor = ((rtor >>2) & 0x3fff) ;

    float hr =  60 /((float)rtor*0.0078125);
    heartRate = (unsigned int)hr;

    unsigned int RR = (unsigned int)rtor* (7.8125) ;  //8ms
    RRinterval = RR;
}


void max30003_HR_RR(max30003_str* max30003)
{
    uint8_t regReadBuff[4];
    max30003RegRead(RTOR, regReadBuff);

    uint32_t RTOR_msb = (uint32_t) (regReadBuff[0]);
    uint32_t RTOR_lsb = (uint32_t) (regReadBuff[1]);
    uint32_t rtor = ((RTOR_msb & 0xFF) <<8 | (RTOR_lsb & 0xFF));
    rtor = ((rtor >>2) & 0x3fff) ;

    //расчёт RR интервала
    max30003->RRinterval = rtor*8 ;  //8ms
    max30003->heartRate  = 6000000 / max30003->RRinterval;// fixme по идее это излишняя информация
}


uint32_t max30003_REG(uint8_t Reg_address)
{
    uint8_t regReadBuff[4];
    max30003RegRead(Reg_address, regReadBuff);
    uint32_t data = 0;
    data |= (((uint32_t)regReadBuff[0]) << 16); // read top 8 bits data
    data |= (((uint32_t)regReadBuff[1]) << 8); // read top 8 bits data
    data |= (((uint32_t)regReadBuff[2])); // read top 8 bits data
    return data;
}

void max30003_GET_ALL(max30003_str* max30003)
{
	max30003->ready    = false;
	max30003->ecg_raw  = getEcgSamples_int();
	max30003->reg_stat = max30003_REG(STATUS);
	if(!(max30003->reg_stat & 0x100000))
	{
		max30003->on_chest = true;
		if(max30003->on_chest_Tim < CHEST_WAIT)
			max30003->on_chest_Tim++;
		else
		{
			max30003->ecg_ok = false;
			max30003_HR_RR(max30003);//смысл узнавать пульс есть только при нормально приделанном датчике.

			if(max30003->heartRate > MAX_HR_HIGH)
			{
				max30003->hr_to_high = true;
				max30003->hr_to_high_Tim = MAX_HR_WAIT;
			}
			else//даже если пульс восстановился ждем
			{
				if(max30003->hr_to_high_Tim > 0)
					max30003->hr_to_high_Tim--;
				if(max30003->hr_to_high_Tim == 0)
					max30003->hr_to_high = false;

			}

			if(max30003->heartRate < MAX_HR_LOW)
			{
				max30003->hr_to_low = true;
				max30003->hr_to_low_Tim = MAX_HR_WAIT;
			}
			else//даже если пульс восстановился ждем
			{
				if(max30003->hr_to_low_Tim > 0)
					max30003->hr_to_low_Tim--;
				if(max30003->hr_to_low_Tim == 0)
					max30003->hr_to_low = false;
			}

		}
	}
	else
	{
		max30003->on_chest_Tim = 0;
		max30003->on_chest = false;
		max30003->ecg_ok = true;
		//max30003->heartRate = HR_ERROR;//fixme пока убрал

		//если нет контакта с сердцем всё можно обнулить
		max30003->hr_to_high = false;
		max30003->hr_to_high_Tim = 0;
		max30003->hr_to_low = false;
		max30003->hr_to_low_Tim = 0;

	}
}
