/**
* Main function with all logic
* deal with threads and other things
*/
static void process(const char * filename, unsigned long k, unsigned long t) {
unsigned long n, i,
start,
end,
rest,
addition;
Multiseq * multiseq;
BestKVals * kval;
MultiseqTreadSpace* threads;
MultiseqTread* thread;
assert_with_message(filename != NULL, "filename can not be NULL");
multiseq = muliseq_new(filename);
kval = best_k_vals_new(k, compare, sizeof(MultiseqPaar));
threads = multiseq_threads(t);
n = rest = muliseq_items(multiseq);
thread = NULL;
while ((thread = multiseq_thread_next(threads)) != NULL) {
addition = calculate_addition(t, thread->id, rest);
start = calculate_start(t, thread->id, rest, addition);
end = rest;
rest = calculate_rest(start);
thread->i = start;
thread->j = end;
thread->multiseq = multiseq;
thread->compare = eval_unit_edist;
pthread_create_or_die(&thread->identifier, eval_seqrange_thread, thread);
}
while ((thread = multiseq_thread_next(threads)) != NULL) {
pthread_join(thread->identifier, NULL);
}
n = muliseq_paars(multiseq);
for (i = 0; i < n; i++) {
const MultiseqPaar * element = muliseq_paar(multiseq, i);
best_k_vals_insert(kval, element);
}
best_k_vals_reset_iter(kval);
for (i = 0; i < k; i++) {
const MultiseqPaar * element = (MultiseqPaar *) best_k_vals_next(kval);
printf("%lu\t%lu\t%lu\n", element->id1, element->id2, element->distance);
}
best_k_vals_delete(kval);
multiseq_threads_delete(threads);
muliseq_delete(multiseq);
}
/**
* Handles the arithmetic operation signal.
*
* @param p0 the internal memory
* @param p1 the knowledge memory
* @param p2 the knowledge memory count
* @param p3 the knowledge memory size
* @param p4 the signal memory
* @param p5 the signal memory count
* @param p6 the signal memory size
* @param p7 the shutdown flag
* @param p8 the signal memory interrupt request flag
* @param p9 the signal memory mutex
* @param p10 the model / signal / operation
* @param p11 the model / signal / operation count
* @param p12 the details / parameters
* @param p13 the details / parameters count
* @param p14 the signal priority (Hand over as reference!)
* @param p15 the signal identification (Hand over as reference!)
* @param p16 the comparison result
*/
void handle_arithmetic_operation(void* p0, void* p1, void* p2, void* p3, void* p4, void* p5, void* p6,
void* p7, void* p8, void* p9, void* p10, void* p11, void* p12, void* p13, void* p14, void* p15, void* p16) {
log_terminated_message((void*) DEBUG_LEVEL_LOG_MODEL, (void*) L"Handle arithmetic operation.");
// The comparison result.
int* r = (int*) p16;
if (*r == *NUMBER_0_INTEGER_MEMORY_MODEL) {
compare_equal_arrays(p16, p10, p11, (void*) ADD_ARITHMETIC_OPERATION_CYBOL_MODEL, (void*) ADD_ARITHMETIC_OPERATION_CYBOL_MODEL_COUNT, (void*) WIDE_CHARACTER_PRIMITIVE_MEMORY_ABSTRACTION);
if (*r != *NUMBER_0_INTEGER_MEMORY_MODEL) {
calculate_addition(p12, p13, p1, p2, p3);
}
}
if (*r == *NUMBER_0_INTEGER_MEMORY_MODEL) {
compare_equal_arrays(p16, p10, p11, (void*) DIVIDE_ARITHMETIC_OPERATION_CYBOL_MODEL, (void*) DIVIDE_ARITHMETIC_OPERATION_CYBOL_MODEL_COUNT, (void*) WIDE_CHARACTER_PRIMITIVE_MEMORY_ABSTRACTION);
if (*r != *NUMBER_0_INTEGER_MEMORY_MODEL) {
//?? calculate_division(p12, p13, p1, p2, p3);
}
}
if (*r == *NUMBER_0_INTEGER_MEMORY_MODEL) {
compare_equal_arrays(p16, p10, p11, (void*) MULTIPLY_ARITHMETIC_OPERATION_CYBOL_MODEL, (void*) MULTIPLY_ARITHMETIC_OPERATION_CYBOL_MODEL_COUNT, (void*) WIDE_CHARACTER_PRIMITIVE_MEMORY_ABSTRACTION);
if (*r != *NUMBER_0_INTEGER_MEMORY_MODEL) {
//?? calculate_multiplication(p12, p13, p1, p2, p3);
}
}
if (*r == *NUMBER_0_INTEGER_MEMORY_MODEL) {
compare_equal_arrays(p16, p10, p11, (void*) SUBTRACT_ARITHMETIC_OPERATION_CYBOL_MODEL, (void*) SUBTRACT_ARITHMETIC_OPERATION_CYBOL_MODEL_COUNT, (void*) WIDE_CHARACTER_PRIMITIVE_MEMORY_ABSTRACTION);
if (*r != *NUMBER_0_INTEGER_MEMORY_MODEL) {
//?? calculate_subtraction(p12, p13, p1, p2, p3);
}
}
}
int main(){
int i,j,n;
printf("sin30=%f\n", sin(3.14/6));
/* double measurement[10] = {10, 19, 29, 38, 51, 59, 69, 80, 91, 101};
double measurement_variance = 1;
double motion[10] = {10, 10, 10, 10, 10, 10, 10, 10, 10, 10};
double motion_variance = 1;
double transpose[4][4] = {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}};
calculate_transpose(*P_matrix, *transpose, 4, 4);
printf("print the transpose: %f, %f, %f, %f\n", transpose[0][0], transpose[0][1], transpose[0][2], transpose[0][3]);
double matrix_mul[4][4] = {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}};
calculate_multiplication(*P_matrix, *transpose, 4, 4, 4, 4, *matrix_mul);
double matrix_add[4][4] = {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}};
calculate_addition(*P_matrix, *transpose, 4, 4, 4, 4, *matrix_add);
printf(" matrix_add: %f, %f, %f, %f\n", matrix_add[0][0], matrix_add[0][1], matrix_add[0][2], matrix_add[0][3]);
double matrix_tobeinversed[2][2] = {{1, 0}, {0, 1}};
double matrix_inverse[2][2] = {{0, 0}, {0, 0}};
calculate_square_inverse(*matrix_tobeinversed, *matrix_inverse);
printf(" matrix_inverse: %f, %f, %f, %f\n", matrix_inverse[0][0], matrix_inverse[0][1], matrix_inverse[1][0], matrix_inverse[1][1]);
for(i=0; i<sizeof(measurement)/sizeof(double); i++){
values = update(values[0], values[1], measurement[i], measurement_variance);
values = predict(values[0], values[1], motion[i], motion_variance );
}*/
double F_transpose[4][4];
calculate_transpose(*F_matrix, *F_transpose, 4, 4);
//print_matrix("F_transpose", *F_transpose, 4, 4);
double H_transpose[4][2];
calculate_transpose(*H_matrix, *H_transpose, 2, 4);
//print_matrix("H_transpose", *H_transpose, 4, 2);
//double start_time = omp_get_wtime ();
struct timeval tv;
gettimeofday(&tv, 0);
double start_mill = (tv.tv_sec) * 1000 + (tv.tv_usec) / 1000 ;
//#pragma omp parallel
{
//#pragma omp for num_threads(1)
for(n=0; n<NUM_MEASUREMENTS; n++){
//int tid = omp_get_thread_num();
//int num_threads = omp_get_num_threads ();
//printf("tid=%d, num_threads=%d\n", tid, num_threads);
//printf("this is the iteration number %d\n", n);
// prediction
//x = (F * x) + u
double matrix_mul[4][1] = {{0}, {0}, {0}, {0}};
calculate_multiplication(*F_matrix, *x_matrix, 4, 4, 4, 1, *matrix_mul);
//print_matrix("matrix_mul", *matrix_mul, 4, 1);
copy_matrix(*matrix_mul, *x_matrix, 4, 1);
////print_matrix("x_matrix", *x_matrix, 4, 1);
//P = F * P * F.transpose()
double matrix_mult_PFt[4][4] = {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}};
calculate_multiplication(*P_matrix, *F_transpose, 4, 4, 4, 4, *matrix_mult_PFt);
//print_matrix("matrix_mult_PFt", *matrix_mult_PFt, 4, 4);
calculate_multiplication(*F_matrix, *matrix_mult_PFt, 4, 4, 4, 4, *P_matrix);
//print_matrix("P_matrix", *P_matrix, 4, 4);
//# measurement update
//Z = matrix([measurements[n]])
double Z_matrix[2][1];
/*for(i=0; i<2; i++){
Z_matrix[i][0] = measurements[i][n] ;
}*/
if(measurements[1][n] >= 0 && measurements[1][n] < PI/2)
{
Z_matrix[0][0] = measurements[0][n]/sqrt((pow(tan(measurements[1][n]), 2) +1));
Z_matrix[1][0] = (measurements[0][n]*tan(measurements[1][n]))/sqrt((pow(tan(measurements[1][n]), 2) +1));
}
else if(measurements[1][n] >= PI/2 && measurements[1][n] < PI)
{
Z_matrix[0][0] = -1*measurements[0][n]/sqrt((pow(tan(measurements[1][n]), 2) +1));
Z_matrix[1][0] = -1*(measurements[0][n]*tan(measurements[1][n]))/sqrt((pow(tan(measurements[1][n]), 2) +1));
}
else if(measurements[1][n] >= PI && measurements[1][n] < 3*PI/2)
{
Z_matrix[0][0] = -1*measurements[0][n]/sqrt((pow(tan(measurements[1][n]), 2) +1));
Z_matrix[1][0] = -1*(measurements[0][n]*tan(measurements[1][n]))/sqrt((pow(tan(measurements[1][n]), 2) +1));
}
else if(measurements[0][n] >= 3*PI/2 && measurements[0][n] < 2*PI)
{
Z_matrix[0][0] = measurements[0][n]/sqrt((pow(tan(measurements[1][n]), 2) +1));
Z_matrix[1][0] = (measurements[0][n]*tan(measurements[1][n]))/sqrt((pow(tan(measurements[1][n]), 2) +1));
}
print_matrix("Z_matrix", *Z_matrix, 2, 1);
//y = Z.transpose() - (H * x)
/*H_matrix[0][0] = atan2(x_matrix[1][0], x_matrix[0][0])/x_matrix[0][0];
H_matrix[1][1] = sqrt(pow(x_matrix[0][0], 2) + pow(x_matrix[1][0], 2))/x_matrix[1][0];
printf("H00=%lf, H11=%lf\n",H_matrix[0][0], H_matrix[1][1] );*/
double matrix_mult_Hx[2][1];
calculate_multiplication(*H_matrix, *x_matrix, 2, 4, 4, 1, *matrix_mult_Hx);
//print_matrix("matrix_mult_Hx", *matrix_mult_Hx, 2, 1);
double y_matrix[2][1];
calculate_subtraction(*Z_matrix, *matrix_mult_Hx, 2, 1, 2, 1, *y_matrix);
//print_matrix("y_matrix", *y_matrix, 2, 1);
//S = H * P * H.transpose() + R
double matrix_mult_PHt[4][2];
calculate_multiplication(*P_matrix, *H_transpose, 4, 4, 4, 2, *matrix_mult_PHt);
//print_matrix("matrix_mult_PHt", *matrix_mult_PHt, 4, 2);
double matrix_mult_HPHt[2][2];
calculate_multiplication(*H_matrix, *matrix_mult_PHt, 2, 4, 4, 2, *matrix_mult_HPHt);
//print_matrix("matrix_mult_HPHt", *matrix_mult_HPHt, 2, 2);
double S_matrix[2][2];
calculate_addition(*matrix_mult_HPHt, *R_matrix, 2, 2, 2, 2, *S_matrix);
//print_matrix("S_matrix", *S_matrix, 2, 2);
//K = P * H.transpose() * S.inverse()
double S_inverse[2][2];
calculate_square_inverse(*S_matrix, *S_inverse);
//print_matrix("S_inverse", *S_inverse, 2, 2);
double matrix_mult_HtSi[4][2];
calculate_multiplication(*H_transpose, *S_inverse, 4, 2, 2, 2, *matrix_mult_HtSi);
//print_matrix("matrix_mult_HtSi", *matrix_mult_HtSi, 4, 2);
double K_matrix[4][2];
calculate_multiplication(*P_matrix, *matrix_mult_HtSi, 4, 4, 4, 2, *K_matrix);
//print_matrix("K_matrix", *K_matrix, 4, 2);
//x = x + (K * y)
double matrix_mult_Ky[4][1];
calculate_multiplication(*K_matrix, *y_matrix, 4, 2, 2, 1, *matrix_mult_Ky);
//print_matrix("matrix_mult_Ky", *matrix_mult_Ky, 4, 1);
double matrix_add_x_Ky[4][1];
calculate_addition(*x_matrix, *matrix_mult_Ky, 4, 1, 4, 1, *matrix_add_x_Ky);
//print_matrix("matrix_add_x_Ky", *matrix_add_x_Ky, 4, 1);
copy_matrix(*matrix_add_x_Ky, *x_matrix, 4, 1);
print_matrix("x_matrix", *x_matrix, 4, 1);
//P = (I - (K * H)) * P
double matrix_mult_KH[4][4];
calculate_multiplication(*K_matrix, *H_matrix, 4, 2, 2, 4, *matrix_mult_KH);
//print_matrix("matrix_mult_KH", *matrix_mult_KH, 4, 4);
double matrix_sub_I_KH[4][4];
calculate_subtraction(*I_matrix, *matrix_mult_KH, 4, 4, 4, 4, *matrix_sub_I_KH);
//print_matrix("matrix_sub_I_KH", *matrix_sub_I_KH, 4, 4);
double matrix_mult_IKHP[4][4];
calculate_multiplication(*matrix_sub_I_KH, *P_matrix, 4, 4, 4, 4, *matrix_mult_IKHP);
//print_matrix("matrix_mult_IKHP", *matrix_mult_IKHP, 4, 4);
copy_matrix(*matrix_mult_IKHP, *P_matrix, 4, 4);
////print_matrix("P_matrix", *P_matrix, 4, 2);
}
}
gettimeofday(&tv, 0);
double finish_mill = (tv.tv_sec) * 1000 + (tv.tv_usec) / 1000 ;
printf("start_mill = %lf\n", start_mill);
printf("finish_mill = %lf\n", finish_mill);
printf("total time for the kalman loop execution = %lf\n", finish_mill-start_mill);
/*double finish_time = omp_get_wtime();
double total_time = finish_time - start_time;
printf("total time for the kalman loop execution = %lf\n", total_time);*/
}
