/*
 * Encoder.cpp
 *
 *  Created on: Mar 17, 2023
 *      Author: Gabriel
 */

#include "Encoder.hpp"

#define pi 3.1415926535

Encoder::Encoder(__IO uint32_t* cnt, __IO uint32_t* arr, uint32_t countsPerRevolution) :
	cnt(cnt), arr(arr), countsPerRevolution(countsPerRevolution)
{

}

Encoder::~Encoder() {

}

void Encoder::callback(GPIO_PinState _direction){
	lastTick = currentTick;
	currentTick = DWT->CYCCNT;
	direction = _direction;
	updated = true;
}

uint32_t Encoder::getCount(){
	lastCount = *cnt;
	return lastCount;
}

int16_t Encoder::getDelta(){
	int16_t delta =  *cnt - lastCount;
	lastCount = *cnt;
	return delta;
}

float Encoder::getOmega(){
	if(!updated){
		return 0;
	}
	updated = false;
	float deltaT = (currentTick - lastTick)/(float)HAL_RCC_GetHCLKFreq();
	if(direction){
		return -8*pi/(countsPerRevolution*deltaT);	// rad/s
	}else{
		return 8*pi/(countsPerRevolution*deltaT);	// rad/s
	}

}

uint32_t Encoder::getCountsPerRevolution(){
	return countsPerRevolution;
}