/*
 * logical level switch I/O thread
 */
msg_t thdJbus485IO(void *arg)
{
    (void)arg;

    chRegSetThreadName("thd JBUS 485 IO");
    while  (initJbus485 () != TRUE) {
        chThdSleepMilliseconds(1000);
        if (chThdShouldTerminate())
            goto cleanAndExit;
    }

    chThdSleepMilliseconds(10);

    do {
        eMBPoll ();
    }  while (!chThdShouldTerminate());


cleanAndExit:
    eMBDisable ();
    eMBClose ();
    DebugTrace ("thdJbusIO 485 Thread Is stopping");

    return 0;
}

// Caller: write thread
msg_t SampleBuffer::exec(const char * filename)
{
  FIL f;
  FRESULT res = f_open(&f, filename, FA_WRITE | FA_CREATE_ALWAYS);
  if (res != FR_OK) {
    tp_write_thread_ = 0;
    return systemstate::FATFS_f_open_error;
  }
  uint8_t buffer_to_write = 0;
  UINT bytes_written = 0;
  msg_t write_errors = 0;

  while (1) {
    if (chThdShouldTerminate())
      break;
    if (buffer_to_write == active_buffer_) {
      chThdSleepMilliseconds(constants::loop_period_ms);
      continue;
    }
    res = f_write(&f, &buffer[buffer_to_write][0],
                  bytes_per_buffer, &bytes_written);
    write_errors += (res != FR_OK) || (bytes_written != bytes_per_buffer);
    buffer_to_write = (buffer_to_write + 1) % number_of_buffers;
  }

  res = f_write(&f, &buffer[active_buffer_][0], buffer_index_, &bytes_written);
  write_errors += (res != FR_OK) || (bytes_written != buffer_index_);
  res = f_close(&f);
  write_errors += (res != FR_OK);

  return write_errors;
}

static msg_t OscAutosendThread(void *arg)
{
  UNUSED(arg);
  uint8_t i;
  const OscNode* node;
  OscChannelData* chd;

  while (!chThdShouldTerminate()) {
    if (osc.autosendDestination == NONE) {
      sleep(250);
    }
    else {
      chd = oscGetChannelByType(osc.autosendDestination);
      i = 0;
      node = oscRoot.children[i++];
      chMtxLock(&chd->lock);
      while (node != 0) {
        if (node->autosender != 0)
          node->autosender(osc.autosendDestination);
        node = oscRoot.children[i++];
      }
      oscSendPendingMessages(osc.autosendDestination);
      chMtxUnlock();
      sleep(osc.autosendPeriod);
    }
  }
  return 0;
}

static msg_t thread1(BaseSequentialStream *chp) {
  systime_t time = chTimeNow();    
  /* 
   * Thread-specific parameters
  */
  chRegSetThreadName("t1");
  static int d = 300;
  static int w = 75000;
  volatile uint32_t i, n;
  while(1) {
  if(chThdShouldTerminate())
    return 0;
  palSetPad(GPIOD, GPIOD_LED3);  
  /* 
   * Deadline for current execution of this task
  */ 
  time += d;            
  chprintf(chp, "%s  N  %d   %d   %d  %d\r\n",  chRegGetThreadName(chThdSelf()), chThdGetPriority(),
						chTimeNow(), time, chThdGetTicks(chThdSelf()));
  /* 
   * Do some "work"
  */
  for(i = 0; i < w; i ++) { 
    n = i / 3;
  }
  chprintf(chp, "%s  X  %d   %d   %d  %d\r\n",  chRegGetThreadName(chThdSelf()), chThdGetPriority(),
						chTimeNow(), time, chThdGetTicks(chThdSelf()));
  palClearPad(GPIOD, GPIOD_LED3);   
  /* 
   * Yield control of CPU until the deadline (which is also beginning of next period)
  */
  chThdSleepUntil(time);
  } 
  return 0;
}

static msg_t clarityMgmtConnectivityMonitoringThd(void *arg)
{
    (void)arg;
    
    #if CH_USE_REGISTRY == TRUE
    chRegSetThreadName(__FUNCTION__);
    #endif

    while (chThdShouldTerminate() == FALSE)
    {
        chThdSleep(MS2ST(500));

#if 0
        /* Check for disconnect */
        clarityMgmtCheckNeedForConnectivty();
#endif

# if 0
        /* Check to see if we can power down */
        clarityMgmtAttemptPowerDown();
#endif

    }

    return CLARITY_SUCCESS;
}

// Driver control task
msg_t
vexOperator( void *arg )
{
    (void)arg;

    // Must call this
    vexTaskRegister("operator");

    // Start manual driving
    StartTask( DriveTask );

    // start the Arm PID task
    StartTask( ArmPidController );
    // start the claw motor controller
    StartTask( ClawController );

    // start manual arm/claw control
    StartTask( ManualArmClawTask );

    // Run until asked to terminate
    while(!chThdShouldTerminate())
        {
        // Don't hog cpu
        vexSleep( 25 );
        }

    return (msg_t)0;
}

static msg_t can_tx(void * p) {
  CANTxFrame txmsg_can1;
  CANTxFrame txmsg_can2;

  (void)p;
  chRegSetThreadName("transmitter");
  txmsg_can1.IDE = CAN_IDE_EXT;
  txmsg_can1.EID = 0x01234567;
  txmsg_can1.RTR = CAN_RTR_DATA;
  txmsg_can1.DLC = 8;
  txmsg_can1.data32[0] = 0x55AA55AA;
  txmsg_can1.data32[1] = 0x00FF00FF;

  txmsg_can2.IDE = CAN_IDE_EXT;
  txmsg_can2.EID = 0x0ABCDEF0;
  txmsg_can2.RTR = CAN_RTR_DATA;
  txmsg_can2.DLC = 8;
  txmsg_can2.data32[0] = 0x66AA66AA;
  txmsg_can2.data32[1] = 0x44FF44FF;

  while (!chThdShouldTerminate()) {
    canTransmit(&CAND1, CAN_ANY_MAILBOX, &txmsg_can1, MS2ST(100));
    canTransmit(&CAND2, CAN_ANY_MAILBOX, &txmsg_can2, MS2ST(100));
    chThdSleepMilliseconds(500);
  }
  return 0;
}

static msg_t uart_proto_ps(void *p)
{
    (void)p;
    CANTxFrame txmsg;
    msg_t tmp;

    chRegSetThreadName("uart_proto");
    while(!chThdShouldTerminate())
    {
        GET_CHAR(tmp);
        switch(tmp) {
            case 0xCB: // Standart frame
                {
                    // Make frame on fly
                    txmsg.IDE = CAN_IDE_STD;
                    GET_CHAR(tmp); // Get flags
                    txmsg.DLC = tmp & 0x0F; // Extract data len from flags
                    txmsg.SID
                    uint8_t len = flags & 0x0F;
                    uint16_t sid;
                    
                }
                break;
            case 0xCE: // Extended frame
                break;
    }
    return 0;
}

void uartpInit(void)
{
    sdStart(&SD_PS, &sc);
    chThdCreateStatic(uart_proto_wa, sizeof(uart_proto_wa), NORMALPRIO + 7, uart_proto_ps, NULL);
}

static msg_t clarityMgmtResponseMonitoringThd(void *arg)
{
    uint32_t attempts = 0;

    (void)arg;
    
    #if CH_USE_REGISTRY == TRUE
    chRegSetThreadName(__FUNCTION__);
    #endif

    while (chThdShouldTerminate() == FALSE)
    {
        if (chMtxTryLock(cc3000Mtx) == TRUE)
        {
            chMtxUnlock();
            attempts = 0;
        }
        else 
        {
            attempts++;
            if (attempts == CC3000_MUTEX_POLL_COUNT)
            {
                unresponsiveCb();
            }
        }

        chThdSleep(MS2ST(CC3000_MUTEX_POLL_TIME_MS));
    }
    return CLARITY_SUCCESS;
}

/**
 * Command processing thread.
 */
msg_t CmdExecutor::main(void){
  this->setName("CmdExecutor");
  eventmask_t evt = 0;
  struct EventListener el_command_long;
  chEvtRegisterMask(&event_mavlink_in_command_long, &el_command_long, EVMSK_MAVLINK_IN_COMMAND_LONG);

  /* wait modems */
  while(GlobalFlags.messaging_ready == 0)
    chThdSleepMilliseconds(50);

  /* main loop */
  while(!chThdShouldTerminate()){
    evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_COMMAND_LONG, MS2ST(50));

    switch (evt){
    case EVMSK_MAVLINK_IN_COMMAND_LONG:
      executeCmd(&mavlink_in_command_long_struct);
      break;

    default:
      break;
    }
  }

  chEvtUnregister(&event_mavlink_in_command_long, &el_command_long);
  chThdExit(0);
  return 0;
}

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;
}

static msg_t
VexSonarTask( void *arg )
{
    tVexSonarChannel    c;

    (void)arg;

    chRegSetThreadName("sonar");

    gptStart( sonarGpt, &vexSonarGpt );

    while(!chThdShouldTerminate())
        {
        if( vexSonars[nextSonar].flags == (SONAR_INSTALLED | SONAR_ENABLED) )
            {
            // ping sonar
            vexSonarPing(nextSonar);

            // wait for next time slot
            // the timer is set to timeout in 40mS but we need a 10mS gap before any more
            // pings can be sent
            chThdSleepUntil(chTimeNow() + 50);

            // calculate echo time
            vexSonars[nextSonar].time = vexSonars[nextSonar].time_f - vexSonars[nextSonar].time_r;

            // was the time too great ?
            if( vexSonars[nextSonar].time > 35000 )
                vexSonars[nextSonar].time = -1;

            // if we have a valid time calculate real distance
            if( vexSonars[nextSonar].time != -1 )
                {
                vexSonars[nextSonar].distance_cm = vexSonars[nextSonar].time   / 58;
                vexSonars[nextSonar].distance_inch = vexSonars[nextSonar].time / 148;
                }
            else
                {
                vexSonars[nextSonar].distance_cm = -1;
                vexSonars[nextSonar].distance_inch = -1;
                }

            // look for next sonar
            for(c=kVexSonar_1;c<kVexSonar_Num;c++)
                {
                if( ++nextSonar == kVexSonar_Num )
                    nextSonar = kVexSonar_1;

                // we need sonar to be installed and enabled
                if( vexSonars[nextSonar].flags == (SONAR_INSTALLED | SONAR_ENABLED) )
                    break;
                }
            }
        else
            // Nothing enabled, just wait
            chThdSleepMilliseconds(25);
        }

    return (msg_t)0;
}

static msg_t thread3(void *p) {

  (void)p;
  while (!chThdShouldTerminate())
    chSemWait(&sem1);
  return 0;
}

msg_t Light::moduleThread(void* arg) {

	(void) arg;

	bool noRecall = true;
	LedData* state;

	while (!chThdShouldTerminate()) {
		chSemWait(&_sem);

		while (TRUE) {
			noRecall = true;

			for (uint8_t i = 0; i < N_LEDS; ++i) {
				if (!data[i].isEmpty()) {
					state = data[i].getHead();

					switch (state->state) {
						case FADE:
							state->current.setRGB(state->startColor.getR() + state->diff.getR() * state->steps / state->totalSteps,
									state->startColor.getG() + state->diff.getG() * state->steps / state->totalSteps,
									state->startColor.getB() + state->diff.getB() * state->steps / state->totalSteps
									);

							leds[i].shine(state->current);

							state->steps++;

							if (state->steps == state->totalSteps) {
								leds[i].shine(state->endColor);
								data[i].pop();

								if (!data[i].isEmpty())
									noRecall = false;
							}
							else
								noRecall = false;

							break;

						case SHINE:
							break;

						case INACTIVE:
							break;
					}
				}
			}

			waitMs(_threadDelay);

			if (noRecall)
				break;
		}
	}

	return (msg_t)0;
}

static msg_t can_tx(void * p) {
  (void)p;
  CANTxFrame txmsg_can1;
  chRegSetThreadName("transmitter");
  txmsg_can1.IDE = CAN_IDE_STD;
  txmsg_can1.EID = 0x412;
  txmsg_can1.RTR = CAN_RTR_DATA;
  
  // read the trimpot via ADC to decide what step commands to send out
  // uses low-level access to implement the simplest possible polled readout

#define AD0CR_PDN                   (1UL << 21)
#define AD0CR_START_NOW             (1UL << 24)
#define AD0CR_CHANNEL5              (1UL << 5)
#define LPC17xx_ADC_CLKDIV          12
#define AD0CR_START_MASK            (7UL << 24)
  
  // set up the ADC and pin
  LPC_SC->PCONP |= (1UL << 12);       // enable ADC power
  LPC_ADC->CR = AD0CR_PDN | (LPC17xx_ADC_CLKDIV << 8) | AD0CR_CHANNEL5;
  LPC_PINCON->PINSEL3 |= 0b11 << 30;  // enable P1.31 as AD0.5

  uint8_t lastCmd = 0;
  while (!chThdShouldTerminate()) {
    chThdSleepMilliseconds(10); // avoid looping without yielding at all
    // perform one ADC conversion
    LPC_ADC->CR &= ~AD0CR_START_MASK;
    LPC_ADC->CR |= AD0CR_START_NOW;
    while ((LPC_ADC->GDR & (1<<31)) == 0)
      ;
    int sample = ((LPC_ADC->GDR >> 4) & 0xFFF) - 0x800; // signed
    
    uint8_t cmd = 0x14; // enable w/ half-step microstepping
    if (sample < 0) {
      cmd ^= 0x02; // direction
      sample = -sample;
    }
    if (sample < 100)
      continue; // dead zone, no stepping
    if (cmd != lastCmd) {
      // transmit a 1-byte "command" packet with the enable and direction bits
      txmsg_can1.DLC = 1;
      txmsg_can1.data8[0] = lastCmd = cmd;
    } else {
      sample /= 2; // >= 50
      // transmit a 0-byte "step" packet, but delay 1..950 ms before doing so
      if (sample > 999)
        sample = 999;
      chThdSleepMilliseconds(1000-sample);
      txmsg_can1.DLC = 0;
    }
    canTransmit(&CAND1, 1, &txmsg_can1, 100);
    echoToBridge(&txmsg_can1);
  }
  return 0;
}

/*
 * Console print server done using synchronous messages. This makes the access
 * to the C printf() thread safe and the print operation atomic among threads.
 * In this example the message is the zero termitated string itself.
 */
static msg_t console_thread(void *arg) {

  (void)arg;
  while (!chThdShouldTerminate()) {
    puts((char *)chMsgWait());
    fflush(stdout);
    chMsgRelease(RDY_OK);
  }
  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;
}

/*------------------------------------------------------------------------*
 * 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;
}

static msg_t CrossFromModemThread(void *arg) {
  chRegSetThreadName("CrossFromModem");
  (void)arg;
  uint8_t c;

  while (!chThdShouldTerminate()) {
    c = sdGet(&SDGSM);
    sdPut(&SDDM, c);
  }
  return 0;
}

static msg_t 
thBlinker(void *arg) 
{
	(void)arg;
	chRegSetThreadName("Blinker");
	while( !chThdShouldTerminate() )
	{
		chThdSleepMilliseconds(1000);
	}
	return 0;
}