#include <hcsr04.h>

int	STATE= 1;


void Gpio_init(uint8_t Pin){
	
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); 
	GPIO_InitTypeDef Init;
	 
	Init.GPIO_Pin =  GPIO_Pin_0;
	Init.GPIO_Mode = GPIO_Mode_OUT;
	Init.GPIO_PuPd = GPIO_PuPd_NOPULL;
	Init.GPIO_Speed = GPIO_Low_Speed;
	GPIO_Init(GPIOA, &Init);

	}

void ICTM_Init(){
	
	TIM_ICInitTypeDef TIM_ICInitStruct;
	TIM_ICInitStruct.TIM_Channel = TIM_Channel_1;
	TIM_ICInitStruct.TIM_ICFilter = 0;
	TIM_ICInitStruct.TIM_ICPolarity = TIM_ICPolarity_Rising;
	TIM_ICInitStruct.TIM_ICPrescaler = TIM_ICPSC_DIV1;
	TIM_ICInitStruct.TIM_ICSelection = TIM_ICSelection_DirectTI;
	TIM_ICInit(TIM2, &TIM_ICInitStruct);
	}

void NVIC_CFG_Init(){
	NVIC_InitTypeDef NVIC_InitStruct;
	NVIC_InitStruct.NVIC_IRQChannel = TIM2_IRQn;
	NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0;
	NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0;
	NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init(&NVIC_InitStruct);
	}
	
void delay(uint16_t ms) {
	TIM_SetCounter(TIM2, 0);
	while (TIM_GetCounter(TIM2) < ms);
	}

void TIM_ResetCounter(TIM_TypeDef* TIMx)
{
  /* Check the parameters */
  assert_param(IS_TIM_ALL_PERIPH(TIMx));

  /* Reset the Counter Register value */
  TIMx->CNT = 0;
}

// Convert float to String     
void ftoa (float f, char* str) { 
    int ipart = (int) f;
    int fpart = (f - ipart) * 1000;
    itoa(ipart, str, 10);
    int i = 0;
    while(str[i++] == '\0');
	i++;
	str[i] = '.';
	itoa(fpart, str + i + 1, 10);
}


void ICTM_Capture(){
	if (TIM_Channel_1){
		if(STATE == 0){ // state = 0 when the first captured value is not captured
			T1 = TIM_GetCapture1(TIM2);
			STATE = 1;
			TI2_Config(TIM2, TIM_ICPolarity_Falling, TIM_ICSelection_DirectTI, 0);
			}
		else if (STATE == 1){
			T2 = TIM_GetCapture1(TIM2);
			TIM_ResetCounter(TIM2);
			if (T1 < T2){ // Diffrence between two capturted timestamp
					DIFF = T2 - T1;
				} 
			else if (T1 > T2){
						DIFF = (0Xffff - T1) + T2;
				}
			DIFF = TIM_cyclesToMs(DIFF);
			DISTANCE = 4 * .034/2;
			STATE = 0;
			TI2_Config(TIM2, TIM_ICPolarity_Rising, TIM_ICSelection_DirectTI, 0);
			TIM_cancelTimer(TIM2);
		}
	}
}


void HCSR04_Init(){
	Gpio_init(1);
	ICTM_Init();
	NVIC_CFG_Init();
}


void HCSR04_Read(void){
	GPIO_WriteBit(TIM2, TIM_Channel_1, TIM_ICPolarity_Rising); // pull the trig pin to high
	delay(10); // 10 us
	GPIO_WriteBit(TIM2, TIM_Channel_1, TIM_ICPolarity_Falling); //  pull the trig pin to low
	TIM_Cmd(TIM2, ENABLE);
}

/*char HC_SR04_getCalibration() {

		return str;

}

void HC_SR04_setCalibration() {
	
		return calibration = DISTANCE;
	}
*/