int main()
{

	std::cout << "just work";
	 XBee xB;
	std::cout << "created xB";
	xB.setup();
	std::cout << "after setup";
	

/*	UDP udp;

	udp.setup();

	for (int i = 0;i < 20;i++) {
	udp.sendCommand(1);
	}

	usleep(3000000);

	for (int i = 0; i < 20;i++) {
	udp.sendCommand(7);	
	}*/

	return 0;
}

/* Transmission Code */
void Transmit_Packet(void)
{   
    /* Create Xbee object */
    XBee xbee = XBee();

    /* Packet to be transmitted */
    uint8_t payload[MAX_SIZE];

    /* Obtain address of receiving end */
    XBeeAddress64 addr64 = XBeeAddress64(0x0, 0x0);
    ZBTxStatusResponse txStatus = ZBTxStatusResponse();

    /* Clear the payload */
    memset(payload, '\0', sizeof(payload));

    /* Debug Packet */
    //payload[0] = '1';
    //payload[1] = '5';

    /* Transfer information into payload */
    memcpy(payload, &G_packet, sizeof(payload));

    for(int i = 0; i < MAX_SIZE; i++)
    { Serial.write(payload[i]);}

    ZBTxRequest zbTx = ZBTxRequest(addr64, payload, sizeof(payload));
  
    /* Transfer the payload */
    xbee.send(zbTx); //!!Prints packet to serial monitor

    /* delay */
    delay(1000);
}

void Monitor::GetPacket(char* data, size_t len)
{
    int i;
    XBee x;

    for(i=0;i<len;i++)
    {
        switch( state )
        {
           case WAIT_FOR_PACKET_START: 
                if( data[i] == XBEE_START_CHAR ) 
                {
                    state = WAIT_FOR_LEN1;
                    //cmdBox->AppendText(_("Got Packet Start\n"));
                }
                break;
           case WAIT_FOR_LEN1: 
                state = WAIT_FOR_LEN2;
                packetsize = data[i];
                packetsize <<= 8;
                packetsize &= 0x0ff00;
                break;
           case WAIT_FOR_LEN2:
                state = RECEIVE_DATA;
                packetsize |= (0x0ff & data[i]);
                packetcount = 0;
                printf("Got Size: %02x\n", packetsize );
                break;
           case RECEIVE_DATA:
                if(data[i] == XBEE_ESC_CHAR)
                    state = GOT_ESCAPE;
                else
                {
                    packet[packetcount++] = data[i];
                    if( packetcount >= packetsize ) state = WAIT_FOR_CHECKSUM;
                }
                //printf("Data: %02x\n", data[i]);
                //cmd.Clear();
                //cmd << data[i];
                //cmdBox->AppendText(cmd);
                break;
           case GOT_ESCAPE:
                packet[packetcount++] = data[i] ^ 0x20;
                if( packetcount >= packetsize ) state = WAIT_FOR_CHECKSUM;
                else state = RECEIVE_DATA;
                break;
           case WAIT_FOR_CHECKSUM:
                if( x.Checksum(packet, packetcount) == data[i] & 0x0ff )
                {

                    printf("Got good checksum\n");
                    Process();
                }
                state = WAIT_FOR_PACKET_START;
                break; 
        }
    }
printf("exit getpacket\n");
}

int testAT()
{
   XBee x;
   int i, j, assert=1;
   char cmd[8];
   char *known = "\x7e\x00\x04\x08\x41\x53\x48\x1b";

   i = x.LocalAT((char*)"SH", (char*)NULL, 2, cmd);

   for(j=0;j<i;j++)
      if( cmd[j] != known[j] ) assert = 0;

   return assert;
}

void XBeeStrategy::run(){


	xbee.readPacket();

	if (xbee.getResponse().isAvailable()) {
		// got something

		if (xbee.getResponse().getApiId() == RX_16_RESPONSE) {
			// got a rx packet
			Rx16Response rx16 = Rx16Response();
			xbee.getResponse().getRx16Response(rx16);

			Serial.println( "**** Incoming transmission ***");
			char destination = rx16.getData(0);
			if(destination == 'a'){

				Serial.println( "Triggering us...");
				char mode = rx16.getData(1);
				if(mode == 'm'){
					Serial.println( "Changing direction");
					int direction = rx16.getData(2);
					int on = rx16.getData(3);
					switch(direction){
						case  'w':
							sputnik.stopMoving();
							sputnik.forward();
							break;
						case  'a':
							sputnik.stopMoving();
							sputnik.turn(1000, 0);
							break;
						case  's':
							sputnik.stopMoving();
							sputnik.backward();
							break;
						case  'd':
							sputnik.stopMoving();
							sputnik.turn(1000, 1);
							break;
						default:
							break;
					}

				}
			}
		}
	}
}

/* Initialize objects */
void setup()
{
  /* Variable Initialization */
  count = 0;

  /* Create XBee Object */
  XBee xbee = XBee();

  /* Initialization */
  init1(); //initialize sensors
  Serial.begin(9600);
  xbee.begin(Serial);
  //Serial.println("Main"); //debug main
  // initHealthSamples();
}

    boolean checkNDResponse() { 
      //mySerial.println("checkNDResponse");
      // wait a small bit so the animation looks good
      if (xbee.readPacket(ND_WAIT_TIME / 6)) {
        // got a response!

        // should be an AT command response
        if (xbee.getResponse().getApiId() == AT_COMMAND_RESPONSE) {
          xbee.getResponse().getAtCommandResponse(atResponse);

          if (atResponse.isOk()) {
            if (atResponse.getCommand()[0] == atCmd[0] && atResponse.getCommand()[1] == atCmd[1] && atResponse.getValueLength() > 3) {

              //mySerial.println(pack(atResponse.getValue()[2],atResponse.getValue()[3],atResponse.getValue()[4],atResponse.getValue()[5]));          
              //mySerial.println(pack(atResponse.getValue()[6],atResponse.getValue()[7],atResponse.getValue()[8],atResponse.getValue()[9]));
              
              addr64 = XBeeAddress64( pack(atResponse.getValue()[2],atResponse.getValue()[3],atResponse.getValue()[4],atResponse.getValue()[5]),pack(atResponse.getValue()[6],atResponse.getValue()[7],atResponse.getValue()[8],atResponse.getValue()[9]) );
              
              
              return true;
            }
          } 
          else {
            //nss.print("Command return error code: ");
            //nss.println(atResponse.getStatus(), HEX);
            nr(1);
          }
        } else {
          //nss.print("Expected AT response but got ");
          //nss.print(xbee.getResponse().getApiId(), HEX);
          nr(2);
        }   
      } else {
        // at command failed
        if (xbee.getResponse().isError()) {
          //nss.print("Error reading packet.  Error code: ");  
          //nss.println(xbee.getResponse().getErrorCode());
          nr(3);
        } 
        else {
          //nss.print("No response from radio");  
          nr(4);
        }
      }
      return false;
    }

EXPORT libxbee::Con::Con(XBee &parent, std::string type, struct xbee_conAddress *address) : parent(parent) {
	xbee_err ret;
	
	if ((xbee = parent.getHnd()) == NULL) throw(XBEE_EINVAL);
	
	if ((ret = xbee_conNew(xbee, &con, type.c_str(), address)) != XBEE_ENONE) throw(ret);
	if ((ret = xbee_conDataSet(con, (void*)this, NULL)) != XBEE_ENONE) {
		xbee_conEnd(con);
		throw(ret);
	}
	try {
		parent.conRegister(this);
	} catch (xbee_err ret) {
		xbee_conEnd(con);
		throw(ret);
	}
}

void XBeeActive::sendRemoteAtCommand()
{
	DEBAG.println("\nSending command sendRemoteAtCommand to the XBee");
	xbee.send(remoteAtRequest);

	// wait up to 5 seconds for the status response
	if (xbee.readPacket(5000))
	{
		// got a response!

		// should be an AT command response
		if (xbee.getResponse().getApiId() == REMOTE_AT_COMMAND_RESPONSE)
		{
			xbee.getResponse().getRemoteAtCommandResponse(remoteAtResponse);

			if (remoteAtResponse.isOk()) {
				DEBAG.print("Command [");
				DEBAG.print(remoteAtResponse.getCommand()[0]);
				DEBAG.print(remoteAtResponse.getCommand()[1]);
				DEBAG.println("] was successful!");

				if (remoteAtResponse.getValueLength() > 0) {
					DEBAG.print("Command value length is ");
					DEBAG.println(remoteAtResponse.getValueLength(), DEC);

					DEBAG.print("Command value: ");

					for (int i = 0; i < remoteAtResponse.getValueLength(); i++) {
						DEBAG.print(remoteAtResponse.getValue()[i], HEX);
						DEBAG.print(" ");
					}

					DEBAG.println("");
				}
			}
			else {
				DEBAG.print("Command returned error code: ");
				DEBAG.println(remoteAtResponse.getStatus(), HEX);
			}
		}
		else
		{
			DEBAG.print("Expected Remote AT response but got ");
			DEBAG.print(xbee.getResponse().getApiId(), HEX);
		}
	}
	else if (xbee.getResponse().isError())
	{
		DEBAG.print("Error reading packet.  Error code: ");
		DEBAG.println(xbee.getResponse().getErrorCode());
	}
	else
	{
		DEBAG.print("No response from radio3");
	}
}

void XBeeActive::sendAtCommand()
{
	int i10;
	xbee.send(atRequest);
	if (xbee.readPacket(5000)) // подождите до 5 секунд для ответа состояния
	{
		if (xbee.getResponse().getApiId() == AT_COMMAND_RESPONSE) // Должна быть команда отклика AT
		{
			xbee.getResponse().getAtCommandResponse(atResponse);

			if (atResponse.isOk())
			{
				//DEBAG.print("Command [");
				//DEBAG.print(atResponse.getCommand()[0]);
				//DEBAG.print(atResponse.getCommand()[1]);
				//DEBAG.println("] was successful!");

				if (atResponse.getValueLength() > 0)
				{
					Len_XBee = atResponse.getValueLength();
					//DEBAG.print("\nCommand value length is - ");
					//DEBAG.println(atResponse.getValueLength(), DEC);
					//DEBAG.print("Command value: ");
					int i11 = Len_XBee - 1;
					info_XBee_data1[0] = 0;
					info_XBee_data1[1] = 0;
					info_XBee_data1[2] = 0;
					info_XBee_data1[3] = 0;

					for (i10 = 0; i10 < atResponse.getValueLength(); i10++)
					{
						info_XBee_data[i10] = atResponse.getValue()[i10];
						//DEBAG.print(info_XBee_data[i10], HEX);
						//DEBAG.print(" ");
						info_XBee_data1[i11] = info_XBee_data[i10];
						i11--;
					}
					//DEBAG.println("");
				}
			}
			else
			{
				//DEBAG.print("Command return error code: ");
				//DEBAG.println(atResponse.getStatus(), HEX);
			}
		}
		else
		{
			//DEBAG.print("Expected AT response but got ");
			//DEBAG.println(xbee.getResponse().getApiId(), HEX);
		}
	}
	else
	{
		if (xbee.getResponse().isError())             // at command failed
		{
			//DEBAG.print("Error reading packet.  Error code: ");
			//DEBAG.println(xbee.getResponse().getErrorCode());
		}
		else
		{
			//DEBAG.println("No response from radio1");
		}
	}
	delay(200);
}

XBeeStrategy::XBeeStrategy(Sputnik _sputnik) : ControllerStrategy( _sputnik){
	xbee.begin(9600);
	Serial.println( "XbeeStrategy Initialized");
}

void XBeeActive::sendAtCommand_ar()
{
	int i10;

	Serial.println("\nSending command sendAtCommand_ar to the XBee");
	xbee.send(arRequestMod);      // send the command

								  // wait up to 5 seconds for the status response
	if (xbee.readPacket(5000))
	{

		// should be an AT command response
		if (xbee.getResponse().getApiId() == AT_COMMAND_RESPONSE)
		{
			xbee.getResponse().getAtCommandResponse(atResponse);

			if (atResponse.isOk())
			{

				//myGLCD.setColor(255 , 0, 0);

				//	Serial.print("Command [");
				//	Serial.print(atResponse.getCommand()[0]);
				//	Serial.print(atResponse.getCommand()[1]);
				//	Serial.println("] was successful!");

				if (atResponse.getValueLength() > 0)
				{
					Len_XBee = atResponse.getValueLength();
					//  Serial.print("Command value length is ");
					//  Serial.println(atResponse.getValueLength(), DEC);
					//  Serial.print("Command value: ");
					int i11 = Len_XBee - 1;
					info_XBee_data1[0] = 0;
					info_XBee_data1[1] = 0;
					info_XBee_data1[2] = 0;
					info_XBee_data1[3] = 0;

					for (i10 = 0; i10 < atResponse.getValueLength(); i10++)
					{
						info_XBee_data[i10] = atResponse.getValue()[i10];
						Serial.print(info_XBee_data[i10], HEX);
						info_XBee_data1[i11] = info_XBee_data[i10];
						i11--;
					}

					// Serial.println("");
				}
			}
			else
			{
				Serial.print("Command return error code: ");
				Serial.println(atResponse.getStatus(), HEX);

				//myGLCD.setColor(255, 0, 0);
				//myGLCD.fillRoundRect(278, 92, 318, 132);
				//myGLCD.setColor(255, 255, 255);
				//myGLCD.drawRoundRect(278, 92, 318, 132);
				//myGLCD.setBackColor(0, 0, 0);
				//delay(200);
				////XBee_alarm();
				//delay(1000);
				//myGLCD.setColor(0, 0, 0);
				//myGLCD.fillRoundRect(278, 92, 318, 132);
				//myGLCD.setColor(0, 0, 0);
				//myGLCD.drawRoundRect(278, 92, 318, 132);
				//	mcp_Out1.digitalWrite(Beep, LOW);                    // 
				delay(300);
			}
		}
		else
		{
			Serial.print("Expected AT response but got ");
			Serial.println(xbee.getResponse().getApiId(), HEX);
		}
	}
	else
	{
		// at command failed
		if (xbee.getResponse().isError())
		{
			Serial.print("Error reading packet.  Error code: ");
			Serial.println(xbee.getResponse().getErrorCode());
		}
		else
		{
			Serial.println("No response from radio2");
		}
	}
}

void XBeeActive::setup()
{
	xbee.begin(Serial1);
	DEBAG.println("\nXBee setup");
}

// Flex sensor task
void flexTask(void* p) {
	Serial.println("IN FLEX TASK");
	while (1) {
		
		if (deviceRdy) {
			int readRaw = analogRead(flexpin);
			//Serial.println(readRaw);
			int frameLen = 0;
			unsigned char outFrame[Q_SIZE];
			unsigned char outMsg[2];
			memcpy(outMsg, "4", 1);
		
			if (readRaw < 490 && handStatus == open) {
				if (counterForFlex[0] >= 5) {
					// send data to rpi to inform that hand has been closed for 2 cycles
					handStatus = close;
					memcpy(&outMsg[1], "1", 1);
					frameLen = xbee.Send(outMsg, 2, outFrame, RPI_ADDR);
					Serial1.write(outFrame, frameLen);
					Serial.println("ON");
					counterForFlex[0] = 0; //reset counter	
				}
				else {
					counterForFlex[0]++;	//increment counter
					counterForFlex[1] = 0;
				}
				
			} else if (readRaw >= 500 && handStatus == close) {
				if (counterForFlex[1] >= 5) {
					// send data to rpi to inform that hand has been opened for 2 cycles;
					handStatus = open;
					memcpy(&outMsg[1], "0", 1);
					frameLen = xbee.Send(outMsg, 2, outFrame, RPI_ADDR);
					Serial1.write(outFrame, frameLen);
					Serial.println("OFF");
					counterForFlex[1] = 0; //reset counter	
				}
				else {
					counterForFlex[1]++;	//reset counters
					counterForFlex[0] = 0;
				}
			}
			else {
				counterForFlex[0] = 0;
				counterForFlex[1] = 0;
			}
		}
		vTaskDelay(200);
	}
}

void setup() {
  Serial.begin(9600);
  xbee.setSerial(Serial);
  centerX = analogRead(JX);
  centerY = analogRead(JY);
  pinMode(SPIN, INPUT);
}

    //discover target node
    void discover() {
      //mySerial.println("discover");
      //if we don't get a address we can't fire
      while (true) {
        //send node discovery
        xbee.send(atRequest);
        
        //default value is that responding XBEE can wait up to six seconds before answering
        //so spamming it with node discoverys might be a bad thing, but waiting so long is booring so
        //we we'll try it and see if it works...
        
        //knight rider on the diodes let's the users know we're looking
        for (int i=0; i<CHANNELS; i++) {
          clearLeds();
          digitalWrite(channels[i % CHANNELS].led_pin,HIGH);

          if (checkNDResponse()) {
            return;
          }      
        }

        for (int i=CHANNELS-1; i>=0; i--) {
          clearLeds();
          digitalWrite(channels[i % CHANNELS].led_pin,HIGH);

          if (checkNDResponse()) {
            return;
          }      
        }
      }
    }

void checkSerialAPI(){
    xbee.readPacket();
    
    if (xbee.getResponse().isAvailable()) {
      
      if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
        xbee.getResponse().getZBRxResponse(rx);
            
        if (rx.getOption() == ZB_PACKET_ACKNOWLEDGED) {
        } else {}
        
        int len = rx.getDataLength();
        char m[len+1];
        for(int i=0;i<len;i++){
          m[i]=rx.getData(i);
        }
        m[len]='\0';

        splitString(m, CMD,DEL,BRI, R, G ,B, TOUT);
        t=millis() + TOUT * 1000;
        
      } else if (xbee.getResponse().getApiId() == MODEM_STATUS_RESPONSE) {
        xbee.getResponse().getModemStatusResponse(msr);
        if (msr.getStatus() == ASSOCIATED) {} 
        else if (msr.getStatus() == DISASSOCIATED) {} 
        else {}
      } else {}
    } else if (xbee.getResponse().isError()) {}
}

void setup() {
  pixels.begin(); 
  t = millis() + TOUT * 1000;
  CMD[0] = 'F';
  
  //Change to Serial1 if using micro
  Serial.begin(9600); 
  xbee.begin(Serial);
}

    void setupold() {
      
      //mySerial.begin(4800);
      //mySerial.println("Hello world");
      
      for (int i= 0; i<CHANNELS; i++) {
        pinMode(channels[i].button_pin,INPUT);
        digitalWrite(channels[i].button_pin,HIGH); //enable internal 20K pullup
        
        pinMode(channels[i].led_pin,OUTPUT);
        //blink leds a bit
        digitalWrite(channels[i].led_pin,HIGH);
        delay(200);
        digitalWrite(channels[i].led_pin,LOW);
      }
      
      //debug led
      //pinMode(13,OUTPUT);
      //digitalWrite(13,HIGH);
      //delay(500);
      //digitalWrite(13,LOW);
      
      xbee.begin(XBEE_BAUD);

      //discover the other XBEE's address
      discover();
      zbTx = ZBTxRequest(addr64, payload, sizeof(payload));
      
      //send a no-op packet so that the xbees can do their magic and find each other
      payload[0] = 254;
      xbee.send(zbTx);
      
      //Flash all leds once so the user knows
      flashAll(500);

      //mySerial.println("Discovered address");
      //mySerial.print("MSB: ");
      //mySerial.println(addr64.getMsb());
      //mySerial.println(addr64.getMsb()==0x0013a200?"Yes!":"NO");
      //mySerial.print("LSB: ");
      //mySerial.println(addr64.getLsb());
      //mySerial.println(addr64.getLsb()==0x403141DA?"Yes!":"NO");
    }

 void loopold() {
   int val;
   int m;
   
   for (uint8_t i= 0; i<CHANNELS; i++) {
     m = millis();
     
     if (channels[i].state == 0 || channels[i].state == 2) {
       val = digitalRead(channels[i].button_pin);
       
       if (channels[i].state == 0 && val == LOW) {
           //a press!, fire!
           uint8_t cc = i;
           
           //special case, we can fire all channels by firing the first button repeatably
           if (i == 0) {
             cc = channel_count;
             channel_count = (channel_count + 1) % CHANNELS;
           } 
           
           //fire!
           payload[0] = cc;
           xbee.send(zbTx);
           
           //set as fired 
           fired_channels |= (1 << cc);  
           digitalWrite(channels[cc].led_pin,HIGH);  
           
           //check if all is fired
           if (fired_channels == B00111111) {
             //wait a bit
             delay(500);
             
             //reset all
             channel_count = 0;
             fired_channels = 0;
             for (int j = 0; j<CHANNELS; j++) {
               channels[j].state = 0;
               digitalWrite(channels[j].led_pin,LOW);
               delay(300);
             }
             break;
           }
       }
       
       if ((channels[i].state == 0 && val == LOW) || (channels[i].state == 2 && val == HIGH)) {
         channels[i].state = (channels[i].state + 1) % 4; //change state 
         channels[i].time = m;
       }
             
     } else if (m - channels[i].time >  THRESHHOLD) {
       channels[i].state = (channels[i].state + 1) % 4; //change state   
     }
   } 
 }