void interpret(TA_Item **ta_table, tBTSUzolPtr root)
{
if((*ta_table)==NULL) //prazdny program, nemame co resit, koncime
{
return;
}
int end= (*ta_table)[0].end;
size_t operation;
tBTSUzolPtr op1, op2, result, pom;
op1=NULL;
op2=NULL;
result=NULL;
pom=NULL;
int *jump_array=NULL;
find_labels(ta_table, &jump_array);
for(int position=0; position<end; position++)
{
ta_Get(ta_table, &operation, &op1, &op2, &result, position);
switch(operation)
{
/******************aritmeticke operace**********************/
case I_ADD:
if(result==NULL)
{
break;
}
if((op1->data.typ==tDouble) && (op2->data.typ==tDouble))
{
result->data.value.d = (op1->data.value.d) + (op2->data.value.d);
result->data.typ=tDouble;
}
else if((op1->data.typ==tString) && (op2->data.typ==tString))
{
string text1, text2;
text1.data = op1->data.value.s;
text2.data = op2->data.value.s;
result->data.value.s = strAppendString(&text1, &text2);
}
else
{
error = ESEM;
ta_Destroy(ta_table);
return;
}
break;
case I_SUB:
if(result==NULL)
{
break;
}
if((op1->data.typ==tDouble) && (op2->data.typ==tDouble))
{
result->data.value.d= op1->data.value.d - op2->data.value.d;
result->data.typ=tDouble;
}
else
{
error = ESEM;
ta_Destroy(ta_table);
return;
}
break;
case I_MUL:
if(result==NULL)
{
break;
}
if((op1->data.typ==tDouble) && (op2->data.typ==tDouble))
{
result->data.value.d= op1->data.value.d * op2->data.value.d;
result->data.typ=tDouble;
}
else
{
error = ESEM;
ta_Destroy(ta_table);
destroy_labels(jump_array);
return;
}
break;
case I_DIV:
if(result==NULL)
{
break;
}
if((op1->data.typ==tDouble) && (op2->data.typ==tDouble))
{
if(op2->data.value.d==0.0)
{
error = EINT;
ta_Destroy(ta_table);
return;
}
result->data.value.d= op1->data.value.d / op2->data.value.d;
result->data.typ=tDouble;
}
else
{
error = ESEM;
ta_Destroy(ta_table);
return;
}
break;
case I_ASSIGN:
if(result==NULL)
{
break;
}
if(op1->data.typ==tBool)
{
result->data.value.b=op1->data.value.b;
result->data.typ=tBool;
}
else if(op1->data.typ==tDouble)
{
result->data.value.d=op1->data.value.d;
result->data.typ=tDouble;
}
else if(op1->data.typ==tString)
{
result->data.value.s = op1->data.value.s;
result->data.typ=tString;
}
else if(op1->data.typ==tNil)
{
result->data.typ=tNil;
result->data.value.b=false;
}
else
{
error = ESEM;
ta_Destroy(ta_table);
return;
}
break;
case I_POW:
if(result==NULL)
{
break;
}
if((op1->data.typ==tDouble) && (op2->data.typ==tDouble))
{
result->data.value.d= pow(op1->data.value.d, op2->data.value.d);
result->data.typ=tDouble;
}
else
{
error = ESEM;
ta_Destroy(ta_table);
destroy_labels(jump_array);
return;
}
break;
/****************porovnani**********************/
case I_EQUAL:
if(result==NULL)
{
break;
}
if(op1->data.typ==tDouble)
{
if(op2->data.typ!=tDouble)
{
result->data.value.b=false;
result->data.typ=tBool;
}
else
{
if(op2->data.value.d==op1->data.value.d)
{
result->data.value.b=true;
result->data.typ=tBool;
}
else
{
result->data.value.b=false;
result->data.typ=tBool;
}
}
}
else if(op1->data.typ==tBool)
{
if(op2->data.typ!=tBool)
{
result->data.value.b=false;
result->data.typ=tBool;
}
else
{
if(op2->data.value.b==op1->data.value.b)
{
result->data.value.b=true;
result->data.typ=tBool;
}
else
{
result->data.value.b=false;
result->data.typ=tBool;
}
}
}
else if (op1->data.typ==tString && op2->data.typ==tString)
{
if(strcmp(op1->data.value.s,op2->data.value.s)==0)
{
result->data.value.b=true;
result->data.typ = tBool;
}
else
{
result->data.value.b=false;
result->data.typ = tBool;
}
}
else
{
error = ESEM;
ta_Destroy(ta_table);
destroy_labels(jump_array);
return;
}
break;
case I_NEQUAL:
if(result==NULL)
{
break;
}
if(op1->data.typ==tDouble)
{
if(op2->data.typ!=tDouble)
{
result->data.value.b=true;
result->data.typ=tBool;
}
else
{
if(op1->data.value.d!=op2->data.value.d)
{
result->data.value.b=true;
result->data.typ=tBool;
}
else
{
result->data.value.b=false;
result->data.typ=tBool;
}
}
}
if(op1->data.typ==tBool)
{
if(op2->data.typ!=tBool)
{
result->data.value.b=true;
result->data.typ=tBool;
}
else
{
if(op1->data.value.b!=op2->data.value.b)
{
result->data.value.b=true;
result->data.typ=tBool;
}
else
{
result->data.value.b=false;
result->data.typ=tBool;
}
}
}
else if (op1->data.typ==tString && op2->data.typ==tString)
{
if(strcmp(op1->data.value.s,op2->data.value.s)!=0)
{
result->data.value.b=true;
result->data.typ = tBool;
}
else
{
result->data.value.b=false;
result->data.typ = tBool;
}
}
else
{
error = ESEM;
ta_Destroy(ta_table);
destroy_labels(jump_array);
return;
}
break;
case I_ELESS:
if(result==NULL)
{
break;
}
if(op1->data.typ==tDouble)
{
if(op2->data.typ!=tDouble)
{
result->data.value.b=false;
result->data.typ=tBool;
}
else
{
if(op1->data.value.d<=op2->data.value.d)
{
result->data.value.b=true;
result->data.typ=tBool;
}
else
{
result->data.value.b=false;
result->data.typ=tBool;
}
}
}
else if(op1->data.typ==tBool)
{
if(op2->data.typ!=tBool)
{
result->data.value.b=false;
result->data.typ=tBool;
}
else
{
if(op2->data.value.b==op1->data.value.b)
{
result->data.value.b=true;
result->data.typ=tBool;
}
else
{
result->data.value.b=false;
result->data.typ=tBool;
}
}
}
else if (op1->data.typ==tString && op2->data.typ==tString)
{
if(strcmp(op1->data.value.s,op2->data.value.s)<=0)
{
result->data.value.b=true;
result->data.typ = tBool;
}
else
{
result->data.value.b=false;
result->data.typ = tBool;
}
}
else
{
error = ESEM;
ta_Destroy(ta_table);
destroy_labels(jump_array);
return;
}
break;
case I_LESS:
if(result==NULL)
{
break;
}
if(op1->data.typ==tDouble)
{
if(op2->data.typ!=tDouble)
{
result->data.value.b=false;
result->data.typ=tBool;
}
else
{
if(op1->data.value.d<op2->data.value.d)
{
result->data.value.b=true;
result->data.typ=tBool;
}
else
{
result->data.value.b=false;
result->data.typ=tBool;
}
}
}
else if (op1->data.typ==tString && op2->data.typ==tString)
{
if(strcmp(op1->data.value.s,op2->data.value.s)<0)
{
result->data.typ = tBool;
result->data.value.b=true;
}
else
{
result->data.typ = tBool;
result->data.value.b=false;
}
}
else
{
error = ESEM;
ta_Destroy(ta_table);
destroy_labels(jump_array);
return;
}
break;
case I_EMORE:
if(result==NULL)
{
break;
}
if(op1->data.typ==tDouble)
{
if(op2->data.typ!=tDouble)
{
result->data.value.b=false;
result->data.typ=tBool;
}
else
{
if(op1->data.value.d>=op2->data.value.d)
{
result->data.value.b=true;
result->data.typ=tBool;
}
else
{
result->data.value.b=false;
result->data.typ=tBool;
}
}
}
else if(op1->data.typ==tBool)
{
if(op2->data.typ!=tBool)
{
result->data.value.b=false;
result->data.typ=tBool;
}
else
{
if(op2->data.value.b==op1->data.value.b)
{
result->data.value.b=true;
result->data.typ=tBool;
}
else
{
result->data.value.b=false;
result->data.typ=tBool;
}
}
}
else if (op1->data.typ==tString && op2->data.typ==tString)
{
if(strcmp(op1->data.value.s,op2->data.value.s)>=0)
{
result->data.value.b=true;
result->data.typ = tBool;
}
else
{
result->data.value.b=false;
result->data.typ = tBool;
}
}
else
{
error = ESEM;
ta_Destroy(ta_table);
destroy_labels(jump_array);
return;
}
break;
case I_MORE:
if(result==NULL)
{
break;
}
if(op1->data.typ==tDouble)
{
if(op2->data.typ!=tDouble)
{
result->data.value.b=false;
result->data.typ=tBool;
}
else
{
if(op1->data.value.d > op2->data.value.d)
{
result->data.value.b=true;
result->data.typ=tBool;
}
else
{
result->data.value.b=false;
result->data.typ=tBool;
}
}
}
else if (op1->data.typ==tString && op2->data.typ==tString)
{
if(strcmp(op1->data.value.s,op2->data.value.s)>0)
{
result->data.value.b=true;
result->data.typ = tBool;
}
else
{
result->data.value.b=false;
result->data.typ = tBool;
}
}
else
{
error = ESEM;
ta_Destroy(ta_table);
destroy_labels(jump_array);
return;
}
break;
/**********vestavene fce*************/
case I_READ:
if(op1==NULL)
{
break;
}
if(op1->data.typ==tDouble)
{
if(result==NULL)
{
return;
}
string text;
strCreate(&text);
int orez=op1->data.value.d;
int ch_znak = 0;
while(orez>0)
{
ch_znak=getchar();
if(result!=NULL)
{
strAppendChar(&text, (char) ch_znak);
}
(orez)--;
}
result->data.value.s=text.data;
result->data.typ = tString;
}
else if (op1->data.typ==tString)
{
int prepinac=-1;
if(strcmp(op1->data.value.s,"*n")==0)
{
prepinac=1;
}
else if(strcmp(op1->data.value.s,"*l")==0)
{
prepinac=2;
}
else if(strcmp(op1->data.value.s,"*a")==0)
{
prepinac=3;
}
else
{
error = ESEM;
}
string text;
strCreate(&text);
int znak=0;
int ret = 0; // pouze overeni scanf
switch(prepinac)
{
case -1:
return;
break;
case 1:
if(result!=NULL)
{
ret = scanf("%lf",&(result->data.value.d));
if(ret != 1) {
error = EINT;
return;
}
result->data.typ=tDouble;
}
break;
case 2:
while(1)
{
{
char ch_znak=0;
znak=getchar();
if((znak==10) || (znak==13))
{
break;
}
ch_znak=znak;
strAppendChar(&text, ch_znak);
}
}
if(result!=NULL)
{
result->data.value.s = text.data;
result->data.typ=tString;
}
break;
case 3:
while(1)
{
{
if((znak=getchar())==EOF)
{
break;
}
char ch_znak=0;
ch_znak=znak;
strAppendChar(&text, ch_znak);
}
}
if(result!=NULL)
{
result->data.value.s=text.data;
result->data.typ=tString;
}
}
}
else
{
error = ESEM;
ta_Destroy(ta_table);
destroy_labels(jump_array);
return;
}
break;
case I_WRITE:
if(op1==NULL)
{
break;
}
if(op1->data.typ==tDouble)
{
printf("%g", op1->data.value.d);
}
else if(op1->data.typ==tString)
{
char *retezec=op1->data.value.s;
if(retezec==NULL)
{
break;
}
for(int i=0; retezec[i]!='\0' ; i++)
{
if( retezec[i]=='\\' )
{
if( retezec[i+1]=='n' )
{
printf("\n");
i++;
}
else if( retezec[i+1]=='t' )
{
printf("\t");
i++;
}
else if( retezec[i+1]=='\\' )
{
printf("\\");
i++;
}
else if( retezec[i+1]=='\"' )
{
printf("\"");
i++;
}
else if( isdigit(retezec[i+1]) && isdigit(retezec[i+2]) && isdigit(retezec[i+3]) )
{
int ascii= atoi(&retezec[i+1]);
putchar(ascii);
i=i+3;
}
}
else
{
putchar(retezec[i]);
}
}
}
else
{
error = EINT;
ta_Destroy(ta_table);
destroy_labels(jump_array);
return;
}
break;
case I_SORT:
if(op1==NULL)
{
break;
}
pom=prep_quickSort(op1);
if(result!=NULL)
{
if(pom->data.typ==tString)
{
result->data.value.s=pom->data.value.s;
result->data.typ=tString;
}
else if(pom->data.typ==tNil)
{
result->data.typ=tNil;
}
}
break;
case I_SUBS:
pom=built_substr(op1, op2, result);
position++;
ta_Get(ta_table, &operation, &op1, &op2, &result, position);
if(result!=NULL)
{
if(pom->data.typ==tString)
{
result->data.value.s=pom->data.value.s;
result->data.typ=tString;
}
else if(pom->data.typ==tNil)
{
result->data.typ=tNil;
}
}
break;
case I_FIND:
pom=find(op1,op2);
if(pom->data.typ==tString)
{
result->data.value.s=pom->data.value.s;
result->data.typ=tString;
}
else if(pom->data.typ==tNil)
{
result->data.typ=tNil;
}
else if(pom->data.typ==tBool)
{
result->data.value.b=pom->data.value.b;
result->data.typ=tBool;
}
else if(pom->data.typ==tDouble)
{
result->data.value.d=pom->data.value.d;
result->data.typ=tDouble;
}
break;
case I_TYPE:
pom=built_type(op1);
if(result!=NULL)
{
result->data.value.s=pom->data.value.s;
result->data.typ=tString;
}
break;
/********************* jumpy a labely ****************************/
case I_LABEL:
break;
case I_TJUMP:
if((op1->data.typ==tBool) && (op1->data.value.b==true))
{
position=jump_array[(*ta_table)[position].jump];
}
else if((op1->data.typ==tDouble) && (op1->data.value.d!=0))
{
position=jump_array[(*ta_table)[position].jump];
}
break;
case I_FJUMP:
if((op1->data.typ==tBool) && (op1->data.value.b==false))
{
position=jump_array[(*ta_table)[position].jump];
}
else if((op1->data.typ==tDouble) && (op1->data.value.d==0))
{
position=jump_array[(*ta_table)[position].jump];
}
break;
case I_JUMP:
position=jump_array[(*ta_table)[position].jump];
break;
case I_FCEJUMP:
pole_back[index_pb]=(position);
index_pb++;
if((index_pb%9)==0) /*naalokovat jinak*/
{
pole_back=advRealloc( pole_back, index_pb*sizeof(int)+ALL_MORE);
}
position=jump_array[(*ta_table)[position].jump];
break;
case I_GOTO:
index_pb--;
if(index_pb<0)
{
return;
}
else
{
position=pole_back[index_pb];
}
break;
}
}
destroy_labels(jump_array);
}
int main()
{
//IMU data
double dx = 0;
float p = 0;
float q = 0;
float r = 0;
float ax = 0;
float ay = 0;
float az = 0;
//Kalman Filter output
float orientation[3]; //phi theta psi: roll pitch yaw
//open io files
FILE* inputfp; //gyro, accel
FILE* outputfp; //kalman filered data
inputfp = fopen("/media/TOSHIBA/College/Senior/S2/CPRE492/KalmanTests/imudata_for_C_code2.txt", "r");
if(inputfp == NULL)
{
printf("can't open input file\n");
return;
}
fseek (inputfp, 0, SEEK_SET );
//test
//printf("inputfp: %d\n", inputfp);
outputfp = fopen("/media/TOSHIBA/College/Senior/S2/CPRE492/KalmanTests/output.txt", "w+");
if(outputfp == NULL)
{
printf("can't open output file\n");
return;
}
//fseek (outputfp, 12, SEEK_SET );
//test
//printf("outputfp: %d\n", outputfp);
//tokenize input
char inputLine[SIZE_LINE];
//char lineToken[SIZE_TOKEN];
char* lineToken;
char delimiters[] = " \t";
//reading file
char readBuffer;
char isReadingLine = 0;
char hasLine = 0;
char tokenCount = 0;
int lineCounter = 0;
char isEnd = 0;
//initialize inputLine to 0's for appending
memset(inputLine, 0, SIZE_LINE);
//test
fseek (inputfp, 0, SEEK_END);
int size = ftell(inputfp);
//fprintf(outputfp, "size of file: %d\n", size);
fseek(inputfp, 0, SEEK_SET);
while(1)
{
//testing
/*
if(lineCounter == 6)
{
break;
}
*/
//read character
if(fread(&readBuffer, 1, 1, inputfp) == 1)
{
//find end of line
if(readBuffer == '\n' || isEnd)
{
hasLine = 1; //make function for this
}
//forming string from line of IMU data
if(readBuffer != '\n')
{
strAppendChar(inputLine, readBuffer);
}
//tokenize IMU data sentence, parse and store important data
if(hasLine)
{
//test
//printf("%s\n", inputLine);
lineToken = strtok(inputLine, delimiters);
tokenCount++;
//test
//printf("token: [%s] count: %d\n\n", lineToken, tokenCount);
//grab all IMU data
while(lineToken != NULL)
{
if(tokenCount == 1)
{
dx = strtof(lineToken, NULL);
//test
//printf("dx: [%.6f]\n", dx);
}
else if(tokenCount == 2)
{
p = strtof(lineToken, NULL);
//test
//printf("p: %.4f\n", p);
}
else if(tokenCount == 3)
{
q = strtof(lineToken, NULL);
//test
//printf("q: %.4f\n", q);
}
else if(tokenCount == 4)
{
r = strtof(lineToken, NULL);
//test
//printf("r: %.4f\n", r);
}
else if(tokenCount == 5)
{
ax = strtof(lineToken, NULL);
//test
//printf("ax: %.4f\n", ax);
}
else if(tokenCount == 6)
{
ay = strtof(lineToken, NULL);
//test
//printf("ay: %.4f\n", ay);
}
else if(tokenCount == 7)
{
az = strtof(lineToken, NULL);
//test
//printf("az: %.4f\n", az);
}
lineToken = strtok(NULL, delimiters);
tokenCount++;
//test
//printf("token: [%s] count: %d\n\n", lineToken, tokenCount);
}
//at this point have IMU data
//conversions
dx = dx * .001; //convert from msecs to secs
p = p*TORADS; //convert from deg/sec to rads/sec
q = q*TORADS;
r = r*TORADS;
Kalman_getOrientation(orientation, dx, p,q,r,ax,ay,az);
//test
fprintf(outputfp, "%.4f\t%.4f\t%.4f\n", orientation[0], orientation[1], orientation[2]);
hasLine = 0;
tokenCount = 0;
lineCounter++;
//test
if(lineCounter == 9999)
{
//fprintf(outputfp, "processed line 9999\n");
//fprintf(outputfp, "seek_cur %d\n", ftell(inputfp));
}
memset(inputLine, 0, SIZE_LINE);
}
}
if(isEnd)
{
break;
}
//find end of file
if(feof(inputfp))
{
isEnd = 1;
}
}
}
