TEST(Packets, PacketParameterReadTest)
{
    std::istringstream inputStream;
    Packet* actualPacket;
    DigitalOutputPacket expectedPacket1(7, true);
    DigitalOutputPacket expectedPacket2(4, false);

    inputStream.str("\xFF\x02\x07\x02\xFF");
    actualPacket = readPacket(inputStream);

    CHECK(actualPacket != NULL);
    CHECK_EQUAL(expectedPacket1, *actualPacket);
    CHECK(expectedPacket2 != *actualPacket);

    delete actualPacket;

    inputStream.str("\xFF\x02\x04\x01\xFF");
    actualPacket = readPacket(inputStream);

    CHECK(actualPacket != NULL);
    CHECK_EQUAL(expectedPacket2, *actualPacket);
    CHECK(expectedPacket1 != *actualPacket);

    delete actualPacket;
}

uint32_t oggAudio::read(uint32_t size, uint8_t *data)
{
uint32_t i;

	if(_wavheader->encoding==WAV_OGG)
	{

		if(!_inBuffer)
			readPacket(&_inBuffer,_buffer,&i);
		if(!_inBuffer) return 0;
		if(size<_inBuffer)
		{
			memcpy(data,_buffer,size);
			memmove(_buffer,_buffer+size,_inBuffer-size);
			_inBuffer-=size;
			aprintf("Ogg:This round read %lu bytes from buffer (asked) \n",size);
			return size;
		}
		memcpy(data,_buffer,_inBuffer);
		i=_inBuffer;
		_inBuffer=0;
		aprintf("This round read %lu bytes asked %lu \n",i,size);
		return i;
	}
	// It is not vorbis audio
	// We have to skip 1 byte header in case of fresh packet
	// Ask the full page
	uint32_t cursize=0;
	uint32_t flags=0;
	uint64_t stamp=0;
	uint8_t *frags,frag;
	uint32_t ssize=0;
	uint32_t red=0;
	uint8_t lenbyte;

	//
	ssize=readPacket(&cursize,data,&flags);
	// First byte is len stuff
	lenbyte=data[0];
	lenbyte=(lenbyte>>6)+((lenbyte&2)<<1);
	lenbyte=1+lenbyte;
	if(ssize<lenbyte)
	{
		printf("Oops:ssize %lu, lenbyte %d\n",ssize,lenbyte);
		return MINUS_ONE;
		
	}
	ssize-=lenbyte;
	memmove(data,data+lenbyte,ssize);
	return ssize;
		
	printf("OGM : Failed reading non vorbis\n");
	return 0;
	


}

void eraseBaseband(int fd, unsigned int begin, unsigned int end) {
    // correct the end address as the boot loader does, but still give it the
    // 'wrong' value
    unsigned int end2 = end;
    if (begin == 0xa0020000) end = 0xa0310000;

    LOG(LOGLEVEL_INFO, "Erasing flash range 0x%08x-0x%08x...\n", begin, end);

    unsigned int base = end - begin;
    if (base == 0) base = 1; // no div by 0, rather wrong values

    ErasePacket packet = {
        .begin = begin,
        .end = end2
    };
    writePacket(fd, 0x805, &packet, ERASE_PACKET_SIZE);
    char buffer[PACKET_SIZE(sizeof(unsigned short))];
    size_t length = readPacket(fd, WRITE_TIMEOUT, buffer, sizeof(buffer));
    unsigned short *job = verifyPacket(buffer, length);
    LOG(LOGLEVEL_DEBUG, "Erase returns: %d\n", job ? *job : 0);

    if (job) {
        EraseStatusReplyPacket *reply;
        int previous = -1;
        do {
            writePacket(fd, 0x806, job, sizeof(*job));
            char buffer2[PACKET_SIZE(ERASE_STATUS_REPLY_PACKET_SIZE)];
            length = readPacket(fd, DEFAULT_TIMEOUT, buffer2, sizeof(buffer2));
            reply = verifyPacket(buffer2, length);
            if (reply) {
                LOG(LOGLEVEL_DEBUG, "Erase status returns: done=%s current=0x%08x w00=%d\n", reply->done ? "yes" : "no", reply->current, reply->w00);
                if (reply->current >= begin && reply->current <= end) {
                    int percent = (reply->current - begin) * 100 / base;
                    if (percent != previous) {
                        LOG(LOGLEVEL_INFO, "Current progress: %u%%\n", percent);
                        previous = percent;
                    }
                } else if (reply->current == 0xa0000000 && reply->done) {
                    LOG(LOGLEVEL_ERROR, "Looks like the erase command failed due to an invalid secpack.\n");
                } else {
                    LOG(LOGLEVEL_INFO, "Current position: 0x%08x\n", reply->current);
                }
            }
        } while (reply && !reply->done);
    } else {
        LOG(LOGLEVEL_ERROR, "Erase command failed!\n");
    }
}

int PubSubClient::loop() {
   if (connected()) {
      long t = millis();
      if (t - lastActivity > KEEPALIVE) {
         _client.write(MQTTPINGREQ);
         _client.write((uint8_t)0);
         lastActivity = t;
      }
      if (_client.available()) {
         uint8_t len = readPacket();
         if (len > 0) {
            uint8_t type = buffer[0]&0xF0;
            if (type == MQTTPUBLISH) {
               if (callback) {
                  uint8_t tl = (buffer[2]<<3)+buffer[3];
                  char topic[tl+1];
                  for (int i=0;i<tl;i++) {
                     topic[i] = buffer[4+i];
                  }
                  topic[tl] = 0;
                  // ignore msgID - only support QoS 0 subs
                  uint8_t *payload = buffer+4+tl;
                  callback(topic,payload,len-4-tl);
               }
            } else if (type == MQTTPINGREQ) {
               _client.write(MQTTPINGRESP);
               _client.write((uint8_t)0);
               lastActivity = t;
            }
         }
      }
      return 1;
   }
   return 0;
}

int PubSubClient::connect(char *id, char* willTopic, uint8_t willQos, uint8_t willRetain, char* willMessage) {
   if (!connected()) {
      if (_client.connect()) {
         nextMsgId = 1;
         uint8_t d[9] = {0x00,0x06,'M','Q','I','s','d','p',MQTTPROTOCOLVERSION};
         uint8_t length = 0;
         int j;
         for (j = 0;j<9;j++) {
            buffer[length++] = d[j];
         }
         if (willTopic) {
            buffer[length++] = 0x06|(willQos<<3)|(willRetain<<5);
         } else {
            buffer[length++] = 0x02;
         }
         buffer[length++] = 0;
         buffer[length++] = (KEEPALIVE/1000);
         length = writeString(id,buffer,length);
         if (willTopic) {
            length = writeString(willTopic,buffer,length);
            length = writeString(willMessage,buffer,length);
         }
         write(MQTTCONNECT,buffer,length);
         while (!_client.available()) {}
         uint8_t len = readPacket();
         
         if (len == 4 && buffer[3] == 0) {
            lastActivity = millis();
            return 1;
         }
      }
      _client.stop();
   }
   return 0;
}

void SerialServer::processTransfer(QByteArray& header)
{
    QDataStream headerStream(&header, QIODevice::ReadOnly);
    
    quint16 startWord;
    quint16 packetCount;
    
    headerStream >> startWord;
    headerStream >> packetCount;
    
    if(startWord != SERIAL_START)
        return;
    qWarning("Got start...");
    qWarning("packetcount=%d", packetCount);
    
    QByteArray compressedData;
    
    for(quint16 i = 0;i < packetCount;i++) {
        QByteArray data;
        if(!readPacket(&data))
            return;
        compressedData += data;
    }
    
    
    QByteArray data = qUncompress(compressedData);
    compressedData.clear();
    compressedData.squeeze();
    
    processData(data);
}

int AudioStreamInstance::readBuffer(int16 *buffer, const int numSamples) {
	debugC(5, kDebugAudio, "readBuffer(buffer, %d)", numSamples);

	if (_stopped)
		return 0;

	handleFade(numSamples);
	int32 leftSamples = numSamples;
	int32 destOffset = 0;
	if ((_bufferOffset + leftSamples) * 2 >= _bufferSize) {
		if (_bufferSize - _bufferOffset * 2 > 0) {
			memcpy(buffer, &_buffer[_bufferOffset], _bufferSize - _bufferOffset * 2);
			leftSamples -= (_bufferSize - _bufferOffset * 2) / 2;
			destOffset += (_bufferSize - _bufferOffset * 2) / 2;
		}
		if (!readPacket())
			return 0;

		_bufferOffset = 0;
	}

	if (leftSamples >= 0) {
		memcpy(buffer + destOffset, &_buffer[_bufferOffset], MIN(leftSamples * 2, _bufferSize));
		_bufferOffset += leftSamples;
	}

	_playedSamples += numSamples;

	return numSamples;
}

AudioStreamInstance::AudioStreamInstance(AudioManager *man, Audio::Mixer *mixer, Common::SeekableReadStream *stream , bool looping, bool deleteFileStreamAtEnd) {
	_compBufferSize = 0;
	_buffer = NULL;
	_bufferSize = 0;
	_bufferMaxSize = 0;
	_mixer = mixer;
	_compBuffer = NULL;
	_bufferOffset = 0;
	_lastSample = 0;
	_lastStepIndex = 0;
	_file = stream;
	_fadingIn = false;
	_fadingOut = false;
	_fadeTime = 0;
	_stopped = false;
	_volume = 255;
	_totalSize = stream->size();
	_currentReadSize = 8;
	_man = man;
	_looping = looping;
	_musicAttenuation = 1000;
	_deleteFileStream = deleteFileStreamAtEnd;
	_playedSamples = 0;

	// preload one packet
	if (_totalSize > 0) {
		_file->skip(8);
		readPacket();
	} else {
		stopNow();
	}

	_soundType = Audio::Mixer::kPlainSoundType;
}

AVFrame *FFMpegDecoder::getFrameAtSec(double sec)
{
	if (videoStream < 0)
	{
		errinfo("no video stream!");
		return NULL;
	}

	AVFrame *pic = NULL;
	seekToSec(sec);
	while (true)
	{
		AVPacket *pkt = readPacket();
		if (pkt == NULL)
			break;

		if (pkt->stream_index != videoStream)
			continue;

		pic = decodeVideo();
		if (pic != NULL)
			break;
	}
	return pic;
}

void prepareFlash(int fd) {
    LOG(LOGLEVEL_INFO, "Preparing flash access...\n");

    short param = 0;
    writePacket(fd, 0x84, &param, sizeof(param));
    char buffer[PACKET_SIZE(CFI1_PACKET_SIZE)];
    size_t length = readPacket(fd, DEFAULT_TIMEOUT, buffer, sizeof(buffer));
    LOG(LOGLEVEL_DEBUG, "CFI Stage 1 returns:\n");
    LOGDO(LOGLEVEL_DEBUG, printBuffer(verifyPacket(buffer, length), CFI1_PACKET_SIZE));

    writePacket(fd, 0x85, NULL, 0);
    length = readPacket(fd, DEFAULT_TIMEOUT, buffer, sizeof(buffer));
    short *unknown = verifyPacket(buffer, length);
    LOG(LOGLEVEL_DEBUG, "CFI Stage 2 returns: %d\n", unknown ? *unknown : 0);
    //LOGDO(LOGLEVEL_DEBUG, printBuffer(buffer, length));
}

SendFileDataPacket::SendFileDataPacket(const char *buffer, size_t bufferSize) :
	DataPacket(DataPacketHeader(buffer, bufferSize))
{
	assert(m_header.getDataSize() + DataPacketHeader::getHeaderSize() <= bufferSize);

	readPacket(buffer, bufferSize);
}

void
service_net ()
{
	while (interrupts > 0)
	{
		NRF_CE_lo;
		uint8_t status = NRF_ReadRegister(NRF_STATUS);
		RXEN_lo;
	        // received a packet
	        if (status & 0x40)
     	   	{                
			printf("Radio received a packet\n");
		 	readPacket();
        	} else if (status & 0x20)
        	{ // packet was sent
                	printf("Radio successfully sent the packet\n");
         		NRF_WriteRegister(NRF_STATUS, 0x10);
		} else if (status & 0x10)
         	{ // packet failed to send              
			printf("Radio failed to send the packet\n");
			NRF_WriteRegister(NRF_STATUS, 0x20);
         	}
		RXEN_hi;
		NRF_CE_hi;
		INT_Disable();
		interrupts--;
		INT_Enable();
	}
}	

 void Demuxer::feedStream(Stream& stream)
 {
     sf::Lock l(m_synchronized);
     
     while ((!didReachEndOfFile() || hasPendingDataForStream(stream)) && stream.needsMoreData())
     {
         AVPacket* pkt = NULL;
         
         pkt = gatherQueuedPacketForStream(stream);
         
         if (!pkt)
             pkt = readPacket();
         
         if (!pkt)
         {
             m_eofReached = true;
         }
         else
         {
             if (!distributePacket(pkt, stream))
             {
                 av_free_packet(pkt);
                 av_free(pkt);
             }
         }
     }
 }

	void PacketReader::onDataReady(Uint8* buf, Uint32 size)
	{
		if (error)
			return;

		mutex.lock();
		if (packet_queue.size() == 0)
		{
			Uint32 ret = 0;
			while (ret < size && !error)
			{
				ret += newPacket(buf + ret, size - ret);
			}
		}
		else
		{
			Uint32 ret = 0;
			IncomingPacket::Ptr pck = packet_queue.back();
			if (pck->read == pck->size) // last packet in queue is fully read
				ret = newPacket(buf, size);
			else
				ret = readPacket(buf, size);

			while (ret < size  && !error)
			{
				ret += newPacket(buf + ret, size - ret);
			}
		}
		mutex.unlock();
	}

void WANDongleSerialPort::rxHandler()
{
  if (((USBEndpoint *) bulk_in)->getState() == USB_TYPE_IDLE) //Success
  {
    buf_in_read_pos = 0;
    buf_in_len = ((USBEndpoint *) bulk_in)->getLengthTransferred(); //Update length
    //lock_rx.unlock();
    rx_mtx.lock();
    lock_rx = false; //Transmission complete
    if(cb_rx_en)
    {
      rx_mtx.unlock();
      listener->readable(); //Call handler from the IRQ context
      //readPacket() should be called by the handler subsequently once the buffer has been emptied
    }
    else
    {
      cb_rx_pending = true; //Queue the callback
      rx_mtx.unlock();
    }

  }
  else //Error, try reading again
  {
    //lock_rx.unlock();
    DBG("Trying again");
    readPacket();
  }
}

AudioStream *ASFStream::createAudioStream() {
	delete _lastPacket;
	_lastPacket = readPacket();

	// TODO
	if (_lastPacket->segments.size() != 1)
		throw Common::Exception("ASFStream::createAudioStream(): Only single segment packets supported");

	Packet::Segment &segment = _lastPacket->segments[0];

	// We should only have one stream in a ASF audio file
	if (segment.streamID != _streamID)
		throw Common::Exception("ASFStream::createAudioStream(): Packet stream ID mismatch");

	// TODO
	if (segment.sequenceNumber != _curSequenceNumber)
		throw Common::Exception("ASFStream::createAudioStream(): Only one sequence number per packet supported");

	// This can overflow and needs to overflow!
	_curSequenceNumber++;

	// TODO
	if (segment.data.size() != 1)
		throw Common::Exception("ASFStream::createAudioStream(): Packet grouping not supported");

	Common::SeekableReadStream *stream = segment.data[0];
	if (_codec)
		return _codec->decodeFrame(*stream);

	return 0;
}

void loop (){
  byte vButton1;
  ARCPOPacket vPacket;
  
  vButton1 = readButton(BUTTON_PIN);
  if (vButton1 == 0){
    ARCPOPacket vP;
    vP.Type = 98;
    vP.Content [0] = 'x';
    vP.Content [1] = 0;
    
    
    writePacket (vP);
    pika(LED1_PIN, 100);
  }
  
  
  vPacket = readPacket();
  
  if (readPacketSuccess()){
    delay(250);
    ProcessPacket (vPacket);
    //Serial.print ("Read a packet -->");
    //DEBUG_TRACE_ARCPOPACKET_TO_SERIAL (vPacket);
  }
  
  //system needs a small delay other keep on reading the same entries (COM problem?)
  delay(250);
}

Frame VideoStream::getNextFrame() {
	unique_ptr<AVFrame, AVFrameDeleter> frame = nullptr;
	if (state != AVERROR_EOF && !eof) {
		// Receive avFrame from decoder
		frame.reset(av_frame_alloc());
		state = avcodec_receive_frame(avCodecContext.get(), frame.get());
		if (state == AVERROR(EAGAIN)) {
			readPacket();
			state = avcodec_receive_frame(avCodecContext.get(), frame.get());
			if(this->firstFramePts == -1) {
			  firstFramePts = pkt.get()->pts;
			}
		}

		assert(state != AVERROR(EINVAL)); // "codec not opened, or it is an encoder" << endl;
		if (state == AVERROR(EAGAIN)) {
			//cout << "output is not available right now - user must try to send new input " << endl;
		}
		else if (state == AVERROR_EOF) {
			//cout << " the decoder has been fully flushed, and there will be no more output frames" << endl;
		}
		else {
			assert(state >= 0);
		}
	}
	Frame f(std::move(frame), *pkt.get());
	f.firstFramePts = firstFramePts; // meh, parent class should be called and know all this.
	return f;
	//return Frame(std::move(frame), *pkt.get());
}

/******************************************************************************
 *
 * @brief   On packet reception, process the data
 *  BSL-based protocol expects:
 *  HEADER  = 0x80
 *  Lenght  = lenght of CMD + [ADDR] + [DATA]
 *  CMD     = 1 byte with the corresponding command
 *  ADDR    = optional address depending on command
 *  DATA    = optional data depending on command
 *  CHKSUM  = 2 bytes (L:H) with CRC checksum of CMD + [ADDR] + [DATA]
 *
 * @return RET_OK: Communication protocol in progress
 *         RET_JUMP_TO_APP: Last byte received, request jump to application
 *****************************************************************************/
uint8_t TI_MSPBoot_CI_Process(void)
{
    uint8_t ret = RET_NO_DATA;
    uint8_t sendAck;

    if (readPacket())    // On complete packet reception
    {
        if ((gPacket.length > PACKET_DESTINATION) && 
            (gPacket.data[PACKET_PROTOCOL] == MSP430_BSL_PROTOCOL) && 
            (gPacket.data[PACKET_DESTINATION] == gPacket.myAddr)) {
            sendAck = verifyPacket();
            ackPacket(sendAck);
            if (sendAck == ACK)
            {
                /* CI_CMD_Intepreter will set up the packet response */
                ret = CI_CMD_Intepreter();
                updatePacket();
                writePacket();
            }
        }

        packetReset();
    }

    return ret;
}

size_t readBaseband(int fd, void *buffer, unsigned short size) {
    LOG(LOGLEVEL_STATUS, "Reading %u bytes from flash...\n", size);

    writePacket(fd, 0x803, &size, sizeof(size));
    void *temp = malloc(PACKET_SIZE(size));
    size_t length = readPacket(fd, DEFAULT_TIMEOUT, temp, PACKET_SIZE(size));

    void *ret = verifyPacket(temp, length);
    if (ret) {
        CmdPacket *packet = (CmdPacket *) temp;

        LOG(LOGLEVEL_DEBUG, "Read returns %d bytes:\n", packet->dataSize);
        LOGDO(LOGLEVEL_DEBUG, printBuffer(ret, packet->dataSize));

        if (packet->dataSize <= size) {
            memcpy(buffer, ret, packet->dataSize);
        } else {
            LOG(LOGLEVEL_WARN, "Warning: The returned data does not fit into the buffer!\n");
            memcpy(buffer, ret, size);
        }

        free(temp);
        return packet->dataSize;
    } else {
        LOG(LOGLEVEL_DEBUG, "Read returns: ERROR!\n");

        free(temp);
        return 0;
    }
}