/*
 * Application entry point.
 */
int main(void) {
  static const evhandler_t evhndl[] = {
    InsertHandler,
    RemoveHandler
  };
  Thread *shelltp = NULL;
  struct EventListener el0, el1;

  /*
   * System initializations.
   * - HAL initialization, this also initializes the configured device drivers
   *   and performs the board-specific initializations.
   * - Kernel initialization, the main() function becomes a thread and the
   *   RTOS is active.
   */
  halInit();
  chSysInit();

  /*
   * Activates the serial driver 2 using the driver default configuration.
   */
  sdStart(&SD2, NULL);

  /*
   * Shell manager initialization.
   */
  shellInit();

  /*
   * Initializes the MMC driver to work with SPI2.
   */
  palSetPadMode(IOPORT2, GPIOB_SPI2NSS, PAL_MODE_OUTPUT_PUSHPULL);
  palSetPad(IOPORT2, GPIOB_SPI2NSS);
  mmcObjectInit(&MMCD1, &SPID2,
                &ls_spicfg, &hs_spicfg,
                mmc_is_protected, mmc_is_inserted);
  mmcStart(&MMCD1, NULL);

  /*
   * Creates the blinker thread.
   */
  chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);

  /*
   * Normal main() thread activity, in this demo it does nothing except
   * sleeping in a loop and listen for events.
   */
  chEvtRegister(&MMCD1.inserted_event, &el0, 0);
  chEvtRegister(&MMCD1.removed_event, &el1, 1);
  while (TRUE) {
    if (!shelltp)
      shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
    else if (chThdTerminated(shelltp)) {
      chThdRelease(shelltp);    /* Recovers memory of the previous shell.   */
      shelltp = NULL;           /* Triggers spawning of a new shell.        */
    }
    chEvtDispatch(evhndl, chEvtWaitOne(ALL_EVENTS));
  }
  return 0;
}

/*
 * Application entry point.
 */
int main(void) {
  static const evhandler_t evhndl[] = {
    TimerHandler,
    InsertHandler,
    RemoveHandler
  };
  static EvTimer evt;
  struct EventListener el0, el1, el2;

  /*
   * System initializations.
   * - HAL initialization, this also initializes the configured device drivers
   *   and performs the board-specific initializations.
   * - Kernel initialization, the main() function becomes a thread and the
   *   RTOS is active.
   */
  halInit();
  chSysInit();

  /*
   * Activates the serial driver 1 using the driver default configuration.
   */
  sdStart(&SD1, NULL);

  /*
   * Buzzer driver initialization.
   */
  buzzInit();

  /*
   * Initializes the MMC driver to work with SPI2.
   */
  mmcObjectInit(&MMCD1);
  mmcStart(&MMCD1, &mmccfg);

  /*
   * Activates the card insertion monitor.
   */
  tmr_init(&MMCD1);

  /*
   * Creates the blinker threads.
   */
  chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
  chThdCreateStatic(waThread2, sizeof(waThread2), NORMALPRIO, Thread2, NULL);

  /*
   * Normal main() thread activity, in this demo it does nothing except
   * sleeping in a loop and listen for events.
   */
  evtInit(&evt, MS2ST(500));            /* Initializes an event timer object.   */
  evtStart(&evt);                       /* Starts the event timer.              */
  chEvtRegister(&evt.et_es, &el0, 0);   /* Registers on the timer event source. */
  chEvtRegister(&inserted_event, &el1, 1);
  chEvtRegister(&removed_event, &el2, 2);
  while (TRUE)
    chEvtDispatch(evhndl, chEvtWaitOne(ALL_EVENTS));
  return 0;
}

/*! \brief sdlog thread.
 *
 * Test logging to microSD card on e407.
 *
 */
msg_t sdlog_thread(void *p) {
    void * arg __attribute__ ((unused)) = p;
    static const evhandler_t evhndl_sdclog[]  = {
        sdc_log_data,
        sdc_log_data,
        sdc_log_data,
        sdc_log_data,
        sdc_log_data
    };
    struct EventListener     el0, el1, el2, el3, el4;

    chRegSetThreadName("sdlog_thread");

#ifdef DEBUG_SDCLOG
    /*chThdSleepMilliseconds(1000);*/
#endif

    SDCLOGDBG("Start sdlog thread\r\n");

    // init structure
    datafile_state.log_sequence  = 0;
    datafile_state.write_errors  = 0;
    datafile_state.sd_log_opened = false;

    sdc_reset_fp_index();

    sdc_init_eod((uint8_t)0xa5);

    // Assert data is halfword aligned
    if(((sizeof(GENERIC_message)*8) % 16) != 0) {
        SDCLOGDBG("%s: GENERIC message is not halfword aligned.\r\n", __func__);
        return (SDC_ASSERT_ERROR);
    }

    // Assert we will not overflow Payload
    if(  (sizeof(MPU9150_read_data)    > (sizeof(datafile_state.log_data.data)-1)) ||
         (sizeof(MPL3115A2_read_data)  > (sizeof(datafile_state.log_data.data)-1)) ||
         (sizeof(ADIS16405_burst_data) > (sizeof(datafile_state.log_data.data)-1))) {
        SDCLOGDBG("%s: DATA size is too large\r\n");
        return (SDC_ASSERT_ERROR);
    }

    chEvtRegister(&mpl3115a2_data_event        ,   &el0, MPL3115A2);
    chEvtRegister(&adis_spi_burst_data_captured,   &el1, ADIS16405);
    chEvtRegister(&mpu9150_data_event          ,   &el2, MPU9150);
    chEvtRegister(&sdc_halt_event              ,   &el3, SENSOR_LOG_HALT);
    chEvtRegister(&sdc_start_event             ,   &el4, SENSOR_LOG_START);
    while(1) {
        if(!fs_stop) {
            chEvtDispatch(evhndl_sdclog, chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(50)));
        } else {
            chEvtDispatch(evhndl_sdclog, chEvtWaitOneTimeout((1<<SENSOR_LOG_START), MS2ST(50)));
        }
    }
    return -1;
}

static msg_t tUsbTx(void *arg) {
  (void)arg;
  chRegSetThreadName("usbTx");


  msg_t msg;
  usbPacket *usbBufp;

  enum {UsbTxComleteID = 0, UsbResetID = 1, UsbConfiguredID = 2};

  EventListener elUsbTxComplete;
  EventListener elUsbReset;
  EventListener elUsbConfigured;

  eventmask_t   activeEvents;


  chEvtRegister(&esUsbTxComplete, &elUsbTxComplete, UsbTxComleteID);
  chEvtRegister(&esUsbReset, &elUsbReset, UsbResetID);
  chEvtRegister(&esUsbConfigured, &elUsbConfigured, UsbConfiguredID);

  // Wait for the USB system to be configured. and clear all other event flags.
  chEvtWaitOne(EVENT_MASK(UsbConfiguredID));
  chEvtGetAndClearEvents(EVENT_MASK(UsbTxComleteID) | EVENT_MASK(UsbResetID));

  while (TRUE) {
    chMBFetch (&usbTXMailbox, &msg, TIME_INFINITE);

    // Check if USB has been reconfigured while waiting for message from sysctrl
    activeEvents = chEvtGetAndClearEvents(EVENT_MASK(UsbConfiguredID));
    if (activeEvents == EVENT_MASK(UsbConfiguredID)) {
      // If so, clear the reset event since it is no longer relevant.
      activeEvents = chEvtGetAndClearEvents(EVENT_MASK(UsbResetID));
    }

    // Typecast Mailbox message to command package pointer for readability
    usbBufp = (usbPacket*)msg;

    // Prepare transmit and start the transmission. This operation will return immediately
    usbPrepareTransmit(usbp, EP_IN, usbBufp->packet, (size_t)usbBufp->size);
    chSysLock();
    usbStartTransmitI(usbp, EP_IN);
    chSysUnlock();

    //Check for events from the USB system.
    activeEvents = chEvtWaitAny(EVENT_MASK(UsbTxComleteID) | EVENT_MASK(UsbResetID));

    if (activeEvents == EVENT_MASK(UsbResetID)) {
      chEvtWaitOne(EVENT_MASK(UsbConfiguredID));
      // Clear any events that has occurred while the usb was not configured.
      chEvtGetAndClearEvents(EVENT_MASK(UsbTxComleteID) | EVENT_MASK(UsbResetID));
    }
    usbFreeMailboxBuffer (usbBufp);
  }
  return 0;
}

/*
 * Application entry point.
 */
int main(void) {
  static const evhandler_t evhndl[] = {
    InsertHandler,
    RemoveHandler
  };
  Thread *shelltp = NULL;
  struct EventListener el0, el1;

  /*
   * System initializations.
   * - HAL initialization, this also initializes the configured device drivers
   *   and performs the board-specific initializations.
   * - Kernel initialization, the main() function becomes a thread and the
   *   RTOS is active.
   */
  halInit();
  chSysInit();

  /*
   * Activates the serial driver 1 and SDC driver 1 using default
   * configuration.
   */
  sdStart(&SD1, NULL);
  sdcStart(&SDCD1, NULL);

  /*
   * Shell manager initialization.
   */
  shellInit();

  /*
   * Activates the card insertion monitor.
   */
  tmr_init(&SDCD1);

  /*
   * Creates the blinker thread.
   */
  chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);

  /*
   * Normal main() thread activity, in this demo it does nothing except
   * sleeping in a loop and listen for events.
   */
  chEvtRegister(&inserted_event, &el0, 0);
  chEvtRegister(&removed_event, &el1, 1);
  while (TRUE) {
    if (!shelltp)
      shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
    else if (chThdTerminated(shelltp)) {
      chThdRelease(shelltp);    /* Recovers memory of the previous shell.   */
      shelltp = NULL;           /* Triggers spawning of a new shell.        */
    }
    chEvtDispatch(evhndl, chEvtWaitOne(ALL_EVENTS));
  }
}

/*------------------------------------------------------------------------*
 * Simulator main.                                                        *
 *------------------------------------------------------------------------*/
int main(void) {
  EventListener sd1fel, sd2fel, tel;

  /*
   * HAL initialization.
   */
  halInit();

  /*
   * ChibiOS/RT initialization.
   */
  chSysInit();

  /*
   * Serial ports (simulated) initialization.
   */
  sdStart(&SD1, NULL);
  sdStart(&SD2, NULL);

  /*
   * Shell manager initialization.
   */
  shellInit();
  chEvtRegister(&shell_terminated, &tel, 0);

  /*
   * Console thread started.
   */
  cdtp = chThdCreateFromHeap(NULL, CONSOLE_WA_SIZE, NORMALPRIO + 1,
                             console_thread, NULL);

  /*
   * Initializing connection/disconnection events.
   */
  cputs("Shell service started on SD1, SD2");
  cputs("  - Listening for connections on SD1");
  (void) sdGetAndClearFlags(&SD1);
  chEvtRegister(&SD1.sevent, &sd1fel, 1);
  cputs("  - Listening for connections on SD2");
  (void) sdGetAndClearFlags(&SD2);
  chEvtRegister(&SD2.sevent, &sd2fel, 2);

  /*
   * Events servicing loop.
   */
  while (!chThdShouldTerminate())
    chEvtDispatch(fhandlers, chEvtWaitOne(ALL_EVENTS));

  /*
   * Clean simulator exit.
   */
  chEvtUnregister(&SD1.sevent, &sd1fel);
  chEvtUnregister(&SD2.sevent, &sd2fel);
  return 0;
}

/*------------------------------------------------------------------------*
 * Simulator main.                                                        *
 *------------------------------------------------------------------------*/
int main(void) {
  EventListener sd1fel, sd2fel, tel;

  /*
   * System initializations.
   * - HAL initialization, this also initializes the configured device drivers
   *   and performs the board-specific initializations.
   * - Kernel initialization, the main() function becomes a thread and the
   *   RTOS is active.
   */
  halInit();
  chSysInit();

  /*
   * Serial ports (simulated) initialization.
   */
  sdStart(&SD1, NULL);
  sdStart(&SD2, NULL);

  /*
   * Shell manager initialization.
   */
  shellInit();
  chEvtRegister(&shell_terminated, &tel, 0);

  /*
   * Console thread started.
   */
  cdtp = chThdCreateFromHeap(NULL, CONSOLE_WA_SIZE, NORMALPRIO + 1,
                             console_thread, NULL);

  /*
   * Initializing connection/disconnection events.
   */
  cputs("Shell service started on SD1, SD2");
  cputs("  - Listening for connections on SD1");
  (void) chIOGetAndClearFlags(&SD1);
  chEvtRegister(chIOGetEventSource(&SD1), &sd1fel, 1);
  cputs("  - Listening for connections on SD2");
  (void) chIOGetAndClearFlags(&SD2);
  chEvtRegister(chIOGetEventSource(&SD2), &sd2fel, 2);

  /*
   * Events servicing loop.
   */
  while (!chThdShouldTerminate())
    chEvtDispatch(fhandlers, chEvtWaitOne(ALL_EVENTS));

  /*
   * Clean simulator exit.
   */
  chEvtUnregister(chIOGetEventSource(&SD1), &sd1fel);
  chEvtUnregister(chIOGetEventSource(&SD2), &sd2fel);
  return 0;
}

void init_fatfs( void )
{
    // Activates the SDC driver 1 using default configuration.
    sdcStart( &SDCD1, NULL );
    
    // Activates the card insertion monitor.
    tmr_init( &SDCD1 );
    
    chEvtRegister( &inserted_event, &el0, 0 );
    chEvtRegister( &removed_event, &el1, 1 );
}

/*------------------------------------------------------------------------*
 * Simulator main.                                                        *
 *------------------------------------------------------------------------*/
int main(void) {

	initTestStream(&testStream);

	/*
	 * System initializations.
	 * - HAL initialization, this also initializes the configured device drivers
	 *   and performs the board-specific initializations.
	 * - Kernel initialization, the main() function becomes a thread and the
	 *   RTOS is active.
	 */
	halInit();
	chSysInit();

	/*
	 * Serial ports (simulated) initialization.
	 */
	sdStart(&SD1, NULL);
	sdStart(&SD2, NULL);

	/*
	 * Console thread started.
	 */
	cdtp = chThdCreateFromHeap(NULL, CONSOLE_WA_SIZE, NORMALPRIO + 1, console_thread, NULL);

	/*
	 * Initializing connection/disconnection events.
	 */
	cputs("Shell service started on SD1, SD2");
	cputs("  - Listening for connections on SD1");
	chEvtRegister(chnGetEventSource(&SD1), &sd1fel, 1);
	cputs("  - Listening for connections on SD2");
	chEvtRegister(chnGetEventSource(&SD2), &sd2fel, 2);

	rusEfiFunctionalTest();

	/*
	 * Events servicing loop.
	 */
	while (!chThdShouldTerminate()) {
		chEvtDispatch(fhandlers, chEvtWaitOne(ALL_EVENTS));
		printPendingMessages();
		chThdSleepMilliseconds(100);
	}

	/*
	 * Clean simulator exit.
	 */
	chEvtUnregister(chnGetEventSource(&SD1), &sd1fel);
	chEvtUnregister(chnGetEventSource(&SD2), &sd2fel);
	return 0;
}

int main(void)
{
  static const evhandler_t evhndl[] = {InsertHandler, RemoveHandler};
  struct EventListener el0, el1;

  // os init
  halInit();
  chSysInit();

  // setup LED pads
  palSetPadMode(GPIOD, 12, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
  palClearPad(GPIOD, 12); // green LED
  palSetPadMode(GPIOD, 15, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
  palClearPad(GPIOD, 15); // blue LED

  // setup pads to USART2 function (connect these pads through RS232 transceiver with PC, terminal emu needs 38400 baud)
  sdStart(&SD2, NULL);
  palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATE(7) | PAL_STM32_OSPEED_HIGHEST); // TX
  palSetPadMode(GPIOA, 3, PAL_MODE_ALTERNATE(7)); // RX

  // setup pads to SPI1 function (connect these pads to your SD card accordingly)
  palSetPadMode(GPIOC, 4, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); // NSS
  palSetPadMode(GPIOA, 5, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST); // SCK
  palSetPadMode(GPIOA, 6, PAL_MODE_ALTERNATE(5)); // MISO
  palSetPadMode(GPIOA, 7, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST); // MOSI
  palSetPad(GPIOC, 4); // set NSS high

  // initialize MMC driver
  mmcObjectInit(&MMCD1, &SPID1, &ls_spicfg, &hs_spicfg, mmc_is_protected, mmc_is_inserted);
  mmcStart(&MMCD1, NULL);

  // ChibiOS has no I2S support yet;
  // codec.c initializes everything necessary
  // except the I2S TX DMA interrupt vector (because it would
  // conflict with the ChibiOS kernel)
  // we can make ChibiOS call our own handler by letting
  // it create the DMA stream for SPI3
  dmaStreamAllocate(SPID3.dmatx,
                    STM32_SPI_SPI3_IRQ_PRIORITY,
                    (stm32_dmaisr_t)I2SDmaTxInterrupt,
                    &SPID3);

  // blink thread; also checks the user button
  chThdCreateStatic(waBlinkThread, sizeof(waBlinkThread), NORMALPRIO, BlinkThread, NULL);

  chEvtRegister(&MMCD1.inserted_event, &el0, 0);
  chEvtRegister(&MMCD1.removed_event, &el1, 1);
  while(TRUE)
  {
    chEvtDispatch(evhndl, chEvtWaitOne(ALL_EVENTS));
  }
}

static msg_t tUsbRx(void *arg) {
  (void)arg;
  chRegSetThreadName("usbRx");

  enum {UsbRxComleteID = 0, UsbResetID = 1, UsbConfiguredID = 2};

  usbPacket *usbBufp;

  EventListener elUsbRxComplete;
  EventListener elUsbReset;
  EventListener elUsbConfigured;

  eventmask_t   activeEvents;


  chEvtRegister(&esUsbRxComplete, &elUsbRxComplete, UsbRxComleteID);
  chEvtRegister(&esUsbReset, &elUsbReset, UsbResetID);
  chEvtRegister(&esUsbConfigured, &elUsbConfigured, UsbConfiguredID);

  // Wait for the USB system to be configured.
  chEvtWaitOne(EVENT_MASK(UsbConfiguredID));
  chEvtGetAndClearEvents(EVENT_MASK(UsbRxComleteID) | EVENT_MASK(UsbResetID));

  while (TRUE) {

    // Allocate buffer space for reception of package in the sysctrl mempool
    usbBufp = usbAllocMailboxBuffer();

    // Prepare receive operation and initiate the usb system to listen
    usbPrepareReceive(usbp, EP_OUT, usbBufp->packet, 64);
    chSysLock();
    usbStartReceiveI(usbp, EP_OUT);
    chSysUnlock();

    // Wait for events from the USB system
    activeEvents = chEvtWaitAny(EVENT_MASK(UsbRxComleteID) | EVENT_MASK(UsbResetID));

    if (activeEvents == EVENT_MASK(UsbResetID)) {
      // If the system was reset, clean up and wait for new configure.
      usbFreeMailboxBuffer (usbBufp);
      chEvtWaitOne(EVENT_MASK(UsbConfiguredID));
      chEvtGetAndClearEvents(EVENT_MASK(UsbRxComleteID) | EVENT_MASK(UsbResetID));
    }
    else {
      // Post pagckage to sysctrl if receive was successful
      usbBufp->size = ep2outstate.rxcnt;
      chMBPost (&usbRXMailbox, (msg_t)usbBufp, TIME_INFINITE);
    }
  }

  return 0;
}

static THD_FUNCTION(cancom_thread, arg) {
	(void)arg;
	chRegSetThreadName("CAN");

	event_listener_t el;
	CANRxFrame rxmsg;
	uint8_t buffer[9];

	chEvtRegister(&CANDx.rxfull_event, &el, 0);

	while(!chThdShouldTerminateX()) {
		if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0) {
			continue;
		}

		while (canReceive(&CANDx, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) {
			buffer[0] = rxmsg.SID;
			for (int i = 0;i < rxmsg.DLC;i++) {
				buffer[i + 1] = rxmsg.data8[i];
			}
			packet_handler_int_process_packet(buffer, rxmsg.DLC + 1);
		}
	}

	chEvtUnregister(&CAND1.rxfull_event, &el);
}

static msg_t can_rx(void *p) {
  struct can_instance *cip = p;
  EventListener el;
  CANRxFrame rxmsg;
  (void)p;
  chRegSetThreadName("receiver");
  chEvtRegister(&cip->canp->rxfull_event, &el, 0);
#if SPC5_CAN_USE_FILTERS
  rxFlag = chEvtGetAndClearFlagsI(&el);
#endif
  while(!chThdShouldTerminate()) {
    if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0)
      continue;
#if !SPC5_CAN_USE_FILTERS
    while (canReceive(cip->canp, CAN_ANY_MAILBOX,
                      &rxmsg, TIME_IMMEDIATE) == RDY_OK) {
      /* Process message.*/
      palTogglePad(PORT_D, cip->led);
    }
#else
    while (canReceive(cip->canp, rxFlag,
                       &rxmsg, TIME_IMMEDIATE) == RDY_OK) {
      /* Process message.*/
      palTogglePad(PORT_D, cip->led);
    }
#endif
  }
  chEvtUnregister(&CAND1.rxfull_event, &el);
  return 0;
}

int main(void) {
  halInit();
  chSysInit();
  uint8_t i;
  event_listener_t tel;

  // Serial Port Setup 
  sdStart(&SD1, NULL);
  palSetPadMode(GPIOC, 4, PAL_MODE_ALTERNATE(7));
  palSetPadMode(GPIOC, 5, PAL_MODE_ALTERNATE(7));

  chprintf((BaseSequentialStream*)&SD1, "Up and Running\n\r");

  palSetPadMode(GPIOB, 3, PAL_MODE_ALTERNATE(6));     /* SCK. */
  palSetPadMode(GPIOB, 4, PAL_MODE_ALTERNATE(6));     /* MISO.*/
  palSetPadMode(GPIOB, 5, PAL_MODE_ALTERNATE(6));     /* MOSI.*/

  palSetPadMode(GPIOC, GPIOC_PIN1, PAL_MODE_OUTPUT_PUSHPULL);
  palSetPad(GPIOC, GPIOC_PIN1);
  palSetPadMode(GPIOC, GPIOC_PIN2, PAL_MODE_OUTPUT_PUSHPULL);
  palClearPad(GPIOC, GPIOC_PIN2);
  palSetPadMode(GPIOC, GPIOC_PIN3, PAL_MODE_INPUT_PULLUP);

  spiStart(&SPID3, &nrf24l01SPI);

  chMtxObjectInit(&nrfMutex);

  //FROM RX---
  extStart(&EXTD1, &extcfg);
  //---

  nrf24l01ObjectInit(&nrf24l01);
  nrf24l01Start(&nrf24l01, &nrf24l01Config);
  
  //FROM RX ---
  extChannelEnable(&EXTD1, 3);
  //-----
  
  initNRF24L01(&nrf24l01);

  chprintf((BaseSequentialStream*)&SD1, "\n\rUp and Running\n\r");
  shellInit();
  chEvtRegister(&shell_terminated, &tel, 0);
  shelltp1 = shellCreate(&shell_cfg1, sizeof(waShell), NORMALPRIO);

  //FROM RX---
  chThdCreateStatic(recieverWorkingArea, sizeof(recieverWorkingArea), NORMALPRIO, receiverThread, NULL);
  //FROM RX^^^^

  /*
  for (i=0;i<32;i++) {
    serialOutBuf[i] = 3;
  }
  */

  for (;;) {
    chEvtDispatch(fhandlers, chEvtWaitOne(ALL_EVENTS));    
  }
}

msg_t WebThread(void *p) {
  EvTimer evt1, evt2;
  EventListener el0, el1, el2;
  uip_ipaddr_t ipaddr;

  (void)p;

  /*
   * Event sources setup.
   */
  chEvtRegister(macGetReceiveEventSource(&ETHD1), &el0, FRAME_RECEIVED_ID);
  chEvtAddFlags(EVENT_MASK(FRAME_RECEIVED_ID)); /* In case some frames are already buffered */

  evtInit(&evt1, MS2ST(500));
  evtStart(&evt1);
  chEvtRegister(&evt1.et_es, &el1, PERIODIC_TIMER_ID);

  evtInit(&evt2, S2ST(10));
  evtStart(&evt2);
  chEvtRegister(&evt2.et_es, &el2, ARP_TIMER_ID);

  /*
   * EMAC driver start.
   */
  macStart(&ETHD1, &mac_config);
  (void)macPollLinkStatus(&ETHD1);

  /*
   * uIP initialization.
   */
  uip_init();
  uip_setethaddr(macaddr);
  uip_ipaddr(ipaddr, IPADDR0, IPADDR1, IPADDR2, IPADDR3);
  uip_sethostaddr(ipaddr);
  httpd_init();

  while (TRUE) {
    chEvtDispatch(evhndl, chEvtWaitOne(ALL_EVENTS));
  }
  return 0;
}

/*! \brief ADIS DIO1 thread
 *
 * For burst mode transactions t_readrate is 1uS
 *
 */
static msg_t Thread_adis_dio1(void *arg) {
	(void)arg;
	static const evhandler_t evhndl_dio1[]       = {
			adis_burst_read_handler,
			//adis_read_id_handler,
			adis_spi_cb_txdone_handler,
			adis_release_bus
	};
	struct EventListener     evl_dio;
	struct EventListener     evl_spi_ev;
	struct EventListener     evl_spi_release;

	chRegSetThreadName("adis_dio");

	chEvtRegister(&adis_dio1_event,           &evl_dio,         0);
	chEvtRegister(&adis_spi_cb_txdone_event,  &evl_spi_ev,      1);
	chEvtRegister(&adis_spi_cb_releasebus,    &evl_spi_release, 2);

	while (TRUE) {
		chEvtDispatch(evhndl_dio1, chEvtWaitOneTimeout((EVENT_MASK(2)|EVENT_MASK(1)|EVENT_MASK(0)), US2ST(50)));
	}
	return -1;
}

static msg_t batterySurveyThd(void *arg)
{
  (void)arg;
  chRegSetThreadName ("battery survey");
  chEvtRegister(&powerOutageSource, &powerOutageListener, 1);
  chThdSleepMilliseconds (2000);

  register_adc_watchdog((uint32_t) ADC1, 4,
      V_ALERT, 0xfff, &powerOutageIsr);

  chEvtWaitOne(EVENT_MASK(1));
  chibios_logFinish ();
  chThdExit(0);
  return 0;
}

static msg_t PollIMUThread(void *arg){
	(void)arg;
	chRegSetThreadName("PollIMU");

	struct EventListener self_el;
	chEvtRegister(&imu_event, &self_el, 2);

	while (TRUE) {
		chEvtWaitAll(EVENT_MASK(0) && EVENT_MASK(4));
		mpu_i2c_read_data(0x3B, 14); // Read accelerometer, temperature and gyro data
		chEvtBroadcastFlags(&imu_event, EVENT_MASK(2));

	}
	return 0;
}

/*! \brief ADIS Newdata Thread
 */
static msg_t Thread_adis_newdata(void *arg) {
	(void)arg;
	chRegSetThreadName("adis_newdata");

	static const evhandler_t evhndl_newdata[]       = {
			adis_newdata_handler
	};
	struct EventListener     evl_spi_cb2;

	chEvtRegister(&adis_spi_cb_newdata, &evl_spi_cb2, 0);

	while (TRUE) {
		chEvtDispatch(evhndl_newdata, chEvtWaitOneTimeout((eventmask_t)1, US2ST(50)));
	}
	return -1;
}

static msg_t Thread_mpu9150_int(void* arg) {
	(void) arg;
	static const evhandler_t evhndl_mpu9150[]       = {
			mpu9150_int_event_handler
	};
	struct EventListener     evl_mpu9150;

	chRegSetThreadName("mpu9150_int");

	chEvtRegister(&mpu9150_int_event,           &evl_mpu9150,         0);

	while (TRUE) {
		chEvtDispatch(evhndl_mpu9150, chEvtWaitOneTimeout(EVENT_MASK(0), MS2ST(50)));
	}
	return -1;
}