/*
 * Carte de gestion de 12 servomoteurs sur bus CAN
 *
 * Christophe Le Lann <contact@totofweb.net>
 * http://www.totofweb.net/robots-projet-66.html
 *
 */

//-----------------------------
// Paramétrages divers
//-----------------------------

#include "../common/can_protocol.h"

//-----------------------------
// Librairies persos
//-----------------------------

#include "periphs/servos.h"
#include "periphs/power.h"
#include "periphs/leds.h"
#include "drivers/can.h"
#include "libs/debug.h"
#include "libs/tprintf.h"

//-----------------------------
// Prototypes
//-----------------------------

// Fonctions
void can_mob_configping(void);
void can_mob_init(void);

//-----------------------------
// Fonctions
//-----------------------------

void can_mob_configping(void) {
	can_mob_select(CAN_MOB_PING);
	can_mob_clear();
	can_mob_setid_std(CAN_ID11_SERVOS_PING);
	can_mob_setmsk_std(CAN_MSK_11B);
	can_mob_setidemsk();
	can_mob_setrtr();
	can_mob_setrtrmsk();
	can_mob_setdlc(0);
	can_mob_irqon(CAN_MOB_PING);
	can_mob_setmode(CAN_MOB_MODE_RX_AUTOREPLY);
}
void can_mob_init(void) {						// Réinitialisation d'un MOb
	can_mob_clear();

	// Configuration du masque d'Identifier
	can_mob_setid_std(CAN_ID11_SERVOS);
	can_mob_setmsk_std(CAN_MSK11_SERVOS);
	can_mob_setidemsk();						// Activation du masque d'IDE : on ne matche que ce qui est en 11 bits

	can_mob_setdlc(8);							// Longueur potentielle des données. On risque d'avoir un DLCW (Data Length Code Warning), mais osf
	can_mob_clearrtrmsk();						// On ne tient pas compte du bit RTR (type quelconque, data ou remote)
	can_mob_setmode(CAN_MOB_MODE_RX_DATA);
}

//-----------------------------
// Fonction principale
//-----------------------------

int main (void) {

	unsigned char mob;
	unsigned char candlc;
	unsigned char candata[8];
	unsigned char i;
	unsigned char uart_servo = 0;
	unsigned char uart_valeur = 0;

	cli();
	power_init();
	servos_init();
	servos_eepromrestore();
	debug_init();
	leds_init();
	can_init(CAN_BAUDRATE, CAN_MODE_ENABLED, NO_INTERRUPTS);

	can_mob_configping();							// Configuration du MOb 0 en autoreply de PING
	for (mob = 1; mob < CAN_NB_MOB; mob++) {
		// Configuration de tous les MObs en réception, tous pour le même message
		// Il en faut plusieurs parce qu'on ne va aller relever la boîte aux lettres
		// que toutes les 20ms, il faut donc que pendant ce temps on puisse réceptionner
		// tous les messages qui se présenteront.

		can_mob_select(mob);
		can_mob_init();
	}
	sei();

	/*
	// Bout de code de test des timings : de 0.4ms à 2.4ms par pas de 0.2ms
	// Avec un masque double au niveau des servos 5 et 6
	// Code volontairement dans le désordre pour tester aussi la fonction d'ordonancement
	servos_enable(0, 0);
	servos_enable(1, 25);
	servos_enable(2, 51);
	servos_enable(11, 255);
	servos_enable(3, 76);
	servos_enable(6, 127);
	servos_enable(4, 102);
	servos_enable(9, 204);
	servos_enable(5, 127);
	servos_enable(7, 153);
	servos_enable(8, 178);
	servos_enable(10, 229);
	*/

	leds_enable(LED_JAUNE);

	debug_msg("\nDemarrage...\n");

	// Chaque boucle a une période de 20ms, par le Timer0
	while (1) {
		servos_waitnewsig();					// On attend le début de la période de 20ms
		servos_genersig();						// Génération des signaux

		leds_disable(LED_VERTE);

		// Interprétation des commandes CAN
		for (mob = 0; mob < CAN_NB_MOB; mob++) {
			if (mob == CAN_MOB_PING) {
				// Réinitialisation, même s'il n'a pas servi
				can_mob_configping();
			} else {
				can_mob_select(mob);
				if (can_mob_state_rxcompleted()) {
					leds_enable(LED_VERTE);		// Led signalant une réception, allumée pendant 20ms
					switch (can_mob_getid_std()) {
						case CAN_ID11_SERVOS_SET: {
							// Le message est censé être constitué de 1 à 4 couples (num, value)
							candlc = can_mob_getdata(candata, 8);
							for (i = 0; (i+1) < candlc; i+=2) {
								if (candata[i] < 12)
									servos_enable(candata[i], candata[i+1]);
							}
							break;
						}
						case CAN_ID11_SERVOS_UNSET: {
							// Le message est censé être constitué de 1 à 8 numéros de servos
							candlc = can_mob_getdata(candata, 8);
							for (i = 0; i < candlc; i++) {
								if (candata[i] < 12)
									servos_disable(candata[i]);
							}
							break;
						}
						case CAN_ID11_SERVOS_POWER: {
							// Activation/désactivation de la partie puissance en fonction du premier paramètre
							candlc = can_mob_getdata(candata, 1);
							if (candlc == 1 && candata[0])
								power_enable();
							else
								power_disable();
							break;
						}
						case CAN_ID11_SERVOS_VIT: {
							// Le message est censé être constitué de 1 à 4 couples (num, value)
							candlc = can_mob_getdata(candata, 8);
							for (i = 0; (i+1) < candlc; i+=2) {
								if (candata[i] < 12)
									servos_setspeed(candata[i], candata[i+1]);
							}
							break;
						}
						case CAN_ID11_SERVOS_SAVE: {
							servos_eepromsave();
							break;
						}
						case CAN_ID11_SERVOS_GPIO: {
							// Le message est censé être constitué de 1 à 4 couples (num, value)
							candlc = can_mob_getdata(candata, 8);
							for (i = 0; (i+1) < candlc; i+=2) {
								if (candata[i] < 12) {
									servos_disable(candata[i]);
									if (candata[i+1]) {
										SRV_00_TO_07_PORT |= (1 << candata[i]);
										SRV_08_TO_11_PORT |= (1 << (candata[i] - 8 + SRV_08_TO_11_SHIFT));
									} else {
										SRV_00_TO_07_PORT &= ~(1 << candata[i]);
										SRV_08_TO_11_PORT &= ~(1 << (candata[i] - 8 + SRV_08_TO_11_SHIFT));
									}
								}
							}
							break;
						}
						case CAN_ID11_SERVOS_PING: {
							// Déjà géré par le cas mob == CAN_MOB_PING
							break;
						}
						default:
							break;
					}
					can_mob_init();					// Réinitialisation du MOb
				}
			}
		}

		// Interprétation des commandes UART
		/*
		 * On peut envoyer :
		 *  - pXX\n où XX est compris entre 00 et 01 pour allumer ou éteindre la puissance
		 *  - sXX\n où XX est compris entre 00 et 11 pour définir le servo qu'on bouge
		 *  - nXX\n où XX est compris entre 00 et 11 pour arrêter le servo
		 *  - XXX\n où XXX est compris entre 000 et 255 pour définir la position
		 */
		while (uart1_RxBuf_GetLength() >= 3 || (uart1_RxBuf_GetLength() > 0 && (uart1_RxBuf_GetAtIndex(0) == '+' || uart1_RxBuf_GetAtIndex(0) == '-'))) {
			leds_enable(LED_VERTE);		// Led signalant une réception, allumée pendant 20ms
			switch (uart1_RxBuf_GetAtIndex(0)) {
				case 'p':
					if (uart1_RxBuf_GetAtIndex(2) == '1') {
						power_enable();
						debug_msg("Power ON\n");
					} else {
						power_disable();
						debug_msg("Power OFF\n");
					}
					uart1_RxBuf_DumpFromFront(3);
					break;
				case 's':
					uart_servo = (uart1_RxBuf_GetAtIndex(1)-'0')*10 + (uart1_RxBuf_GetAtIndex(2)-'0');
					if (uart_servo < 12) {
						tprintf(uart1_putc_buffered, "Servo %u\n", (unsigned int)uart_servo);
					}
					uart1_RxBuf_DumpFromFront(3);
					break;
				case 'n':
					uart_servo = (uart1_RxBuf_GetAtIndex(1)-'0')*10 + (uart1_RxBuf_GetAtIndex(2)-'0');
					if (uart_servo < 12) {
						servos_disable(uart_servo);
						tprintf(uart1_putc_buffered, "Servo %u : arret\n", (unsigned int)uart_servo);
					}
					uart1_RxBuf_DumpFromFront(3);
					break;
				case '+':
					uart_valeur++;
					servos_enable(uart_servo, uart_valeur);
					tprintf(uart1_putc_buffered, "Valeur %u\n", (unsigned int)uart_valeur);
					uart1_RxBuf_DumpFromFront(1);
					break;
				case '-':
					uart_valeur--;
					servos_enable(uart_servo, uart_valeur);
					tprintf(uart1_putc_buffered, "Valeur %u\n", (unsigned int)uart_valeur);
					uart1_RxBuf_DumpFromFront(1);
					break;
				default:
					if (uart1_RxBuf_GetAtIndex(0) >= '0' && uart1_RxBuf_GetAtIndex(0) <= '2' && uart1_RxBuf_GetAtIndex(1) >= '0' && uart1_RxBuf_GetAtIndex(1) <= '9' && uart1_RxBuf_GetAtIndex(0) >= '0' && uart1_RxBuf_GetAtIndex(2) <= '9') {
						uart_valeur = (uart1_RxBuf_GetAtIndex(0)-'0')*100 + (uart1_RxBuf_GetAtIndex(1)-'0')*10 + (uart1_RxBuf_GetAtIndex(2)-'0');
						servos_enable(uart_servo, uart_valeur);
						tprintf(uart1_putc_buffered, "Valeur %u\n", (unsigned int)uart_valeur);
					}
					uart1_RxBuf_DumpFromFront(3);
					break;
			}
		}

		servos_prepare();						// Calcul des nouvelles valeurs
	}

	return 0;
}