void AudioTest1::init()
{
    uart = Uart::getInstance();
    uart->init((volatile uint32_t *)UART_BASE);
    uart->puts("AudioTest1 StartUp...\r\n");
    i2cBus = new I2C((volatile uint32_t *)SND_I2C_SCL_BASE, (volatile uint32_t *)SND_I2C_SDA_BASE);
    soundCard = new SoundCard(i2cBus, (volatile uint32_t *)AUDIO_IF_0_BASE);
    soundCard->init();
}

void Uart::rxChar( void *src, char c )
{
    Uart *com;

    com = (Uart*)src;
    
    com->RXBuffer->putChar( c );
    if( com->echo ){
        com->TXBuffer->pushChar( c );
    } // if
    if( com->pipe ){
        com = (Uart*)com->getComUart( com->pipe );
        if( com ){
            com->TXBuffer->pushChar( c );
        } // if
    } // if
}

void* do_uart_thread(void* data) {
    
    Uart *x = (Uart*)data;
    struct timespec tim,tim2;
    uint8_t c;
    int i = 0;
    tim.tv_sec = 0;
    tim.tv_nsec = 100000L;
    while(1){
        nanosleep(&tim,&tim2);
        if(x->isReady() == READY){
            int n = x->readApi(&c);  // Doc data tu comport
            if(n > 0){
                x->getData(c);               
            }        
        }     
    }
}

// -------------------------------------------------------------------------
// UartManagerCL::scan
// -------------------------------------------------------------------------
// Regarde ce qui est connecte sur les ports non ouverts
// -------------------------------------------------------------------------
void UartManagerCL::scan(UartInfo infos[UART_PORT_NBR]) 
{
    for(int i=0; i<UART_PORT_NBR; i++) {
        infos[i].id = UART_NONE;
        if (uartListByPort_[i] != NULL){
            continue;
	}
        Uart uart;
	if (uart.open((UartPort)i)) {
            uart.ping();
            infos[i]=uart.getInfo();
            if (infos[i].id != UART_NONE) {
                LOG_DEBUG("Port %d: %s is connected\n", i, infos[i].name);
            } else {
                LOG_DEBUG("Port %d: no device\n", i);
            }
            uart.close();
	} else {
            LOG_DEBUG("Port %d: cannot be open... It is probably used by another application\n", i);
	}
    }
}

    virtual int getAction(void *data, int len,uint8_t *dataOut){
        uint8_t *rxBuf = (uint8_t*)data;
        memcpy(rxBuf,data,len);
        int txLength = 0;
        int tem = State::STATE_FREE;
        int temMode = rxBuf[ ZMQ_INSTRUCTION_IND];
        int temAddr = rxBuf[ ZMQ_ADDRESS_IND];
        uint16_t temData;
        int rxEnd = len;
        if(rxBuf[ZMQ_LENGTH_IND] == 2){ // neu nhan duoc du lieu co 2 byte
            temData = ((uint16_t)rxBuf[rxEnd - 5])*256  + (uint16_t)rxBuf[rxEnd - 4];
        } else{
            temData = rxBuf[rxEnd - 3];
        }
        if(temMode == INS_WRITE){
            std::cout<<"-I-Received Instruction Write Data\n";
            this->data[temAddr] = temData;
            switch (temAddr){
                case Address::ADR_UP:
                    mUart.putBytes(data,len);
                    mUart.putBytes(data,len);
                    break;
                case Address::ADR_DOWN:
                    mUart.putBytes(data,len);
                    mUart.putBytes(data,len);
                    break;
                case Address::ADR_STOP:
                    mUart.putBytes(data,len);
                    mUart.putBytes(data,len);
                    break;
                case Address::ADR_HEIGHT_SP:   
                    mUart.putBytes(data,len);
                    mUart.putBytes(data,len);
                    break;
                case Address::ADR_SAVE2POS1:
                    mUart.putBytes(data,len);
                    mUart.putBytes(data,len);
                    break;
                case Address::ADR_MOVE2POS1:
                    mUart.putBytes(data,len);

                    mUart.putBytes(data,len);
                    break;
                case Address::ADR_SAVE2POS2:
                    mUart.putBytes(data,len);
                    mUart.putBytes(data,len);
                    break;
                case Address::ADR_MOVE2POS2:
                    mUart.putBytes(data,len);
                    mUart.putBytes(data,len);
                    break;
                case Address::ADR_SAVE2POS3:
                    mUart.putBytes(data,len);
                    mUart.putBytes(data,len);
                    break;
                case Address::ADR_MOVE2POS3:
                    mUart.putBytes(data,len);
                    mUart.putBytes(data,len);
                    break;
                case Address::ADR_SAVE2POS4:
                    mUart.putBytes(data,len);
                    mUart.putBytes(data,len);
                    break;
                case Address::ADR_MOVE2POS4:
                    mUart.putBytes(data,len);
                    mUart.putBytes(data,len);
                    break;

                    
            }       
        } 
        else if(temMode == INS_READ){
            txLength = ZMQ_ADDRESS_IND + 4 + 2;
            uint8_t txBuf[txLength];

            txBuf[ZMQ_START_BYTE1_IND] = ZMQ_START_BYTE1;
            txBuf[ZMQ_START_BYTE2_IND] = ZMQ_START_BYTE2;
            txBuf[ZMQ_LENGTH_IND] = 2;
            txBuf[ZMQ_ADDRESS_IND] = temAddr;
            temData = this->data[temAddr];
            txBuf[ZMQ_ADDRESS_IND + 1] = (uint8_t)(temData >> 8);
            txBuf[ZMQ_ADDRESS_IND + 2] = (uint8_t)temData;

            #ifdef CONSOLE
                printf("TX Length = %d\n",txLength);
            #endif
            txBuf[txLength - 3] = this->getChecksum(txBuf,txLength);
            txBuf[txLength - 1] = ZMQ_STOP_BYTE1;
            txBuf[txLength - 2] = ZMQ_STOP_BYTE2;
            tem = State::getInstance()->STATE_GET_DATA;
            memcpy(dataOut,txBuf,txLength);
        }

void Beam::update()
{
    uint32_t button = InfraRood::getInstance()->read();
    cs->setHex(button);
    Uart *uart = Uart::getInstance();

    switch (button >> 16)
    {
    case ::TerasicRemote::A:
        uart->puts("A\r\n");
        break;
    case TerasicRemote::B:
        uart->puts("B\r\n");
        break;
    case TerasicRemote::C:
        uart->puts("C\r\n");
        break;
    case TerasicRemote::POWER:
        uart->puts("Power\r\n");
        break;
    case TerasicRemote::NUM_0:
        uart->puts("0\r\n");
        break;
    case TerasicRemote::NUM_1:
        uart->puts("1\r\n");
        break;
    case TerasicRemote::NUM_2:
        uart->puts("2\r\n");
        break;
    case TerasicRemote::NUM_3:
        uart->puts("3\r\n");
        break;
    case TerasicRemote::NUM_4:
        uart->puts("4\r\n");
        break;
    case TerasicRemote::NUM_5:
        uart->puts("5\r\n");
        break;
    case TerasicRemote::NUM_6:
        uart->puts("6\r\n");
        break;
    case TerasicRemote::NUM_7:
        uart->puts("7\r\n");
        break;
    case TerasicRemote::NUM_8:
        uart->puts("8\r\n");
        break;
    case TerasicRemote::NUM_9:
        uart->puts("9\r\n");
        break;
    case TerasicRemote::CH_UP:
        uart->puts("Channel Up\r\n");
        break;
    case TerasicRemote::CH_DOWN:
        uart->puts("Channel Down\r\n");
        break;
    case TerasicRemote::VOL_UP:
        uart->puts("Volume Up\r\n");
        break;
    case TerasicRemote::VOL_DOWN:
        uart->puts("Volume Down\r\n");
        break;
    case TerasicRemote::MENU:
        uart->puts("Menu\r\n");
        break;
    case TerasicRemote::RETURN:
        uart->puts("Return\r\n");
        break;
    case TerasicRemote::PLAY:
        uart->puts("Play\r\n");
        break;
    case TerasicRemote::LEFT:
        uart->puts("Left\r\n");
        break;
    case TerasicRemote::RIGHT:
        uart->puts("Right\r\n");
        break;
    case TerasicRemote::MUTE:
        uart->puts("Mute\r\n");
        break;
    default:
        uart->puts("Onbekende knop ingeduwd\r\n");
        break;
    }
}

/// Entry point of application.
int main(){
  //Pin connected to the rx pin of the Qik motor controller.
  int qTx = 25;
  //Pin connected to the tx pin of the Qik motor controller.
  int qRx = 26;
  //Baud rate used to communicate with the Qik motor controller.
  int qBaud = 115200;
  
  //Pin connected to the photointerrupter of the encoder on the left motor.
  int ePin1 = 10;
  //Pin connected to the photointerrupter of the encoder on the right motor.
  int ePin2 = 11;
  
  //Pin connected to the ultrasonic sensor at the front of the rosbee on the left side.
  int ussPin1 = 5;
  //Pin connected to the ultrasonic sensor at the front of the rosbee in the middle.
  int ussPin2 = 6;
  //Pin connected to the ultrasonic sensor at the front of the rosbee on the right side.
  int ussPin3 = 7;
  //Pin connected to the ultrasonic sensor at the back of the rosbee on the left side.
  int ussPin4 = 8;
  //Pin connected to the ultrasonic sensor at the back of the rosbee on the right side.
  int ussPin5 = 4;
  //Pin connected to the ultrasonic sensor at the back of hte rosbee int the middle.
  int ussPin6 = 9;
  
  //If you wish to send debug information to the console you need to make the propeller wait a sec.
  //The propeller is faster then the startup of the console. This will result in data being missed. 
  //Uncomment while debugging.
  //sleep(1);
  
  //Uart object for communication.
  Uart uart;
  //Qik object for motor control.
  Qik qik{qTx,qRx,qBaud};
  //Stop  motor 1.
  //This is done so the rosbee won't drive away and/or stop while the 
  //program is rebooted.
  qik.setMotorSpeed(Qik::Motor::M0,0);
 
  //Stop  motor 2.
  //This is done so the rosbee won't drive away and/or stop while the 
  //program is rebooted.
  qik.setMotorSpeed(Qik::Motor::M1,0);
  
  //Encoder object for the left motor.
  Encoder enc1{ePin1};
  //Encoder object fot the right motor.
  Encoder enc2{ePin2};
  
  //Ultrasonic sensor object for the sensor front left. 
  UltraSonicSensor uss1(ussPin1);
  //Ultrasonic sensor object for the sensor front middle.
  UltraSonicSensor uss2(ussPin2);
  //Ultrasonic sensor object for the sensor front right.
  UltraSonicSensor uss3(ussPin3);
  //Ultrasonic sensor object for the sensor back left.
  UltraSonicSensor uss4(ussPin4);
  //Ultrasonic sensor object for the sensor front right.
  UltraSonicSensor uss5(ussPin5);
  
  //Variables used for communcation.
  //cmd = command byte received.
  //value = follow byte received.
  //rtn = byte to be send.
  //intRtn = int(4 bytes) to be send.
  //speed = motor speed (can be negative).
  char cmd, value, rtn;
  int intRtn;
  signed char speed;
  
  //Run forever.
  //The rosbee is expected to work as long as it has power.
  //Therefore  this loop never needs to end.
  while(true){
    //Get the command byte.
    //This will block if no byte is available.
    cmd = uart.readChar();
    
    //Check which command to execute.
    //This is just a epic long switch case.
    //There was honestly no better way to do this that does
    //not require making infinite classes.
    switch(cmd){
      //Motors
      //Commands regarding the motor controllers.
      case PPP::SET_SPEED_M0:
        speed = uart.readChar();
        qik.setMotorSpeed(Qik::Motor::M0,speed);
        break;
      case PPP::SET_SPEED_M1:
        speed = uart.readChar();
        qik.setMotorSpeed(Qik::Motor::M1,speed);
        break;
      case PPP::SET_BRAKE_SPEED_M0:
        value = uart.readChar();
        qik.setBrakePower(Qik::Motor::M0,value);
        break;
      case PPP::SET_BRAKE_SPEED_M1:
//.........这里部分代码省略.........

int main() {
    char buf[5] = {'h', 'e', 'l', 'l', 'o'};
    Uart test = Uart(1);
    test.transmit(buf, 5);
    
}

extern "C" void LIBH_read(uint64_t addr, uint8_t *buf, int size) {
    if (addr >= 0x80000000 && addr < 0x80001000) {
        // GPIO
    } else if (uart0.isAddrValid(addr)) {
        uart0.read(addr, buf, size);
    } else if (irqctrl.isAddrValid(addr)) {
        irqctrl.read(addr, buf, size);
    } else if (addr >= 0x80005000 && addr < 0x80006000) {
        // GP timers
    } else {
        std::cout << "Unmapped access\n";
    }
}

static CameraPacket getPacket(void)
{
    T2CON = 0x0020;                 // disabled, prescaled, internal oscillator
    TMR2 = 0;                       // clear the timer
    PR2 = CLOCKS_PER_SEC/64/10 - 1; // interrupt at 1kHz (1ms)
    _T2IE = ON;                     // enable Timer2 to interrupt
    _T2IF = OFF;                    // clear the interrupt flag
    _T2IP = 4;                      // set priority to real-time
    T2CONbits.TON = 1;

    CameraPacket inPacket = newCameraPacket();
    while(cameraComPort.size() < 8) if(!T2CONbits.TON) break;
    cameraComPort.receive(inPacket.raw,8);
    return inPacket;
}

INTERRUPT_HANDLER(radioIrq, EXTI1_vector)
{
  uint8_t status = radio.readStatus ();
  radio.writeRegister (STATUS, status);
  //status &=0xF0;
  if (status&(1 << 6))
  {
     value.previus = value.current;
      radio.writeRegister (STATUS, status);
      value.current = radio.receiveByte ();
      uart1.transmit ("Data received: ");
      uart1.transmit (value.current);
      for (uint8_t i=0;i<5;++i)
      {
        if (value.current& (1 << i))triacs [i]->set();
        else triacs [i]->clear ();
      }
  }
  /*switch (status>>4)
  {
  case 1:
    {
      radio.comm (FLUSH_TX);
      uart1.transmit ("Max number retransmit");
      break;
    }
  case 2:
    {
      uart1.transmit ("Data sent");
      break;
    }
  case 4:
    {
      value.previus = value.current;
      radio.writeRegister (STATUS, status);
      value.current = radio.receiveByte ();
      uart1.transmit ("Data received: ");
      uart1.transmit (value.current);
      for (uint8_t i=0;i<5;++i)
      {
        if (value.current& (1 << i))triacs [i]->set();
        else triacs [i]->clear ();
      }
      break;
    }
  }   */
  
}

extern "C" void LIBH_create_dispatcher(void *entry_point) {
    DWORD tmp;
    tcs_simple *ctx = (tcs_simple *)idle_stack;
    ctx->regs[REG_RA] = (uint64_t)idle_task_entry;
    ctx->regs[REG_A0] = (uint64_t)entry_point;
    LIBH_create_thread(idle_stack, IDLE_STACK_SIZE, 0, 0);
    current_idx = 0;
    //printf_s("Event[%d] idle_tick set\n", current_idx);
    SetEvent(vecThreads[current_idx].hEvent);

    while (1) {
        tmp = SuspendThread(vecThreads[current_idx].hThread);
        wasPreemtiveSwitch = false;

        irqctrl.raise_interrupt(3); // GPTimer

        if (_kbhit()) {
            uart0.putRx(_getch());
            irqctrl.raise_interrupt(1);
        }

        if (wasPreemtiveSwitch) {
            wasPreemtiveSwitch = false;
        } else {
            // @retvalue 0 the specified thread was not suspended. 
            //           1, the specified thread was suspended but was restarted. 
            //          >1, the specified thread is still suspended.
            tmp = ResumeThread(vecThreads[current_idx].hThread);
        }

        Sleep(100);
    }
}

void LXSAMD21DMX::startOutput ( void ) {
	if ( _direction_pin != DIRECTION_PIN_NOT_USED ) {
		digitalWrite(_direction_pin, HIGH);
	}

	if ( _interrupt_status == ISR_INPUT_ENABLED ) {
		stop();
	}
	
	if ( _interrupt_status == ISR_DISABLED ) {	//prevent messing up sequence if already started...
	  SerialDMX.begin(DMX_BREAK_BAUD, (uint8_t)SERIAL_8N2);
  
	  // Assign pin mux to SERCOM functionality (must come after SerialDMX.begin)
	  pinPeripheral(PIN_DMX_RX, PIO_SERCOM_ALT);
	  pinPeripheral(PIN_DMX_TX, PIO_SERCOM_ALT);
	  
	  _interrupt_status = ISR_OUTPUT_ENABLED;
	  _shared_dmx_data = dmxData();
	  _shared_dmx_slot = 0;              
	  _shared_dmx_state = DMX_STATE_START;

	  SERCOM4->USART.INTENSET.reg =  SERCOM_USART_INTENSET_TXC | SERCOM_USART_INTENSET_ERROR;
	  SERCOM4->USART.DATA.reg = 0;  

	}
}

static CameraError retrievePic(String imgName)
{
    setPowerOutput(ON);
    /**
     * Increasing Power On Period
     *
     * Although the data sheet says that the camera needs ~1.5 sec to start-up,
     * in practice 2 sec makes for a much more reliable situation.
     */
    wait(2000);//1500);

    // initialize camera and image storage
    cameraComPort.init();
    cameraComPort.baudrate(14400);
    //imageFile = getFileStream();
    imageFile = FSfopen(imgName, "w");
    CameraError error = NO_FILE;

    //if(imageFile.open)
    if(imageFile)
    {
        //FSfclose(FSfopen(imgName, "w")); // erase file
        //imageFile.open(imgName); // open file
        error = getPicture();
        FSfclose(imageFile);
        imageFile = NullPtr;
        //imageFile.close();
    }

    //imageFile.free();

    setPowerOutput(OFF);
    wait(1000);

    return error;
}

void SERCOM3_Handler()
{
  Serial.IrqHandler();
}

void UART1_RX_TX_IRQHandler(void)
{
    SerialCloud.IrqHandler();
    Dash.wakeFromSleep();
}

void UART2_RX_TX_IRQHandler(void)
{
    Serial2.IrqHandler();
    Dash.wakeFromSleep();
}

void SERCOM4_Handler()
{
  Serial2.IrqHandler();
}

static CameraError getPicture(void)
{
    toCam = newCameraPacket(),
    fromCam = newCameraPacket();
    toCam.cmdToken = SYNC;
    int syncTries = 0;

    //if(!imageFile.open)
    if(!imageFile)
        return NO_FILE;

    /* try and get sync with camera */
    while(!(fromCam.cmdToken == ACK && fromCam.parameter1 == SYNC)) // until ACK received
    {
        if(syncTries == 3)
            cameraComPort.baudrate(115200);
        cameraComPort.flush();
        sendPacket(toCam); // send SYNC command
        pause(25);
        fromCam = getPacket(); // get reply

        if(syncTries < 7)
            ++syncTries;
        else
            return NO_HANDSHAKE;
    }

    /* acknowledge camera's sync request */
    toCam = newCameraPacket(); // initialize packet
    toCam.cmdToken = ACK;      // acknowledge
    toCam.parameter1 = SYNC;   // the sync

    syncTries = 0;
    while(!(fromCam.cmdToken == SYNC)) // wait for camera SYNC request
    {
        fromCam = getPacket();

        if(syncTries < 7)
            ++syncTries;
        else
            return NO_SYNC;
    }

    sendPacket(toCam);

    /* configure camera */
    toCam = newCameraPacket(); // initialize packet
    toCam.cmdToken = INITIAL;  // initial configuration
    toCam.parameter1 = 0x04;   // 115,200 baud rate
    toCam.parameter2 = 0x87;   // compress color
    toCam.parameter3 = 0x01;   // 80x60 preview resolution
    toCam.parameter4 = 0x07;   // 640x480 compression resolution
    sendPacket(toCam);

    /* if camera acknowledges changes, change the UART baud rate */
    fromCam = getPacket();
    if(!(fromCam.cmdToken == ACK && fromCam.parameter1 == INITIAL))
        return NO_INITIAL;// if reconfiguration was not successful

    cameraComPort.baudrate(115200); // change UART baud rate

    /* specify image quality */
    toCam = newCameraPacket(); // initialize packet
    toCam.cmdToken = QUALITY;  // configure picture quality
    toCam.parameter1 = 0x00;   // to be the best
    sendPacket(toCam);

    /* if camera acknowledges change, then get an image */
    fromCam = getPacket();
    if(!(fromCam.cmdToken == ACK && fromCam.parameter1 == QUALITY))
        return NO_QUALITY;// if reconfiguration was not successful

    /* get an image */
    toCam = newCameraPacket(); // initialize packet
    toCam.cmdToken = GET_PIC;  // configure picture quality
    toCam.parameter1 = 0x05;   // get full size image
    sendPacket(toCam);

    /* if camera acknowledges request, then retrieve image data */
    fromCam = getPacket();
    if(!(fromCam.cmdToken == ACK && fromCam.parameter1 == GET_PIC))
        return NO_GET_PIC; // if request was not successful

    /* get image size */
    fromCam = getPacket();
    if(!(fromCam.cmdToken == DATA))
        return NO_DATA; // if request was not successful

    /* read data size */
    picsize = bytesToGet = fromCam.parameter2 + fromCam.parameter3 * 0x100LL + fromCam.parameter4 * 0x10000LL;

    int cnt = 0;

    /* store the image */
    while(bytesToGet > 0) // until all bytes retrieved
    {
        tempSize = cameraComPort.receive(tempBuffer, RX_BUFFER_SIZE); // receive the bytes
        //imageFile.write(tempBuffer,tempSize); // store the bytes
        FSfwrite(tempBuffer, sizeof(char), tempSize, imageFile);
        bytesToGet -= tempSize; // update bytes remaining
//.........这里部分代码省略.........

static void sendPacket(CameraPacket packetToSend)
{
    cameraComPort.send(packetToSend.raw,8);
}