/*

   IN_D_X ... PH2
   IN_D_Y ... PJ2
   OUT_D_X ... PK5
   OUT_D_Y ... PK3
   IN_S_X ... PE3 ... OC0D
   IN_S_Y ... PF3 ... OC0D
   OUT_S_X ... PC3 ... OC0D  
   OUT_S_Y ... PD3 ... OC0D
   PSW1 ... PE7
   PSW2 ... PE6
   PSW3 ... PE5
   PSW4 ... PE4  
   
*/

#include <avr/interrupt.h>
#include <avr/io.h>
#include <string.h>
#include <stdio.h>
#include <stddef.h>
#include "main.h"
#include "pinout.h"
#include <simpleavrio.h>
#include <interrupt.h>
#include <delay.h>
#include <avr/io.h>
#include <avr/sleep.h>
#include <clksys_driver.h>

volatile uint32_t MOV = 0, MOVEMENT = 0, ONLY_FIRST = 0, GET_S_Y_UP = 0, GET_S_Y_DOWN, PUT_S_Y_UP = 0, PUT_S_Y_DOWN = 0, SET_LL = 0;
volatile uint32_t ARRAY[3], LL_S[2] = { 0, 1}, LL_D[2] = { 0, 1}, i = 0, COUNT_TIME_X = 0, COUNT_TIME_Y = 0, COUNT_X = 0, COUNT_Y = 0, INTER_STEP_Y = 0, INTER_STEP_X = 0; 

 void Interrupt_IN_S_X ()				// Indikace přerušení (sestupné hrany signálu STEP osa X - PE3)  !!!!!!!!!!!!!!!!!! NA VSTUP  - KONEKTOR JE PŘIVEDENA NÁBĚŽNÁ HRANA, NA PIN PROCESORU SESTUPNÁ - REAGUJE NA NÁBĚŽNOU HRANU Z PLC
{
	PORTE.PIN3CTRL = PORT_ISC_FALLING_gc;									// reaguje na náběžnou hranu
	PORTE.INT0MASK =  0x8;													// aktivní přerušení na pinu 3, port E
	PORTE.INTCTRL = PORT_INT0LVL_HI_gc;										// nastavení přerušení INT0
	PMIC.CTRL |= PMIC_HILVLEN_bm;											// nejvyšší priorita přerušení
	sei();																	// aktivuje globální přerušení
}

//Přerušení po přivedení sestupné hrany na IN_S_X(PE3)
ISR (PORTE_INT0_vect)
{   
	PORTE.INTFLAGS = 0x01; // clear INT0IF flag.
	
	INTER_STEP_X = 1;
	
	if ((INTER_STEP_Y == 0 && IN_D_X == 0 && OUT_D_X == 0 && OUT_D_Y == 0) || (INTER_STEP_Y == 0 && IN_D_X == 0 && OUT_D_X == 0 && OUT_D_Y == 1) || (INTER_STEP_Y == 0 && IN_D_X == 0 && OUT_D_X == 1 && OUT_D_Y == 0))		// pohyb doleva
	{
		_delay_us(6);
		OUT_D_X = LL_D[1];
		OUT_D_Y = LL_D[1];
		_delay_us(3);
	}
	else if ((INTER_STEP_Y == 0 && IN_D_X == 1 && OUT_D_X == 1 && OUT_D_Y == 0)|| (INTER_STEP_Y == 0 && IN_D_X == 1 && OUT_D_X == 0 && OUT_D_Y == 1) || (INTER_STEP_Y == 0 && IN_D_X == 1 && OUT_D_X == 1 && OUT_D_Y == 1))		// pohyb doprava
	{
		_delay_us(6);
		OUT_D_X = LL_D[0];
		OUT_D_Y = LL_D[0];
		_delay_us(3);
	}
	
	 if (INTER_STEP_Y == 0 && IN_D_X == 0)	// pohyb doleva
	{	
		MOVEMENT = 1;
		_delay_us(3);
		OUT_S_X = LL_S[1];
		OUT_S_Y = LL_S[1];
		_delay_us(3);
		OUT_S_X = LL_S[0];
		OUT_S_Y = LL_S[0];
		ARRAY[0] = ARRAY[1];
		ARRAY[1] = ARRAY[2];
		ARRAY[2] = MOVEMENT;
	}	
	else if (INTER_STEP_Y == 0 && IN_D_X == 1)	// pohyb doprava
	{
		MOVEMENT = 2;
		_delay_us(3);
		OUT_S_X = LL_S[1];
		OUT_S_Y = LL_S[1];
		_delay_us(3);
		OUT_S_X = LL_S[0];
		OUT_S_Y = LL_S[0];
		ARRAY[0] = ARRAY[1];
		ARRAY[1] = ARRAY[2];
		ARRAY[2] = MOVEMENT;
	}

	if((ARRAY[0] == 4) && (ARRAY[1] == 8) && (ARRAY[2] == 1))	    //1
	{
		//SIG1 = 0;
		//SIG2 = 0;
		//ERR_LED = 0;
		PUT_S_Y_DOWN++;
	}
	else if((ARRAY[0] == 3) && (ARRAY[1] == 7) && (ARRAY[2] == 1)) //2
	{
		//SIG1 = 0;
		//SIG2 = 0;
		//ERR_LED = 1;
		GET_S_Y_UP++;
	}
	else if((ARRAY[0] == 3) && (ARRAY[1] == 5) && (ARRAY[2] == 2)) //3
	{
		//SIG1 = 0;
		//SIG2 = 1;
		//ERR_LED = 0;
		GET_S_Y_UP++;  
	}
	else if((ARRAY[0] == 4) && (ARRAY[1] == 6) && (ARRAY[2] == 2)) //4
	{
		//SIG1 = 0;
		//SIG2 = 1;
		//ERR_LED = 1;
		GET_S_Y_DOWN++;
	}
	
	 if (INTER_STEP_X == 1 && INTER_STEP_Y == 1)
 	{
		if((IN_D_X == 0 && IN_D_Y == 0 && OUT_D_X == 1 && OUT_D_Y == 0)|| (IN_D_X == 0 && IN_D_Y == 0 && OUT_D_X == 0 && OUT_D_Y == 1) || (IN_D_X == 0 && IN_D_Y == 0 && OUT_D_X == 0 && OUT_D_Y == 0))				// pohyb doprava - dolů
		{
			MOVEMENT = 8;
			MOV = 8;
			_delay_us(6);
			OUT_D_X = LL_D[1]; 
			OUT_D_Y = LL_D[1];
			_delay_us(3); 
		}
 		else if((IN_D_X == 0 && IN_D_Y == 1 && OUT_D_X == 1 && OUT_D_Y == 0) || (IN_D_X == 0 && IN_D_Y == 1 && OUT_D_X == 0 && OUT_D_Y == 1) || (IN_D_X == 0 && IN_D_Y == 1 && OUT_D_X == 0 && OUT_D_Y == 0))		// pohyb doprava - nahoru
 		{
			MOVEMENT = 7;
			MOV = 7;
 			_delay_us(6);
 			OUT_D_X = LL_D[1];
 			OUT_D_Y = LL_D[1];
 			_delay_us(3);
 		}
		else if((IN_D_X == 1 && IN_D_Y == 1 && OUT_D_X == 1 && OUT_D_Y == 0) || (IN_D_X == 1 && IN_D_Y == 1 && OUT_D_X == 0 && OUT_D_Y == 1) || (IN_D_X == 1 && IN_D_Y == 1 && OUT_D_X == 1 && OUT_D_Y == 1))		// pohyb doleva - nahoru
		{
			MOVEMENT = 5;
			MOV = 5;
			_delay_us(6);
			OUT_D_X = LL_D[0];
			OUT_D_Y = LL_D[0];
			_delay_us(3);
		}
		else if((IN_D_X == 1 && IN_D_Y == 0 && OUT_D_X == 0 && OUT_D_Y == 1) || (IN_D_X == 1 && IN_D_Y == 0 && OUT_D_X == 1 && OUT_D_Y == 0) || (IN_D_X == 1 && IN_D_Y == 0 && OUT_D_X == 1 && OUT_D_Y == 1))		// pohyb doleva-dolů
		{
			MOVEMENT = 6;
			MOV = 6;
			_delay_us(6);
			OUT_D_X = LL_D[0];
			OUT_D_Y = LL_D[0];
			_delay_us(3);
		}
		
			ARRAY[0] = ARRAY[1];
			ARRAY[1] = ARRAY[2];
			ARRAY[2] = MOVEMENT;
			
			if(((ARRAY[0] == 1) || (ARRAY[0] == 2)) && (ARRAY[1] == 0) && (ARRAY[2] == 7) && (ONLY_FIRST != 1))
			{	
				ONLY_FIRST = 1;
				PUT_S_Y_UP++;
			}
			else if(((ARRAY[0] == 1) || (ARRAY[0] == 2)) && (ARRAY[1] == 0) && (ARRAY[2] == 8) && (ONLY_FIRST != 2))
			{	
				ONLY_FIRST = 2;
				PUT_S_Y_DOWN++;
			}
			else if(((ARRAY[0] == 1) || (ARRAY[0] == 2)) && (ARRAY[1] == 0) && (ARRAY[2] == 5) && (ONLY_FIRST != 3))
			{
				ONLY_FIRST = 3;
				PUT_S_Y_UP++;
			}
			else if(((ARRAY[0] == 1) || (ARRAY[0] == 2)) && (ARRAY[1] == 0) && (ARRAY[2] == 6) && (ONLY_FIRST != 4))
			{
				ONLY_FIRST = 4;
				PUT_S_Y_DOWN++;
			}
			else if(((ARRAY[0] == 3) || (ARRAY[0] == 4)) && (ARRAY[1] == 7) && (ARRAY[2] == 7) && (ONLY_FIRST != 5))
			{
				ONLY_FIRST = 5;
				GET_S_Y_UP++;
			}
			else if(((ARRAY[0] == 3) || (ARRAY[0] == 4)) && (ARRAY[1] == 8) && (ARRAY[2] == 8) && (ONLY_FIRST != 6))
			{
				ONLY_FIRST = 6;
				GET_S_Y_DOWN++;
			}
			else if(((ARRAY[0] == 3) || (ARRAY[0] == 4)) && (ARRAY[1] == 5) && (ARRAY[2] == 5) && (ONLY_FIRST != 7))
			{
				ONLY_FIRST = 7;
				GET_S_Y_UP++;
			}
			else if(((ARRAY[0] == 3) || (ARRAY[0] == 4)) && (ARRAY[1] == 6) && (ARRAY[2] == 6) && (ONLY_FIRST != 8))
			{
				ONLY_FIRST = 8;
				GET_S_Y_DOWN++;
			}
		
		if (IN_D_X == 1 && IN_D_Y == 1)		//  5 
		{
			_delay_us(3);
			OUT_S_X = LL_S[1];
			_delay_us(3);
			OUT_S_X = LL_S[0];
			_delay_us(3);
			OUT_S_X = LL_S[1];
			_delay_us(3);
			OUT_S_X = LL_S[0];
		}
 		else if (IN_D_X == 0 && IN_D_Y == 1)	// 7 
 		{
 			_delay_us(3);
 			OUT_S_Y = LL_S[1];
 			_delay_us(3);
 			OUT_S_Y = LL_S[0];
 			_delay_us(3);
 			OUT_S_Y = LL_S[1];
 			_delay_us(3);
 			OUT_S_Y = LL_S[0];
 		}
	   else	if (IN_D_X == 0 && IN_D_Y == 0)		// 8
		{
			_delay_us(3);
			OUT_S_X = LL_S[1];
			_delay_us(3);
			OUT_S_X = LL_S[0];
			_delay_us(3);
			OUT_S_X = LL_S[1];
			_delay_us(3);
			OUT_S_X = LL_S[0];
		}
		else if (IN_D_X == 1 && IN_D_Y == 0)	// 6
		{
			_delay_us(3);
			OUT_S_Y = LL_S[1];
			_delay_us(3);
			OUT_S_Y = LL_S[0];
			_delay_us(3);
			OUT_S_Y = LL_S[1];
			_delay_us(3);
			OUT_S_Y = LL_S[0];
		}
 	}
	INTER_STEP_Y = 0;
	COUNT_X = 1;

}

void Interrupt_IN_S_Y ()				// Indikace přerušení (sestupné hrany signálu STEP osa Y - PF3)
{
	PORTF.PIN3CTRL = PORT_ISC_FALLING_gc;									// reaguje na sestupnou hranu
	PORTF.INT1MASK =  0x8;													// aktivní přerušení na pinu 3, port F
	PORTF.INTCTRL = PORT_INT1LVL_HI_gc;										// nastavení přerušení INT1
	PMIC.CTRL |= PMIC_HILVLEN_bm;											// nejvyšší priorita přerušení
	sei();																	// aktivuje globální přerušení
}

//Přerušení po přivedení sestupné hrany na IN_S_X(PF3)
ISR (PORTF_INT1_vect)
{

		PORTF.INTFLAGS = 0x01; // clear INT1IF flag.
		
		INTER_STEP_Y = 1;
		
		if ((INTER_STEP_X == 0 && IN_D_Y == 0 && OUT_D_X == 0 && OUT_D_Y == 0) || (INTER_STEP_X == 0 && IN_D_Y == 0 && OUT_D_X == 0 && OUT_D_Y == 1) || (INTER_STEP_X == 0 && IN_D_Y == 0 && OUT_D_X == 1 && OUT_D_Y == 1))			// pohyb dolů
		{ 
			MOVEMENT = 4;
			_delay_us(6);
			OUT_D_X = LL_D[1];
			OUT_D_Y = LL_D[0];
			_delay_us(3);
		}
		else if ((INTER_STEP_X == 0 && IN_D_Y == 1 && OUT_D_X == 0 && OUT_D_Y == 0) || (INTER_STEP_X == 0 && IN_D_Y == 1 && OUT_D_X == 1 && OUT_D_Y == 0) || (INTER_STEP_X == 0 && IN_D_Y == 1 && OUT_D_X == 1 && OUT_D_Y == 1))	// pohyb nahoru
		{	
			MOVEMENT = 3;
			_delay_us(6);
			OUT_D_X = LL_D[0];
			OUT_D_Y = LL_D[1];
			_delay_us(3);
		}
		else 
		{
			MOVEMENT = 0;
		}
		
		if (INTER_STEP_X == 0 && IN_D_Y == 0)		// pohyb dolů
		{ 
			MOVEMENT = 4;
			if (GET_S_Y_DOWN == 0)
			{
				_delay_us(3);
				OUT_S_X = LL_S[1];
				OUT_S_Y = LL_S[1];
				_delay_us(3);
				OUT_S_X = LL_S[0];
				OUT_S_Y = LL_S[0];
				if (PUT_S_Y_DOWN > 0)
				{
					_delay_us(3);
					OUT_S_X = LL_S[1];
					OUT_S_Y = LL_S[1];
					_delay_us(3);
					OUT_S_X = LL_S[0];
					OUT_S_Y = LL_S[0];
					PUT_S_Y_DOWN--;
				}
			}
			else if (GET_S_Y_DOWN > 0)
			{
				GET_S_Y_DOWN--;
			}
		}
		else if (INTER_STEP_X == 0 && IN_D_Y == 1)	// pohyb nahoru
		{
			MOVEMENT = 3;
			if (GET_S_Y_UP == 0)
			{
				_delay_us(3);
				OUT_S_X = LL_S[1];
				OUT_S_Y = LL_S[1];
				_delay_us(3);
				OUT_S_X = LL_S[0];
				OUT_S_Y = LL_S[0];
				if (PUT_S_Y_UP > 0)
				{
					_delay_us(3);
					OUT_S_X = LL_S[1];
					OUT_S_Y = LL_S[1];
					_delay_us(3);
					OUT_S_X = LL_S[0];
					OUT_S_Y = LL_S[0];
					PUT_S_Y_UP--;
				}
			}	
		else if (GET_S_Y_UP > 0)
		{
			GET_S_Y_UP--;
		}
}
		 
		INTER_STEP_X = 0;
		COUNT_Y = 1;
		
		if (COUNT_TIME_Y > 150000)
		{
			COUNT_TIME_Y = 100000;
		}
		
	ARRAY[0] = ARRAY[1];
	ARRAY[1] = ARRAY[2];
	ARRAY[2] = MOVEMENT;
	

if((ARRAY[0] == 1) && (ARRAY[1] == 0) && (ARRAY[2] == 4))		//5
{
   	//SIG1 = 1;
   	//SIG2 = 0;
   	//ERR_LED = 0;
	PUT_S_Y_DOWN++;
}
else if((ARRAY[0] == 1) && (ARRAY[1] == 0) && (ARRAY[2] == 3)) // 6
{
   	//SIG1 = 1;
   	//SIG2 = 0;
   	//ERR_LED = 1;
	PUT_S_Y_UP++;
}
else if((ARRAY[0] == 2) && (ARRAY[1] == 0) && (ARRAY[2] == 4)) // 7
{
   	//SIG1 = 1;
   	//SIG2 = 1;
   	//ERR_LED = 0;
	PUT_S_Y_DOWN++;
}
else if((ARRAY[0] == 2) && (ARRAY[1] == 0) && (ARRAY[2] == 3)) // 8
{
   	//SIG1 = 1;
   	//SIG2 = 1;
   	//ERR_LED = 1;
	PUT_S_Y_UP++;
}

}

// Nastaví interní RC oscilátor 32 MHz
void clock_init(void)
{
	OSC.CTRL |= OSC_RC32MEN_bm;
	while(!(OSC.STATUS & OSC_RC32MRDY_bm));
	CCP = CCP_IOREG_gc;
	CLK.CTRL = (CLK.CTRL & ~CLK_SCLKSEL_gm) | CLK_SCLKSEL_RC32M_gc;
}

void logic_level_init(void)
{
				if (PSW1 == 0 && SET_LL != 1)						// přepnutí na PNP logiku po zapnutí/restartu
				{
					LL_S[0] = 1;
					LL_S[1] = 0;
					LL_D[0] = 1;
					LL_D[1] = 0;
					SET_LL = 1;
				}
				else if (PSW1 == 1 && SET_LL != 1)					// přepnutí na NPN logiku po zapnutí/restartu
				{
					LL_S[0] = 0;
					LL_S[1] = 1;
					LL_D[0] = 0;
					LL_D[1] = 1;
					SET_LL = 1;
				}
}

int main (void)
{  	
	PORTC.OUT = 0x00;
  	PORTC.DIR = 0b00001000;
	PORTD.OUT = 0x00;
	PORTD.DIR = 0b00001111;
	PORTE.OUT = 0b00001000;
  	PORTE.DIR = 0b00000000;
	PORTF.OUT = 0b00001000;
  	PORTF.DIR = 0b00000000;
  	PORTH.OUT = 0b00000100;
  	PORTH.DIR = 0b00110000;
	PORTJ.OUT = 0b00000100;
  	PORTJ.DIR = 0b00000000;
	PORTK.OUT = 0x00;
  	PORTK.DIR = 0b00101000;

	clock_init();
	_delay_ms(500);
	logic_level_init();
	_delay_ms(500);
	Interrupt_IN_S_Y ();
 	Interrupt_IN_S_X ();

		do {
			ERR_LED = 1;
						
  			_delay_us(10);
  			if (MOV != 0)										// pokud je aktivní sdružený pohyb, začne detekovat zastavení pohybu v jedné ose na víc jak 90 ms
  			{
  				if ( COUNT_X == 0)								// pokud není pohyb v ose X
  				{
  					COUNT_TIME_X = COUNT_TIME_X + 10;			// přičte 10 us - čas bez pohybu v rámci jednoho cyklu
  				}
  				else											// pokud je aktivní pohyb v ose X
  				{
  					COUNT_TIME_X = 0;							// vynuluje čas bez pohybu v ose X
  					COUNT_X = 0;								// vynuluje proměnnou pro indikaci pohybu v ose X
  				}
  		
  				if (COUNT_X > 90000)							// pokud je čas bez pohybu v ose X větší jak 90 ms, byl ukončen sdružený pohyb
  				{
  					MOV = 0;									// vynuluje se proměnná s indexem sdruženého pohybu
					ONLY_FIRST = 0;								// vynuluje se proměnná pro zajištění správné korekce kroků ve sdružených pohybech
 				}
  		
  				if (COUNT_Y == 0)								// TO SAMO PRO Y
  				{
  					COUNT_TIME_Y = COUNT_TIME_Y + 10;
  				}
  				else
  				{
  					COUNT_TIME_Y = 0;
  					COUNT_Y = 0;
  				}
  			
  				if (COUNT_Y > 90000)
 				{
  					MOV = 0;
  					ONLY_FIRST = 0;
  				}
  			}
	
								
			} while (1);
	
}