PUBLIC void AppColdStart(void) {
	/* Disable watchdog if enabled by default */
#ifdef WATCHDOG_ENABLED
	//vAHI_WatchdogStop(); // By default enabled with max timeout 16392 ms
#endif

	vCoordinator_Init();

	while (1) {
		vProcessEventQueues();
		
		switch (sCoordData.sSystem.eState) {
		case E_STATE_INIT:
			sCoordData.sSystem.u8Channel = CHANNEL_MIN;
			sCoordData.sSystem.eState = E_STATE_START_ENERGY_SCAN;
			break;
		case E_STATE_START_ENERGY_SCAN:
			PRINT("Energy scan\n");
			vStartEnergyScan();
			sCoordData.sSystem.eState = E_STATE_ENERGY_SCANNING;
			break;
		case E_STATE_ENERGY_SCANNING:
			break;
		case E_STATE_DISASSOCIATE_DEVICES:
        	vCheckNodeDisassociation();

        	if(sCoordData.sNode.u8AssociatedNodes == 0)
        	{
        		PRINT("Start coordinator again\n");
        		sCoordData.sSystem.eState = E_STATE_START_COORDINATOR;
        	}
        	break;

		case E_STATE_START_COORDINATOR:
			vStartCoordinator();
			PRINT("Network started\n");
			sCoordData.sSystem.eState = E_STATE_RUNNING_IDLE;
			break;
		case E_STATE_RUNNING_IDLE:
			break;
		}
	}
}

PUBLIC void vCoordinator_Init(void) {
	memset(&sCoordData, 0, sizeof(tsCoordData));

	uint32 *pu32Mac;

	sCoordData.sSystem.sConfig.u16PanID = PAN_ID;
	sCoordData.sSystem.eState = E_STATE_INIT;

	/* Initialize stack and hardware interfaces. We aren't using callbacks at all, just monitoring the upward queues in a loop */
	(void) u32AHI_Init();
	(void) u32AppQApiInit(NULL, NULL, NULL);

	pvMac = pvAppApiGetMacHandle();
	psPib = MAC_psPibGetHandle(pvMac);

	/* Set Pan ID and short address in PIB (also sets match registers in hardware) */
	MAC_vPibSetPanId(pvMac, sCoordData.sSystem.sConfig.u16PanID);
	MAC_vPibSetShortAddr(pvMac, COORD_ADDR);

	/* Fill in mac and short address */
	sCoordData.sSystem.u16ShortAddr = COORD_ADDR;
	pu32Mac = pvAppApiGetMacAddrLocation();
    sCoordData.sSystem.u32MacHi = pu32Mac[0];
    sCoordData.sSystem.u32MacLo = pu32Mac[1];

	/* Allow nodes to associate */
	psPib->bAssociationPermit = TRUE;
	psPib->u8BeaconPayloadLength = MAC_MAX_BEACON_PAYLOAD_LEN;

	/* Enable receiver to be on when idle */
	MAC_vPibSetRxOnWhenIdle(pvMac, TRUE, FALSE);
}

void vProcessEventQueues(void)
{
	MAC_MlmeDcfmInd_s *psMlmeInd;
	MAC_McpsDcfmInd_s *psMcpsInd;

	/* Check for anything on the MCPS upward queue */
	do {
		psMcpsInd = psAppQApiReadMcpsInd();
		if (psMcpsInd != NULL)
		{
			vProcessIncomingData(psMcpsInd);
			vAppQApiReturnMcpsIndBuffer(psMcpsInd);
		}
	} while (psMcpsInd != NULL);

	/* Check for anything on the MLME upward queue */
	do {
		psMlmeInd = psAppQApiReadMlmeInd();
		if (psMlmeInd != NULL)
		{
			vProcessIncomingMlme(psMlmeInd);
			vAppQApiReturnMlmeIndBuffer(psMlmeInd);
		}
	} while (psMlmeInd != NULL);
}

void vProcessIncomingMlme(MAC_MlmeDcfmInd_s *psMlmeInd)
{
	switch (psMlmeInd->u8Type)
	{
	case MAC_MLME_IND_ASSOCIATE:
		/* Only allow nodes to associate if network has been started */
		if (sCoordData.sSystem.eState >= E_STATE_RUNNING_IDLE)
		{
			vHandleNodeAssociation(psMlmeInd);
		}
		break;
	case MAC_MLME_DCFM_SCAN:
		if (psMlmeInd->uParam.sDcfmScan.u8Status == MAC_ENUM_SUCCESS)
		{
			if (psMlmeInd->uParam.sDcfmScan.u8ScanType == MAC_MLME_SCAN_TYPE_ENERGY_DETECT)
			{
				if (sCoordData.sSystem.eState == E_STATE_ENERGY_SCANNING)
				{
					vHandleEnergyScanResponse(psMlmeInd);
					sCoordData.sSystem.eState = E_STATE_START_COORDINATOR;
				}
			}
		}
		break;
	case MAC_MLME_IND_DISASSOCIATE:
		break;
	case MAC_MLME_DCFM_DISASSOCIATE:
		break;
	case MAC_MLME_IND_ORPHAN:
		if(sCoordData.sSystem.eState >= E_STATE_RUNNING_IDLE)
		{
			vHandleNodeOrphaning(psMlmeInd);
		}
		break;
	case MAC_MLME_IND_COMM_STATUS:
		/* callback from MAC to NWK as result of communication with other node by previous primitive (orphan rsp or associate rsp) */
    	switch(psMlmeInd->uParam.sIndCommStatus.u8Status)
    	{
			case MAC_ENUM_SUCCESS:
				PRINT("Comm success\n");
				break;
			case MAC_ENUM_TRANSACTION_EXPIRED:
				PRINT("Comm transaction expired\n");
				break;
			case MAC_ENUM_TRANSACTION_OVERFLOW:
				PRINT("Comm transaction overflow\n");
				break;
			case MAC_ENUM_NO_ACK:
				PRINT("No ack\n");
				break;
			default:
				PRINT("Com status: %d\n", psMlmeInd->uParam.sIndCommStatus.u8Status);
    	}
		break;
	default:
		break;
	}
}

void vStartEnergyScan(void) {
	/* Structures used to hold data for MLME request and response */
	MAC_MlmeReqRsp_s sMlmeReqRsp;
	MAC_MlmeSyncCfm_s sMlmeSyncCfm;

	/* Start energy detect scan */
	sMlmeReqRsp.u8Type = MAC_MLME_REQ_SCAN;
	sMlmeReqRsp.u8ParamLength = sizeof(MAC_MlmeReqStart_s);
	sMlmeReqRsp.uParam.sReqScan.u8ScanType = MAC_MLME_SCAN_TYPE_ENERGY_DETECT;
	sMlmeReqRsp.uParam.sReqScan.u32ScanChannels = SCAN_CHANNELS;
	sMlmeReqRsp.uParam.sReqScan.u8ScanDuration = ENERGY_SCAN_DURATION;

	vAppApiMlmeRequest(&sMlmeReqRsp, &sMlmeSyncCfm);
}

void vHandleEnergyScanResponse(MAC_MlmeDcfmInd_s *psMlmeInd)
{
	PRINT("Handle energy response\n");
	uint8 i = 0;
	uint8 u8MinEnergy;

	u8MinEnergy = (psMlmeInd->uParam.sDcfmScan.uList.au8EnergyDetect[0]);

	/* Search list to find quietest channel */
	/* Note High power module can't work in channel 26 */
	while (i < psMlmeInd->uParam.sDcfmScan.u8ResultListSize)
	{
		/* Keep spectrum measurements */
		if ((psMlmeInd->uParam.sDcfmScan.uList.au8EnergyDetect[i]) < u8MinEnergy)
		{
			u8MinEnergy = (psMlmeInd->uParam.sDcfmScan.uList.au8EnergyDetect[i]);
			if (i != (psMlmeInd->uParam.sDcfmScan.u8ResultListSize - 1))
			{
				sCoordData.sSystem.u8Channel = i + CHANNEL_MIN;
			}
		}
		i++;
	}
}

void vStartCoordinator(void)
{
	/* Structures used to hold data for MLME request and response */
	MAC_MlmeReqRsp_s sMlmeReqRsp;
	MAC_MlmeSyncCfm_s sMlmeSyncCfm;

	/* Start Pan */
	sMlmeReqRsp.u8Type = MAC_MLME_REQ_START;
	sMlmeReqRsp.u8ParamLength = sizeof(MAC_MlmeReqStart_s);
	sMlmeReqRsp.uParam.sReqStart.u16PanId = sCoordData.sSystem.sConfig.u16PanID;
	sMlmeReqRsp.uParam.sReqStart.u8Channel = sCoordData.sSystem.u8Channel;
	sMlmeReqRsp.uParam.sReqStart.u8BeaconOrder = 0x0f; /* No beacons */
	sMlmeReqRsp.uParam.sReqStart.u8SuperframeOrder = 0x0f; /* No superframe */
	sMlmeReqRsp.uParam.sReqStart.u8PanCoordinator = TRUE;
	sMlmeReqRsp.uParam.sReqStart.u8BatteryLifeExt = FALSE;
	sMlmeReqRsp.uParam.sReqStart.u8Realignment = FALSE;
	sMlmeReqRsp.uParam.sReqStart.u8SecurityEnable = FALSE;

	vAppApiMlmeRequest(&sMlmeReqRsp, &sMlmeSyncCfm);
}

void vHandleNodeAssociation(MAC_MlmeDcfmInd_s *psMlmeInd)
{
    MAC_MlmeReqRsp_s   sMlmeReqRsp;
    MAC_MlmeSyncCfm_s  sMlmeSyncCfm;
    uint16 u16ShortAddress;

    /* Default to PAN access denied */
    uint8 u8AssocStatus = 2;

    /* Default short address */
    u16ShortAddress = SHORT_BROADCAST_ADDRESS;

    /* Check that the device only wants to use a short address */
    if (psMlmeInd->uParam.sIndAssociate.u8Capability & 0x80)
    {
    	/* We assume that an already known mac address is a new association request from the already connected node (e.g. after reset) */
    	/* We will replace the known node and reassociate the new one */
    	u16ShortAddress = u16Coordinator_FindShortAddress(psMlmeInd->uParam.sIndAssociate.sDeviceAddr.u32H, psMlmeInd->uParam.sIndAssociate.sDeviceAddr.u32L);
    	PRINT("Association request from %x\n",psMlmeInd->uParam.sIndAssociate.sDeviceAddr.u32L);

    	if(!u16ShortAddress)
    	{
    		/* New mac address */
    		if(sCoordData.sNode.u8AssociatedNodes < MAX_NODES)
    		{
    			vNodeAddressAssignment(psMlmeInd->uParam.sIndAssociate.sDeviceAddr);
    			/* Don't forget to update the short address */
    			u16ShortAddress = sCoordData.sNode.asAssocNodes[sCoordData.sNode.u8AssociatedNodes - 1].u16ShortAddr;
				
                /* Assume association succeeded */
                u8AssocStatus = 0;
    		}
    	}
    }

    /* Create association response */
    sMlmeReqRsp.u8Type = MAC_MLME_RSP_ASSOCIATE;
    sMlmeReqRsp.u8ParamLength = sizeof(MAC_MlmeRspAssociate_s);
    memcpy((void *)&sMlmeReqRsp.uParam.sRspAssociate.sDeviceAddr, (void *)&psMlmeInd->uParam.sIndAssociate.sDeviceAddr, MAC_EXT_ADDR_LEN);
    sMlmeReqRsp.uParam.sRspAssociate.u16AssocShortAddr = u16ShortAddress;
    sMlmeReqRsp.uParam.sRspAssociate.u8Status = u8AssocStatus;
    sMlmeReqRsp.uParam.sRspAssociate.u8SecurityEnable = FALSE;

    /* Send association response. There is no confirmation for an association response, hence no need to check */
    vAppApiMlmeRequest(&sMlmeReqRsp, &sMlmeSyncCfm);
}