#include "canfestival.h"

#include "Master.h"
#include "Slave.h"
#include "TestMasterSlave.h"


static UNS32 OnMasterMap1Update(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex)
{
	eprintf("OnSlaveMap1Update:%d\n", SlaveMap1);
	return 0;
}

s_BOARD SlaveBoard = {"0", "125K"};
s_BOARD MasterBoard = {"1", "125K"};

/***************************  INIT  *****************************************/
static void InitNodes(CO_Data* d, UNS32 id)
{
	/****************************** INITIALISATION SLAVE *******************************/
	if(strcmp(SlaveBoard.baudrate, "none")) {
		setNodeId(&TestSlave_Data, 0x02);

		setState(&TestSlave_Data, Initialisation);
	}

	/****************************** INITIALISATION MASTER *******************************/
	if(strcmp(MasterBoard.baudrate, "none")){
 		RegisterSetODentryCallBack(&TestMaster_Data, 0x2000, 0, &OnMasterMap1Update);
		
		// Defining the node Id
		setNodeId(&TestMaster_Data, 0x01);

		setState(&TestMaster_Data, Initialisation);
	}
}

/***************************  EXIT  *****************************************/
void Exit(CO_Data* d, UNS32 id)
{
	masterSendNMTstateChange(&TestMaster_Data, 0x02, NMT_Reset_Node);    

	//Stop master
	setState(&TestMaster_Data, Stopped);
}


int TestMasterSlave_start (void)
{
	TimerInit();

	if(strcmp(SlaveBoard.baudrate, "none")){
		
		TestSlave_Data.heartbeatError = TestSlave_heartbeatError;
		TestSlave_Data.initialisation = TestSlave_initialisation;
		TestSlave_Data.preOperational = TestSlave_preOperational;
		TestSlave_Data.operational = TestSlave_operational;
		TestSlave_Data.stopped = TestSlave_stopped;
		TestSlave_Data.post_sync = TestSlave_post_sync;
		TestSlave_Data.post_TPDO = TestSlave_post_TPDO;
		TestSlave_Data.storeODSubIndex = TestSlave_storeODSubIndex;
		TestSlave_Data.post_emcy = TestSlave_post_emcy;

		if(!canOpen(&SlaveBoard,&TestSlave_Data)){
			eprintf("Cannot open Slave Board (%s,%s)\n",SlaveBoard.busname, SlaveBoard.baudrate);
			return 1;
		}
	}
	if(strcmp(MasterBoard.baudrate, "none")){
		
		TestMaster_Data.heartbeatError = TestMaster_heartbeatError;
		TestMaster_Data.initialisation = TestMaster_initialisation;
		TestMaster_Data.preOperational = TestMaster_preOperational;
		TestMaster_Data.operational = TestMaster_operational;
		TestMaster_Data.stopped = TestMaster_stopped;
		TestMaster_Data.post_sync = TestMaster_post_sync;
		TestMaster_Data.post_TPDO = TestMaster_post_TPDO;
		TestMaster_Data.post_emcy = TestMaster_post_emcy;
		TestMaster_Data.post_SlaveBootup=TestMaster_post_SlaveBootup;
		
		if(!canOpen(&MasterBoard,&TestMaster_Data)){
			eprintf("Cannot open Master Board (%s,%s)\n",MasterBoard.busname, MasterBoard.baudrate);
			if(strcmp(MasterBoard.baudrate, "none")) canClose(&TestMaster_Data);
			return 1;
		}
	}

	// Start timer thread
	StartTimerLoop(&InitNodes);

	return 0;
}

void TestMasterSlave_stop (void)
{
	eprintf("Finishing.\n");
	
	// Stop timer thread
	StopTimerLoop(&Exit);
	
	// Close CAN devices (and can threads)
	if(strcmp(SlaveBoard.baudrate, "none")) canClose(&TestSlave_Data);
	if(strcmp(MasterBoard.baudrate, "none")) canClose(&TestMaster_Data);

	TimerCleanup();
	eprintf("End.\n");
}