/*
 * pse_stepper_planer.c
 *
 *  Created on: Aug 14, 2023
 *      Author: leo
 */

#include <stdint.h>

#include "lvgl.h"

#include "PSE_unit.h"

#include "pse_stepper_planer.h"

void pse_stepper_planer_tick(pse_unit* unit){
	pse_stepper_conf* c = unit->stepper_conf;
	pse_stepper_status* s = unit->stepper_status;

	HAL_GPIO_TogglePin(c->STEP_GPIO_Port, c->STEP_GPIO_Pin);
	s->steps_counter += (pse_sp_get_dir(unit)?1:-1) * HAL_GPIO_ReadPin(c->STEP_GPIO_Port, c->STEP_GPIO_Pin);
	if(s->stop_at_limit && s->steps_counter == s->stop_steps){
		pse_sp_jog_speed(c, s, 0);
		pse_sp_stop_axis(c);
	}
}

void pse_sp_start_axis(pse_stepper_conf* conf, pse_stepper_status* status){
	if(status->stop_at_limit && status->steps_counter == status->stop_steps) return;
	
	HAL_GPIO_WritePin(conf->EN_GPIO_Port, conf->EN_GPIO_Pin, GPIO_PIN_RESET);
	HAL_TIM_Base_Start_IT(conf->tim);
}
void pse_sp_stop_axis(pse_stepper_conf* conf){
	HAL_TIM_Base_Stop_IT(conf->tim);
	HAL_GPIO_WritePin(conf->EN_GPIO_Port, conf->EN_GPIO_Pin, GPIO_PIN_SET);
}

void pse_sp_set_dir(pse_unit* unit, int dir){
	pse_stepper_conf* conf = unit->stepper_conf;
	HAL_GPIO_WritePin(conf->DIR_GPIO_Port, conf->DIR_GPIO_Pin, unit->reversed?-dir:dir);
}

int pse_sp_get_dir(pse_unit* unit){
	pse_stepper_conf* conf = unit->stepper_conf;
	int val = HAL_GPIO_ReadPin(conf->DIR_GPIO_Port, conf->DIR_GPIO_Pin);
	return (unit->reversed?-val:val);
}

// https://en.wikipedia.org/wiki/Integer_square_root#Using_only_integer_division
static uint16_t int_sqrt(uint32_t s){
	if (s <= 1)
		return s;

	uint32_t x0 = s / 2;

	uint32_t x1 = (x0 + s / x0) / 2;

	while (x1 < x0){
		x0 = x1;
		x1 = (x0 + s / x0) / 2;
	}
	return x0;
}

void pse_stepper_planer_compute_sps(pse_unit* unit){
	pse_stepper_conf* c = unit->stepper_conf;
	pse_stepper_status* s = unit->stepper_status;
	
	// magic constants are used to approximate pi and other multiplicatives constant as a rationnal
	uint64_t numerator = (uint64_t)14323 * unit->syringe->diameter * unit->syringe->diameter * PSE_STEPPER_TIMER_CLOCK * PSE_STEPPER_SCREW_PITCH;
	uint64_t denominator = (uint64_t)607887 * unit->flow * PSE_STEPPER_STEPS_PER_ROTATION;
	uint64_t res = numerator / denominator;

	uint64_t min_scale = PSE_STEPPER_TIMER_CLOCK * 1000 / (2 * PSE_STEPPER_WARN_MAX_KSPS);
	if(res < min_scale){
		lv_obj_t * mbox1 = lv_msgbox_create(NULL, "WARNING", "aaa", NULL, true);
		lv_obj_t* text = lv_msgbox_get_text(mbox1);
		lv_label_set_text_fmt(text, "Fast movement selected, this may lead to undefined behavior (%llu ksps)", PSE_STEPPER_TIMER_CLOCK * 1000 / res);
		lv_obj_center(mbox1);
		return;
	}
	else if(res >= ((uint64_t)1 << 32)){
		lv_obj_t * mbox1 = lv_msgbox_create(NULL, "Error", "Invalid configuration entered (period > 2^32)", NULL, true);
		lv_obj_center(mbox1);
		return;
	}

	if(res < 65536){
		s->tim_presc = 0;
		s->tim_period = res - 1;
		__HAL_TIM_SET_AUTORELOAD(c->tim, s->tim_period);
		__HAL_TIM_SET_PRESCALER(c->tim, s->tim_presc);
	}
	else{
		uint16_t sqrt = int_sqrt(res);
		s->tim_presc = sqrt - 1;
		s->tim_period = sqrt;
		__HAL_TIM_SET_AUTORELOAD(c->tim, s->tim_period);
		__HAL_TIM_SET_PRESCALER(c->tim, s->tim_presc);
	}
}

void pse_sp_set_dir_all(pse_unit* units, int units_num, int dir){
	for(int i = 0; i < units_num; i++){
		pse_sp_set_dir(&units[i], dir);
	}
}

void pse_sp_start_all(pse_unit* units, int units_num){
	for(int i = 0; i < units_num; i++){
		if(units[i].enabled){
			pse_sp_start_axis(units[i].stepper_conf, units[i].stepper_status);
		}
	}
}
void pse_sp_stop_all(pse_unit* units, int units_num){
	for(int i = 0; i < units_num; i++){
		if(units[i].enabled)
			pse_sp_stop_axis(units[i].stepper_conf);
	}
}

void pse_sp_jog_speed(pse_stepper_conf* conf, pse_stepper_status* sta, int status){
	if(status){
		__HAL_TIM_SET_AUTORELOAD(conf->tim, PSE_STEPPER_JOG_PERIOD);
		__HAL_TIM_SET_PRESCALER(conf->tim, PSE_STEPPER_JOG_PRESC);
	}
	else{
		__HAL_TIM_SET_AUTORELOAD(conf->tim, sta->tim_period);
		__HAL_TIM_SET_PRESCALER(conf->tim, sta->tim_presc);
	}
}