void decoderStateFree(decoderstate* state) {
int i;
for(i = 0; i < state->numBlock; i++) {
mFree(state->blocks[i]);
mFree(state->coefficients[i]);
free(state->isSentPacketInBlock[i]);
}
if(state->numBlock > 0) {
free(state->nPacketsInBlock);
free(state->blocks);
free(state->coefficients);
free(state->isSentPacketInBlock);
}
if(state->nDataToSend > 0) {
free(state->dataToSend);
}
for(i = 0; i < state->nAckToSend; i++) {
free(state->ackToSend[i]);
}
if(state->nAckToSend > 0) {
free(state->ackToSend);
free(state->ackToSendSize);
}
free(state->lossBuffer);
free(state);
}
void close_ekf() {
free(vectR);
free_map();
mFree(mQ,U_SIZE);
mFree(mV,X_SIZE);
mFree(mPpred,X_SIZE);
mFree(mPcorr,X_SIZE);
}
int MRtmpSource::onUnPublish()
{
// do clean
m_sources.erase(m_url);
mFree(m_videoSh);
mFree(m_audioSh);
mFree(m_metadata);
return E_SUCCESS;
}
int matrixTest(){
matrix* a, *b, *c;
// Create & Destroy Matrices
a = getRandomMatrix(1000,1000);
b = getRandomMatrix(1000,1000);
c = mMul(*a, *b);
mFree(a);
mFree(b);
mFree(c);
mFree(mCreate(0,0));
return true;
}
void MAMF0StrictArray::clear()
{
vector<MAMF0Any *>::iterator iter;
for (iter = values.begin(); iter != values.end(); ++iter) {
MAMF0Any *any = *iter;
mFree(any);
}
values.clear();
}
void MAMFObject::clear()
{
for (unsigned int i = 0; i < values.size(); ++i) {
pair<MString, MAMF0Any *> &p = values.at(i);
MAMF0Any *any = p.second;
mFree(any);
}
values.clear();
}
int MRtmpSource::onMetadata(MRtmpMessage &msg)
{
dispatch(msg);
mFree(m_metadata);
m_metadata = new MRtmpMessage;
*m_metadata = msg;
log_trace("metadata update.");
return E_SUCCESS;
}
int MRtmpSource::onAudio(MRtmpMessage &msg)
{
dispatch(msg);
addToGop(msg);
if (MFlashVideoInfo::audioIsSequenceHeader(msg.payload)) {
mFree(m_audioSh);
m_audioSh = new MRtmpMessage;
*m_audioSh = msg;
log_trace("audio SH update.");
}
return E_SUCCESS;
}
void EKF_Correction(ekf_data xpred, ekf_data *xcorr, float *obs ){
float expMeas[NUM_SENS]; // Expected measurements
int idseg[NUM_SENS]; // Segment id for expected measurements
int idJac[NUM_SENS]; // To store the indexes required to build the Jacobian
// float px_s, py_s, th_s; // Sensor odo
odo s_odo[NUM_SENS];
float cth, sth;
int i;
int h_line; // Number of lines for the Jacobian
float **mJ;
float **dMeas; // z-h
cth=cos(xpred.th);
sth=sin(xpred.th);
// Compute the expected measurements
for (i=0; i<NUM_SENS; i++) {
// pxs= px + cos(th)*px_sb -sin(th)*py_sb
s_odo[i].x= xpred.x+cth*sa[i][SX]-sth*sa[i][SY];
// pys= py + sin(th)*px_sb +cos(th)*py_sb
s_odo[i].y= xpred.y+sth*sa[i][SX]+cth*sa[i][SY];
// pth_s= th + th_sb
s_odo[i].th= xpred.th+sa[i][STH];
ExpSensReading(s_odo[i], expMeas+i, idseg+i);
#ifdef EKF_DEBUG
printf("expM: %f\tidseg: %d\n",expMeas[i],idseg[i]);
printf("s_b -- x: %f\ty: %f\t th: %f\n",sa[i][SX],sa[i][SY],sa[i][STH]);
printf("s_odo -- x: %f\ty: %f\t th: %f\n",s_odo[i].x,s_odo[i].y,s_odo[i].th);
#endif
}
// Let us find out the good observations along with the good expected measurements
h_line=0;
for (i=0; i<NUM_SENS; i++) {
idJac[i]=-1;
if ( obs[i] < MAX_DIST && expMeas[i]<MAX_DIST) {
idJac[h_line]=i; // Id of the "used" sensors
h_line++;
}
}
//#ifdef EKF_DEBUG
printf("\nh_line: %d\n", h_line);
for (i=0; i<NUM_SENS; i++) {
printf("%d [%d]\n",idJac[i],i);
}
//#endif
// Main data Structure for the Jacobian
// mJ = (float **) malloc(sizeof(float *)* h_line);
mBuild(&mJ, h_line, X_SIZE);
// Compute the Jacobian
for (i=0; i<h_line; i++) {
float num, den, d_num, d_den, s_num, s_den;
float cths, sths;
// *(mJ+i)=(float *) malloc(sizeof(float)*X_SIZE);
// Compute the Jacobian
// Let us consider the prediction robot odo x=(px, py, th)
// Let us consider the sensor base odometry (px_sb, py_sb, th_sb)
// Let us consider the sensor odometry (px_s, py_s, th_s)
// | a*px_s + b*py_s +c | |num|
// h(x,M) = ------------------------ = ----
// | a*cos(th_s) + b*sin(th_s) |den|
// with:
// pxs= px + cos(th)*px_sb -sin(th)*py_sb
// pys= py + sin(th)*px_sb +cos(th)*py_sb
// pth_s= th + th_sb
//
cths=cos(s_odo[idJac[i]].th);
sths=sin(s_odo[idJac[i]].th);
num = map[idseg[idJac[i]]][A]*s_odo[idJac[i]].x +
map[idseg[idJac[i]]][B]*s_odo[idJac[i]].y +
map[idseg[idJac[i]]][C];
s_num = sign(num);
den = map[idseg[idJac[i]]][A]*cths+ map[idseg[idJac[i]]][B]*sths;
s_den = sign(den);
//
// d num d |num| d num
// ------ = a --> ------- = sign(num)* ------ = sign(num) * a
// d px d px d px
//
// d num d |num| d num
// ------ = b --> ------- = sign(num)* ------ = sign(num) * b
// d py d py d py
//
// d num
// d_num = ------ = a*(-px_sb*sin(th)-py_sb*cos(th))+b*(px_sb*cos(th)-py_sb*sin(th))
// d th
// d num
// d_den = ------ = -a*sin(th_s)+b*cos(th_s)
// d th
//
//
d_num = map[idseg[idJac[i]]][A]*(-sa[idJac[i]][SX]*sth-sa[idJac[i]][SY]*cth)+
map[idseg[idJac[i]]][B]* (sa[idJac[i]][SX]*cth -sa[idJac[i]][SY]*sth );
d_den = -map[idseg[idJac[i]]][A]*sths + map[idseg[idJac[i]]][B]*cths;
// Let us build the Jacobian
//
// d h 1 d |num| 1
// ---- = ----- * ------- = ----- * sign(num) * a
// d px |den| d px |den|
//
mJ[i][0]=s_num*map[idseg[idJac[i]]][A]/fabs(den);
// d h 1 d |num| 1
// ---- = ----- * ------- = ----- * sign(num) * b
// d py |den| d py |den|
//
mJ[i][1]=s_num*map[idseg[idJac[i]]][B]/fabs(den);;
// d h sign(num)*d_num*den - sign(den)*d_den*num
// ---- = ------------------------------------------
// d tx den^2
//
mJ[i][2]=(s_num*d_num*den - s_den*d_den*num)/(den*den);
}
//#ifdef EKF_DEBUG
printf("\n");
for (i=0; i<NUM_SENS; i++)
printf("Sensor: %d\tReading: %f\tWall: %d\n",i+1,expMeas[i],(idseg[i]>-1)?idseg[i]+1:idseg[i]);
printf("Xpre --- x: %f\ty: %f\t th: %f\n",xpred.x,xpred.y,xpred.th);
//#endif
for (i=0; i<h_line; i++)
printf("Jx: %f\tJy: %f\tJth: %f\n",mJ[i][0],mJ[i][1],mJ[i][2]);
float **mP1;
float **mP2;
float **mP3;
float **mK;
// Let us build the matrix
// P (n x n) J (q x n)
// P1=mPpred*J^T (n x n) x (n x q) = (n x q)
mBuild(&mP1,X_SIZE,h_line);
mMultTr2(mP1, X_SIZE, h_line, mPpred, X_SIZE, X_SIZE, mJ, h_line, X_SIZE);
// printf("\nP1=Ppred*J^T\n");
// mPrint(mP1,X_SIZE,h_line);
// P2= J*P1 (q x n) x (n x q) = (q x q)
mBuild(&mP2,h_line,h_line);
mMult(mP2,h_line, h_line, mJ, h_line, X_SIZE, mP1, X_SIZE, h_line);
// printf("\nP2=J*P1\n");
// mPrint(mP2,h_line,h_line);
// P2= P2+R (q x q) + (q x q) = (q x q)
for (i=0; i<h_line; i++)
mP2[i][i]+=vectR[idJac[i]];
// printf("\nP2=P2+R\n");
// mPrint(mP2,h_line,h_line);
// P3= inv(P2) (q x q)
mBuild(&mP3,h_line,h_line);
mInv(mP3,h_line,h_line,mP2,h_line,h_line);
// printf("\nP3=inv(P2)\n");
// mPrint(mP3,h_line,h_line);
// K= P1*P3 (n x q) x (q x q) = (n x q)
mBuild(&mK,X_SIZE,h_line);
mMult(mK,X_SIZE,h_line,mP1, X_SIZE, h_line, mP3, h_line, h_line);
printf("\nK=P1*P3\n");
mPrint(mK,X_SIZE,h_line);
// To build the innovation vector
mBuild(&dMeas, h_line,1);
for (i=0; i<h_line; i++) {
dMeas[i][0]=obs[idJac[i]]-expMeas[idJac[i]];
}
// mPrint(dMeas,h_line,1);
float **mP4;
float **mP5;
float **mInn;
// Pk = (I-KJ)P
// K*J
// (n x q) x (q x n) = (n x n)
mBuild(&mP4,X_SIZE,X_SIZE);
mMult(mP4,X_SIZE,X_SIZE,mK,X_SIZE,h_line,mJ,h_line,X_SIZE);
// printf("\nmJ\n");
// mPrint(mJ,h_line,X_SIZE);
// printf("\nmP4\n");
// mPrint(mP4,X_SIZE,X_SIZE);
// (I-K*J)
mEye(&mP5,X_SIZE);
mSub2(mP5,X_SIZE,X_SIZE,mP4,X_SIZE,X_SIZE);
// (I-K*J)*P
mMult(mPcorr, X_SIZE, X_SIZE, mP5, X_SIZE, X_SIZE, mPpred, X_SIZE, X_SIZE);
printf("\nmPcorr\n");
mPrint(mPcorr,X_SIZE,X_SIZE);
// K*vec
// (n x h_line) x (h_line x 1)
mBuild(&mInn, X_SIZE, 1);
mMult(mInn, X_SIZE, 1, mK, X_SIZE, h_line, dMeas, h_line, 1);
printf("\ndMeas\n");
mPrint(dMeas,h_line,1);
printf("\nmInn\n");
mPrint(mInn,X_SIZE,1);
// Xpost
xcorr->x = xpred.x + mInn[0][0];
xcorr->y = xpred.y + mInn[1][0];
xcorr->th = xpred.th + mInn[2][0];
// Free Memory
mFree(mJ,h_line);
mFree(mP1,X_SIZE);
mFree(mP2,h_line);
mFree(mP3,h_line);
mFree(mP4,X_SIZE);
mFree(mP5,X_SIZE);
mFree(mK,X_SIZE);
mFree(mInn,X_SIZE);
mFree(dMeas,h_line);
}
void EKF_Prediction(ekf_data xprev, ekf_data *xpred, float *u){
float ctha,stha;
float **mA; //[X_SIZE][X_SIZE];
float **mB; //[X_SIZE][U_SIZE];
float **mPprev; //[X_SIZE][X_SIZE];
float **mP1; //[X_SIZE][X_SIZE];
float **mP2; //[X_SIZE][X_SIZE];
int i,j;
ctha= cos(xprev.th+ u[U_W]*Tc/2);
stha= sin(xprev.th+ u[U_W]*Tc/2);
// State update
xpred->x = xprev.x + u[U_V]*Tc*ctha;
xpred->y = xprev.y + u[U_V]*Tc*stha;
xpred->th = xprev.th + u[U_W]*Tc;
// Populate Matrices
// A = [ 1 0 -v*tc*sin(th + w*tc/2)
// 0 1 v*tc*cos(th + w*tc/2)
// 0 0 1 ]
mBuild(&mA,X_SIZE,X_SIZE);
// row 1
mA[0][0]=1;
mA[0][1]=0;
mA[0][2]=-u[U_V]*Tc*stha;
// row 2
mA[1][0]=0;
mA[1][1]=1;
mA[1][2]=u[U_V]*Tc*ctha;
// row 3
mA[2][0]=0;
mA[2][1]=0;
mA[2][2]=1;
// B = [ tc*cos(th + w*tc/2) -v*tc*sin(th + w*tc/2)*tc/2
// tc*sin(th + w*tc/2) v*tc*cos(th + w*tc/2)*tc/2
// 0 tc ]
mBuild(&mB,X_SIZE,U_SIZE);
// row 1
mB[0][0]=Tc*ctha;
mB[0][1]=mA[0][2]*Tc/2;
// row 2
mB[1][0]=Tc*stha;
mB[1][1]=mA[1][2]*Tc/2;
// row 3
mB[2][0]=0;
mB[2][1]=Tc;
// Covariance Update (The second term is neglected)
// Pprev= A*P*A' + B*Q*B' + V
// Allocate matrix
mBuild(&mPprev,X_SIZE,X_SIZE);
mBuild(&mP1,X_SIZE,X_SIZE);
mBuild(&mP2,X_SIZE,X_SIZE);
// mPprex = xprev.P
// The pPrev at time k is the mPcorr at time k-1
printf("\nmPcorr k-1\n");
mCopy(mPprev,X_SIZE,X_SIZE,mPcorr,X_SIZE,X_SIZE);
mPrint(mPprev,X_SIZE,X_SIZE);
// mP1 = P*A';
mMultTr2(mP1,X_SIZE,X_SIZE,mPprev,X_SIZE,X_SIZE,mA,X_SIZE,X_SIZE);
// mP2 = A*P*A'
mMultTr2(mP2,X_SIZE,X_SIZE,mA,X_SIZE,X_SIZE,mP1,X_SIZE,X_SIZE);
// mPpred = A*P*A' + V
mAdd2(mPpred,X_SIZE,X_SIZE,mV,X_SIZE,X_SIZE);
printf("\nmPpred k\n");
mPrint(mPpred,X_SIZE,X_SIZE);
// Free memory
mFree(mA,X_SIZE);
mFree(mB,X_SIZE);
mFree(mPprev,X_SIZE);
mFree(mP1,X_SIZE);
mFree(mP2,X_SIZE);
}
void extractData(decoderstate* state) {
do_debug("in extractData\n");
// We only try to extract data on current block.
int nPackets = state->nPacketsInBlock[0], i, firstNonDecoded = -1;
uint8_t factor;
uint16_t size;
// Find the first non-decoded line :
for(i = 0; i<nPackets; i++) {
// Look for decoded packets to send
if((isZeroAndOneAt(state->coefficients[0]->data[i], i, BLKSIZE)) && !(state->isSentPacketInBlock[0][i])) {
memcpy(&size, state->blocks[0]->data[i], 2);
size = ntohs(size);
do_debug("Got a new decoded packet of size %u to send to the application ! o/\n", size);
// Append to the sending buffer
state->dataToSend = realloc(state->dataToSend, (state->nDataToSend + size) * sizeof(uint8_t));
memcpy(state->dataToSend + state->nDataToSend, state->blocks[0]->data[i] + 2, size);
state->nDataToSend += size;
state->isSentPacketInBlock[0][i] = true;
}
if( ! isZeroAndOneAt(state->coefficients[0]->data[i], i, BLKSIZE)) {
firstNonDecoded = i;
break;
}
}
do_debug("FirstNonDecoded = %d\n", firstNonDecoded);
if(firstNonDecoded == -1) {
if((nPackets == BLKSIZE) && (state->isSentPacketInBlock[0][BLKSIZE - 1])) {
// The entire block has been decoded AND sent
do_debug("An entire block has been decoded and sent, switch to next block.\n");
mFree(state->blocks[0]);
mFree(state->coefficients[0]);
free(state->isSentPacketInBlock[0]);
for(i = 0; i < state->numBlock - 1; i++) {
state->blocks[i] = state->blocks[i+1];
state->coefficients[i] = state->coefficients[i+1];
state->nPacketsInBlock[i] = state->nPacketsInBlock[i+1];
state->isSentPacketInBlock[i] = state->isSentPacketInBlock[i+1];
}
state->numBlock--;
state->currBlock++;
state->blocks = realloc(state->blocks, state->numBlock * sizeof(matrix*));
state->coefficients = realloc(state->coefficients, state->numBlock * sizeof(matrix*));
state->isSentPacketInBlock = realloc(state->isSentPacketInBlock, state->numBlock * sizeof(int*));
state->nPacketsInBlock = realloc(state->nPacketsInBlock, state->numBlock * sizeof(int));
}
return;
}
// Try to decode it
for(i = 0; i<nPackets; i++) {
if(i!=firstNonDecoded) {
factor = state->coefficients[0]->data[firstNonDecoded][i];
rowMulSub(state->coefficients[0]->data[firstNonDecoded], state->coefficients[0]->data[i], factor, BLKSIZE);
rowMulSub(state->blocks[0]->data[firstNonDecoded], state->blocks[0]->data[i], factor, PACKETSIZE);
}
}
if(isZeroAndOneAt(state->coefficients[0]->data[firstNonDecoded], firstNonDecoded, BLKSIZE)) {
// We decoded something => call recursively, in case there's something else waiting, or we finished the block
do_debug("Something got decoded\n");
extractData(state);
}
}
void handleInCoded(decoderstate* state, uint8_t* buffer, int size) {
do_debug("in handleInCoded\n");
datapacket* packet = bufferToData(buffer, size);
matrix *coeffs, *tmp;
int bufLen, i, delta;
uint8_t* dataVector;
uint8_t* coeffVector;
uint8_t ackBuffer[100];
uint16_t loss, total;
//printf("Data received :\n");
//dataPacketPrint(*packet);
do_debug("p->packetNumber = %2x\n", packet->packetNumber);
// ~~ Allocate blocks & coefficient matrix if necessary ~~
while(state->currBlock + state->numBlock - 1 < packet->blockNo) {
do_debug("CurrBlock = %d, numBlock = %d, blockNo of received Data = %d\n", state->currBlock, state->numBlock, packet->blockNo);
state->blocks = realloc(state->blocks, (state->numBlock + 1) * sizeof(matrix*));
state->blocks[state->numBlock] = mCreate(BLKSIZE, PACKETSIZE);
state->coefficients = realloc(state->coefficients, (state->numBlock + 1) * sizeof(matrix*));
state->coefficients[state->numBlock] = mCreate(BLKSIZE, BLKSIZE);
state->nPacketsInBlock = realloc(state->nPacketsInBlock, (state->numBlock + 1) * sizeof(int));
state->nPacketsInBlock[state->numBlock] = 0;
state->isSentPacketInBlock = realloc(state->isSentPacketInBlock, (state->numBlock + 1) * sizeof(int*));
state->isSentPacketInBlock[state->numBlock] = malloc(BLKSIZE * sizeof(int));
for(i = 0; i<BLKSIZE; i++) {
state->isSentPacketInBlock[state->numBlock][i] = false;
}
state->numBlock ++;
}
if(packet->blockNo >= state->currBlock) {
if((packet->packetNumber & BITMASK_NO) >= state->nPacketsInBlock[packet->blockNo - state->currBlock]) { // Try to append
// Compute coefficients
coeffs = mCreate(1, BLKSIZE);
dataVector = calloc(PACKETSIZE, sizeof(uint8_t));
coeffVector = calloc(BLKSIZE, sizeof(uint8_t));
if(((packet->packetNumber) & BITMASK_FLAG) == FLAG_CLEAR) {
coeffs->data[0][((packet->packetNumber) & BITMASK_NO)] = 1;
} else if(((packet->packetNumber) & BITMASK_FLAG) == FLAG_CODED) {
srandom(packet->seqNo);
tmp = getRandomMatrix(1, BLKSIZE);
memcpy(coeffs->data[0], tmp->data[0], packet->packetNumber & BITMASK_NO);
mFree(tmp);
} else {
printf("handleInCoded : received a bogus data packet. DIE.");
exit(1);
}
// ~~ Append to the matrix and eventually decode ~~
memcpy(dataVector, packet->payloadAndSize, packet->size);
memcpy(coeffVector, coeffs->data[0], BLKSIZE);
mFree(coeffs);
if(appendCodedPayload(state, coeffVector, dataVector, packet->blockNo - state->currBlock)) {
do_debug("Received an innovative packet\n");
state->stats_nInnovative++;
} else {
do_debug("Received packet was not innovative. Drop.\n");
if(packet->blockNo - state->currBlock == 0) {
state->stats_nAppendedNotInnovativeGaloisFirstBlock++;
} else {
state->stats_nAppendedNotInnovativeGaloisOtherBlock++;
}
}
free(dataVector);
free(coeffVector);
} else {
do_debug("Received packet has NO chance to be innovative. Drop.\n");
state->stats_nAppendedNotInnovativeCounter++;
}
} else {
do_debug("Packet received for an outdated block. Drop.\n");
state->stats_nOutdated++;
}
// ~~ Try to decode ~~
if((state->numBlock > 0) && (state->nPacketsInBlock[0] > 0)) {
do_debug("Calling extractData() while numBlock = %d, currBlock = %d, nPacketInBlock[0] = %d\n", state->numBlock, state->currBlock, state->nPacketsInBlock[0]);
extractData(state);
}
// ~~ Update the loss information buffer ~~
delta = packet->seqNo - state->lastSeqReceived;
if(delta > 0) {
for(i = state->lossBuffer->currentIndex + 1; i < (state->lossBuffer->currentIndex + delta); i++) {
state->lossBuffer->isReceived[i % LOSS_BUFFER_SIZE] = false;
}
state->lossBuffer->isReceived[(state->lossBuffer->currentIndex + delta) % LOSS_BUFFER_SIZE] = true;
state->lossBuffer->currentIndex = (state->lossBuffer->currentIndex + delta) % LOSS_BUFFER_SIZE;
} else {
state->lossBuffer->isReceived[(state->lossBuffer->currentIndex + delta + LOSS_BUFFER_SIZE) % LOSS_BUFFER_SIZE] = true;
}
state->lastSeqReceived = max(state->lastSeqReceived, packet->seqNo);
// ~~ Send an ACK back ~~
ackpacket ack;
ack.ack_dofs = malloc(DOFS_LENGTH * sizeof(uint8_t));
for(i = 0; i < DOFS_LENGTH; i++) {
if(state->numBlock > i) { // If the i-th block is allocated
// Include the number of packets received
ack.ack_dofs[i] = state->nPacketsInBlock[i];
} else {
// Otherwise, let it just be zero
ack.ack_dofs[i] = 0;
}
}
ack.ack_seqNo = packet->seqNo;
ack.ack_currBlock = state->currBlock;
countLoss(*state, &loss, &total);
ack.ack_loss = loss;
ack.ack_total = total;
//printf("ACK to send :\n");
//ackPacketPrint(ack);
ackPacketToBuffer(ack, ackBuffer, &bufLen);
free(ack.ack_dofs);
state->ackToSend = realloc(state->ackToSend, (state->nAckToSend + 1) * sizeof(uint8_t*));
state->ackToSendSize = realloc(state->ackToSendSize, (state->nAckToSend + 1) * sizeof(int));
state->ackToSend[state->nAckToSend] = malloc(bufLen * sizeof(uint8_t));
memcpy(state->ackToSend[state->nAckToSend], ackBuffer, bufLen);
state->ackToSendSize[state->nAckToSend] = bufLen;
state->nAckToSend ++;
free(packet->payloadAndSize);
free(packet);
}
int codingTest(){
struct timeval startTime, endTime;
encoderstate* encState = encoderStateInit();
decoderstate* decState = decoderStateInit();
uint8_t inputBuffer[INPUT_LENGTH], buf1[2 * PACKETSIZE], buf2[2 * PACKETSIZE], type;
int totalBytesSent = 0, totalBytesReceived = 0, totalAckSent = 0, totalAckReceived = 0, totalDataPacketReceived = 0, totalDataPacketSent = 0, nDataPacketSent = 0;
int i, j, buf1Len, buf2Len;
muxstate mState;
mState.sport = 10; mState.dport = 10; mState.remote_ip = 10;
int nRounds = CLEAR_PACKETS, sendSize;
float timeElapsed;
matrix* randomMatrix = getRandomMatrix(1, INPUT_LENGTH);
memcpy(inputBuffer, randomMatrix->data[0], INPUT_LENGTH);
mFree(randomMatrix);
gettimeofday(&startTime, NULL);
for(i = 0; i<nRounds; i++){
//if(regulator()){
//printf("\n~~~~Starting round %d~~~~~\n", i);
//encoderStatePrint(*encState);
//decoderStatePrint(*decState);
//printf("~~~~~~~~~\n");
//}
//sendSize = (int)(((0.8 + 0.2 *random())/RAND_MAX) * INPUT_LENGTH);
sendSize = PACKETSIZE - 20;
//printf("Adding %d to the encoder\n", sendSize);
handleInClear(encState, inputBuffer, sendSize);
totalBytesReceived += sendSize;
// Send ACKs
for(j = 0; j < decState->nAckToSend; j++){
bufferToMuxed(decState->ackToSend[j], buf1, decState->ackToSendSize[j], &buf1Len, mState, TYPE_ACK);
muxedToBuffer(buf1, buf2, buf1Len, &buf2Len, &mState, &type);
totalAckSent++;
if(((1.0 * random())/RAND_MAX) > LOSS){
onAck(encState, buf2, buf2Len);
//printf("Sent an ACK\n");
totalAckReceived++;
} else {
//printf("Lost an ACK\n");
}
}
// Free
for(j = 0; j< decState->nAckToSend;j++){
free(decState->ackToSend[j]);
}
free(decState->ackToSend);
decState->ackToSend = 0;
free(decState->ackToSendSize);
decState->ackToSendSize = 0;
decState->nAckToSend = 0;
// Send coded data packets from the encoder
for(j = 0; j < encState->nDataToSend; j++){
bufferToMuxed(encState->dataToSend[j], buf1, encState->dataToSendSize[j], &buf1Len, mState, TYPE_DATA);
muxedToBuffer(buf1, buf2, buf1Len, &buf2Len, &mState, &type);
totalDataPacketSent += buf2Len;
nDataPacketSent++;
if(((1.0 * random())/RAND_MAX) > LOSS){
handleInCoded(decState, buf2, buf2Len);
//printf("Sent a DATA packet from buf1:%d to buf2:%d\n", buf1Len, buf2Len);
totalDataPacketReceived += buf2Len;
} else {
//printf("Lost a data packet\n");
}
}
// Free
for(j = 0; j< encState->nDataToSend;j++){
free(encState->dataToSend[j]);
}
free(encState->dataToSend);
encState->dataToSend=0;
free(encState->dataToSendSize);
encState->dataToSendSize = 0;
encState->nDataToSend = 0;
if(decState->nDataToSend > 0){
//printf("Sent %d decoded bytes to the application\n", decState->nDataToSend);
totalBytesSent += decState->nDataToSend;
free(decState->dataToSend);
decState->dataToSend = 0;
decState->nDataToSend = 0;
}
if(regulator()){
printf("\n~~~~End of round %d~~~~~\n", i);
encoderStatePrint(*encState);
decoderStatePrint(*decState);
printf("~~~~~~~~~\n");
}
//usleep(10000 + (1.0 * random() /RAND_MAX) * 1000);
}
gettimeofday(&endTime, NULL);
timeElapsed = 1.0 * (endTime.tv_sec - startTime.tv_sec) + ((endTime.tv_usec - startTime.tv_usec) / 1000000.0);
encoderStatePrint(*encState);
decoderStatePrint(*decState);
printf("During the %d rounds and %f s, %d bytes has been received by the encoder ; %d has been sent to the application.\n%d bytes of Data Packets has been sent, %d received.\n%d Ack has been sent, %d received.\n Simulated loss rate = %f %%. Transmission efficiency = %f %%. Transmission speed = %f MB/s. Data packet per Rounds = %f\n", nRounds, timeElapsed, totalBytesReceived, totalBytesSent, totalDataPacketSent, totalDataPacketReceived, totalAckSent, totalAckReceived, LOSS, 1.0 * totalBytesSent / totalDataPacketReceived, totalBytesSent / (1024 * 1024 * timeElapsed), 1.0 * nDataPacketSent / nRounds);
encoderStateFree(encState);
decoderStateFree(decState);
return true;
}
int main(int argc, char *argv[]) {
int i,j;
float **m1;
float **m2;
float **m3;
float **m4;
float **m5;
float *diag4;
float **m1T;
float **M1;
float M2[5][5];
int row1, col1; // n x m
int row1T,col1T; // m x n
int row2, col2; // n x n
int row3, col3; // n x n
int row4, col4; // n x n
int row5, col5; // n x n
if (argc<2) {
printf("Usage: %s randSeed\n",argv[0]);
return -1;
}
// Test of functions that allocate the memory required to store the resulting matrix
row1=ROW;
col1=COL;
mRand(&m1,row1,col1,10,atoi(argv[1]));
mPrint(m1,row1,col1);
// Transposition
mTranspB(&m1T,&row1T,&col1T,m1,row1,col1);
mPrint(m1T,row1T,col1T);
// Multiplication
mMultB(&m2,&row2,&col2,m1,row1,col1,m1T,row1T,col1T);
mPrint(m2,row2,col2);
// Inversion
mInvB(&m3,&row3,&col3,m2,row2,col2);
mPrint(m3,row3,col3);
// Diagonal matrix
row4=ROW;
col4=ROW;
diag4= (float *) malloc(sizeof(float)*row4);
for (i=0; i<row4; i++)
diag4[i]=i+1;
mDiag(&m4,row4,diag4);
mPrint(m4,row4,col4);
mFree(m1,row1);
mFree(m1T,row1T);
mFree(m2,row2);
mFree(m3,row3);
mFree(m4,row4);
// Test of functions that assume the memory required to store the
// resulting matrix to be already allocated
// row1=row2=row3=row4=row1T=0;
// col1=col2=col3=col4=col1T=0;
// Random Creation
mRand(&m1,row1,col1,10,atoi(argv[1]));
mPrint(m1,row1,col1);
mBuild(&m1T,row1T,col1T);
mBuild(&m2,row2,col2);
mBuild(&m3,row3,col3);
mBuild(&m4,row4,col4);
// Transposition
mTransp(m1T,row1T,col1T,m1,row1,col1);
mPrint(m1T,row1T,col1T);
// Multiplication
mMult(m2,row2,col2,m1,row1,col1,m1T,row1T,col1T);
mPrint(m2,row2,col2);
// Inversion
mInv(m3,row3,col3,m2,row2,col2);
mPrint(m3,row3,col3);
// Diagonal matrix
row4=ROW;
col4=ROW;
diag4= (float *) malloc(sizeof(float)*row4);
for (i=0; i<row4; i++)
diag4[i]=i+1;
// mDiag(&m4,row4,diag4);
// mPrint(m4,row4,col4);
// Mult
mPrint(m1T,row1T,col1T);
mFree(m2,row2);
mRand(&m2,row2,col2,10,atoi(argv[1])*5);
mPrint(m2,row2,col2);
mFree(m4,row4);
row4=row2;
col4=row1T;
mBuild(&m4,row4,col4);
mMultTr2(m4,row4,col4,m2,row2,col2,m1T,row1T,col1T);
mPrint(m4,row4,col4);
// Copy
row5=row4;
col5=col4;
printf("\ncopy\n");
mBuild(&m5,row5,col5);
mPrint(m5,row5,col5);
mCopy(m5,row5,col5,m4,row4,col4);
mPrint(m4,row4,col4);
mPrint(m5,row5,col5);
mFree(m1,row1);
mFree(m1T,row1T);
mFree(m2,row2);
mFree(m3,row3);
mFree(m4,row4);
// To check copy to and from float[r][c]
// So far only squared matrix are copied!
printf("\nLast\n");
mRand(&M1,5,5,10,atoi(argv[1])*5);
mPrint(M1,5,5);
mCopyM2A(5,M2,M1);
for(i=0; i<5; i++)
for(j=0; j<5; j++)
M2[i][j]*=100;
mCopyA2M(M1,5,M2);
mPrint(M1,5,5);
return 0;
}
