/** Pushes back a char in the queue */
void q_push_back_data (FIFOQ *q, char data, off_t offset) {
// Create new Node
Node *n = malloc(sizeof(n));
n->data = data;
n->offset = offset;
q_push_back(q, n); // push using our common q_push_back function
}
int hadlock_algo(Pair* source, Pair* target){
int i, j, v1, v2, res;
Pair p, v, step[4] = {{1,0},{0,1},{-1,0},{0,-1}};
/* initialize all cells like unvisited */
for(i=0; i<sz.r; ++i){
for(j=0; j<sz.c; ++j){
W[i][j] = -1;
}
}
q_clear();
p_set(W,source,0);
q_push(source);
while(!q_empty()){
p = q_pop();
for(i=0; i<4; ++i){
v = p_sum(step+i,&p);
v1 = p_get(Z,&p);
v2 = p_get(Z,&v);
if(p_in(&v) && (p_get(W,&v)==-1 || (p_get(W,&v)>p_get(W,&p))) && (v1-v2 <= 3) && (v1-v2 >= -1) && can_go(&v)){
if(p_mhtn_norm(&v,target) < p_mhtn_norm(&p,target)){
p_set(W,&v,p_get(W,&p));
q_push_back(&v);
}else{ /* > */
/*if(!(p_get(W,&v)!=-1 || (p_get(W,&v)==p_get(W,&p)+1))){ */
p_set(W,&v,p_get(W,&p)+1);
q_push_front(&v);
}
if(p_eq(&v,target)){
break;
}
}
}
if(p_eq(&v,target)) break;
}
res = p_get(W,target);
if(res != -1) res = 2*res+p_mhtn_norm(source,target);
return res;
}
