void TIM1_CC_IRQHandler(void) {
    float max_dv, tiny_dp, vel_req;
    int32_t pos_err;

    TIMER_LED_ON;

	TIM1->SR &= ~TIM_SR_CC1IF;	// clear capture/compare 1 event flag

	m1.active = 0;
	if (m1.current_period == 0) {
		// initiate movement
		// Calculate initial period needed for the calculation of acceleration
		m1.current_period = stepper_setf(1.0f / sqrtf(2.0f / (float) STEPPER_MAX_A));
		// No movement in this iteration
		m1.dir = 0;
		if(m1.pos_cmd - m1.current_pos > 0) {
//			m1.dir = 1;
			STEPPER_SET_DIR_POS;
		} else if(m1.pos_cmd - m1.current_pos < 0){
//			m1.dir = -1;
			STEPPER_SET_DIR_NEG;
		}

		// Set the counter to CCR1 value, so the first timer loop will not cause compare match
		TIM1->CNT = TIM1->CCR1;
		TIM1->EGR = TIM_EGR_UG;		// Generate update event (reload registers)
		TIM1->SR &= ~TIM_SR_UIF;	// clear update event flag
		TIM1->CCER |= TIM_CCER_CC1E;	// Enable PWM output
		TIM1->CR1 |= TIM_CR1_CEN;	// Count up, Enable timer
	}
    m1.current_pos += m1.dir;
	/* compute max change in velocity per servo period */
	max_dv = STEPPER_MAX_A * (float) m1.current_period / (float) CPU_CLK;
	/* calculate desired velocity */
	if (m1.enabled) {
		/* planner enabled, request a velocity that tends to drive
		   pos_err to zero, but allows for stopping without position
		   overshoot */
		pos_err = m1.pos_cmd - m1.current_pos;

		/* positive and negative errors require some sign flipping to
		   avoid sqrt(negative) */
		if (pos_err > 0) {
			vel_req = -max_dv + sqrtf(2.0f * (float) STEPPER_MAX_A * (float) pos_err + max_dv * max_dv);
			/* mark planner as active */
			m1.active = 1;
		} else if (pos_err < 0) {
			vel_req = max_dv - sqrtf(-2.0f * (float) STEPPER_MAX_A * (float) pos_err + max_dv * max_dv);
			/* mark planner as active */
			m1.active = 1;
		} else {
			/* within 'tiny_dp' of desired pos, no need to move */
			vel_req = 0.0;
		}
	} else {
		/* planner disabled, request zero velocity */
		vel_req = 0.0;
		/* and set command to present position to avoid movement when
		   next enabled */
		m1.pos_cmd = m1.current_pos;
	}
	/* limit velocity request */
	if (vel_req > m1.vmax) {
		vel_req = m1.vmax;
	} else if (vel_req < -m1.vmax) {
		vel_req = -m1.vmax;
	}
	/* ramp velocity toward request at accel limit */
	if (vel_req > m1.current_vel + max_dv) {
		m1.current_vel += max_dv;
	} else if (vel_req < m1.current_vel - max_dv) {
		m1.current_vel -= max_dv;
	} else {
		m1.current_vel = vel_req;
	}

    // TODO poprawić - gubi pierwszy(?) krok
    if(m1.current_vel > 0) {
    	m1.dir = 1;
    	STEPPER_SET_DIR_POS;
    } else if(m1.current_vel < 0) {
    	m1.dir = -1;
    	STEPPER_SET_DIR_NEG;
    }
    /* check for still moving */
    if(m1.current_vel != 0.0) {
		/* yes, mark planner active */
		m1.active = 1;
    }
    /* integrate velocity to get new position */
    //m1.current_pos += m1.current_vel * m1.current_period;
    if(m1.active == 0) {
    	stepper_stop();
    } else {
    	m1.current_period = stepper_setf(fabsf(m1.current_vel));
    }
    TIMER_LED_OFF;

}