#define F_CPU 16000000UL

#include <avr/io.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <avr/interrupt.h>
#include <math.h>
#include "ioport.h"
#include "stepper.h"
#include "pinout.h"
#include "uart.h"
#include <util/delay.h>

uint8_t  debugging = 1;											//DEBUG-VARIABLE - Meldungen einblenden (1)
uint8_t  first_home = 1;
uint8_t  first_calc = 1;

uint8_t stepper_command;										//1= forward, 0= reverse
uint8_t stepper_dir;											//1= forward, 0= reverse (Ri. HOME)
uint8_t akt_dir;

#define STEPPER_OFF_DELAY 100

uint8_t stepper_delay = 1;
uint8_t stepper_d;
uint8_t stepper_max_min = 0;
uint8_t stepper_home = 0;
uint8_t stepper_rev_home = 0;

uint16_t stepper_off_delay;

uint16_t step_counter = 0;
uint16_t position_request;
uint16_t delta = 0;
uint16_t ticks_rampe;
uint16_t ticks_rampe_beschl;
uint16_t ticks_rampe_brems;
uint16_t ticks_rampe_akt;

volatile uint8_t anzahl_overflows;
volatile uint8_t rest_ticks;
volatile uint8_t tick_abgelaufen;

char Ausgabestring[30];




uint16_t Stepper_max = 4300;										//Eigentlich 4276 - aber geht sowieso nur bis Anschlag

uint32_t Beschleunigung_Schritte;
uint32_t Bremsung_Schritte;
float   Schritt_Faktor;
float   Beschleunigung_Zeit;
float   Bremsung_Zeit;
uint64_t Frequenz;
float   Beschleunigung_Multiplikator;
float   Bremsung_Multiplikator;
float   Multiplikator_angepasst;
float   Timer_Ticks_1;
uint16_t Timer_Ticks_1_int;
float   Timer_Ticks_n;
float   Timer_Ticks_old;
uint16_t Timer_Ticks_n_int;
float   Q_Faktor;







ISR(TIMER2_COMP_vect)									//Hier wird der Rest der Tick-Wartezeit durchlaufen und das Verarbeitungs-Flag gesetzt
{													
	TIMSK &= ~(1<<OCIE2);								//Compare Interrupt deaktivieren	
	tick_abgelaufen = 1;	
}


ISR(TIMER2_OVF_vect)									//Da wir nur einen 8-Bit-Timer haben durchlaufen wir mehrere Durchgänge
{
	anzahl_overflows --;
	
	PORTB ^= (1<<PB3);
	
	if(!anzahl_overflows)
	{
		TIMSK &= ~(1<<TOIE2);							//Timer Overflow Interrupt deaktivieren			
		OCR2 = rest_ticks;
		TCNT2 = 0;										//Zähler auf 0
		TIMSK |= (1<<OCIE2);							//Compare Interrupt aktivieren	
		TIFR  |= (1<<OCF2);								//Output Compare Match Flag löschen (Zähler scharfschalten)
	}
}


void timer2_init (uint16_t u_ticks)						//Wird für jede Tick-Warterunde gestartet
{
	tick_abgelaufen = 0;
	anzahl_overflows = u_ticks / 256;

	PORTB ^= (1<<PB2);

	TCCR2 = (1<<CS22);									//Takt/64
	TCNT2 = 0;											//Zähler auf 0
	TIMSK |= (1<<TOIE2);								//Timer Overflow Interrupt aktivieren	
	TIFR  |= (1<<TOV2);									//Timer Overflow Flag löschen (Zähler scharfschalten)
}



void update_stepper()
{
	
	if (stepper_run)
	{
		if(INPUT(LS_HOME) && !stepper_dir)
		{
			if(debugging)
			{
				uart_putstr("Home erkannt\r\n");
				uart_putstr( itoa( step_counter, Ausgabestring, 10 ) );
				uart_putstr("\r\n");
			}
			stepper_home = 1;
			step_counter = 0;
			stepper_run = STEPPER_READY;
			stepper_max_min = 0;
			first_calc = 1;
		}
		else if(!INPUT(LS_HOME))
		{
			stepper_home = 0;
		}

		if(INPUT(LS_REV_HOME) && stepper_dir)
		{
			if(debugging)
			{
				uart_putstr("Rev Home erkannt\r\n");
				uart_putstr( itoa( step_counter, Ausgabestring, 10 ) );
				uart_putstr("\r\n");
			}
			stepper_rev_home = 1;
			stepper_run = STEPPER_READY;
			stepper_max_min = 0;
			first_calc = 1;
		}	
		else if(!INPUT(LS_REV_HOME))
		{
			stepper_rev_home = 0;
		}

		if(step_counter == position_request)
		{
			if(debugging)
			{
				uart_putstr("Counter = Request\r\n");
			}
			stepper_run = STEPPER_READY;
			stepper_max_min = 0;
			first_calc = 1;
		}

	}

			
	switch(stepper_command)
	{
		case CMD_STEP:
			if(debugging)
				uart_putstr("STEP\r\n");
			
			stepper_off_delay = STEPPER_OFF_DELAY;

			if(stepper_dir)							// Vorwärts gehts, Richtung Top
			{
				if(!stepper_rev_home)
				{
					stepper_run = STEPPER_ACTIVE;
					position_request = step_counter + 2;
				}				
			}
			else									// Zurück, Richtung Home
			{
				if(!stepper_home)
				{
					stepper_run = STEPPER_ACTIVE;
					position_request = step_counter - 2;
				}
			}
			ticks_rampe_akt = 0;
			break;
			
		case CMD_HOME:
			if(debugging)
				uart_putstr("HOME\r\n");
			if(!stepper_home)
			{
				stepper_dir = 0;
				stepper_run = STEPPER_ACTIVE;
				stepper_max_min = 1;			
				stepper_off_delay = STEPPER_OFF_DELAY;
				position_request = 0;
			}
			ticks_rampe_akt = 0;
			break;
			
		case CMD_TOP:
			if(debugging)
				uart_putstr("TOP\r\n");
			if(!stepper_rev_home)
			{
				stepper_dir = 1;
				stepper_run = STEPPER_ACTIVE;
				stepper_max_min = 1;
				stepper_off_delay = STEPPER_OFF_DELAY;	
				position_request = Stepper_max;	
			}
			ticks_rampe_akt = 0;
			break;
			
		case CMD_GOTO:
			if(debugging)
				uart_putstr("GOTO\r\n");
			if(step_counter == position_request)
			{
				stepper_max_min = 0;
				stepper_run = STEPPER_READY;
			}
			else if(step_counter < position_request)
			{
				if(!stepper_rev_home)
				{
					stepper_dir = 1;
					stepper_run = STEPPER_ACTIVE;
					stepper_max_min = 1;
					stepper_off_delay = STEPPER_OFF_DELAY;
				}
			}
			else
			{
				if(!stepper_home)
				{
					stepper_dir = 0;
					stepper_run = STEPPER_ACTIVE;
					stepper_max_min = 1;
					stepper_off_delay = STEPPER_OFF_DELAY;
				}
			}
			ticks_rampe_akt = 0;
			break;


		case CMD_GOTO_BACKW:
			if(debugging)
				uart_putstr("GOTO_BACKW\r\n");
			if(position_request < step_counter)									//Wenn x Schritte zurück > aktueller Position
				position_request = step_counter - position_request;				//	von der aktuellen Position aus x Schritte rückwärts (Ri. Start)
			else
			{																	//sonst geht es nach Hause
				position_request = 0;
			}
			if(step_counter == position_request)								
			{
				stepper_max_min = 0;
				stepper_run = STEPPER_READY;
			}
			else if(step_counter < position_request)							
			{
				if(!stepper_rev_home)
				{				
					stepper_dir = 1;
					stepper_run = STEPPER_ACTIVE;
					stepper_max_min = 1;
					stepper_off_delay = STEPPER_OFF_DELAY;
				}
			}
			else
			{
				if(!stepper_home)
				{				
					stepper_dir = 0;
					stepper_run = STEPPER_ACTIVE;
					stepper_max_min = 1;
					stepper_off_delay = STEPPER_OFF_DELAY;
				}
			}
			ticks_rampe_akt = 0;
			break;

	}

	stepper_command = 0;

/*	Groß geschrieben = High-Pegel

	1	PA0	full / HALF
	2	PA1
	4	PA2
	8	PA3	
	16	PA4	
	32	PA5	ENABLE/disable
	64	PA6	CW/ccw
	128	PA7	Step (active low - Impuls bei steigender Flanke)
*/


	if(first_calc && stepper_run)
	{
		if(debugging)
		{
			uart_putstr(" First Calc & Stepper Run\r\n");
		}
		if(step_counter < position_request)
			delta = position_request - step_counter;
		else if(step_counter > position_request)
			delta = step_counter - position_request;
		else
			delta = 0;
			
		if(delta > 20)
		{		
			ticks_rampe = delta;
			if(debugging)
			{
				uart_putstr( itoa(ticks_rampe, Ausgabestring, 10 ) );
				uart_putstr(" Ticks Rampe\r\n");
			}
			

			Schritt_Faktor =				(Beschleunigung_Schritte + Bremsung_Schritte);
			if(Schritt_Faktor > ticks_rampe)
			{				
				Schritt_Faktor = ticks_rampe /	Schritt_Faktor;
			}
			else
				Schritt_Faktor = 1;
			if(debugging)
			{
				uart_putstr( dtostrf(Schritt_Faktor, 2, 13, Ausgabestring ) );
				uart_putstr(" Schrittfaktor\r\n");
			}
			
			ticks_rampe_beschl = Beschleunigung_Schritte * Schritt_Faktor;
			if(debugging)
			{
				uart_putstr( itoa(ticks_rampe_beschl, Ausgabestring, 10 ) );
				uart_putstr(" Ticks Rampe beschl\r\n");
			}

			ticks_rampe_brems  = ticks_rampe - (Bremsung_Schritte * Schritt_Faktor);			
			if(debugging)
			{
				uart_putstr( itoa(ticks_rampe_brems, Ausgabestring, 10 ) );
				uart_putstr(" Ticks Rampe brems\r\n");
			}
			
		}

		timer2_init(Timer_Ticks_1_int);
		
		Timer_Ticks_n	  = Timer_Ticks_1;
		Timer_Ticks_n_int = Timer_Ticks_n;
		
		first_calc = 0;
	}


	if(tick_abgelaufen)
	{	
		if(stepper_run)
		{			
			akt_dir = stepper_dir;	
				STEPPER_PORT |= 0x20;						// Enable auf high (active) 
				_delay_us(1);
				STEPPER_PORT &= 0xBF;						// Richtung egalisieren (= Richtung HOME)
				_delay_us(1);
				if(akt_dir)
				{
					STEPPER_PORT |= 0x40;					// Richtung TOP zuweisen
					_delay_us(1);
				}
				STEPPER_PORT &= 0x7F;						// Clock auf low ziehen
				_delay_us(1);
				STEPPER_PORT |= 0x80;						// Steigende Flanke an Clock löst Step aus
				_delay_us(5);
				STEPPER_PORT &= 0xDF;						// Ausschalten (enable auf low)
		}

		if(stepper_run)
		{
			if(stepper_max_min)
			{
//				stepper_off_delay = STEPPER_OFF_DELAY;
			}
			else
			{
				stepper_run = 0;
			}
			if(stepper_dir)
			{
				if (!stepper_rev_home)
				{		
					step_counter ++;
					ticks_rampe_akt ++;
				}					
			}
			else
			{
				if (!stepper_home)
				{			
					step_counter --;
					ticks_rampe_akt ++;
				}					
			}
		}
		
		if(stepper_run)
		{
			if(delta > 20)							//Rampe berechnen
			{
				Timer_Ticks_old = Timer_Ticks_n;
				
				if(ticks_rampe_akt <= ticks_rampe_beschl)
				{
					Q_Faktor      = Timer_Ticks_old * Timer_Ticks_old * Beschleunigung_Multiplikator;
					if(debugging)
					{
						uart_putstr( dtostrf(Q_Faktor, 6, 10, Ausgabestring ) );
					}

					Timer_Ticks_n = Timer_Ticks_old * (1 + Q_Faktor + 1.5 * Q_Faktor * Q_Faktor);
				}
				else if (ticks_rampe_akt >= ticks_rampe_brems)
				{
					Q_Faktor      = Timer_Ticks_old * Timer_Ticks_old * Bremsung_Multiplikator;
					if(debugging)
					{
						uart_putstr( dtostrf(Q_Faktor, 6, 10, Ausgabestring ) );
						uart_putstr(" ; ");
					}
					
					Timer_Ticks_n = Timer_Ticks_old * (1 + Q_Faktor + 1.5 * Q_Faktor * Q_Faktor);					
				}
				else
				{
					Timer_Ticks_n = Timer_Ticks_old;					
				}
				
				if(debugging)
				{
					uart_putstr( dtostrf(Timer_Ticks_n, 6, 10, Ausgabestring ) );
					uart_putstr(" Akt Ticks\r\n");
				}
			
				Timer_Ticks_n_int = Timer_Ticks_n;
			}
			
			timer2_init(Timer_Ticks_n_int);
				
		}
	}

}


void stepper_init()
{
	SET_DDR(LS_LED);
	OUTPUT_ON(LS_LED);
	
	STEPPER_DDR   = 0xFF;
	STEPPER_PORT  = 0x80;								// Alles aus (Full Step bei active low), Clock high (Generell: Wechsel auf high = Schritt)
	
	position_request = Stepper_max;
	step_counter = Stepper_max / 2;
	
	Beschleunigung_Zeit				= (float)(VMax - VStart) / Beschleunigung;
	if(debugging)
	{
		uart_putstr( dtostrf( Beschleunigung_Zeit, 6, 10, Ausgabestring ) );
		uart_putstr(" Beschl Zeit\r\n");
	}
	
	Bremsung_Zeit					= (float)(VMax - VEnd  ) / Bremsung;
	if(debugging)
	{
		uart_putstr( dtostrf( Bremsung_Zeit, 6, 10, Ausgabestring ) );
		uart_putstr(" Brems Zeit\r\n");
	}
	

	Beschleunigung_Schritte	= ((uint32_t)((uint32_t)VMax * (uint32_t)VMax) - (uint32_t)((uint32_t)VStart * (uint32_t)VStart)) / (uint32_t)(2 * (uint32_t)Beschleunigung);
	if(debugging)
	{
		uart_putstr( ultoa( Beschleunigung_Schritte, Ausgabestring, 10 ) );
		uart_putstr(" Beschl Ticks\r\n");
	}
	
	Bremsung_Schritte		= ((uint32_t)((uint32_t)VMax * (uint32_t)VMax) - (uint32_t)((uint32_t)VEnd   * (uint32_t)VEnd  )) / (uint32_t)(2 * (uint32_t)Bremsung);
	if(debugging)
	{
		uart_putstr( ultoa( Bremsung_Schritte, Ausgabestring, 10 ) );
		uart_putstr(" Brems Ticks\r\n");
	}

	
	Frequenz						= F_CPU / Prescaler;
	if(debugging)
	{
		uart_putstr( ultoa( Frequenz, Ausgabestring, 10 ) );
		uart_putstr(" Frequenz\r\n");
	}
	
	Beschleunigung_Multiplikator	= (float)Beschleunigung / (Frequenz * Frequenz) * -1;;
	if(debugging)
	{
		uart_putstr( dtostrf( Beschleunigung_Multiplikator, 1, 14, Ausgabestring ) );
		uart_putstr(" Beschl Mult\r\n");
	}
	
	Bremsung_Multiplikator			= (float)Bremsung 	    / (Frequenz * Frequenz);
	if(debugging)
	{
		uart_putstr( dtostrf( Bremsung_Multiplikator, 1, 14, Ausgabestring ) );
		uart_putstr(" Brems Multr\n");	
	}
	
	Timer_Ticks_1                   = (float) Frequenz / sqrt(VStart * VStart + 2 * Beschleunigung);
	Timer_Ticks_1_int               = Timer_Ticks_1;
	if(debugging)
	{
		uart_putstr( dtostrf( Timer_Ticks_1, 9, 5, Ausgabestring ) );
		uart_putstr(" Ticks Start\r\n");
	}
}