#include <stdio.h>
#include <math.h>

#define M_PI 3.1415
#define uint32_t	int
#define int32_t	int
#define KROK	0.1//mm
#define STEPS_PER_MM	(1 / KROK)  // 10 kroków na 1 mm
#define MAX_STEPS_PER_MS  4 

#define L_MAX	200//mm
#define T_MAX	3000//3s
#define T_KROK	10//co 10ms
uint32_t KlusLenTable[(T_MAX / T_KROK) + 1];

uint32_t CheckMaxWysuwLen(uint32_t L, uint32_t T);
void KlusMaxLenTable(uint32_t* KlusLenTable);


//
#define G	10//m/s2
uint32_t g_time = 40;
//uint32_t GTable[g_time +1];
uint32_t droga, velocity;

int main(void) {
	
	for(int i=0; i<=g_time; i++) {
		droga = 5*i*i;
		velocity = G*i;
		printf("droga=%d prędkość=%d\n", droga, velocity);
	}
	
	
	
//	KlusMaxLenTable(KlusLenTable);
	
	//printf("%d \n", tim);
//	if (tim > T) printf("OK %d \n", tim);
//	else printf("Za duży wysuw %d \n", tim);
	
	return 0;
}

uint32_t CheckMaxWysuwLen(uint32_t Lmax, uint32_t T)
{
	float position;
	uint32_t steps;
	uint32_t currentPosition = 0;
	uint32_t targetPosition;
	int delta;
	int t, len = 0;

	if (0 == T) return len;

	for (len = 0; len <= Lmax; len++)
	{
		for (t = 0; t <= T; t++)
		{
			position = (STEPS_PER_MM * len / 2) * (sin((2.0 * t / T) * M_PI - M_PI/2) + 1);
			steps = roundf(position);// * STEPS_PER_MM);
			targetPosition = steps;
			delta = targetPosition - currentPosition;
			if( abs(delta) > MAX_STEPS_PER_MS) break;
			currentPosition = targetPosition;
		}
		if (t < T ) return len-1;
	}
	return Lmax;
}
void KlusMaxLenTable(uint32_t* KlusLenTable)
{
	int k = 0, t;
	
	//tabela do wklejenia do kodu
	printf("KlusMaxLenTable[%d] = {", T_MAX/T_KROK + 1);
	for (t = 0; t <= T_MAX; t += T_KROK, k++)//co 10mm
	{
		KlusLenTable[k] = CheckMaxWysuwLen(L_MAX, t);
		printf("%d, ", KlusLenTable[k]);
	}
	printf("}\n");
	//podglad
		for (k = 0; k <= T_MAX/T_KROK; k++)//co 10mm
	{
		printf("t=%d Lmax=%d, ", k*T_KROK, KlusLenTable[k]);
	}

}