/*** Mbed Includes ***/
#include "SimpleSpirit1.h"
#include "radio_spi.h"
//#include "main.h"


//180327 HJM : 디버그 라인
//180619 HJM : 디버그는 전부 중지.
//#define CUBEBITE_DEBUG
//#define	NDEBUG
//#define	DEBUG_IRQ
//#define	HEAVY_DEBUG


//180619 HJM : init 재시작을 위한 카운팅 변수
static int iErrorCounting = 0;
#define	MAX_ERROR_COUNTING	3
#define RETURN_ERROR_NUMBER     100


#define SPIRIT_GPIO_IRQ			(SPIRIT_GPIO_0)
//#define SPIRIT_GPIO_IRQ			(SPIRIT_GPIO_2)
//#define SPIRIT_GPIO_IRQ			(SPIRIT_GPIO_1)	//EV-COG-AD3029LZ -> GPIO 8 -> P0_08

static uint16_t last_state;
#define SPIRIT1_STATUS()		((last_state = (uint16_t)refresh_state()) & SPIRIT1_STATE_STATEBITS)

#define XO_ON                   (0x1)

#define BUSYWAIT_UNTIL(cond, millisecs) \
        do { \
            uint32_t start = us_ticker_read(); \
            uint32_t limit = (uint32_t)millisecs*1000U; \
            while (!(cond)) { \
                uint32_t now = us_ticker_read(); \
                if((now - start) > limit) break; \
            } \
        } while(0)

#define st_lib_spirit_irqs		SpiritIrqs

#define STATE_TIMEOUT           (100)


#define	CUBEBITE_OUTPUT_POWER_DBM_TEST	0


// betzw: switching force & back from standby is on some devices quite unstable
#define USE_STANDBY_STATE

/*** Class Implementation ***/
/** Static Class Variables **/
SimpleSpirit1 *SimpleSpirit1::_singleton = NULL;






/** Constructor **/
SimpleSpirit1::SimpleSpirit1(PinName mosi, PinName miso, PinName sclk,
                             PinName irq, PinName cs, PinName sdn,
                             PinName led, uint32_t uint32SClkSettingValue) :
    _spi(mosi, miso, sclk),
    _irq(irq),
    _chip_select(cs),
    _shut_down(sdn),
    _led(led),
    _current_irq_callback(),
    _rx_receiving_timeout(),
    _uint32SpiClkSettingValue(uint32SClkSettingValue)
{
}






//180619 HJM : 굳이 spi init을 한 번 더한다고 문제가 없는 거라면 ?? 테스트 해보자.
/** Init Function **/
void SimpleSpirit1::init()
{
    printf("[INIT_REVISE]init start...\n");
    //if (false == isItDoItSPIInit)
//    {
//    	isItDoItSPIInit = true;

    /*cubebite HJM, reset irq disable counter and irq callback & disable irq */
    _nr_of_irq_disables = 0;		// @@
//    disable_spirit_irq();

    /*cubebite HJM, unselect chip */
    chip_unselect();

    /*cubebite HJM, configure spi */
    _spi.format(8, 0); /* 8-bit, mode = 0, [order = SPI_MSB] only available in mbed3 */

    _spi.frequency(_uint32SpiClkSettingValue);	//HJM 초기 값, 6500000
    printf("Setting spi clk : [%d]\r\n", _uint32SpiClkSettingValue);

    //    _spi.frequency(13000000); // 13MHz (i.e. max speed allowed for Spirit1)
    //    _spi.frequency(6500000); // 6.5MHz (i.e. max speed allowed for Spirit1)
    //    _spi.frequency(4333333); // 4.3333...MHz (i.e. max speed allowed for Spirit1)
    //    _spi.frequency(2600000); // 2.6MHz (i.e. max speed allowed for Spirit1)
    //    _spi.frequency(1444444); // 1.4444...MHz (i.e. max speed allowed for Spirit1)
    //	_spi.frequency(764705); // 0.6470...MHz (i.e. max speed allowed for Spirit1)
    //	_spi.frequency(393939); // 0.3939...MHz (i.e. max speed allowed for Spirit1)
//    }

    //if (false == isItDoItRFInit)
//	{
//		isItDoItRFInit = true;

    //180619 HJM : 여기서 부터 sprit1 init 부분
    /*cubebite HJM, install irq handler */
    _irq.mode(PullUp);
    _irq.fall(Callback<void()>(this, &SimpleSpirit1::IrqHandler));



    /*cubebite HJM, init cube vars */
    spirit_on = OFF;
    last_rssi = 0 ; //MGR
    last_sqi = 0 ;  //MGR


    /*cubebite HJM, set frequencies */
    printf("[INIT_REVISE]set frequencies...");
    radio_set_xtal_freq(XTAL_FREQUENCY);			//cubebite HJM, 50MHz, 180619 HJM : 단순히 변수 셋팅
    mgmt_set_freq_base((uint32_t)BASE_FREQUENCY);	// @@
    printf("OK\n");

    /*cubebite HJM , restart board */
    enter_shutdown();
    exit_shutdown();

    /*cubebite HJM, soft core reset */
    cmd_strobe(SPIRIT1_STROBE_SRES);







    //180619 HJM : 여기서 부터는 init while문이 들어감.
    /*cubebite HJM, Configures the SPIRIT1 radio part */
    //180119 HJM : 433MHz 설정으로 변경 후 테스트.
    printf("[INIT_REVISE]set SPIRIT1 radio...\n");
    SRadioInit x_radio_init = {
        XTAL_OFFSET_PPM,                       // Xtal offset in ppm
        (uint32_t)BASE_FREQUENCY,	// HJM 수정 후 테스트 중 base frequency
        (uint32_t)CHANNEL_SPACE,	// channel space
        CHANNEL_NUMBER,             // channel number, HJM : 뭐지 0 인줄 알았더니 1로 셋팅되어있네/
        MODULATION_SELECT,		// modulation select
        DATARATE,                 // HJM 수정 후 테스트 중
        (uint32_t)FREQ_DEVIATION, // frequency deviation
        (uint32_t)BANDWIDTH       // channel filter bandwidth
    };





    uint8_t ui8Return;
    ui8Return = radio_init(&x_radio_init);
    if (RETURN_ERROR_NUMBER == ui8Return) 
    {
        reset_board();
        return;
    }
    printf("test 9, ui8Return : [%d]\n", ui8Return);




    /*180117 HJM : 송신 신호 세기 */
    radio_set_pa_level_dbm(0,POWER_DBM);	///hjm test
//    	radio_set_pa_level_dbm(0,10.0);
    //    radio_set_pa_level_dbm(0,CUBEBITE_OUTPUT_POWER_DBM_TEST);
    radio_set_pa_level_max_index(0);




    /* Configures the SPIRIT1 packet handler part*/
    PktBasicInit x_basic_init = {
        PREAMBLE_LENGTH,
        SYNC_LENGTH,
        SYNC_WORD,
        LENGTH_TYPE,
        LENGTH_WIDTH,
        CRC_MODE,
        CONTROL_LENGTH,
        EN_ADDRESS,
        EN_FEC,
        EN_WHITENING
    };
    pkt_basic_init(&x_basic_init);






    /* Enable the following interrupt sources, routed to GPIO */
    irq_de_init(NULL);
    irq_clear_status();
    irq_set_status(TX_DATA_SENT, S_ENABLE);
    irq_set_status(RX_DATA_READY,S_ENABLE);
    irq_set_status(RX_DATA_DISC, S_ENABLE);
    irq_set_status(VALID_SYNC, S_ENABLE);
    irq_set_status(TX_FIFO_ERROR, S_ENABLE);
    irq_set_status(RX_FIFO_ERROR, S_ENABLE);
#ifndef RX_FIFO_THR_WA
    irq_set_status(TX_FIFO_ALMOST_EMPTY, S_ENABLE);
    irq_set_status(RX_FIFO_ALMOST_FULL, S_ENABLE);
#endif // !RX_FIFO_THR_WA




    /* Configure Spirit1 */
    radio_persistent_rx(S_ENABLE);
    qi_set_sqi_threshold(SQI_TH_0);
    qi_sqi_check(S_ENABLE);
    qi_set_rssi_threshold_dbm(CCA_THRESHOLD);	//180205 HJM : RRSI 셋팅 값 수정 테스트
    timer_set_rx_timeout_stop_condition(SQI_ABOVE_THRESHOLD);
    timer_set_infinite_rx_timeout();
    radio_afc_freeze_on_sync(S_ENABLE);
    calibration_rco(S_ENABLE);

    spirit_on = OFF;
    CLEAR_TXBUF();
    CLEAR_RXBUF();
    _spirit_tx_started = false;
    _is_receiving = false;

    /* Configure the radio to route the IRQ signal to its GPIO 3 */
    SGpioInit x_gpio_init = {
        SPIRIT_GPIO_IRQ,
        SPIRIT_GPIO_MODE_DIGITAL_OUTPUT_LP,
        SPIRIT_GPIO_DIG_OUT_IRQ
    };
    spirit_gpio_init(&x_gpio_init);






    /* Setup CSMA/CA */
    CsmaInit x_csma_init = {
        S_ENABLE,         // enable persistent mode
        TBIT_TIME_64,     // Tcca time
        TCCA_TIME_3,      // Lcca length
        5,                // max nr of backoffs (<8)
        1,                // BU counter seed
        8                 // BU prescaler
    };
    csma_ca_init(&x_csma_init);

#ifdef USE_STANDBY_STATE
    /* Puts the SPIRIT1 in STANDBY mode (125us -> rx/tx) */
    cmd_strobe(SPIRIT1_STROBE_STANDBY);
#endif // USE_STANDBY_STATE

//	}
    printf("[INIT_REVISE]init end.\n");
}





static volatile int tx_fifo_remaining = 0;            // to be used in irq handler
static volatile int tx_buffer_pos = 0;                // to be used in irq handler
static const volatile uint8_t *tx_fifo_buffer = NULL; // to be used in irq handler
int SimpleSpirit1::send(const void *payload, unsigned int payload_len, bool use_csma_ca)
{
    /* Checks if the payload length is supported */
//    printf("\r\n[SEND] 1, MAX_PACKET_LEN : [%d]\n", MAX_PACKET_LEN);
//    printf("[SEND] 2, payload_len : [%d]\n", payload_len);

    if(payload_len > MAX_PACKET_LEN) 
    {
//        printf("[SEND] \n");
        return RADIO_TX_ERR;
    }









	printf("send [1");
//    disable_spirit_irq();

    BUSYWAIT_UNTIL(SPIRIT1_STATUS() == SPIRIT1_STATE_RX, STATE_TIMEOUT);
#ifndef NDEBUG
    if((last_state & SPIRIT1_STATE_STATEBITS) != SPIRIT1_STATE_RX) {
        debug("\r\nAssert failed in: %s (%d): state=%x\r\n", __func__, __LINE__, last_state>>1);
    }
#endif


    if((last_state & SPIRIT1_STATE_STATEBITS) != SPIRIT1_STATE_RX) 
    {
        //180619 HJM : 여기서 뻗지는 않음, 그냥 send error 만 띄울 뿐. 하지만 이것도 카운팅 걸어두고 몇 번(MAX_ERROR_COUNTING) 이상이면 init을 타도록 해야 하겠다.
        debug("\r\nAssert failed in: %s (%d): state=%x\r\n", __func__, __LINE__, last_state>>1);
        printf("[SEND] error \n");
        
        if (0x3 == (last_state>>1))
        {
        	return RADIO_TX_ERR_RESET;
//        	reset_board();
        }
        
        if (0x40 == (last_state>>1))
        {
			return RADIO_TX_ERR_RESET;	
        	
        }
        //180619 HJM : 아예 여기서 걸리면 흘러가게 냅두면 spirit1 이 꼬인다.
        ++iErrorCounting;
        if (iErrorCounting >= MAX_ERROR_COUNTING)
        {
        	iErrorCounting = 0;    	
        	return RADIO_TX_ERR_RESET;
        }
//
        return RADIO_TX_ERR;
    }
    iErrorCounting = 0;


	printf(" 2");
    //if (SPIRIT1_STATE_RX == SPIRIT1_STATUS())
//    {
//    	printf("[SEND] error \n");
//
//    	return RADIO_TX_ERR;
//    }

    //180323 HJM : IRQ 선이 동작하지 않기 때문에 여기에 현재 sprit1의 상태가 비지 상태인지 체크하고 송신.











    /* Reset State to Ready */
    printf(" 3");
//    printf("[SEND] 3, set_ready_state() call,  \n");
    set_ready_state();

	printf(" 4");
//	printf("[SEND] 4, cmd_strobe() call, SPIRIT1_STROBE_FTX \n");
    cmd_strobe(SPIRIT1_STROBE_FTX); // flush TX FIFO buffer

#ifndef NDEBUG
    debug_if(!(linear_fifo_read_num_elements_tx_fifo() == 0), "\r\nAssert failed in: %s (%d)\r\n", __func__, __LINE__);
#endif

	printf(" 5");
//	printf("[SEND] 5, pkt_basic_set_payload_length() call, \n");
    pkt_basic_set_payload_length(payload_len); // set desired payload len

    if(use_csma_ca) {
//        printf("[SEND] 6, csma_ca_state() call, \n");
        csma_ca_state(S_ENABLE); // enable CSMA/CA
    }

	printf(" 6");
    /* Init buffer & number of bytes to be send */
//    printf("[SEND] 7, csma_ca_state() call, \n");
    tx_fifo_remaining = payload_len;
    tx_fifo_buffer = (const uint8_t*)payload;

	printf(" 7");
    int8_t fifo_available = SPIRIT_MAX_FIFO_LEN; // fill-up whole fifo
    int8_t to_send = (tx_fifo_remaining > fifo_available) ? fifo_available : tx_fifo_remaining;

	printf(" 8");
    tx_fifo_remaining -= to_send;

	printf(" 9");
    /* Fill FIFO Buffer */
    if(to_send > 0) {
//    	printf("[SEND] 8, if(to_send > 0), if(to_send > 0) \n");
        spi_write_linear_fifo(to_send, (uint8_t*)&tx_fifo_buffer[0]);
//        printf("[SEND] 9, if(to_send > 0), if(to_send > 0) \n");
    }
	printf(" 10");
    tx_buffer_pos = to_send;
    _spirit_tx_started = true;

	printf(" 11");
//    enable_spirit_irq();
//    printf(" 6");















    /* Start transmitting */
//    printf("[SEND] 10, Start transmitting \n");
	printf(" 12");
    cmd_strobe(SPIRIT1_STROBE_TX);

	printf(" 13");
    while(tx_fifo_remaining != 0); // wait until not everything is yet send (evtl. by irq handler)

    /*
    if (SPIRIT1_STATUS() == SPIRIT1_STATE_TX)
    	{

    	}
    	else if (SPIRIT1_STATUS() == SPIRIT1_STATE_RX)
    	{

    	}
    	else if (SPIRIT1_STATUS() == SPIRIT1_STATE_)
    	{

    	}
    */











	printf(" 14");
//	printf("[SEND] 11, Start transmitting end, BUSYWAIT_UNTIL \n");
    BUSYWAIT_UNTIL(!_spirit_tx_started, STATE_TIMEOUT);

























#ifdef HEAVY_DEBUG
    debug("\r\n%s (%d): state=%x, _spirit_tx_started=%d\r\n", __func__, __LINE__, SPIRIT1_STATUS()>>1, _spirit_tx_started);
#endif

	printf(" 15");
    if(use_csma_ca) {
        csma_ca_state(S_DISABLE); // disable CSMA/CA
    }











    //180323 HJM : irq가 작동하지 않아 수동으로 irq 값을 읽어 오기로 결정됬다.
    //이걸 while 로 가둬서 send 하기 전에 검토하면 된다.
	printf(" 16");
    int iCounting = 0;
    while(1) 
    {
//        printf(" 16-1");
        st_lib_spirit_irqs x_irq_status;

        irq_get_status(&x_irq_status);
#ifdef	CUBEBITE_DEBUG
        printf("while\n");
#endif
        iCounting = iCounting + 1;

//	    BUSYWAIT_UNTIL(SPIRIT1_STATUS() == SPIRIT1_STATE_TX, STATE_TIMEOUT);


//		printf(" 16-2");
        if(x_irq_status.IRQ_TX_DATA_SENT) 
        {

#ifdef DEBUG_IRQ
//            uint32_t *tmp = (uint32_t*)&x_irq_status;
//            debug_if(!((*tmp) & IRQ_TX_DATA_SENT_MASK), "\r\nAssert failed in: %s (%d)\r\n", __func__, __LINE__);
//            debug_if(tx_fifo_remaining != 0, "\r\nAssert failed in: %s (%d)\r\n", __func__, __LINE__);
#endif
//			printf(" 16-3");
            if(_spirit_tx_started) 
            {
#ifdef	CUBEBITE_DEBUG
                printf("[IRQ] if(x_irq_status.IRQ_TX_DATA_SENT) in, if(_spirit_tx_started) in \n");
#endif

            }

//			printf(" 16-4");
            x_irq_status.IRQ_TX_FIFO_ALMOST_EMPTY = S_RESET;
            tx_fifo_buffer = NULL;

            _spirit_tx_started = false;

            break;
        } 
        else 
        {
#ifdef	CUBEBITE_DEBUG
            printf("not yet sent!!!\n");
#endif	//CUBEBITE_DEBUG	
        }





//			printf(" 17");
        /* The IRQ_TX_FIFO_ALMOST_EMPTY notifies an nearly empty TX fifo */
        if(x_irq_status.IRQ_TX_FIFO_ALMOST_EMPTY) {
//        	printf("1\n");
//            printf("[IRQ] if(x_irq_status.IRQ_TX_FIFO_ALMOST_EMPTY) in\n");

            //uint32_t *tmp = (uint32_t*)&x_irq_status;
//            debug_if(!((*tmp) & IRQ_TX_FIFO_ALMOST_EMPTY_MASK), "\r\nAssert failed in: %s (%d)\r\n", __func__, __LINE__);
//            debug_if(!_spirit_tx_started, "\r\nAssert failed in: %s (%d)\r\n", __func__, __LINE__);
//            debug_if(tx_fifo_buffer == NULL, "\r\nAssert failed in: %s (%d)\r\n", __func__, __LINE__);

            int8_t fifo_available = SPIRIT_MAX_FIFO_LEN/2; // fill-up half fifo
            int8_t to_send = (tx_fifo_remaining > fifo_available) ? fifo_available : tx_fifo_remaining;

            tx_fifo_remaining -= to_send;

            /* Fill FIFO Buffer */
            if(to_send > 0) {
                spi_write_linear_fifo(to_send, (uint8_t*)&tx_fifo_buffer[tx_buffer_pos]);
            }
            tx_buffer_pos += to_send;
        }



        /* TX FIFO underflow/overflow error */
        if(x_irq_status.IRQ_TX_FIFO_ERROR) 
        {
#ifdef	CUBEBITE_DEBUG
            printf("[IRQ] if(x_irq_status.IRQ_TX_FIFO_ERROR) in\n");
#endif	//CUBEBITE_DEBUG		

            //uint32_t *tmp = (uint32_t*)&x_irq_status;
//            debug("\r\n%s (%d): irq=%x\r\n", __func__, __LINE__, *tmp);
//            debug_if(!((*tmp) & IRQ_TX_FIFO_ERROR_MASK), "\r\nAssert failed in: %s (%d)\r\n", __func__, __LINE__);

            //if(_spirit_tx_started)
//            {
//                _spirit_tx_started = false;
//                /* call user callback */
//                if(_current_irq_callback)
//                {
//                    _current_irq_callback(TX_ERR);
//                }
//            }

            /* reset data still to be sent */
            tx_fifo_remaining = 0;
        }





		
        if (1000 == iCounting) 
        {
            //180327 HJM : 10번이나 검색을 했는데 IRQ가 이상이 있으면 정말 이상이 있다고 생각하고 그냥 break
#ifdef  CUBEBITE_DEBUG
            printf("if (1000 == iCounting) \n");
#endif
            iCounting = 0;
            return RADIO_TX_ERR_RESET;

        }


    }



























	printf(" 18");
    cmd_strobe(SPIRIT1_STROBE_RX); // Return to RX state

//	printf("[SEND] 12, Start transmitting end, BUSYWAIT_UNTIL \n");
//    disable_spirit_irq();
    if(_spirit_tx_started) {
        // in case of state timeout
        _spirit_tx_started = false;
//        enable_spirit_irq();

//        printf("[SEND] 13, if(_spirit_tx_started), in case of state timeout, return RADIO_TX_ERR \n");
		printf(" 19, err\n");
        return RADIO_TX_ERR;
    } else {
//        enable_spirit_irq();
        /* call user callback */
        if(_current_irq_callback) {
            _current_irq_callback(TX_DONE);
        }
//        printf("[SEND] 13, else, in case of state timeout, return RADIO_TX_OK \n");
		printf(" 19, ok\n");
        return RADIO_TX_OK;
    }
}

/** Set Ready State **/
int SimpleSpirit1::set_ready_state(void)
{
    uint16_t state;

//    disable_spirit_irq();

    _spirit_tx_started = false;
    _is_receiving = false;
    stop_rx_timeout();

    cmd_strobe(SPIRIT1_STROBE_FRX);
    CLEAR_RXBUF();
    CLEAR_TXBUF();

    state = SPIRIT1_STATUS();
    if(state == SPIRIT1_STATE_STANDBY) {
//        printf("[SEND] 3-1, set_ready_state() call, if(state == SPIRIT1_STATE_STANDBY) in \n");
        cmd_strobe(SPIRIT1_STROBE_READY);
    } else if(state == SPIRIT1_STATE_RX) {
//        printf("[SEND] 3-2, set_ready_state() call, if(state == SPIRIT1_STATE_RX) in \n");
        cmd_strobe(SPIRIT1_STROBE_SABORT);
    } else if(state != SPIRIT1_STATE_READY) 
    {
#ifndef NDEBUG
        debug("\r\nAssert failed in: %s (%d): state=%x\r\n", __func__, __LINE__, state>>1);
#endif
		return RETURN_ERROR_NUMBER;

	//	off();
//		reset_board();
//		on();
//		printf("[SEND] 3-3, set_ready_state() call, else if(state != SPIRIT1_STATE_READY) in, something is wrong. \n");
    }

    BUSYWAIT_UNTIL((SPIRIT1_STATUS() == SPIRIT1_STATE_READY) && ((last_state & XO_ON) == XO_ON), STATE_TIMEOUT);
    if(last_state != (SPIRIT1_STATE_READY | XO_ON)) {
        error("\r\nSpirit1: failed to become ready (%x) => pls. reset!\r\n", last_state);
//        enable_spirit_irq();
        return RETURN_ERROR_NUMBER + 2;
    }

    irq_clear_status();

//    enable_spirit_irq();
}

int SimpleSpirit1::off(void)
{
    if(spirit_on == ON) {
        /* Disables the mcu to get IRQ from the SPIRIT1 */
//        disable_spirit_irq();

        /* first stop rx/tx */
        set_ready_state();

#ifdef USE_STANDBY_STATE
        /* Puts the SPIRIT1 in STANDBY */
        cmd_strobe(SPIRIT1_STROBE_STANDBY);
        BUSYWAIT_UNTIL(SPIRIT1_STATUS() == SPIRIT1_STATE_STANDBY, STATE_TIMEOUT);
        if((last_state & SPIRIT1_STATE_STATEBITS) != SPIRIT1_STATE_STANDBY) {
            error("\r\nSpirit1: failed to enter standby (%x)\r\n", last_state>>1);
            return 1;
        }
#endif // USE_STANDBY_STATE

        spirit_on = OFF;
        _nr_of_irq_disables = 1;
    }
    return 0;
}

int SimpleSpirit1::on(void)
{
    if(spirit_on == OFF) {
        set_ready_state();

        /* now we go to Rx */
        cmd_strobe(SPIRIT1_STROBE_RX);

        BUSYWAIT_UNTIL(SPIRIT1_STATUS() == SPIRIT1_STATE_RX, STATE_TIMEOUT);
        if((last_state & SPIRIT1_STATE_STATEBITS) != SPIRIT1_STATE_RX) {
            error("\r\nSpirit1: failed to enter rx (%x) => retry\r\n", last_state>>1);
        }

        /* Enables the mcu to get IRQ from the SPIRIT1 */
        spirit_on = ON;
#ifndef NDEBUG
        debug_if(!(_nr_of_irq_disables == 1), "\r\nAssert failed in: %s (%d)\r\n", __func__, __LINE__);
#endif
//        enable_spirit_irq();
    }

#ifndef NDEBUG
    if(SPIRIT1_STATUS() != SPIRIT1_STATE_RX) {
        debug("\r\nAssert failed in: %s (%d): state=%x\r\n", __func__, __LINE__, last_state>>1);
    }
#endif

    return 0;
}

uint8_t SimpleSpirit1::refresh_state(void)
{
    uint8_t mcstate;

    SpiritSpiReadRegisters(MC_STATE0_BASE, 1, &mcstate);

    return mcstate;
}

int SimpleSpirit1::read(void *buf, unsigned int bufsize)
{
//    disable_spirit_irq();
    printf("read() 1 ");

    /* Checks if the RX buffer is empty */
    if(IS_RXBUF_EMPTY()) {
#ifndef NDEBUG
        debug("\r\nBuffer is empty\r\n");
#endif
        printf("[DEBUG] 2-1");
        set_ready_state();

        cmd_strobe(SPIRIT1_STROBE_RX);
        BUSYWAIT_UNTIL(SPIRIT1_STATUS() == SPIRIT1_STATE_RX, STATE_TIMEOUT);
//        enable_spirit_irq();
        return 0;
    }

    if(bufsize < spirit_rx_len) {
//        enable_spirit_irq();

        /* If buf has the correct size */
#ifndef NDEBUG
        debug("\r\nTOO SMALL BUF\r\n");
#endif
        return 0;
    } else {
        /* Copies the packet received */

        memcpy(buf, spirit_rx_buf, spirit_rx_len);
        bufsize = spirit_rx_len;
        printf("recv 3-1");

        CLEAR_RXBUF();
        printf(" 3-2");

//        enable_spirit_irq();
        printf(" 3-3 read ok\n");

        return bufsize;
    }

}

int SimpleSpirit1::channel_clear(void)
{
    float rssi_value;
    /* Local variable used to memorize the SPIRIT1 state */
    uint8_t spirit_state = ON;

    if(spirit_on == OFF) {
        /* Wakes up the SPIRIT1 */
        on();
        spirit_state = OFF;
    }

#ifndef NDEBUG
    if(SPIRIT1_STATUS() != SPIRIT1_STATE_RX) {
        debug("\r\nAssert failed in: %s (%d): state=%x\r\n", __func__, __LINE__, last_state>>1);
    }
#endif

//    disable_spirit_irq();

    /* Reset State to Ready */
    set_ready_state();

    /* Stores the RSSI value */
    rssi_value = qi_get_rssi_dbm();

//    enable_spirit_irq();

    /* Puts the SPIRIT1 in its previous state */
    if(spirit_state==OFF) {
        off();
#ifndef NDEBUG
#ifdef USE_STANDBY_STATE
        if(SPIRIT1_STATUS() != SPIRIT1_STATE_STANDBY) {
#else
        if(SPIRIT1_STATUS() != SPIRIT1_STATE_READY) {
#endif
            debug("\r\nAssert failed in: %s (%d): state=%x\r\n", __func__, __LINE__, last_state>>1);
        }
#endif
    } else {
//        disable_spirit_irq();

        set_ready_state();

        cmd_strobe(SPIRIT1_STROBE_RX);
        BUSYWAIT_UNTIL(SPIRIT1_STATUS() == SPIRIT1_STATE_RX, STATE_TIMEOUT);
        if((last_state & SPIRIT1_STATE_STATEBITS) != SPIRIT1_STATE_RX) {
            error("\r\nSpirit1: (#2) failed to enter rx (%x) => retry\r\n", last_state>>1);
        }

//        enable_spirit_irq();

#ifndef NDEBUG
        if(SPIRIT1_STATUS() != SPIRIT1_STATE_RX) {
            debug("\r\nAssert failed in: %s (%d): state=%x\r\n", __func__, __LINE__, last_state>>1);
        }
#endif
    }

    /* Checks the RSSI value with the threshold */
    if(rssi_value<CCA_THRESHOLD) {
        return 0;
    } else {
        return 1;
    }
}

int SimpleSpirit1::get_pending_packet(void)
{
    return !IS_RXBUF_EMPTY();
}

/** Spirit Irq Callback **/
/* betzw - TODO: use threaded interrupt handling when `MBED_CONF_RTOS_PRESENT` is defined (see `atmel-rf-driver`) */
void SimpleSpirit1::IrqHandler()
{
#ifdef	CUBEBITE_DEBUG
    printf("[CUBEBITE] IrqHandler() call !\r\n");
#endif	//CUBEBITE_DEBUG        

    st_lib_spirit_irqs x_irq_status;

    /* get interrupt source from radio */
    irq_get_status(&x_irq_status);

    /* The IRQ_TX_DATA_SENT notifies the packet has been sent. Puts the SPIRIT1 in RX */
    if(x_irq_status.IRQ_TX_DATA_SENT) {
        /* betzw - NOTE: MUST be handled before `IRQ_RX_DATA_READY` for Nanostack integration!
                                                       Logically, Nanostack only expects the "DONE" after "SUCCESS" (if it gets
                                                       DONE before SUCCESS, it assumes you're not going to bother to send SUCCESS).
        */
#ifdef DEBUG_IRQ
        uint32_t *tmp = (uint32_t*)&x_irq_status;
        debug_if(!((*tmp) & IRQ_TX_DATA_SENT_MASK), "\r\nAssert failed in: %s (%d)\r\n", __func__, __LINE__);
        debug_if(tx_fifo_remaining != 0, "\r\nAssert failed in: %s (%d)\r\n", __func__, __LINE__);
#endif

        if(_spirit_tx_started) {
            printf("[IRQ] if(x_irq_status.IRQ_TX_DATA_SENT) in, if(_spirit_tx_started) in \n");
            _spirit_tx_started = false;

            /* call user callback */
            if(_current_irq_callback) {
                _current_irq_callback(TX_DONE);
            }
        }

        /* Disable handling of other TX flags */
        x_irq_status.IRQ_TX_FIFO_ALMOST_EMPTY = S_RESET;
        tx_fifo_buffer = NULL;
    }

#ifndef RX_FIFO_THR_WA
    /* The IRQ_TX_FIFO_ALMOST_EMPTY notifies an nearly empty TX fifo */
    if(x_irq_status.IRQ_TX_FIFO_ALMOST_EMPTY) {
#ifdef DEBUG_IRQ
        uint32_t *tmp = (uint32_t*)&x_irq_status;
        debug_if(!((*tmp) & IRQ_TX_FIFO_ALMOST_EMPTY_MASK), "\r\nAssert failed in: %s (%d)\r\n", __func__, __LINE__);
        debug_if(!_spirit_tx_started, "\r\nAssert failed in: %s (%d)\r\n", __func__, __LINE__);
        debug_if(tx_fifo_buffer == NULL, "\r\nAssert failed in: %s (%d)\r\n", __func__, __LINE__);
#endif

        int8_t fifo_available = SPIRIT_MAX_FIFO_LEN/2; // fill-up half fifo
        int8_t to_send = (tx_fifo_remaining > fifo_available) ? fifo_available : tx_fifo_remaining;

        tx_fifo_remaining -= to_send;

        /* Fill FIFO Buffer */
        if(to_send > 0) {
            spi_write_linear_fifo(to_send, (uint8_t*)&tx_fifo_buffer[tx_buffer_pos]);
        }
        tx_buffer_pos += to_send;
    }
#endif // !RX_FIFO_THR_WA

    /* TX FIFO underflow/overflow error */
    if(x_irq_status.IRQ_TX_FIFO_ERROR) {
#ifdef DEBUG_IRQ
        uint32_t *tmp = (uint32_t*)&x_irq_status;
        debug("\r\n%s (%d): irq=%x\r\n", __func__, __LINE__, *tmp);
        debug_if(!((*tmp) & IRQ_TX_FIFO_ERROR_MASK), "\r\nAssert failed in: %s (%d)\r\n", __func__, __LINE__);
#endif
        if(_spirit_tx_started) {
            _spirit_tx_started = false;
            /* call user callback */
            if(_current_irq_callback) {
                _current_irq_callback(TX_ERR);
            }
        }

        /* reset data still to be sent */
        tx_fifo_remaining = 0;
    }

    /* The IRQ_RX_DATA_READY notifies a new packet arrived */
    if(x_irq_status.IRQ_RX_DATA_READY) {
#ifdef DEBUG_IRQ
        uint32_t *tmp = (uint32_t*)&x_irq_status;
        debug_if(!((*tmp) & IRQ_RX_DATA_READY_MASK), "\r\nAssert failed in: %s (%d)\r\n", __func__, __LINE__);
#endif

        if(!_is_receiving) { // spurious irq?!? (betzw: see comments on macro 'RX_FIFO_THR_WA'!)
#ifdef HEAVY_DEBUG
            debug("\r\n%s (%d): irq=%x\r\n", __func__, __LINE__, *tmp);
#endif
        } else {
            _is_receiving = false; // Finished receiving
            stop_rx_timeout();

            spirit_rx_len = pkt_basic_get_received_pkt_length();

#ifdef DEBUG_IRQ
            debug_if(!(spirit_rx_len <= MAX_PACKET_LEN), "\r\n%s (%d): irq=%x\r\n", __func__, __LINE__, *tmp);
#endif

            if(spirit_rx_len <= MAX_PACKET_LEN) {
                uint8_t to_receive = spirit_rx_len - _spirit_rx_pos;
                if(to_receive > 0) {
                    spi_read_linear_fifo(to_receive, &spirit_rx_buf[_spirit_rx_pos]);
                    _spirit_rx_pos += to_receive;
                }
            }

            cmd_strobe(SPIRIT1_STROBE_FRX);

            last_rssi = qi_get_rssi(); //MGR
            last_sqi  = qi_get_sqi();  //MGR

            /* call user callback */
            if((_spirit_rx_pos == spirit_rx_len) && _current_irq_callback) {
                _current_irq_callback(RX_DONE);
            }

            /* Disable handling of other RX flags */
            x_irq_status.IRQ_RX_FIFO_ALMOST_FULL = S_RESET;
        }
    }

#ifndef RX_FIFO_THR_WA
    /* RX FIFO almost full */
    if(x_irq_status.IRQ_RX_FIFO_ALMOST_FULL) {
#ifdef DEBUG_IRQ
        uint32_t *tmp = (uint32_t*)&x_irq_status;
        debug_if(!((*tmp) & IRQ_RX_FIFO_ALMOST_FULL_MASK), "\r\nAssert failed in: %s (%d)\r\n", __func__, __LINE__);
#endif
        if(!_is_receiving) { // spurious irq?!?
#ifdef DEBUG_IRQ
            debug("\r\n%s (%d): irq=%x\r\n", __func__, __LINE__, *tmp);
#endif
        } else {
            uint8_t fifo_available = linear_fifo_read_num_elements_rx_fifo();
            if((fifo_available + _spirit_rx_pos) <= MAX_PACKET_LEN) {
                spi_read_linear_fifo(fifo_available, &spirit_rx_buf[_spirit_rx_pos]);
                _spirit_rx_pos += fifo_available;
            } else {
#ifdef DEBUG_IRQ
                debug("\r\n%s (%d): irq=%x\r\n", __func__, __LINE__, *tmp);
#endif
            }
        }
    }
#endif // !RX_FIFO_THR_WA

    /* Reception errors */
    if((x_irq_status.IRQ_RX_FIFO_ERROR) || (x_irq_status.IRQ_RX_DATA_DISC)) {
#ifdef DEBUG_IRQ
        uint32_t *tmp = (uint32_t*)&x_irq_status;
        debug("\r\n%s (%d): irq=%x\r\n", __func__, __LINE__, *tmp);
        debug_if(!((*tmp) & (IRQ_RX_FIFO_ERROR_MASK | IRQ_RX_DATA_DISC_MASK)), "\r\nAssert failed in: %s (%d)\r\n", __func__, __LINE__);
#endif
        rx_timeout_handler();
        if(_spirit_tx_started) {
            _spirit_tx_started = false;
            /* call user callback */
            if(_current_irq_callback) {
                _current_irq_callback(TX_ERR);
            }
        }
    }

    /* The IRQ_VALID_SYNC is used to notify a new packet is coming */
    if(x_irq_status.IRQ_VALID_SYNC) {
#ifdef DEBUG_IRQ
        uint32_t *tmp = (uint32_t*)&x_irq_status;
        debug_if(!((*tmp) & IRQ_VALID_SYNC_MASK), "\r\nAssert failed in: %s (%d)\r\n", __func__, __LINE__);
#endif
        /* betzw - NOTE: there is a race condition between Spirit1 receiving packets and
         *               the MCU trying to send a packet, which gets resolved in favor of
         *               sending.
         */
        if(_spirit_tx_started) {
#ifdef DEBUG_IRQ
            debug("\r\n%s (%d): irq=%x\r\n", __func__, __LINE__, *tmp);
#endif
        } else {
            _is_receiving = true;
            start_rx_timeout();
        }
    }
}
