/**
\brief Request a new (free) packet buffer.

Component throughout the protocol stack can call this function is they want to
get a new packet buffer to start creating a new packet.

\note Once a packet has been allocated, it is up to the creator of the packet
      to free it using the openqueue_freePacketBuffer() function.

\returns A pointer to the queue entry when it could be allocated, or NULL when
         it could not be allocated (buffer full or not synchronized).
*/
OpenQueueEntry_t* openqueue_getFreePacketBuffer(uint8_t creator) {
   uint8_t i;
   INTERRUPT_DECLARATION();
   DISABLE_INTERRUPTS();
   
   // refuse to allocate if we're not in sync
   if (ieee154e_isSynch()==FALSE && creator > COMPONENT_IEEE802154E){
     ENABLE_INTERRUPTS();
     return NULL;
   }
   
   // if you get here, I will try to allocate a buffer for you
   
   // walk through queue and find free entry
   for (i=0;i<QUEUELENGTH;i++) {
      if (openqueue_vars.queue[i].owner==COMPONENT_NULL) {
         openqueue_vars.queue[i].creator=creator;
         openqueue_vars.queue[i].owner=COMPONENT_OPENQUEUE;
         ENABLE_INTERRUPTS(); 
         return &openqueue_vars.queue[i];
      }
   }
   ENABLE_INTERRUPTS();
   return NULL;
}

//Puts the thread with the ID tid to sleep
void CALLING_CONVENTION CX86Scheduler::sleepThread(pid_t tid, size_t count)
{
	if (count == 0)
		return;
	bool islocked = false;
	bool isrunning = true;

	DISABLE_INTERRUPTS();	//Turn off preemption
	m_lock.acquireSpinlock();
	iterator_t itr = m_runningList.findValue((PTHREAD)tid);
	if (!m_runningList.isEntry(itr))
	{
		isrunning = false;
		//Not on the running list, scan the blocked list
		itr = m_blockedList.findValue((PTHREAD)tid);
		if (!m_blockedList.isEntry(itr))
		{
			//Thread does not exist, release lock and return
			m_lock.releaseSpinlock();
			ENABLE_INTERRUPTS();
			return;
		}
	}
	//OK, we found the thread
	PTHREAD thread = (PTHREAD)tid;
	thread->waitcount += count;
	if (isrunning)
	{
		m_runningList.removeAt(itr);
		m_blockedList.insertToTail(thread);
	}
	m_lock.releaseSpinlock();
	ENABLE_INTERRUPTS();
}

// Threads return here and space is freed
void mythread_cleanup()
{
	// Unblock thread blocked by join
	DISABLE_INTERRUPTS();
	int id = running_thread[1]->thread.blocking_id;
	if (id > 0) {
		Node * temp = 0xffffffff;
		temp = lookup_node(running_thread[1]->thread.blocking_id, WAITING); //Blocking ID was not the expected value Camtendo 11/4
		if (temp != 0xffffffff) // not found
		{
			Node * blocked_node = (Node *) malloc(sizeof(Node));
			blocked_node->thread = temp->thread;
			blocked_node->thread.scheduling_status = READY;
			remove_node(temp, WAITING);
			add_node(blocked_node, READY);
		}
	}
	ENABLE_INTERRUPTS();
	alt_printf("COMPLETED.\n");
	DISABLE_INTERRUPTS();
	free(running_thread[1]->thread.context);
	running_thread[1]->thread.scheduling_status = DONE;
	ENABLE_INTERRUPTS();
	while(TRUE);
}

/*
 *	Record a usimple_lock just acquired on
 *	the current processor.
 *
 *	MACH_RT:  Preemption has been disabled by lock
 *	acquisition, so it's safe to use the cpu number
 *	specified by the caller.
 */
void
usl_stack_push(
	usimple_lock_t	l,
	int		mycpu)
{
	spl_t		s;

	if (uslock_stack_enabled == FALSE)
		return;

	DISABLE_INTERRUPTS(s);
	assert(uslock_stack_index[mycpu] >= 0);
	assert(uslock_stack_index[mycpu] < USLOCK_STACK_DEPTH);
	if (uslock_stack_index[mycpu] >= USLOCK_STACK_DEPTH) {
		printf("usl_stack_push (cpu 0x%x):  too many locks (%d)",
		       mycpu, uslock_stack_index[mycpu]);
		printf(" disabling stacks\n");
		uslock_stack_enabled = FALSE;
		ENABLE_INTERRUPTS(s);
		return;
	}
	uslock_stack[mycpu][uslock_stack_index[mycpu]] = l;
	uslock_stack_index[mycpu]++;
	ENABLE_INTERRUPTS(s);
}

void main(){

	OUTPUT_LOW(LCD_RW); //Che do ghi 
	LCD_Init();        //Khoi tao LCD

	LCD_PutCmd(0x01);  //Xoa man hinh

	ENABLE_INTERRUPTS(INT_TIMER0); //Kich hoat ngat ngoai
	SETUP_TIMER_0(RTCC_INTERNAL|RTCC_DIV_32); //Xung kich noi va chia truoc 32
	ENABLE_INTERRUPTS(GLOBAL); //Cho phep ngat toan cuc
	SET_TIMER0(100); //Bat dau dem tu 100, khi tran Timer0 duoc 1ms

	while (True){ //Duy tri hoat dong cua vi dieu khien

		if (dem > N_max){
			dem = 0;
			LCD_PutCmd(0x01);
		}

		LCD_SetPosition(0x00); //Cot 1 dong 1
		LCD_PutChar("Dem so:");
		LCD_SetPosition(0x07); //Cot 8 dong 1
		printf(LCD_PutChar,"%lu",dem);

	}
}

 OpenQueueEntry_t* openqueue_macGetDataPacket(open_addr_t* toNeighbor) {
   uint8_t i;
   INTERRUPT_DECLARATION();
   DISABLE_INTERRUPTS();
   if (toNeighbor->type==ADDR_64B) {
      // a neighbor is specified, look for a packet unicast to that neigbhbor
      for (i=0;i<QUEUELENGTH;i++) {
         if (openqueue_vars.queue[i].owner==COMPONENT_RES_TO_IEEE802154E &&
            packetfunctions_sameAddress(toNeighbor,&openqueue_vars.queue[i].l2_nextORpreviousHop)) {
            ENABLE_INTERRUPTS();
            return &openqueue_vars.queue[i];
         }
      }
   } else if (toNeighbor->type==ADDR_ANYCAST) {
      // anycast case: look for a packet which is either not created by RES
      // or an KA (created by RES, but not broadcast)
      for (i=0;i<QUEUELENGTH;i++) {
         if (openqueue_vars.queue[i].owner==COMPONENT_RES_TO_IEEE802154E &&
             ( openqueue_vars.queue[i].creator!=COMPONENT_RES ||
                (
                   openqueue_vars.queue[i].creator==COMPONENT_RES &&
                   packetfunctions_isBroadcastMulticast(&(openqueue_vars.queue[i].l2_nextORpreviousHop))==FALSE
                )
             )
            ) {
            ENABLE_INTERRUPTS();
            return &openqueue_vars.queue[i];
         }
      }
   }
   ENABLE_INTERRUPTS();
   return NULL;
}

void process_reorg_encrypt_restart(void)
{
	intrpt_state_t	prev_intrpt_state;
	enc_info_t	*encr_ptr;
	int		gtmcrypt_errno;
	gd_segment	*seg;
	sgmnt_addrs	*csa;

	csa = reorg_encrypt_restart_csa;
	assert(NULL != csa);	/* caller should have ensured this */
	/* Opening handles for encryption is a heavyweight operation. Caller should have ensured we are not in crit for
	 * any region when the new key handles are opened for any one region. Assert that.
	 */
	assert(0 == have_crit(CRIT_HAVE_ANY_REG));
	DEFER_INTERRUPTS(INTRPT_IN_CRYPT_RECONFIG, prev_intrpt_state);
	encr_ptr = csa->encr_ptr;
	assert(NULL != encr_ptr);
	DBG_RECORD_CRYPT_RECEIVE(csa->hdr, csa, csa->nl, process_id, encr_ptr);
	seg = csa->region->dyn.addr;
	INIT_DB_OR_JNL_ENCRYPTION(csa, encr_ptr, seg->fname_len, seg->fname, gtmcrypt_errno);
	if (0 != gtmcrypt_errno)
	{
		ENABLE_INTERRUPTS(INTRPT_IN_CRYPT_RECONFIG, prev_intrpt_state);
		GTMCRYPT_REPORT_ERROR(gtmcrypt_errno, rts_error, seg->fname_len, seg->fname);
	}
	reorg_encrypt_restart_csa = NULL;
	ENABLE_INTERRUPTS(INTRPT_IN_CRYPT_RECONFIG, prev_intrpt_state);
}

/*
 *	Eliminate the entry for a usimple_lock
 *	that had been active on the current processor.
 *
 *	MACH_RT:  Preemption has been disabled by lock
 *	acquisition, and we haven't yet actually
 *	released the hardware lock associated with
 *	this usimple_lock, so it's safe to use the
 *	cpu number supplied by the caller.
 */
void
usl_stack_pop(
	usimple_lock_t	l,
	int		mycpu)
{
	unsigned int	i, index;
	spl_t		s;

	if (uslock_stack_enabled == FALSE)
		return;

	DISABLE_INTERRUPTS(s);
	assert(uslock_stack_index[mycpu] > 0);
	assert(uslock_stack_index[mycpu] <= USLOCK_STACK_DEPTH);
	if (uslock_stack_index[mycpu] == 0) {
		printf("usl_stack_pop (cpu 0x%x):  not enough locks (%d)",
		       mycpu, uslock_stack_index[mycpu]);
		printf(" disabling stacks\n");
		uslock_stack_enabled = FALSE;
		ENABLE_INTERRUPTS(s);
		return;
	}
	index = --uslock_stack_index[mycpu];
	for (i = 0; i <= index; ++i) {
		if (uslock_stack[mycpu][i] == l) {
			if (i != index)
				uslock_stack[mycpu][i] =
					uslock_stack[mycpu][index];
			ENABLE_INTERRUPTS(s);
			return;
		}
	}
	ENABLE_INTERRUPTS(s);
	panic("usl_stack_pop:  can't find usimple_lock 0x%x", l);
}

void openserial_startInput() {
   INTERRUPT_DECLARATION();
   
   if (openserial_vars.inputBufFill>0) {
      openserial_printError(COMPONENT_OPENSERIAL,ERR_INPUTBUFFER_LENGTH,
                            (errorparameter_t)openserial_vars.inputBufFill,
                            (errorparameter_t)0);
      DISABLE_INTERRUPTS();
      openserial_vars.inputBufFill=0;
      ENABLE_INTERRUPTS();
   }
   
   uart_clearTxInterrupts();
   uart_clearRxInterrupts();      // clear possible pending interrupts
   uart_enableInterrupts();       // Enable USCI_A1 TX & RX interrupt
   
   DISABLE_INTERRUPTS();
   openserial_vars.busyReceiving  = FALSE;
   openserial_vars.mode           = MODE_INPUT;
   openserial_vars.reqFrameIdx    = 0;
#ifdef FASTSIM
   uart_writeBufferByLen_FASTSIM(
      openserial_vars.reqFrame,
      sizeof(openserial_vars.reqFrame)
   );
   openserial_vars.reqFrameIdx = sizeof(openserial_vars.reqFrame);
#else
   uart_writeByte(openserial_vars.reqFrame[openserial_vars.reqFrameIdx]);
#endif
   ENABLE_INTERRUPTS();
}

owerror_t idmanager_setMyID(open_addr_t* newID) {
   INTERRUPT_DECLARATION();
   DISABLE_INTERRUPTS();
   switch (newID->type) {
     case ADDR_16B:
        memcpy(&idmanager_vars.my16bID,newID,sizeof(open_addr_t));
        break;
     case ADDR_64B:
        memcpy(&idmanager_vars.my64bID,newID,sizeof(open_addr_t));
        break;
     case ADDR_PANID:
        memcpy(&idmanager_vars.myPANID,newID,sizeof(open_addr_t));
        break;
     case ADDR_PREFIX:
        memcpy(&idmanager_vars.myPrefix,newID,sizeof(open_addr_t));
        break;
     case ADDR_128B:
        //don't set 128b, but rather prefix and 64b
     default:
        openserial_printCritical(COMPONENT_IDMANAGER,ERR_WRONG_ADDR_TYPE,
              (errorparameter_t)newID->type,
              (errorparameter_t)1);
        ENABLE_INTERRUPTS();
        return E_FAIL;
   }
   ENABLE_INTERRUPTS();
   return E_SUCCESS;
}

void main(){
    float distance = 0;
    
    OUTPUT_LOW(LCD_RW);
    LCD_Init();
    LCD_PutCmd(0x01);
    
    SETUP_TIMER_1(T1_INTERNAL|T1_DIV_BY_4);
    SETUP_CCP1(CCP_CAPTURE_RE);
    
    ENABLE_INTERRUPTS(INT_CCP1);
    ENABLE_INTERRUPTS(GLOBAL);
    
    
    while (True){
        Trigger();
        
        while (echo == 0){
            ;
        }
        
        distance = value*0.8/58;
        LCD_PutCmd(0x01);
        LCD_SetPosition(0x00);
        LCD_PutChar("Distance:");
        LCD_SetPosition(0x40);
        printf(LCD_PutChar,"%.2fcm",distance);
        delay_ms(1000);
    }
}

uint8_t openserial_getInputBuffer(uint8_t* bufferToWrite, uint8_t maxNumBytes) {
    uint8_t numBytesWritten;
    uint8_t inputBufFillLevel;
    INTERRUPT_DECLARATION();

    //<<<<<<<<<<<<<<<<<<<<<<<
    DISABLE_INTERRUPTS();
    inputBufFillLevel = openserial_vars.inputBufFillLevel;
    ENABLE_INTERRUPTS();
    //>>>>>>>>>>>>>>>>>>>>>>>

    if (maxNumBytes<inputBufFillLevel-1) {
        openserial_printError(
            COMPONENT_OPENSERIAL,
            ERR_GETDATA_ASKS_TOO_FEW_BYTES,
            (errorparameter_t)maxNumBytes,
            (errorparameter_t)inputBufFillLevel-1
        );
        numBytesWritten = 0;
    } else {
        numBytesWritten = inputBufFillLevel-1;
        //<<<<<<<<<<<<<<<<<<<<<<<
        DISABLE_INTERRUPTS();
        memcpy(bufferToWrite,&(openserial_vars.inputBuf[1]),numBytesWritten);
        ENABLE_INTERRUPTS();
        //>>>>>>>>>>>>>>>>>>>>>>>
    }

    return numBytesWritten;
}

/**
 * Send the given byte to the USART. It is added to the transmit buffer, and asynchronously
 * transmitted.
 *
 * @param c     Byte to write out on the serial port
 */
void serPutByte(BYTE c) {
    //Check if buffer is full.
    #ifdef SER_WAIT_FOR_TXBUF
    //Wait until a byte is transmitted by the interrupt routine and buffer has place again.
    while (busIsTxBufFull(BUSID)) {
        FAST_USER_PROCESS();
    }
    #else
    if (busIsTxBufFull(BUSID)) {
        serStat |= SER1_TXBUF_OVERRUN;
        return;
    }
    #endif

    //Enter critical section
    DISBALE_INTERRUPTS();

    //If we are not currently TXing, the TX buffer will be empty. No need to transmit via buffer,
    //just write direct to TXREG
    if ( !PIE1_TXIE)
    {
        TXREG = c;      //Send byte
        PIE1_TXIE = 1;  //Indicate that we are currently TXing
        ENABLE_INTERRUPTS();
    }
    //We are currently TXing. This means that the TXIF will soon be set after the current byte
    //has been transmitted, and the TX buffer will be checked for any unsent data. Add current
    //byte to TX buffer.
    else {
        //Add byte to TX buffer, and update buffer pointers
        busPutByteTxBuf(BUFID, c);
        
        ENABLE_INTERRUPTS();
    }
}

void main(){
   TRISE = 0x00; //Chan OUTPUT

   ENABLE_INTERRUPTS(INT_TIMER0); //Kich hoat ngat tran Timer0
   SETUP_TIMER_0(RTCC_INTERNAL|RTCC_DIV_32); //Xung kich noi va chia truoc 32
   ENABLE_INTERRUPTS(GLOBAL); //Cho phep ngat toan cuc
   SET_TIMER0(100); //Bat dau dem tu 100, khi tran Timer0 duoc 1ms

   while (True){ //Duy tri hoat dong cua vi dieu khien
      ;    
   }
}

 bool neighbors_isPreferredParent(open_addr_t* address) {
   uint8_t i;
   INTERRUPT_DECLARATION();
   DISABLE_INTERRUPTS();
   for (i=0;i<MAXNUMNEIGHBORS;i++) {
      if (isThisRowMatching(address,i) && neighbors_vars.neighbors[i].parentPreference==MAXPREFERENCE) {
    	  ENABLE_INTERRUPTS();
         return TRUE;
      }
   }
   ENABLE_INTERRUPTS();
   return FALSE;
}

void openserial_stop() {
    uint16_t temp_openserial_input_buffer_fill_level;
    INTERRUPT_DECLARATION();
    DISABLE_INTERRUPTS();
    temp_openserial_input_buffer_fill_level = openserial_vars.input_buffer_fill_level;
    ENABLE_INTERRUPTS();
    uart_disableInterrupts();              // disable USCI_A1 TX & RX interrupt
    DISABLE_INTERRUPTS();
    openserial_vars.mode=MODE_OFF;
    ENABLE_INTERRUPTS();
    if (temp_openserial_input_buffer_fill_level>0) {
        uint8_t temp_openserial_received_command;
        DISABLE_INTERRUPTS();
        temp_openserial_received_command = openserial_vars.received_command;
        ENABLE_INTERRUPTS();
        switch (temp_openserial_received_command) {
        case 'R': //Trigger IDManager about isRoot
            idmanager_triggerAboutRoot();
            break;
        case 'B': //Trigger IDManager about isBridge
            idmanager_triggerAboutBridge();
            break;
        case 'T': //Trigger TCPInject
            tcpinject_trigger();
            break;
        case 'U': //Trigger UDPInject
            udpinject_trigger();
            break;
        case 'E': //Trigger ICMPv6Echo
            icmpv6echo_trigger();
            break;
        case 'O': //Trigger ICMPv6Router
            icmpv6router_trigger();
            break;
        case 'P': //Trigger ICMPv6RPL
            icmpv6rpl_trigger();
            break;
        case 'D': //Trigger OpenBridge (called only by moteProbe)
            openbridge_trigger();
            break;
        default:
            openserial_printError(COMPONENT_OPENSERIAL,ERR_UNSUPPORTED_COMMAND,
                                  (errorparameter_t)temp_openserial_received_command,
                                  (errorparameter_t)0);
            DISABLE_INTERRUPTS();
            openserial_vars.input_buffer_fill_level = 0;
            ENABLE_INTERRUPTS();
            break;
        }
    }
}

/* Unwind a trigger level, pop all the associated mv_stents and dispense with the base frame.
 * Note that this unwind restores the condition handler stack pointer and correct gtm_trigger_depth value in
 * order to maintain these values properly in the event of a major unwind. This routine is THE routine to use to unwind
 * trigger base frames in all cases due to the cleanups it takes care of.
 */
void gtm_trigger_fini(boolean_t forced_unwind, boolean_t fromzgoto)
{
	if (0 == (frame_pointer->type & SFT_TRIGR))
		GTMASSERT;		/* Would normally be an assert but potential frame stack damage so severe
					   and resulting debug difficulty that we GTMASSERT instead. */
	/* Unwind the trigger base frame */
	op_unwind();
	/* restore frame_pointer stored at msp (see base_frame.c) */
	frame_pointer = *(stack_frame**)msp;
	msp += SIZEOF(stack_frame *);		/* Remove frame save pointer from stack */
	if (!forced_unwind)
	{	/* Remove the "do not return to me" flag only on non-error unwinds. Note this flag may have already been
		 * turned off by an earlier tp_restart if this is not an implicit_tstart situation.
		 */
		assert(!tp_pointer->implicit_tstart || (SFF_TRIGR_CALLD & frame_pointer->flags));
		frame_pointer->flags &= SFF_TRIGR_CALLD_OFF;
		DBGTRIGR((stderr, "gtm_trigger_fini: turning off SFF_TRIGR_CALLD (2) in frame 0x"lvaddr"\n", frame_pointer));
	} else
	{	/* Error unwind, make sure certain cleanups are done */
#		ifdef DEBUG
		assert(!dollar_tlevel || (tstart_trigger_depth <= gtm_trigger_depth));
		if (tstart_trigger_depth == gtm_trigger_depth) /* Unwinding gvcst_put() so get rid of flag it potentially set */
			donot_INVOKE_MUMTSTART = FALSE;
#		endif
		if (tp_pointer)
		{	/* This TP transaction can never be allowed to commit if this is the first trigger
			 * (see comment in tp_frame.h against "cannot_commit" field for details).
			 */
			if ((0 == gtm_trigger_depth) && !fromzgoto)
			{
				DBGTRIGR((stderr, "gtm_trigger: cannot_commit flag set to TRUE\n"))
				tp_pointer->cannot_commit = TRUE;
			}
			if ((tp_pointer->fp == frame_pointer) && tp_pointer->implicit_tstart)
				OP_TROLLBACK(-1); /* We just unrolled the implicitly started TSTART so unroll what it did */
		}
	}
	DBGTRIGR((stderr, "gtm_trigger: POP: frame_pointer 0x%016lx  ctxt value: 0x%016lx\n", frame_pointer, ctxt));
	/* Re-allow interruptions now that our base frame is gone */
	if (forced_unwind)
	{	/* Since we are being force-unwound, we don't know the state of things except that it it should be either
		 * the state we set it to or the ok-to-interrupt state. Assert that and if we are changing the state,
		 * be sure to run the deferred handler.
		 */
		assert((INTRPT_IN_TRIGGER_NOMANS_LAND == intrpt_ok_state) || (INTRPT_OK_TO_INTERRUPT == intrpt_ok_state));
		ENABLE_INTERRUPTS(intrpt_ok_state);
	} else
	{	/* Normal unwind should be ok with this macro */
		ENABLE_INTERRUPTS(INTRPT_IN_TRIGGER_NOMANS_LAND);
	}
}

 OpenQueueEntry_t* openqueue_resGetReceivedPacket() {
   uint8_t i;
   INTERRUPT_DECLARATION();
   DISABLE_INTERRUPTS();
   for (i=0;i<QUEUELENGTH;i++) {
      if (openqueue_vars.queue[i].owner==COMPONENT_IEEE802154E_TO_RES &&
          openqueue_vars.queue[i].creator==COMPONENT_IEEE802154E) {
    	  ENABLE_INTERRUPTS();
    	  return &openqueue_vars.queue[i];
      }
   }
   ENABLE_INTERRUPTS();
   return NULL;
}

OpenQueueEntry_t* openqueue_macGetEBPacket() {
   uint8_t i;
   INTERRUPT_DECLARATION();
   DISABLE_INTERRUPTS();
   for (i=0;i<QUEUELENGTH;i++) {
      if (openqueue_vars.queue[i].owner==COMPONENT_SIXTOP_TO_IEEE802154E &&
          openqueue_vars.queue[i].creator==COMPONENT_SIXTOP) {
         ENABLE_INTERRUPTS();
         return &openqueue_vars.queue[i];
      }
   }
   ENABLE_INTERRUPTS();
   return NULL;
}

void main(){
    TRISB = 0XFF; //Chan INPUT
    TRISE = 0x00; //Chan OUTPUT

    PORT_B_PULLUPS(0x01); //Noi dien tro len nguon
    PORTE = 0x00; //Set gia tri ban dau la muc 0

    ENABLE_INTERRUPTS(INT_EXT); //Kich hoat ngat ngoai
    EXT_INT_EDGE(H_TO_L); //Chon canh ngat, cao xuong thap
    ENABLE_INTERRUPTS(GLOBAL); //Cho phep ngat toan cuc

    while (True){ //Duy tri hoat dong cua vi dieu khien
        ;
    }
}