/*
 *
 * Les "négociations" sont les envois de commande du PC au pic, avec confirmation par le pic.
 * Pour des raisons techniques, les négociations ne peuvent se faire que lorsque les signaux
 * ne sont pas envoyés aux servos, ce qui laisse 10ms de négociations toutes les 20ms.
 * Tous les signaux envoyés au pic sont à envoyer en hexadecimal à 57600 bauds.
 * Les leds sont considérées comme des servos. Leur valeurs de commande possibles sont donc 0x00 (éteinte) ou 0xFF (allumée)
 *
 * Le pic annonce qu'il est opérationnel en envoyant "ok"
 *
 * Le début des négociations est signalé par le pic par l'envoi d'un "a".
 *
 * On peut alors envoyer la commande de start pour signaler qu'on va donner une commande : 0xFF
 * -> Le pic approuve en répondant "o", s'il y a une erreur il renvoie "e" et il faut recommencer l'envoi de la commande.
 * On envoie alors l'adresse du servo, comprise entre 0 et 30 (0x00 à 0x1E).
 * -> Le pic approuve en répondant "o", s'il y a une erreur il renvoie "e" et il faut recommencer l'envoi de la commande.
 * On envoie alors la commande de position, comprise entre 0 et 29 (0x00 à 0xFF), sachant que le neutre du servo est "environ" à 15 (mais tous les servos ne sont pas parfaits)
 * -> Le pic approuve en répondant "o", s'il y a une erreur il renvoie "e" et il faut recommencer l'envoi de la commande.
 * Cette négociation dure en tout environ 100µs
 *
 * On peut recommencer cet envoi autant de fois que nécessaire jusqu'au signal de fin des négociations
 *
 * La fin des négociations est signalée par le pic par l'envoi d'un "b". (18ms après l'envoi du début)
 *
 */

#include <18F452.h>                              // Inclusion du Header correspondant au µC utilisé
#fuses HS,NOWDT,NOPROTECT,NOBROWNOUT,NOLVP       // Paramétrage des fusibles du microcontrôleur
#use delay(clock=10000000)                       // PIC à 10MHz
#use rs232(baud=57600, xmit=PIN_C6,rcv=PIN_C7)   // connexion série sur RC6 et RC7 à 57600 bauds

//******************************
// Defines
//******************************

#define SV01 PIN_B7
#define SV02 PIN_B6
#define SV03 PIN_B5
#define SV04 PIN_B4
#define SV05 PIN_B3
#define SV06 PIN_B2
#define SV07 PIN_B1
#define SV08 PIN_B0
#define SV09 PIN_D7
#define SV10 PIN_D6
#define SV11 PIN_D5
#define SV12 PIN_D4
#define SV13 PIN_C5
#define SV14 PIN_C4
#define SV15 PIN_D3
#define SV16 PIN_D2
#define SV17 PIN_D1
#define SV18 PIN_D0
#define SV19 PIN_C3
#define SV20 PIN_C2
#define SV21 PIN_C1
#define SV22 PIN_C0
#define SV23 PIN_E2
#define SV24 PIN_E1
#define SV25 PIN_E0
#define SV26 PIN_A5
#define SV27 PIN_A4
#define SV28 PIN_A3
#define SV29 PIN_A2
#define SV30 PIN_A1
#define SV31 PIN_A0

//******************************
// Variables globales
//******************************

int etat[32];
int commande[32] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int nb100us;

//******************************
// Timers & Interruptions
//******************************

// ne dois être appelé que pendant la pause de 10ms entre les signaux,
// sinon les signaux seront malformés.
#INT_RDA
void main_rs232rx() {
	int cmd, servo;
	cmd = getc();
	if (cmd == 0xFF) { // FF de start
		printf("o");
		cmd = getc();
		if (cmd < 31) { // adresse du servo
			servo = cmd;
			printf("o");
			cmd = getc();
			commande[servo] = cmd; // commande
			printf("o");
		} else {
			printf("e");
		}
	} else {
		printf("e");
	}
	nb100us++;
}

//******************************
// Programme principal
//******************************

void main () {

	int temps_ecoule = 0; // par pas de 7.8125µs (256*7.8125µ = 2ms)

	enable_interrupts(INT_RDA);
	enable_interrupts(GLOBAL);

	delay_ms(500);
	printf("ok");

	while (true) {

		// t=0ms
		
		nb100us = 0;

		// Allumage-extinction des LED
		if (commande[26] > 0) { output_high(SV27); etat[26] = 1; }
		else { output_low(SV27); etat[26] = 0; }
		if (commande[27] > 0) { output_high(SV28); etat[27] = 1; }
		else { output_low(SV28); etat[27] = 0; }
		if (commande[28] > 0) { output_high(SV29); etat[28] = 1; }
		else { output_low(SV29); etat[28] = 0; }
		if (commande[29] > 0) { output_high(SV30); etat[29] = 1; }
		else { output_low(SV30); etat[29] = 0; }
		if (commande[30] > 0) { output_high(SV31); etat[30] = 1; }
		else { output_low(SV31); etat[30] = 0; }

		// t=~0ms

		// On s'occupe du signal des servos 1 à 7
		output_high(SV01); etat[0] = 1;
		output_high(SV02); etat[1] = 1;
		output_high(SV03); etat[2] = 1;
		output_high(SV04); etat[3] = 1;
		output_high(SV05); etat[4] = 1;
		output_high(SV06); etat[5] = 1;
		output_high(SV07); etat[6] = 1;
		delay_us(500);
		for (temps_ecoule = 0; temps_ecoule <  30; temps_ecoule++) {
			if (etat[0] == 1 && temps_ecoule >= commande[0]) { output_low(SV01); etat[0] = 0; }
			if (etat[1] == 1 && temps_ecoule >= commande[1]) { output_low(SV02); etat[1] = 0; }
			if (etat[2] == 1 && temps_ecoule >= commande[2]) { output_low(SV03); etat[2] = 0; }
			if (etat[3] == 1 && temps_ecoule >= commande[3]) { output_low(SV04); etat[3] = 0; }
			if (etat[4] == 1 && temps_ecoule >= commande[4]) { output_low(SV05); etat[4] = 0; }
			if (etat[5] == 1 && temps_ecoule >= commande[5]) { output_low(SV06); etat[5] = 0; }
			if (etat[6] == 1 && temps_ecoule >= commande[6]) { output_low(SV07); etat[6] = 0; }

			delay_us(45);
		}

		// t=~2ms

		// On s'occupe du signal des servos 8 à 14
		output_high(SV08); etat[7]  = 1;
		output_high(SV09); etat[8]  = 1;
		output_high(SV10); etat[9]  = 1;
		output_high(SV11); etat[10] = 1;
		output_high(SV12); etat[11] = 1;
		output_high(SV13); etat[12] = 1;
		output_high(SV14); etat[13] = 1;
		delay_us(500);
		for (temps_ecoule = 0; temps_ecoule <  30; temps_ecoule++) {
			if (etat[7]  == 1 && temps_ecoule >= commande[7])  { output_low(SV08); etat[7]  = 0; }
			if (etat[8]  == 1 && temps_ecoule >= commande[8])  { output_low(SV09); etat[8]  = 0; }
			if (etat[9]  == 1 && temps_ecoule >= commande[9])  { output_low(SV10); etat[9]  = 0; }
			if (etat[10] == 1 && temps_ecoule >= commande[10]) { output_low(SV11); etat[10] = 0; }
			if (etat[11] == 1 && temps_ecoule >= commande[11]) { output_low(SV12); etat[11] = 0; }
			if (etat[12] == 1 && temps_ecoule >= commande[12]) { output_low(SV13); etat[12] = 0; }
			if (etat[13] == 1 && temps_ecoule >= commande[13]) { output_low(SV14); etat[13] = 0; }

			delay_us(45);
		}

		// t=~4ms

		// On s'occupe du signal des servos 15 à 21
		output_high(SV15); etat[14] = 1;
		output_high(SV16); etat[15] = 1;
		output_high(SV17); etat[16] = 1;
		output_high(SV18); etat[17] = 1;
		output_high(SV19); etat[18] = 1;
		output_high(SV20); etat[19] = 1;
		output_high(SV21); etat[20] = 1;
		delay_us(500);
		for (temps_ecoule = 0; temps_ecoule <  30; temps_ecoule++) {
			if (etat[14] == 1 && temps_ecoule >= commande[14]) { output_low(SV15); etat[14] = 0; }
			if (etat[15] == 1 && temps_ecoule >= commande[15]) { output_low(SV16); etat[15] = 0; }
			if (etat[16] == 1 && temps_ecoule >= commande[16]) { output_low(SV17); etat[16] = 0; }
			if (etat[17] == 1 && temps_ecoule >= commande[17]) { output_low(SV18); etat[17] = 0; }
			if (etat[18] == 1 && temps_ecoule >= commande[18]) { output_low(SV19); etat[18] = 0; }
			if (etat[19] == 1 && temps_ecoule >= commande[19]) { output_low(SV20); etat[19] = 0; }
			if (etat[20] == 1 && temps_ecoule >= commande[20]) { output_low(SV21); etat[20] = 0; }

			delay_us(45);
		}

		// t=~6ms

		// On s'occupe du signal des servos 22 à 26
		output_high(SV22); etat[21] = 1;
		output_high(SV23); etat[22] = 1;
		output_high(SV24); etat[23] = 1;
		output_high(SV25); etat[24] = 1;
		output_high(SV26); etat[25] = 1;
		delay_us(500);
		for (temps_ecoule = 0; temps_ecoule <  30; temps_ecoule++) {
			if (etat[21] == 1 && temps_ecoule >= commande[21]) { output_low(SV22); etat[21] = 0; }
			if (etat[22] == 1 && temps_ecoule >= commande[22]) { output_low(SV23); etat[22] = 0; }
			if (etat[23] == 1 && temps_ecoule >= commande[23]) { output_low(SV24); etat[23] = 0; }
			if (etat[24] == 1 && temps_ecoule >= commande[24]) { output_low(SV25); etat[24] = 0; }
			if (etat[25] == 1 && temps_ecoule >= commande[25]) { output_low(SV26); etat[25] = 0; }

			delay_us(45);
		}

		// t=~8ms

		printf("a"); // début des négociations
		// Ici, on attends environ 10ms, mais il faut prendre en compte que les envois série
		// prennent du temps donc il faut enlevé autant de fois 100µs qu'il y a eu de
		// transfert série
		for (; nb100us < 100; nb100us++)
			delay_us(100);
		printf("b"); // fin des négociations

		// t=~20ms
	}
}