#include <avr/io.h>
#include <avr/interrupt.h> 
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h> 
#include <avr/pgmspace.h>
#define F_CPU 9.6e6
#include <util/delay.h>

#define FOSC 8000000 // Clock Speed
#define BAUD 115200UL
#define BaseDeTempsSEC 3

// Berechnungen zur Baudrate:

#define UBRR_VAL ((FOSC+BAUD*8)/(BAUD*16)-1)   // clever runden

#define usart_buffer_max_size 64u
#define usart_command_max_size 10

/***************************************************/

char usart_command[usart_command_max_size + 1] = {0};
char usart0_tx_buffer[usart_buffer_max_size ];
char usart1_tx_buffer[usart_buffer_max_size ];
volatile uint8_t usart_command_size = 0;
volatile uint8_t usart0_tx_buffer_size = 0;
volatile uint8_t usart0_tx_buffer_start = 0;
volatile uint8_t usart1_tx_buffer_size = 0;
volatile uint8_t usart1_tx_buffer_start = 0;
/***************************************************/
/* chaînes de caracteres statiques servant a stocker les echos(reponses du moteur de commande) */
  char machaine1[30];
  char machaine2[30];
  char machaine3[30];
  char machaine4[30];
  char machaine5[30];
  char machaine6[30];
  char machaine7[30];

/****************************************************/
    /* Configuration USART0 and USART1 */
	   /* Set baud rate */
void USART_Init (unsigned int ubrr)
{
  UBRR0H = (unsigned char)(ubrr>>8);
  UBRR0L = (unsigned char) ubrr;
  UBRR1H = (unsigned char)(ubrr>>8);
  UBRR1L = (unsigned char) ubrr;
  
/* Enable receiver and transmitter and set frame format: 8data, 2stop bit */ 

  UCSR0B = (1<<RXEN0)|(1<<TXEN0)|(1<<RXCIE0)|(0<<TXCIE0)|(0<<UCSZ02)|(0<<UDRIE0);
  UCSR0C = (1<<USBS0) | (1<<UCSZ01)|(1<<UCSZ00);

  UCSR1B = (1<<RXEN1)|(1<<TXEN1)|(1<<RXCIE1)|(0<<TXCIE1)|(0<<UCSZ12)|(0<<UDRIE1);
  UCSR1C = (1<<USBS1) |(1<<UCSZ11)|(1<<UCSZ10);
  
} 

/**********************************************************/

/*-- function receive USART0 --*/

unsigned char USART0_Receive (void)
{
    while(!(UCSR0A & (1<<RXC0)) ); // Wait for data to be received 
    
     return UDR0; // Get and return received data from buffer 
}


/*-- function transmit USART0 --*/

void USART0_Transmit (unsigned char data0)
{
  while ( !(UCSR0A & (1<<UDRE0)) ); // Wait for empty transmit buffer  
 
   UDR0 = data0; // Put data into buffer, sends the data 
} 

/*-- function receive USART1 --*/

unsigned char USART1_Receive (void)
{
  while(!(UCSR1A & (1<<RXC1)) );  // Wait for data to be received 
    
   return UDR1; // Get and return received data from buffer 
}

/*-- function transmit USART1 --*/  

void USART1_Transmit (unsigned char data1)
{
  while ( !(UCSR1A & (1<<UDRE1)) ); // Wait for empty transmit buffer  
    
   UDR1 = data1; // Put data into buffer, sends the data  
}

/**********************************************************/
/* Configuration du Timer1 Overflow */
void Timer1_Init(void)
{       // Timer 1 overflow 
  TCCR1B |= (1<< CS12)|(1<<CS11)|(1<< CS10); // Reglage du Prescaler a 1024 
  TIMSK1 |= (0 << TOIE1);            // Timer1 Overflow Interrupt unenable
}

/* Fonction d´interruption du Timer1 Overflow */
/* Cette fonction permet de faire une pause lorsque la
platine aura achevee son deplacement */
ISR(TIMER1_OVF_vect)
{
	static unsigned char counter1 = 0;
	counter1++;
	if (counter1 == BaseDeTempsSEC)
	{
	  counter1 = 0;
      TIMSK1 |= (0 << TOIE1); // Deactivation Timer1 Overflow
	}
}


/**********************************************************/
/*-Aktivierung der Datenübertragung über RS485 von USART1-*/

 void RS485_Init (void)
{
  DDRD = (1<<PD4)|(1<<PD3)|(1<<PD5); //Steuer- und Datenausgang aktivieren
  PORTD = (1<<PD4);	//auf Senden stellen (Aktivierung Datensendung)
}

/**********************************************************/
           /* Configuration des entrees et sorties */

void configuration(void) 
{
  // Pin 0 jusqu´a 7 definient comme entrees pour les boutons poussoirs
  DDRC = 0x00; 

  // Activation Pull-up interne a partir du Pin 0 jusqu´a 7
   PORTC = 0xff; 

  // Definir Pin 0 jusqu´a 7 comme sorties pour les LEDs 
   DDRA = 0xff;
}
/**********************************************************/

/* Ajout dans une file */
void USART0_QueueIn(char c)
{
  int i;
  

  if (usart0_tx_buffer_size < usart_buffer_max_size)
  {
    i = (usart0_tx_buffer_size + usart0_tx_buffer_start) % usart_buffer_max_size;
    usart0_tx_buffer[i] = c;
    ++usart0_tx_buffer_size;
  }
}

/* Sortie d'une file */
/* J'utilise le caractère nul pour dire qu'il ne reste rien dans la file */
/* Ca signifie que la file n'est pas prête à recevoir du binaire */
char USART0_QueueOut(void)
{
  char c;

  if (usart0_tx_buffer_size == 0)
    return 0;
  c = usart0_tx_buffer[usart0_tx_buffer_start];
  --usart0_tx_buffer_size;
  usart0_tx_buffer_start = (usart0_tx_buffer_start + 1) % usart_buffer_max_size;
  return c;
}

/* Ajout dans une file */
void USART1_QueueIn(char c)
{
  int i;
  
  if (usart1_tx_buffer_size < usart_buffer_max_size)
  {
    i = (usart1_tx_buffer_size + usart1_tx_buffer_start) % usart_buffer_max_size;
    usart1_tx_buffer[i] = c;
    ++usart1_tx_buffer_size;
  }
}

/* Sortie d'une file */
/* J'utilise le caractère nul pour dire qu'il ne reste rien dans la file */
/* Ca signifie que la file n'est pas prête à recevoir du binaire */
char USART1_QueueOut(void)
{
  char c;

  if (usart1_tx_buffer_size == 0)
    return 0;
  c = usart1_tx_buffer[usart1_tx_buffer_start];
  --usart1_tx_buffer_size;
  usart1_tx_buffer_start = (usart1_tx_buffer_start + 1) % usart_buffer_max_size;
  return c;
}

/* Envoie une réponse sur l'USART0 via la file de transfert */
static void USART0_Send(const char *s)
{
  int i;
  
  for (i = 0; s[i] != 0; ++i)
    USART0_QueueIn(s[i]);
  if (usart0_tx_buffer_size > 0)
    UCSR0B |= 1 << UDRIE0;
}

/* Envoie une commande sur l'USART1 via la file de transfert */
static void USART1_Send(const char *s)
{
  int i;
  
  for (i = 0; s[i] != 0; ++i)
    USART1_QueueIn(s[i]);
  if (usart1_tx_buffer_size > 0)
    UCSR1B |= 1 << UDRIE1;
}

/* Traitement de la commande */
static void ProcessCommand(void)
{
  int i;
  int x;
  int y;
  char x_moteur[12];
  char y_moteur[12];
  

  /* On extrait le X et le Y, puis on génère les commandes moteurs */
  for (i = 0; i < usart_command_size; ++i)
    if (usart_command[i] == ',')
      break;
  if (i <= 0 || i >= usart_command_size - 1)
  {
    /* On n'a pas trouvé la virgule au milieu de la chaîne -> erreur */
    USART0_Send("\x15");  /* NAK , L´envoi est un echec */
    usart_command_size = 0;
    return;
  }

  /* Je transforme volontairement x et y en int pour te permettre des contrôles ou des calculs */
  usart_command[i] = 0;
  usart_command[usart_command_size] = 0;
  x = atoi(usart_command);
  y = atoi(usart_command + i + 1);
  usart_command_size = 0;
  /* Envoi des commandes moteurs */
  itoa(x, x_moteur, 10);
  itoa(y, y_moteur, 10);
  
  /* Position x_moteur a envoyer */
  USART1_Send("#3s");
  USART1_Send(x_moteur);
  USART1_Send("\r");
  USART1_Receive(); // Wait for response to be received from USART1
  machaine1[30] = USART1_Receive(); // stockage de la reponse du Moteur de commande

  USART1_Send("#3A\r"); // Activation de l´envoi de x_moteur via USART1
  USART1_Receive(); // Wait for response to be received from USART1
  machaine2[30] = USART1_Receive(); // stockage de la reponse du Moteur de commande
  
  /* Position y_moteur a envoyer */   
  USART1_Send("#2s");
  USART1_Send(y_moteur);
  USART1_Send("\r");
  USART1_Receive(); // Wait for response to be received from USART1
  machaine3[30] = USART1_Receive(); // stockage de la reponse du Moteur de commande
   
  USART1_Send("#2A\r"); // Activation de l´envoi de y_moteur via USART1
  USART1_Receive(); // Wait for response to be received from USART1
  machaine4[30] = USART1_Receive(); // stockage de la reponse du Moteur de commande

  USART1_Send("#3C\r"); // Lecture de la position actuelle
  USART1_Receive(); // Wait for response to be received from USART1
  machaine5[30] = USART1_Receive(); // stockage de la reponse du Moteur de commande

  USART1_Send("#2C\r"); // Lecture de la position actuelle
  USART1_Receive(); // Wait for response to be received from USART1
  machaine6[30] = USART1_Receive(); // stockage de la reponse du Moteur de commande

  }

/* La fonction d´interruption de reception du byte */
/* Cette fonction est active lorsque RXCIE0 = 1 */ 
ISR(USART0_RX_vect)
{
  char data;
  data = UDR0;

  if (data == '\r')
  {
    /* Traitement de la commande ("/n X,Y /r") */
     ProcessCommand();
     	  
   }
  else if (data == '\n')
  {
    /* Démarrage d'une nouvelle commande */
    usart_command_size = 0;
	machaine1[0] = '\0'; // Vidage de la chaine
    machaine2[0] = '\0'; // Vidage de la chaine
    machaine3[0] = '\0'; // Vidage de la chaine
    machaine4[0] = '\0'; // Vidage de la chaine
    machaine5[0] = '\0'; // Vidage de la chaine
    machaine6[0] = '\0'; // Vidage de la chaine
   }

  else
  {
    /* Quand on ne reçoit ni \r ni \n, on enregistre le caractère dans la commande */
    if (usart_command_size < usart_command_max_size)
    {
      usart_command[usart_command_size] = data;
      ++usart_command_size;
    }
  }
}

ISR(USART1_RX_vect)
{
 char machainereponse[30] ;
 machainereponse[30] = UDR1;

 switch(machainereponse[30])
 {
   
   case '3sx_moteur\r':
   machainereponse[0] = 0 ; // Vider la chaine
   break;

   case '3A\r':
   machainereponse[0] = 0 ; // Vider la chaine
   break;

   case '2sy_moteur\r':
   machainereponse[0] = 0 ; // Vider la chaine
   break;

   case '2A\r':
   machainereponse[0] = 0 ; // Vider la chaine
   break;

   case '3C\r':
   machainereponse[0] = 0 ; // Vider la chaine
   break;

   case '2C\r':
   machainereponse[0] = 0 ; // Vider la chaine
   TIMSK1 |= (1 << TOIE1); // Activation de la fonction d´interruption Timer1 Overflow
   USART0_Send("\x06"); // ACK --> l´envoi est un succes
   break;

   default:
   break;    

 }
 

} 



/* La fonction d´interruption d´envoi de byte */
/* Cette fonction est active lorsque UDRIE0 = 1 */
ISR(USART0_UDRE_vect)
{
  UDR0 = USART0_QueueOut();
  /* S'il n'y a plus de données à envoyer on arrete l'emission */
  if (usart0_tx_buffer_size == 0)
    UCSR0B &= ~(1 << UDRIE0);
}

/* La fonction d´interruption d´envoi de byte */
/* Cette fonction est active lorsque UDRIE1 = 1 */
ISR(USART1_UDRE_vect)
{
  UDR1 = USART1_QueueOut();
  /* S'il n'y a plus de données à envoyer on arrete l'emission */
  if (usart1_tx_buffer_size == 0)
    UCSR1B &= ~(1 << UDRIE1);
}

void ParametrageMoteur(void)
 { 
   USART1_Send("#2c\r"); // Considerer la position actuelle comme origine du repere sur l´axe x 
   /* Attente reponse moteur de commande via USART1 */
   USART1_Receive(); // Wait for response to be received from USART1 
   machaine7[30] = USART1_Receive();
   machaine7[0] = 0;

   USART1_Send("#3c\r"); // Considerer la position actuelle comme origine du repere sur l´axe y
   /* Attente reponse moteur de commande via USART1 */
   USART1_Receive();// Wait for response to be received from USART1
   machaine7[30] = USART1_Receive();
   machaine7[0] = 0;
   
   USART1_Send("#2p2\r");// Mode de travail: deplacement absolu --> p = 2
   /* Attente reponse moteur de commande via USART1 */
   USART1_Receive();// Wait for response to be received from USART1
   machaine7[30] = USART1_Receive();
   machaine7[0] = 0;

   USART1_Send("#3p2\r");
   /* Attente reponse moteur de commande via USART1*/
   USART1_Receive();// Wait for response to be received from USART1	
   machaine7[30] = USART1_Receive();
   machaine7[0] = 0;      
 }
  

int main (void)
{ 
  USART_Init(UBRR_VAL); // Initialisation de l´USART0 et l´USART1
  Timer1_Init(); // Compteur permettant de faire une pause apres de deplacement de la platine
  RS485_Init(); // Contrôl de l´envoi des positions via l´USART1
  configuration(); // Configuration des entrees et sorties des PORTA, PORTC
/* TODO: Ici il faut paramétrer les moteurs (mode de positionnement etc.) */
  ParametrageMoteur(); // Mode de Positionnement et de travail
  sei(); // Activation des fonctions d´interruption
 
  
  while (1) // Endlosschleife
  {
   	 
  }
  
}