/*
 * Routine to put is in PM0. We also need to do some housekeeping if the stats
 * or the energest module is enabled
 */
static void
enter_pm0(void)
{
  ENERGEST_OFF(ENERGEST_TYPE_CPU);
  ENERGEST_ON(ENERGEST_TYPE_LPM);

  /* We are only interested in IRQ energest while idle or in LPM */
  ENERGEST_IRQ_RESTORE(irq_energest);

  /*
   * After PM0 we don't need to adjust the system clock. Thus, saving the time
   * we enter Deep Sleep is only required if we are keeping stats.
   */
  if(LPM_CONF_STATS) {
    sleep_enter_time = RTIMER_NOW();
  }

  assert_wfi();

  /* We reach here when the interrupt context that woke us up has returned */
  LPM_STATS_ADD(0, RTIMER_NOW() - sleep_enter_time);

  /* Remember IRQ energest for next pass */
  ENERGEST_IRQ_SAVE(irq_energest);

  ENERGEST_ON(ENERGEST_TYPE_CPU);
  ENERGEST_OFF(ENERGEST_TYPE_LPM);
}

/*---------------------------------------------------------------------------*/
static void
show_leds(unsigned char changed)
{
  if(changed & LEDS_GREEN) {
    /* Green did change */
    if((invert ^ leds) & LEDS_GREEN) {
      ENERGEST_ON(ENERGEST_TYPE_LED_GREEN);
    } else {
      ENERGEST_OFF(ENERGEST_TYPE_LED_GREEN);
    }
  }
  if(changed & LEDS_YELLOW) {
    if((invert ^ leds) & LEDS_YELLOW) {
      ENERGEST_ON(ENERGEST_TYPE_LED_YELLOW);
    } else {
      ENERGEST_OFF(ENERGEST_TYPE_LED_YELLOW);
    }
  }
  if(changed & LEDS_RED) {
    if((invert ^ leds) & LEDS_RED) {
      ENERGEST_ON(ENERGEST_TYPE_LED_RED);
    } else {
      ENERGEST_OFF(ENERGEST_TYPE_LED_RED);
    }
  }
  leds_arch_set(leds ^ invert);
}

/*---------------------------------------------------------------------------*/
static int
transmit(unsigned short payload_len)
{
  if(tx_in_progress) {
    return RADIO_TX_COLLISION;
  }
  tx_in_progress = 1;

  /* Energest */
  if(listen_on) {
    ENERGEST_OFF(ENERGEST_TYPE_LISTEN);
  }
  ENERGEST_ON(ENERGEST_TYPE_TRANSMIT);

  /* Transmit and wait */
  vMMAC_StartPhyTransmit(&tx_frame_buffer,
                         E_MMAC_TX_START_NOW |
                         (send_on_cca ? E_MMAC_TX_USE_CCA : E_MMAC_TX_NO_CCA));

  if(poll_mode) {
    BUSYWAIT_UNTIL(u32MMAC_PollInterruptSource(E_MMAC_INT_TX_COMPLETE), MAX_PACKET_DURATION);
  } else {
    if(in_ack_transmission) {
      /* as nested interupts are not possible, the tx flag will never be cleared */
      BUSYWAIT_UNTIL(FALSE, MAX_ACK_DURATION);
    } else {
      /* wait until the tx flag is cleared */
      BUSYWAIT_UNTIL(!tx_in_progress, MAX_PACKET_DURATION);
    }
  }

  /* Energest */
  ENERGEST_OFF(ENERGEST_TYPE_TRANSMIT);
  if(listen_on) {
    ENERGEST_ON(ENERGEST_TYPE_LISTEN);
  }
  tx_in_progress = 0;

  /* Check error code */
  int ret;
  uint32_t tx_error = u32MMAC_GetTxErrors();
  if(tx_error == 0) {
    ret = RADIO_TX_OK;
    RIMESTATS_ADD(acktx);
  } else if(tx_error & E_MMAC_TXSTAT_ABORTED) {
    ret = RADIO_TX_ERR;
    RIMESTATS_ADD(sendingdrop);
  } else if(tx_error & E_MMAC_TXSTAT_CCA_BUSY) {
    ret = RADIO_TX_COLLISION;
    RIMESTATS_ADD(contentiondrop);
  } else if(tx_error & E_MMAC_TXSTAT_NO_ACK) {
    ret = RADIO_TX_NOACK;
    RIMESTATS_ADD(noacktx);
  } else {
    ret = RADIO_TX_ERR;
  }
  return ret;
}

void ST_RadioTransmitCompleteIsrCallback(StStatus status,
                                           u32 txSyncTime,
                                           boolean framePending)
{

  ENERGEST_OFF(ENERGEST_TYPE_TRANSMIT); 
  ENERGEST_ON(ENERGEST_TYPE_LISTEN);
  LED_TX_OFF();
  
  last_tx_status = status;
  
  if(status == ST_SUCCESS || status == ST_PHY_ACK_RECEIVED){
      CLEAN_TXBUF();
  }
  else {

      if(RETRY_CNT_GTZ()){
          // Retransmission
          LED_TX_ON();
          if(ST_RadioTransmit(stm32w_txbuf)==ST_SUCCESS){
              
              ENERGEST_OFF(ENERGEST_TYPE_LISTEN);
              ENERGEST_ON(ENERGEST_TYPE_TRANSMIT);
               
              PRINTF("stm32w: retransmission.\r\n");
              
              DEC_RETRY_CNT();
          }
          else {
              CLEAN_TXBUF();
              LED_TX_OFF();
              PRINTF("stm32w: retransmission failed.\r\n");
          }
      }
      else {
          CLEAN_TXBUF();
      }      
  }
  
  /* Debug outputs. */
  if(status == ST_SUCCESS || status == ST_PHY_ACK_RECEIVED){
      PRINTF("stm32w: return status TX_END\r\n");
  }
  else if (status == ST_MAC_NO_ACK_RECEIVED){
      PRINTF("stm32w: return status TX_END_NOACK\r\n");
  }
  else if (status == ST_PHY_TX_CCA_FAIL){
      PRINTF("stm32w: return status TX_END_CCA_FAIL\r\n");
  }
  else if(status == ST_PHY_TX_UNDERFLOW){
      PRINTF("stm32w: return status TX_END_UNDERFLOW\r\n");
  }
  else {
      PRINTF("stm32w: return status TX_END_INCOMPLETE\r\n");
  }
}

/*---------------------------------------------------------------------------*/
void
m25p16_read(uint8_t * addr, uint8_t * buff, uint8_t buff_len)
{
  uint8_t i;

  select();
  ENERGEST_ON(ENERGEST_TYPE_FLASH_READ);

#if M25P16_READ_FAST
  bit_bang_write(M25P16_I_FAST_READ);
#else
  bit_bang_write(M25P16_I_READ);
#endif

  /* Write the address, MSB in addr[0], bits [7:5] of the MSB: 'don't care' */
  for(i = 0; i < 3; i++) {
    bit_bang_write(addr[i]);
  }

  /* For FAST_READ, send the dummy byte */
#if M25P16_READ_FAST
  bit_bang_write(M25P16_DUMMY_BYTE);
#endif

  for(i = 0; i < buff_len; i++) {
    buff[i] = ~bit_bang_read();
  }
  ENERGEST_OFF(ENERGEST_TYPE_FLASH_READ);
  deselect();
}

void TC1_Handler(void)
{
	volatile uint32_t ul_dummy;
	/* Clear status bit to acknowledge interrupt */
	ul_dummy = tc_get_status(TC0,1);
	
	#if RTIMER_DEBUG
	if ((ul_dummy & TC_SR_CPCS) == TC_SR_CPCS) {
	#endif
		tc_stop(TC0,1);	// Crucial, since the execution may take long time
	#if RTIMER_DEBUG
		printf("Compare event %16x\r\n", ul_dummy);
	#endif
		tc_disable_interrupt(TC0,1,TC_IDR_CPCS);  // Disable the next compare interrupt XXX XXX
		ENERGEST_ON(ENERGEST_TYPE_IRQ);
		rtimer_run_next(); /* Run the posted task. */
		ENERGEST_OFF(ENERGEST_TYPE_IRQ);
		
	#if RTIMER_DEBUG
	}
	else {
		printf("Unknown interrupt.\n\n");
	}
	#endif
	
}

ISR(UART1RX, uart1_rx_interrupt)
{
  uint8_t c;
  ENERGEST_ON(ENERGEST_TYPE_IRQ);

  if(!(URXIFG1 & IFG2)) {
    /* Edge detect if IFG not set? */
    U1TCTL &= ~URXSE; /* Clear the URXS signal */
    U1TCTL |= URXSE;  /* Re-enable URXS - needed here?*/
    rx_in_progress = 1;
    LPM4_EXIT;
  } else {
    rx_in_progress = 0;
    /* Check status register for receive errors. */
    if(URCTL1 & RXERR) {
      c = RXBUF1;   /* Clear error flags by forcing a dummy read. */
    } else {
      c = RXBUF1;
      if(uart1_input_handler != NULL) {
	if(uart1_input_handler(c)) {
	  LPM4_EXIT;
	}
      }
    }
  }

  ENERGEST_OFF(ENERGEST_TYPE_IRQ);
}

/*---------------------------------------------------------------------------*/
int
xmem_pread(void *_p, int size, unsigned long offset)
{
  unsigned char *p = _p;
  const unsigned char *end = p + size;
  int s;
  wait_ready();

  ENERGEST_ON(ENERGEST_TYPE_FLASH_READ);

  s = splhigh();
  SPI_FLASH_ENABLE();

  spi_tx(SPI_FLASH_INS_READ);
  spi_tx(offset >> 16);	/* MSB */
  spi_tx(offset >> 8);
  spi_tx(offset >> 0);	/* LSB */
  
  FASTSPI_CLEAR_RX();
  for(; p < end; p++) {
    unsigned char u;
    FASTSPI_RX(u);
    *p = ~u;
  }

  SPI_FLASH_DISABLE();
  splx(s);

  ENERGEST_OFF(ENERGEST_TYPE_FLASH_READ);

  return size;
}

/*---------------------------------------------------------------------------*/
void
cc26xx_uart_isr(void)
{
  char the_char;
  uint32_t flags;

  ENERGEST_ON(ENERGEST_TYPE_IRQ);

  power_and_clock();

  /* Read out the masked interrupt status */
  flags = ti_lib_uart_int_status(UART0_BASE, true);

  /* Clear all UART interrupt flags */
  ti_lib_uart_int_clear(UART0_BASE, CC26XX_UART_INTERRUPT_ALL);

  if((flags & CC26XX_UART_RX_INTERRUPT_TRIGGERS) != 0) {
    /*
     * If this was a FIFO RX or an RX timeout, read all bytes available in the
     * RX FIFO.
     */
    while(ti_lib_uart_chars_avail(UART0_BASE)) {
      the_char = ti_lib_uart_char_get_non_blocking(UART0_BASE);

      if(input_handler != NULL) {
        input_handler((unsigned char)the_char);
      }
    }
  }

  ENERGEST_OFF(ENERGEST_TYPE_IRQ);
}

interrupt(PORT1_VECTOR) port1_isr (void) {
  ENERGEST_ON(ENERGEST_TYPE_IRQ);
  /* ADXL345_IFG.x goes high when interrupt occurs, use to check what interrupted */
  if ((ADXL345_IFG & ADXL345_INT1_PIN) && !(ADXL345_IFG & BV(CC2420_FIFOP_PIN))){
    /* Check if this should be suppressed or not */
    if(timer_expired(&suppressTimer1)) {
      timer_set(&suppressTimer1, SUPPRESS_TIME_INT1);
      ADXL345_IFG &= ~ADXL345_INT1_PIN;   // clear interrupt flag
      process_poll(&accmeter_process);
      LPM4_EXIT;
    }
  } else if ((ADXL345_IFG & ADXL345_INT2_PIN) && !(ADXL345_IFG & BV(CC2420_FIFOP_PIN))){
    /* Check if this should be suppressed or not */
    if(timer_expired(&suppressTimer2)) {
      timer_set(&suppressTimer2, SUPPRESS_TIME_INT2);
      ADXL345_IFG &= ~ADXL345_INT2_PIN;   // clear interrupt flag
      process_poll(&accmeter_process);
      LPM4_EXIT;
    }
  } else {
    /* CC2420 interrupt */
    if(cc2420_interrupt()) {
      LPM4_EXIT;
    }
  }
  ENERGEST_OFF(ENERGEST_TYPE_IRQ);
}

/*---------------------------------------------------------------------------*/
void
uart_isr(void)
{
  uint16_t mis;

  ENERGEST_ON(ENERGEST_TYPE_IRQ);

  /* Store the current MIS and clear all flags early, except the RTM flag.
   * This will clear itself when we read out the entire FIFO contents */
  mis = (uint16_t)UARTIntStatus(UART0_BASE, true); 
  
  HWREG(UART0_BASE | UART_O_ICR) = 0x0000FFBF;

  if(mis & (UART_INT_RX | UART_INT_RT)) {
    if(input_handler != NULL) {
      input_handler((unsigned char)UARTCharGetNonBlocking(UART0_BASE));
    } else {
      /* To prevent an Overrun Error, we need to flush the FIFO even if we
       * don't have an input_handler. Use mis as a data trash can */
      mis = (uint16_t)UARTCharGetNonBlocking(UART0_BASE);
      }
  } else if(mis & (UART_INT_OE | UART_INT_BE | UART_INT_FE)) {
    /* ISR triggered due to some error condition */
    reset();
  }

  ENERGEST_OFF(ENERGEST_TYPE_IRQ);
}

/*---------------------------------------------------------------------------*/
int
xmem_pread(void *_p, int size, unsigned long offset)
{
  unsigned char *p = _p;
  const unsigned char *end = p + size;
  int s;
  wait_ready();

  ENERGEST_ON(ENERGEST_TYPE_FLASH_READ);

  s = splhigh();
  SPI_FLASH_ENABLE();

  SPI_WRITE_FAST(SPI_FLASH_INS_READ);
  SPI_WRITE_FAST(offset >> 16);	/* MSB */
  SPI_WRITE_FAST(offset >> 8);
  SPI_WRITE_FAST(offset >> 0);	/* LSB */
  SPI_WAITFORTx_ENDED();
  
  SPI_FLUSH();
  for(; p < end; p++) {
    unsigned char u;
    SPI_READ(u);
    *p = ~u;
  }

  SPI_FLASH_DISABLE();
  splx(s);

  ENERGEST_OFF(ENERGEST_TYPE_FLASH_READ);

  return size;
}

/*---------------------------------------------------------------------------*/
void
lpm_exit()
{
  if((REG(SYS_CTRL_PMCTL) & SYS_CTRL_PMCTL_PM3) == SYS_CTRL_PMCTL_PM0) {
    /* We either just exited PM0 or we were not sleeping in the first place.
     * We don't need to do anything clever */
    return;
  }

  LPM_STATS_ADD(REG(SYS_CTRL_PMCTL) & SYS_CTRL_PMCTL_PM3,
                RTIMER_NOW() - sleep_enter_time);

  /* Adjust the system clock, since it was not counting while we were sleeping
   * We need to convert sleep duration from rtimer ticks to clock ticks and
   * this will cost us some accuracy */
  clock_adjust((clock_time_t)
               ((RTIMER_NOW() - sleep_enter_time) / RTIMER_CLOCK_TICK_RATIO));

  /* Restore system clock to the 32 MHz XOSC */
  select_32_mhz_xosc();

  /* Restore PMCTL to PM0 for next pass */
  REG(SYS_CTRL_PMCTL) = SYS_CTRL_PMCTL_PM0;

  /* Remember IRQ energest for next pass */
  ENERGEST_IRQ_SAVE(irq_energest);

  ENERGEST_ON(ENERGEST_TYPE_CPU);
  ENERGEST_OFF(ENERGEST_TYPE_LPM);
}

/*---------------------------------------------------------------------------*/
void
cc26xx_rf_cpe0_isr(void)
{
  ENERGEST_ON(ENERGEST_TYPE_IRQ);

  if(!rf_core_is_accessible()) {
    printf("RF ISR called but RF not ready... PANIC!!\n");
    if(rf_core_power_up() != RF_CORE_CMD_OK) {
      PRINTF("rf_core_power_up() failed\n");
      return;
    }
  }

  ti_lib_int_master_disable();

  if(HWREG(RFC_DBELL_NONBUF_BASE + RFC_DBELL_O_RFCPEIFG) & RX_FRAME_IRQ) {
    process_poll(&rf_core_process);
  }

  if(RF_CORE_DEBUG_CRC) {
    if(HWREG(RFC_DBELL_NONBUF_BASE + RFC_DBELL_O_RFCPEIFG) & RX_NOK_IRQ) {
      rx_nok_isr();
    }
  }

  /* Clear interrupt flags */
  HWREG(RFC_DBELL_NONBUF_BASE + RFC_DBELL_O_RFCPEIFG) = 0x0;
  ti_lib_int_master_enable();

  ENERGEST_OFF(ENERGEST_TYPE_IRQ);
}

/*---------------------------------------------------------------------------*/
void
uart_isr(uint8_t uart)
{
  uint32_t uart_base;
  uint16_t mis;

  ENERGEST_ON(ENERGEST_TYPE_IRQ);

  uart_base = uart_regs[uart].base;

  /* Store the current MIS and clear all flags early, except the RTM flag.
   * This will clear itself when we read out the entire FIFO contents */
  mis = REG(uart_base | UART_MIS) & 0x0000FFFF;

  REG(uart_base | UART_ICR) = 0x0000FFBF;

  if(mis & (UART_MIS_RXMIS | UART_MIS_RTMIS)) {
    while(!(REG(uart_base | UART_FR) & UART_FR_RXFE)) {
      if(input_handler[uart] != NULL) {
        input_handler[uart]((unsigned char)(REG(uart_base | UART_DR) & 0xFF));
      } else {
        /* To prevent an Overrun Error, we need to flush the FIFO even if we
         * don't have an input_handler. Use mis as a data trash can */
        mis = REG(uart_base | UART_DR);
      }
    }
  } else if(mis & (UART_MIS_OEMIS | UART_MIS_BEMIS | UART_MIS_FEMIS)) {
    /* ISR triggered due to some error condition */
    reset(uart_base);
  }

  ENERGEST_OFF(ENERGEST_TYPE_IRQ);
}

/*---------------------------------------------------------------------------*/
void
halTimer1Isr(void)
{
  if(INT_TIM1FLAG & INT_TIMUIF) {
    rtimer_clock_t now, clock_to_wait;
    /* Overflow event. */
    /* PRINTF("O %4x.\r\n", TIM1_CNT); */
    /* printf("OV "); */
    time_msb++;
    now = ((rtimer_clock_t) time_msb << 16) | TIM1_CNT;
    clock_to_wait = next_rtimer_time - now;

    if(clock_to_wait <= 0x10000 && clock_to_wait > 0) { 
      /* We must now set the Timer Compare Register. */
      TIM1_CCR1 = (uint16_t) clock_to_wait;
      INT_TIM1FLAG = INT_TIMCC1IF;
      INT_TIM1CFG |= INT_TIMCC1IF;      /* Compare 1 interrupt enable. */
    }
    INT_TIM1FLAG = INT_TIMUIF;
  } else {
    if(INT_TIM1FLAG & INT_TIMCC1IF) {
      /* Compare event. */
      INT_TIM1CFG &= ~INT_TIMCC1IF;       /* Disable the next compare interrupt */
      PRINTF("\nCompare event %4x\r\n", TIM1_CNT);
      PRINTF("INT_TIM1FLAG %2x\r\n", INT_TIM1FLAG);
      ENERGEST_ON(ENERGEST_TYPE_IRQ);
      rtimer_run_next();
      ENERGEST_OFF(ENERGEST_TYPE_IRQ);
      INT_TIM1FLAG = INT_TIMCC1IF;
    }
  }
}

interrupt(TIMERB1_VECTOR) timerb1 (void) {
  ENERGEST_ON(ENERGEST_TYPE_IRQ);
  if(TBIV == TBIV_CCR1) {
  	sw_ext++;
  }
  ENERGEST_OFF(ENERGEST_TYPE_IRQ);
}

/*---------------------------------------------------------------------------*/
ISR(UNMI, unmi_interrupt)       /* user non-maskable interrupts */
{    
  ENERGEST_ON(ENERGEST_TYPE_CPU);
    
  PIN_SET(LED_ERROR);           /* use PIN_SET instead of LED_ON */
  switch (SYSUNIV) {
    case SYSUNIV_NMIIFG:        /* non-maskable interrupt */
      while(1);
      break;
    case SYSUNIV_OFIFG:         /* oscillator fault */
      WAIT_FOR_OSC();           /* try to clear the fault flag */
      break;
    case SYSUNIV_ACCVIFG:       /* Flash Memory Access Violation */
      /* will be accidentially triggered after flash programming when using
       * a voltage between 2.4 and 2.6V (errata SYS12) */
      while(1);
      break;
    case SYSUNIV_SYSBERRIV:
      while(1);
      break;
    default:
      while(1);
      break;
  }
  PIN_CLR(LED_ERROR);
  
  ENERGEST_OFF(ENERGEST_TYPE_CPU);
}

void stm32_flash_read(uint32_t address, void * data, uint32_t length)
{
  uint8_t	* pdata = (uint8_t *)address;
  ENERGEST_ON(ENERGEST_TYPE_FLASH_READ);
  memcpy(data, pdata, length);
  ENERGEST_OFF(ENERGEST_TYPE_FLASH_READ);
}

/*---------------------------------------------------------------------------*/
static void
abc_recv ( struct abc_conn *c )
{
	mst_t *rcv_msg;
	uint8_t decrypted[MSG_LEN];
	int j, i;
        rcv_msg = packetbuf_dataptr();
	printf("\nmessage received\n");
	for(i = 0; i < rcv_msg->len; i++)
	{
		printf ( "%u:", rcv_msg->dat[i]);
	}
	printf("\n");
        ENERGEST_OFF ( ENERGEST_TYPE_CPU );
        ENERGEST_OFF ( ENERGEST_TYPE_LPM );
        ENERGEST_OFF ( ENERGEST_TYPE_TRANSMIT );
        ENERGEST_OFF ( ENERGEST_TYPE_LISTEN );
        ENERGEST_ON ( ENERGEST_TYPE_CPU );
        ENERGEST_ON ( ENERGEST_TYPE_LPM );
        ENERGEST_ON ( ENERGEST_TYPE_TRANSMIT );
        ENERGEST_ON ( ENERGEST_TYPE_LISTEN );

        last.lpm = energest_type_time ( ENERGEST_TYPE_LPM );
        last.transmit = energest_type_time ( ENERGEST_TYPE_TRANSMIT );
        last.listen = energest_type_time ( ENERGEST_TYPE_LISTEN );
        last.cpu = energest_type_time ( ENERGEST_TYPE_CPU );
	
	j = ecc_decrypt(decrypted, MSG_LEN, rcv_msg->dat, rcv_msg->len, prKey_alice);
	
        diff.cpu = energest_type_time ( ENERGEST_TYPE_CPU ) - last.cpu;
        diff.lpm = energest_type_time ( ENERGEST_TYPE_LPM ) - last.lpm;
        diff.transmit = energest_type_time ( ENERGEST_TYPE_TRANSMIT ) - last.transmit;
        diff.listen = energest_type_time ( ENERGEST_TYPE_LISTEN ) - last.listen;
        if ( j > 0 ) {
                printf ( "\nDecrypted data\n" );
                for ( i =0; i < MSG_LEN; i++ ) {
                        printf ( "%u:", decrypted[i] );
                }
                printf ( "\n" );

                printf ( "Clock ticks recorded by\nCPU = %ld \nLPM = %ld \nTRANSMITTER = %ld\nRECEIVER = %ld\n",diff.cpu, diff.lpm, diff.transmit, diff.listen );
        }
        else
	{
		printf("couldn't decrypt\n");
	}
}