routing no queue

routing no queue

#define KEY_TYPE float
#define VALUE_TYPE uint
#include "sorting.cl"

//QUEUE
typedef int4 QueueElement;

#define QueueElementCopySteps 3

#define  QueueElementEmpty 0
#define  QueueElementReady 1
#define  QueueElementReading 2
#define  QueueElementRead 0
void checkAndSetQueueState(volatile global QueueElement* element, uint state, uint expected)
{
  volatile global int* pstate = ((volatile global int*)element) + 3;
  while(atomic_cmpxchg(pstate, expected, state) != expected);
}
void setQueueState(volatile global QueueElement* element, uint state)
{
  volatile global int* pstate = ((volatile global int*)element) + 3;
  uint old = atomic_xchg(pstate, state);
  //check: old + 1 % (QueueElementReading+1) == state
}
uint getQueueState(volatile global QueueElement* element)
{
  return element->w;
}

int3 getBlockId(volatile global QueueElement* element)
{
  return (*element).xyz;
}

void setBlockId(volatile global QueueElement* element, int3 block)
{
  (*element).xyz = block;
}

struct _QueueGlobal
{
  uint front;
  uint back;
  int filllevel;
  uint activeBlocks;
  uint processedBlocks;
  uint sortingBarrier;
  int debug;
  uint mincost;
};
typedef struct _QueueGlobal QueueGlobal;

#define linAccess3D(id, dim) \
  ((id).x + ( (id).y + (id).z  * (dim.y) )* (dim).x)

float atomic_min_float(volatile global float* p, float val)
{
  return as_float(atomic_min((volatile global uint*)(p),as_uint(val)));
}

void Enqueue(volatile global QueueGlobal* queueInfo,
             volatile global QueueElement* queue,
             volatile global float* queuePriority,
             int3 block,
             float priority,
             int2 blocks,
             uint queueSize)
{
  //check if element is already in tehe Queue:

  volatile global float* myQueuePriority = queuePriority + linAccess3D(block, blocks);

  //update priority:
  float old_priority = atomic_min_float(myQueuePriority, priority);

  //if there is a priority, it is in the queue
  if(old_priority != MAXFLOAT)
    return;
  else
  {
    //add to queue

    //inc size
    uint filllevel = atomic_inc(&queueInfo->filllevel);

    //make sure there is enough space?
    //assert(filllevel < queueSize)

    //insert a new element
    uint pos = atomic_inc(&queueInfo->back)%queueSize;

    //make sure it is free?
    //queue[pos] == QueueElementEmpty

    //insert the data
    setBlockId(queue + pos, block);
    write_mem_fence(CLK_GLOBAL_MEM_FENCE);

    //increase the number of active Blocks
    atomic_inc(&queueInfo->activeBlocks);

    //activate the queue element
    setQueueState(queue + pos,  QueueElementReady);

  }
}

bool Dequeue(volatile global QueueGlobal* queueInfo,
             volatile global QueueElement* queue,
             volatile global float* queuePriority,
             int3* element,
             int2 blocks,
             uint queueSize)
{
  //is there something to get?
  if(atomic_dec(&queueInfo->filllevel) <= 0)
  {
    atomic_inc(&queueInfo->filllevel);
    return false;
  }

  //pop the front of the queue
  uint pos = atomic_inc(&queueInfo->front)%queueSize;

  //check for barrier
  uint barrierPos = queueInfo->sortingBarrier;
  uint back = queueInfo->back%queueSize;
  while(barrierPos <= pos && barrierPos > back)
  {
    barrierPos = queueInfo->sortingBarrier;
    queueInfo->debug += 100000;
  }

  //wait for the element to be written/change to reading
  checkAndSetQueueState(queue + pos, QueueElementReading, QueueElementReady);

  //read
  *element = getBlockId(queue + pos);

  //remove priority (mark as not in queue)
  volatile global float* myQueuePriority = queuePriority + linAccess3D(*element, blocks);
  atomic_xchg(myQueuePriority, MAXFLOAT);

  //free queue element
  setQueueState(queue + pos, QueueElementRead);
  return true;
}
//

float getPenalty(read_only image2d_t costmap, int2 pos)
{
  const sampler_t sampler = CLK_FILTER_NEAREST
                          | CLK_NORMALIZED_COORDS_FALSE
                          | CLK_ADDRESS_CLAMP_TO_EDGE;

  return read_imagef(costmap, sampler, pos).x;
}

__kernel void clearQueueLink(global float* queuePriority)
{
  queuePriority[get_global_id(0)] = MAXFLOAT;
}

//it might be better to do that via opengl or something (arbitrary node shapes etc)
__kernel void prepareRouting(read_only image2d_t costmap,
                           global float* routecost,
                           global QueueElement* queue,
                           global float* queuePriority,
                           global QueueGlobal* queueInfo,
                           uint queueSize,
                           global uint4* startingPoints,
                           const uint numStartingPoints,
                           const int2 dim,
                           const int2 numBlocks)
{
  __local bool hasStartingPoint;

  int3 id = {get_global_id(0), get_global_id(1), get_global_id(2)};

  if(get_local_id(0) == 0 && get_local_id(1) == 0 && get_local_id(2) == 0)
    hasStartingPoint = false;
  barrier(CLK_LOCAL_MEM_FENCE);

  if(get_local_id(0) < dim.x && get_local_id(1) < dim.y)
  {
    float cost = 0.01f*MAXFLOAT;
    if(startingPoints[id.z].x <= id.x && id.x <= startingPoints[id.z].z &&
        startingPoints[id.z].y <= id.y && id.y <= startingPoints[id.z].w)
    {
        cost = 0;
        hasStartingPoint = true;
    }
    //prepare data
    routecost[dim.x*id.y+id.x + dim.x*dim.y*id.z] = cost;
    barrier(CLK_LOCAL_MEM_FENCE);
    if(hasStartingPoint && get_local_id(0) == 0 && get_local_id(1) == 0)
    {
      //insert into queue
      int3 block = (int3)(get_group_id(0), get_group_id(1), id.z);
      Enqueue(queueInfo, queue, queuePriority, block, 0, numBlocks, queueSize);
    }
  }
}

local float* accessLocalCost(local float* l_costs, int2 id)
{
  return l_costs + (id.x+1 + (id.y+1)*(get_local_size(0)+2));
}

void copyInLocalData(global float* routecost,
           int3 blockid,
           local float* l_costs,
           int2 dim)
{
  blockid.x *= get_local_size(0);
  blockid.y *= get_local_size(1);

  //copy cost to local
  int local_linid = get_local_id(1)*get_local_size(0) + get_local_id(0);
  for(int i = local_linid; i < (get_local_size(0)+2)*(get_local_size(1)+2); i += get_local_size(0)*get_local_size(1))
  {
    int2 global_pos = (int2)(blockid.x, blockid.y) + (int2)(i%(get_local_size(0)+2),i/(get_local_size(0)+2)) - (int2)(1,1);

    float incost = 0.01f*MAXFLOAT;
    if(global_pos.x > 0 && global_pos.x < dim.x &&
       global_pos.y > 0 && global_pos.y < dim.y)
      incost = routecost[dim.x*global_pos.y + global_pos.x + dim.x*dim.y*blockid.z];
    l_costs[i] = incost;
  }
}

void copyOutLocalData(global float* routecost,
           int3 blockid,
           local float* l_costs,
           int2 dim)
{
  //copy cost to global
  int2 global_pos = (int2)(get_local_size(0)*blockid.x + get_local_id(0), get_local_size(1)*blockid.y + get_local_id(1));
  if(global_pos.x < dim.x && global_pos.y < dim.y)
    routecost[dim.x*global_pos.y + global_pos.x + dim.x*dim.y*blockid.z] = *accessLocalCost(l_costs, (int2)(get_local_id(0), get_local_id(1)));
}

bool route(read_only image2d_t costmap,
           global float* routecost,
           int3 blockid,
           local float* l_costs,
           int2 dim)
{
  copyInLocalData(routecost, blockid, l_costs, dim);

  //route as long as there is change
  local bool change;
  change = true;
  barrier(CLK_LOCAL_MEM_FENCE);

  int2 localid = (int2)(get_local_id(0),get_local_id(1));
  int2 globalid = (int2)(blockid.x*get_local_size(0)+get_local_id(0),blockid.y*get_local_size(1)+get_local_id(1));

  //iterate over it
  float myPenalty = getPenalty(costmap, globalid);
  float local * myval = accessLocalCost(l_costs, localid);
  int counter = -1;
  while(change)
  {
    change = false;
    barrier(CLK_LOCAL_MEM_FENCE);

    float lastmyval = *myval;

    int2 offset;
    for(offset.y = -1; offset.y  <= 1; ++offset.y)
      for(offset.x = -1; offset.x <= 1; ++offset.x)
      {
        //TODO d is either 1 or M_SQRT2 - maybe it is faster to substitute
        float d = native_sqrt((float)(offset.x*offset.x+offset.y*offset.y));
        float penalty = 0.5f*(myPenalty + getPenalty(costmap, globalid + offset));
        float c_other = *accessLocalCost(l_costs, localid + offset);
        //TODO use custom params here
        *myval = min(*myval, c_other + 0.01f*d + d*penalty);
      }

    if(*myval != lastmyval)
      change = true;

    barrier(CLK_LOCAL_MEM_FENCE);
    ++counter;
  }

  copyOutLocalData(routecost, blockid, l_costs, dim);
  return counter > 0;
}

void minBorderCosts(local float* l_costs,
                    local float borderCosts[4])
{
  if(get_local_id(0) < 4)
    borderCosts[get_local_id(0)] = MAXFLOAT;
  barrier(CLK_LOCAL_MEM_FENCE);
  float myCost = *accessLocalCost( l_costs, (int2)(get_local_id(0), get_local_id(1)) );
  int target = -1;
  if(get_local_id(0) == 0)
    atomic_min((local uint*)(&borderCosts[0]), as_uint(myCost));
  else if(get_local_id(0) == get_local_size(0) - 1)
    atomic_min((local uint*)(&borderCosts[2]), as_uint(myCost));
  if(get_local_id(1) == 0)
    atomic_min((local uint*)(&borderCosts[1]), as_uint(myCost));
  else if(get_local_id(1) == get_local_size(1) - 1)
    atomic_min((local uint*)(&borderCosts[3]), as_uint(myCost));
  barrier(CLK_LOCAL_MEM_FENCE);
}

void routing_worker(read_only image2d_t costmap,
                      global float* routecost,
                      volatile global QueueElement* queue,
                      volatile global float* queuePriority,
                      volatile global QueueGlobal* queueInfo,
                      uint queueSize,
                      const int2 dim,
                      const int2 numBlocks,
                      const uint processedBlocksThreshold,
                      local float* l_costs)
{
  while(queueInfo->activeBlocks > 0 && queueInfo->processedBlocks < processedBlocksThreshold)
  {
    local int3 blockId;
    if(get_local_id(0) == 0 && get_local_id(1) == 0)
    {
      int3 element;
      if(Dequeue(queueInfo, queue, queuePriority, &element, numBlocks, queueSize))
        blockId = element;
      else
        blockId.x = -1;
    }
    barrier(CLK_LOCAL_MEM_FENCE);

    if(blockId.x > -1)
    {
      //process it
      bool change = route(costmap, routecost, blockId, l_costs, dim);

      //add surrounding ones
      if(change)
      {
        local float borderCosts[4];
        minBorderCosts(l_costs, borderCosts);
        if(get_local_id(0) == 0 && get_local_id(1) == 0)
        {
          int3 block = blockId;
          if(blockId.x - 1 >= 0)
          {
            block.x = blockId.x - 1;
            Enqueue(queueInfo, queue, queuePriority, block, borderCosts[0], numBlocks, queueSize);
            block.x = blockId.x;
          }
          if(blockId.x + 1 < numBlocks.x)
          {
            block.x = blockId.x + 1;
            Enqueue(queueInfo, queue, queuePriority, block, borderCosts[2], numBlocks, queueSize);
            block.x = blockId.x;
          }
          if(blockId.y - 1 >= 0)
          {
            block.y = blockId.y - 1;
            Enqueue(queueInfo, queue, queuePriority, block, borderCosts[1], numBlocks, queueSize);
          }
          if(blockId.y + 1 < numBlocks.y)
          {
            block.y = blockId.y + 1;
            Enqueue(queueInfo, queue, queuePriority, block, borderCosts[3], numBlocks, queueSize);
          }
        }
      }
      //else if(get_local_id(0) == 0 && get_local_id(1) == 0)
      //  atomic_inc(&queueInfo->debug);

      //update counter
      if(get_local_id(0) == 0 && get_local_id(1) == 0)
      {
        atomic_dec(&queueInfo->activeBlocks);
        atomic_inc(&queueInfo->processedBlocks);
      }

    }
    barrier(CLK_LOCAL_MEM_FENCE);
  }
}

uint wrappedAroundQueueId(uint id, uint queueSize)
{
  return min(id, id + queueSize);
}
void sortQueue(volatile global QueueElement* queue,
               volatile global QueueGlobal* queueInfo,
               volatile global float* queuePriorities,
               uint queueSize,
               const int2 numBlocks,
               const uint processedBlocksThreshold,
               local float* l_priorites,
               local uint* l_ids)
{
  local bool ok;
  uint nextsort = queueSize + 1;
  uint linid = get_local_id(1) * get_local_size(0) + get_local_id(0);
  uint threads = get_local_size(0)*get_local_size(1);
  uint sortMargin =  4*threads;

  //prepare local ids
  l_ids[linid] = linid;
  l_ids[threads + linid] = threads + linid;

  while(queueInfo->activeBlocks > 0 && queueInfo->processedBlocks < processedBlocksThreshold)
  {
    ok = true;
    barrier(CLK_LOCAL_MEM_FENCE);

    //restart at back?
    if(nextsort > queueSize)
      nextsort = queueInfo->back % queueSize; //the back might just be adding

    //is the distance bigger than the margin? (ring buffer)
    uint margin = wrappedAroundQueueId(nextsort - (queueInfo->front%queueSize), queueSize);
    if(margin < sortMargin)
    {
      //margin is too small, dont even bother
      nextsort = queueSize + 1;
      continue;
    }

    //copy the data in
    uint in_out_id0 = wrappedAroundQueueId(nextsort - 2*threads + linid, queueSize);
    //make sure it is ready
    int err_counter = 100;
    while(getQueueState(queue + in_out_id0) != QueueElementReady && --err_counter);
    if(!err_counter) ok = false;
    int3 blockId0 = getBlockId(queue + in_out_id0);
    l_priorites[linid] = queuePriorities[linAccess3D(blockId0, numBlocks)];

    uint in_out_id1 = wrappedAroundQueueId(nextsort - threads + linid, queueSize);
    //make sure it is ready
    err_counter = 100;
    while(getQueueState(queue + in_out_id1) != QueueElementReady && --err_counter);
    if(!err_counter) ok = false;
    int3 blockId1 = getBlockId(queue + in_out_id1);
    l_priorites[threads +  linid] = queuePriorities[linAccess3D(blockId1, numBlocks)];

    barrier(CLK_LOCAL_MEM_FENCE);

    if(!ok) //loading did not work
      continue;

    //sort
    bitonicSort(linid,l_priorites,l_ids,2*get_local_size(0),true);

    uint target0 = l_ids[linid];
    uint target1 = l_ids[threads + linid];

    //copy blockids around
    #define copyBlockIds(COMPONENT) \
      barrier(CLK_LOCAL_MEM_FENCE); \
      l_ids[target0] = blockId0.COMPONENT; \
      l_ids[target1] = blockId1.COMPONENT; \
      barrier(CLK_LOCAL_MEM_FENCE); \
      blockId0.COMPONENT = l_ids[linid]; \
      blockId1.COMPONENT = l_ids[threads + linid];

    copyBlockIds(x);
    copyBlockIds(y);
    copyBlockIds(z);

    #undef copyBlockIds

    //check if we can write it back
    margin = wrappedAroundQueueId(nextsort - (queueInfo->front%queueSize), queueSize);
    if(margin > 3*threads)
    {
      //set the barrier
      queueInfo->sortingBarrier =  wrappedAroundQueueId(nextsort - 2*threads, queueSize);
      write_mem_fence(CLK_GLOBAL_MEM_FENCE);

      //check if it is still ok
      margin = wrappedAroundQueueId(nextsort - (queueInfo->front%queueSize), queueSize);
      if(margin > 2*threads)
      {
        //write back
        setBlockId(queue + in_out_id0, blockId0);
        setBlockId(queue + in_out_id1, blockId1);
      }
      //remove barrier
      queueInfo->sortingBarrier = queueSize + 1;
      write_mem_fence(CLK_GLOBAL_MEM_FENCE);
      queueInfo->debug++;
    }
    else
    {
      //increase sort margin
      sortMargin += threads;
      //restart sort at the beginning
      nextsort = queueSize + 1;
      queueInfo->debug+= 1000;
    }

    //reset local ids
    l_ids[linid] = linid;
    l_ids[threads + linid] = threads + linid;
  }
}

__kernel void routing(read_only image2d_t costmap,
                      global float* routecost,
                      volatile global QueueElement* queue,
                      volatile global float* queuePriority,
                      volatile global QueueGlobal* queueInfo,
                      uint queueSize,
                      const int2 dim,
                      const int2 numBlocks,
                      const uint processedBlocksThreshold,
                      local float* l_data)
{
  //if(get_group_id(0) + get_group_id(1) + get_group_id(2) == 0)
  ////sorter
  //  sortQueue(queue,
  //            queueInfo,
  //            queuePriority,
  //            queueSize,
  //            numBlocks,
  //            processedBlocksThreshold,
  //            l_data,
  //            (local uint*)( l_data+2*get_local_size(0)*get_local_size(1) ) );
  //else
    //worker
     routing_worker(costmap,
                    routecost,
                    queue,
                    queuePriority,
                    queueInfo,
                    queueSize,
                    dim,
                    numBlocks,
                    processedBlocksThreshold,
                    l_data);
}

__kernel void getMinimum(global const float* routecost,
                         volatile global QueueGlobal* queueInfo,
                         volatile global ushort2* coordinates,
                         const int2 dim,
                         int targets)
{
  float cost = 0;
  for(int i = 0; i < targets; ++i)
    cost += routecost[dim.x*get_global_id(1) + get_global_id(0) + dim.x*dim.y*i];

  __local uint localmin;
  __local uint2 coords;
  localmin = as_uint(MAXFLOAT);

  barrier(CLK_LOCAL_MEM_FENCE);

  atomic_min(&localmin, as_uint(cost));

  barrier(CLK_LOCAL_MEM_FENCE);

  if(localmin == as_uint(cost))
    coords = (uint2)(get_global_id(0), get_global_id(1));

  barrier(CLK_LOCAL_MEM_FENCE);

  if( (get_local_id(0) + get_local_id(1)) == 0 )
  {
    uint minv = as_uint(localmin);
    //compress it
    minv = (minv << 1) & 0xFFFFFF00;
    //combine with block id
    uint groupv = (get_group_id(0) + get_group_id(1)*get_num_groups(0)) & 0xFF;
    uint combined = minv | groupv;
    uint old = atomic_min(&queueInfo->mincost, combined);
    if(old > combined)
    {
      //write coords to array entry
      coordinates[groupv] = (ushort2)(coords.x,coords.y);
    }
  }
}

uint borderId(int2 lid, int2 lsize)
{
  uint id = lid.x;
  if(lid.x==lsize.x-1)
    id += lid.y;
  else if(lid.y==lsize.y-1)
    id = lsize.x*2 + lsize.y - 3 - lid.x;
  else if(lid.x == 0 && lid.y > 0)
    id = 2*(lsize.x + lsize.y - 2) - lid.y;
  return id;
}

__kernel void calcInOut(global const float* routecost,
                        global uint* blockroutes,
                        local int* l_cost,
                        local int* l_origin,
                        const int2 start,
                        const int2 dim)
{
  int3 coord = (int3)(get_global_id(0), get_global_id(1), get_global_id(2));
  float mincost = MAXFLOAT;
  if(coord.x >= 0 && coord.x < dim.x &&
     coord.y >= 0 && coord.y < dim.y)
    mincost =  routecost[coord.x + dim.x*coord.y + dim.x*dim.y*coord.z];

  int2 dir = (int2)(0,0);
  for(int y = -1; y <= 1; ++y)
    for(int x = -1; x <= 1; ++x)
    {
      coord = (int3)(get_global_id(0)+x, get_global_id(1)+y, get_global_id(2));
      if(coord.x >= 0 && coord.x < dim.x &&
         coord.y >= 0 && coord.y < dim.y)
      {
        float tcost =  routecost[coord.x + dim.x*coord.y + dim.x*dim.y*coord.z];
        if(tcost < mincost)
        {
          mincost = tcost;
          dir = (int2)(x,y);
        }
      }
    }
  //store dir and cost
  int2 lid = (int2)(get_local_id(0), get_local_id(1));
  int2 fromid = lid + dir;

  bool outside = fromid.x < 0 ||
                fromid.y < 0 ||
                fromid.x >= get_local_size(0) ||
                fromid.y >= get_local_size(1);

  int cost = (dir.x == 0 && dir.y == 0) ? 0 :
             (outside ? 1 : (2*get_local_size(0)*get_local_size(1)));

  int origin = (dir.x == 0 && dir.y == 0) ? ( ( (1 + lid.y) << 8 ) | (lid.x + 1)) :
               (outside ? ( ( (1 + fromid.y) << 8 ) | (1 + fromid.x ) ) : -1);

  int loffset = lid.x + lid.y*(int)get_local_size(0);
  int lfrom = fromid.x + fromid.y*(int)get_local_size(0);
  l_cost[loffset] = cost;
  l_origin[loffset] = origin;

  __local bool changed;
  uint count = 0;
  do
  {
    changed = false;
    barrier(CLK_LOCAL_MEM_FENCE);
    if(origin == -1)
    {
      if(l_cost[loffset] != l_cost[lfrom]+1)
      {
        l_cost[loffset] = l_cost[lfrom]+1;
        l_origin[loffset] = l_origin[lfrom];
        changed = true;
      }
    }
    barrier(CLK_LOCAL_MEM_FENCE);
  }
  while(changed);

  bool border =  get_local_id(0) == 0 ||
                 get_local_id(1) == 0 ||
                 get_local_id(0) == get_local_size(0)-1 ||
                 get_local_id(1) == get_local_size(1)-1;

  uint info = (l_cost[loffset] << 16) | (l_origin[loffset] &0xFFFF);

  if(border)
  {
    //store the info
    uint localoffset = borderId((int2)(get_local_id(0),get_local_id(1)), (int2)(get_local_size(0),get_local_size(1)));
    uint groupid = get_group_id(0)+get_group_id(1)*get_num_groups(0) + get_group_id(2)*get_num_groups(0)*get_num_groups(1);
    uint elements_per_group = 2*(get_local_size(0) + get_local_size(1) - 2);
    //blockroutes[0] = info;
    blockroutes[ get_global_size(2) + groupid * elements_per_group + localoffset] =  info;
  }

  if( start.x == get_global_id(0) && start.y == get_global_id(1) )
  {
    //this is the starting point, we also need to save this info
    blockroutes[ get_global_id(2) ] =  info;
  }

}

uint blockInOutCost(uint data)
{
  return data >> 16;
}
int2 blockInOutOrigin(uint data)
{
  int x = data & 0xFF;
  int y = (data >> 8) & 0xFF;
  return (int2)(x-1,y-1);
}
uint pack(int2 d)
{
  return (d.x << 16) | d.y;
}
int2 unpack(uint d)
{
  return (int2)(d >> 16, d & 0xFFFF);
}

__kernel void calcInterBlockRoute(global const uint* blockroutes,
                                  global uint2* interblockroutes,
                                  const int2 start,
                                  const int2 blocks,
                                  const int2 blocksize)
{
  uint target = get_global_id(0);
  uint startinfo = blockroutes[target];
  uint elements_per_group = 2*(blocksize.x + blocksize.y - 2);
  uint inoffset = get_global_size(0) + target*blocks.x*blocks.y*elements_per_group;
  uint outoffset = blocks.x*blocks.y*target + get_global_size(0);
  uint blockcount = 0;

  uint newcost = blockInOutCost(startinfo);
  int2 localorigin = blockInOutOrigin(startinfo);
  interblockroutes[outoffset++] = (uint2)(0,pack(start));

  int2 block = (int2)(start.x/blocksize.x, start.y/blocksize.y);
  uint sumcost = 0;
  int2 blockdiff = (int2)(0,0);

  do
  {
    sumcost += newcost;
    int2 currentpos = block*blocksize + localorigin;
    blockdiff = (int2)(localorigin.x < 0 ? -1 : (localorigin.x >= blocksize.x ? 1 : 0),
                       localorigin.y < 0 ? -1 : (localorigin.y >= blocksize.y ? 1 : 0));
    block.x += blockdiff.x;
    block.y += blockdiff.y;
    int2 localpos = currentpos - block*blocksize;

    int localoffset = borderId(localpos, blocksize);
    interblockroutes[outoffset++] = (uint2)(sumcost,pack(currentpos));

    uint currentinfo = blockroutes[inoffset + (block.y*blocks.x + block.x)*elements_per_group + localoffset];
    newcost = blockInOutCost(currentinfo);
    localorigin = blockInOutOrigin(currentinfo);
    ++blockcount;

  } while(blockdiff.x != 0 || blockdiff.y != 0);

  interblockroutes[target].x = blockcount;
  interblockroutes[target].y = sumcost+1;
}

__kernel void constructRoute(global const float* routecost,
                             global const uint2* interblockroutes,
                             global int2* route,
                             const int maxblocks_pertarget,
                             const int maxpoints_pertarget,
                             const int numtargets,
                             const int2 dim,
                             const int2 blocks)
{
  uint target = get_global_id(0)/maxblocks_pertarget;
  uint element = get_global_id(0)%maxblocks_pertarget;
  if(element >= interblockroutes[target].x)
    return;

  int offset = maxpoints_pertarget*target + interblockroutes[blocks.x*blocks.y*target + numtargets + element].x;
  int endoffset = maxpoints_pertarget*target + interblockroutes[blocks.x*blocks.y*target + numtargets + element + 1].x;
  int2 pos = unpack( interblockroutes[blocks.x*blocks.y*target + numtargets + element].y );

  route[offset++] = pos;
  //follow the route
  while(offset <= endoffset)
  {
    float mincost = MAXFLOAT;
    int2 next = pos;
    for(int y = -1; y <= 1; ++y)
      for(int x = -1; x <= 1; ++x)
      {
        int2 coord = (int2)(pos.x+x, pos.y+y);
        if(coord.x >= 0 && coord.x < dim.x &&
           coord.y >= 0 && coord.y < dim.y)
        {
          float tcost =  routecost[coord.x + dim.x*coord.y + dim.x*dim.y*target];
          if(mincost > tcost)
          {
            mincost = tcost;
            next = coord;
          }
        }
      }
    pos = next;
    route[offset++] = pos;
  }
}

__kernel void route_no_queue(read_only image2d_t costmap,
                      global float* routecost,
                      const int2 dim,
                      local float* l_costs)
{
  int3 blockid = {get_group_id(0), get_group_id(1), get_global_id(2)};
  route(costmap, routecost, blockid, l_costs, dim);
  return;
}

#if 0
  int3 id = {get_global_id(0), get_global_id(1), get_global_id(2)};

  //copy cost to local
  int local_linid = get_local_id(1)*get_local_size(0) + get_local_id(0);
  for(int i = local_linid; i < (get_local_size(0)+2)*(get_local_size(1)+2); i += get_local_size(0)*get_local_size(1))
  {
    int2 global_pos = (int2)(get_group_id(0)*get_local_size(0),get_group_id(1)*get_local_size(1));
    global_pos += (int2)(i%(get_local_size(0)+2),i/(get_local_size(0)+2)) - (int2)(1,1);

    float incost = 0.01f*MAXFLOAT;
    if(global_pos.x > 0 && global_pos.x < dim.x &&
       global_pos.y > 0 && global_pos.y < dim.y)
      incost = routecost[dim.x*global_pos.y + global_pos.x + dim.x*dim.y*id.z];
    l_costs[i] = incost;
  }

  local bool change;
  if(local_linid == 0)
    change = true;
  barrier(CLK_LOCAL_MEM_FENCE);

  int2 localid = (int2)(get_local_id(0),get_local_id(1));
  int2 globalid = (int2)(get_global_id(0),get_global_id(1));

  //iterate over it
  float myPenalty = getPenalty(costmap, globalid);
  float local * myval = accessLocalCost(l_costs, localid);
  while(change)
  {
    if(local_linid == 0)
      change = false;
    barrier(CLK_LOCAL_MEM_FENCE);

    float lastmyval = *myval;

    int2 offset;
    for(offset.y = -1; offset.y  <= 1; ++offset.y)
      for(offset.x = -1; offset.x <= 1; ++offset.x)
      {

        float d = native_sqrt((float)(offset.x*offset.x+offset.y*offset.y));
        float penalty = 0.5f*(myPenalty + getPenalty(costmap, globalid + offset));
        float c_other = *accessLocalCost(l_costs, localid + offset);
        *myval = min(*myval, c_other + 0.01f*d + d*penalty);
      }

    if(*myval != lastmyval)
      change = true;

    barrier(CLK_LOCAL_MEM_FENCE);
  }

  //write back
  routecost[dim.x*globalid.y + globalid.x + dim.x*dim.y*id.z] = *myval;
}
#endif

#if 0

/*
 The values stored in the resulting map contain the cost the reach each node and
 which is its predecessor on the path there. It contains 32 Bits for each node
 where the first two bits represent the direction of the predecessor and the
 remaining bits form the actual cost of reaching the node.
 */

constant uint MASK_VALUE        = 0x3fffffff;
constant uint MASK_DIRECTION    = 0xc0000000;
constant uint VALUE_NOT_VISITED = 0xffffffff;

//------------------------------------------------------------------------------
// Heap
//------------------------------------------------------------------------------
typedef struct
{
  global uint *values; //!< The values to be sorted
  global uint *heap;   //!< Sorted indices (by values)
  uint size;           //!< Current heap size
  int2 dim;            //!< Dimensions of values
} data_t;

#define _value(index) (d->values[ d->heap[index] ] & MASK_VALUE)
#define _parent(index) (index / 2)
#define _child_left(index) (2 * index + 1)
#define _child_right(index) (2 * index + 2)

/**
 * @param index The index of the node to insert
 */
void heap_insert( data_t *d,
                  const uint index )
{
  uint cur = d->size++;
  uint val = _value(cur);
  uint parent = _parent(cur);

  // move up
  while( cur && val < _value(parent) )
  {
    d->heap[ cur ] = d->heap[ parent ];

    cur = parent;
    parent = _parent(cur);
  }

  d->heap[ cur ] = index;
}

/**
 *
 * @return Index of the minimum node
 */
uint heap_removeMin( data_t *d )
{
  // Remove first element
  uint cur = 0,
       front = d->heap[ cur ],
       child = _child_left(cur);

  // Propagate hole down the tree
  while( child < d->size )
  {
    // Change to right child if it exists and is smaller than the left one
    if( child + 1 < d->size && _value(child + 1) < _value(child) )
      child += 1;

    d->heap[ cur ] = d->heap[ child ];
    cur = child;
    child = _child_left(cur);
  }

  // Put last element into hole...
  uint index = d->heap[ --d->size ];
  uint val = _value(index);
  uint parent = _parent(cur);

  // ...and let it bubble up
  while( cur && val < _value(parent) )
  {
    d->heap[ cur ] = d->heap[ parent ];

    cur = parent;
    parent = _parent(cur);
  }

  d->heap[ cur ] = index;
  return front;
}

//------------------------------------------------------------------------------
// Dijkstra
//------------------------------------------------------------------------------
float getPenalty(read_only image2d_t costmap, int x, int y)
{
  const sampler_t sampler = CLK_FILTER_NEAREST
                          | CLK_NORMALIZED_COORDS_FALSE
                          | CLK_ADDRESS_CLAMP_TO_EDGE;

  return read_imagef(costmap, sampler, (int2)(x, y)).x;
}

//------------------------------------------------------------------------------
// Kernel
//------------------------------------------------------------------------------

#define makeIndex(pos, dim) (pos.y * dim.x + pos.x)

__kernel void test_kernel( read_only image2d_t costmap,
                           global uint* nodes,
                           global uint* queue,
                           const int2 dim,
                           const int2 start,
                           const int2 target )
{
  data_t data = {
    .values = nodes,
    .heap = queue,
    .size = 0,
    .dim = dim
  };

  const float link_width = 4,
              alpha_L = 0.01,
              alpha_P = 2.99,
              // simplify and scale the cost calculation a bit...
              factor = 100 * (0.5/alpha_L * alpha_P * link_width + 1);

  // Initialize all to "Not visited" and startnode to zero
  for(uint i = 0; i < dim.x * dim.y; ++i)
    nodes[i] = VALUE_NOT_VISITED;

  uint start_index = makeIndex(start, dim);
  nodes[ start_index ] = 0;
  heap_insert(&data, start_index);
  int count = 10;
  uint max_size = 0;
  // Start with Dijkstra
  do
  {
    max_size = max(data.size, max_size);
    uint index = heap_removeMin(&data);
    int px = index % dim.x, // DON'T use int2 -> don't know why but % doesn't work...
        py = index / dim.x;

    // Update all 8 neighbours
    for( int x = max(px - 1, 0); x <= min(px + 1, dim.x); ++x )
      for( int y = max(py - 1, 0); y <= min(py + 1, dim.y); ++y )
      {
        if( px == x && py == y )
          continue;

        int index_n = makeIndex((int2)(x, y), dim);

        if( nodes[index_n] == VALUE_NOT_VISITED )
          heap_insert(&data, index_n);

        float len = (x == px || y == py) ? 1 : 1.4f;
        float cost = factor
                   * len
                   * (getPenalty(costmap, x, y) + getPenalty(costmap, px, py));

        int total_cost = nodes[index] + (int)cost;

        // update/relax
        if( total_cost < nodes[index_n] )
        {
          nodes[index_n] = total_cost;
        }
      }
  } while( data.size && count-- );

  nodes[ 0 ] = max_size;
}

#endif

#define KEY_TYPE float
#define VALUE_TYPE uint
#include "sorting.cl"

//QUEUE
typedef int4 QueueElement;

#define QueueElementCopySteps 3

#define  QueueElementEmpty 0
#define  QueueElementReady 1
#define  QueueElementReading 2
#define  QueueElementRead 0
void checkAndSetQueueState(volatile global QueueElement* element, uint state, uint expected)
{
  volatile global int* pstate = ((volatile global int*)element) + 3;
  while(atomic_cmpxchg(pstate, expected, state) != expected);
}
void setQueueState(volatile global QueueElement* element, uint state)
{
  volatile global int* pstate = ((volatile global int*)element) + 3;
  uint old = atomic_xchg(pstate, state);
  //check: old + 1 % (QueueElementReading+1) == state
}
uint getQueueState(volatile global QueueElement* element)
{
  return element->w;
}

int3 getBlockId(volatile global QueueElement* element)
{
  return (*element).xyz;
}

void setBlockId(volatile global QueueElement* element, int3 block)
{
  (*element).xyz = block;
}

struct _QueueGlobal
{
  uint front;
  uint back;
  int filllevel;
  uint activeBlocks;
  uint processedBlocks;
  uint sortingBarrier;
  int debug;
  uint mincost;
};
typedef struct _QueueGlobal QueueGlobal;

#define linAccess3D(id, dim) \
  ((id).x + ( (id).y + (id).z  * (dim.y) )* (dim).x)

float atomic_min_float(volatile global float* p, float val)
{
  return as_float(atomic_min((volatile global uint*)(p),as_uint(val)));
}

void Enqueue(volatile global QueueGlobal* queueInfo,
             volatile global QueueElement* queue,
             volatile global float* queuePriority,
             int3 block,
             float priority,
             int2 blocks,
             uint queueSize)
{
  //check if element is already in tehe Queue:

  volatile global float* myQueuePriority = queuePriority + linAccess3D(block, blocks);

  //update priority:
  float old_priority = atomic_min_float(myQueuePriority, priority);

  //if there is a priority, it is in the queue
  if(old_priority != MAXFLOAT)
    return;
  else
  {
    //add to queue

    //inc size
    uint filllevel = atomic_inc(&queueInfo->filllevel);

    //make sure there is enough space?
    //assert(filllevel < queueSize)

    //insert a new element
    uint pos = atomic_inc(&queueInfo->back)%queueSize;

    //make sure it is free?
    //queue[pos] == QueueElementEmpty

    //insert the data
    setBlockId(queue + pos, block);
    write_mem_fence(CLK_GLOBAL_MEM_FENCE);

    //increase the number of active Blocks
    atomic_inc(&queueInfo->activeBlocks);

    //activate the queue element
    setQueueState(queue + pos,  QueueElementReady);

  }
}

bool Dequeue(volatile global QueueGlobal* queueInfo,
             volatile global QueueElement* queue,
             volatile global float* queuePriority,
             int3* element,
             int2 blocks,
             uint queueSize)
{
  //is there something to get?
  if(atomic_dec(&queueInfo->filllevel) <= 0)
  {
    atomic_inc(&queueInfo->filllevel);
    return false;
  }

  //pop the front of the queue
  uint pos = atomic_inc(&queueInfo->front)%queueSize;

  //check for barrier
  uint barrierPos = queueInfo->sortingBarrier;
  uint back = queueInfo->back%queueSize;
  while(barrierPos <= pos && barrierPos > back)
  {
    barrierPos = queueInfo->sortingBarrier;
    queueInfo->debug += 100000;
  }

  //wait for the element to be written/change to reading
  checkAndSetQueueState(queue + pos, QueueElementReading, QueueElementReady);

  //read
  *element = getBlockId(queue + pos);

  //remove priority (mark as not in queue)
  volatile global float* myQueuePriority = queuePriority + linAccess3D(*element, blocks);
  atomic_xchg(myQueuePriority, MAXFLOAT);

  //free queue element
  setQueueState(queue + pos, QueueElementRead);
  return true;
}
//

float getPenalty(read_only image2d_t costmap, int2 pos)
{
  const sampler_t sampler = CLK_FILTER_NEAREST
                          | CLK_NORMALIZED_COORDS_FALSE
                          | CLK_ADDRESS_CLAMP_TO_EDGE;

  return read_imagef(costmap, sampler, pos).x;
}

__kernel void clearQueueLink(global float* queuePriority)
{
  queuePriority[get_global_id(0)] = MAXFLOAT;
}

//it might be better to do that via opengl or something (arbitrary node shapes etc)
__kernel void prepareRouting(read_only image2d_t costmap,
                           global float* routecost,
                           global QueueElement* queue,
                           global float* queuePriority,
                           global QueueGlobal* queueInfo,
                           uint queueSize,
                           global uint4* startingPoints,
                           const uint numStartingPoints,
                           const int2 dim,
                           const int2 numBlocks)
{
  __local bool hasStartingPoint;

  int3 id = {get_global_id(0), get_global_id(1), get_global_id(2)};

  if(get_local_id(0) == 0 && get_local_id(1) == 0 && get_local_id(2) == 0)
    hasStartingPoint = false;
  barrier(CLK_LOCAL_MEM_FENCE);

  if(get_local_id(0) < dim.x && get_local_id(1) < dim.y)
  {
    float cost = 0.01f*MAXFLOAT;
    if(startingPoints[id.z].x <= id.x && id.x <= startingPoints[id.z].z &&
        startingPoints[id.z].y <= id.y && id.y <= startingPoints[id.z].w)
    {
        cost = 0;
        hasStartingPoint = true;
    }
    //prepare data
    routecost[dim.x*id.y+id.x + dim.x*dim.y*id.z] = cost;
    barrier(CLK_LOCAL_MEM_FENCE);
    if(hasStartingPoint && get_local_id(0) == 0 && get_local_id(1) == 0)
    {
      //insert into queue
      int3 block = (int3)(get_group_id(0), get_group_id(1), id.z);
      Enqueue(queueInfo, queue, queuePriority, block, 0, numBlocks, queueSize);
    }
  }
}

local float* accessLocalCost(local float* l_costs, int2 id)
{
  return l_costs + (id.x+1 + (id.y+1)*(get_local_size(0)+2));
}

void copyInLocalData(global float* routecost,
           int3 blockid,
           local float* l_costs,
           int2 dim)
{
  blockid.x *= get_local_size(0);
  blockid.y *= get_local_size(1);

  //copy cost to local
  int local_linid = get_local_id(1)*get_local_size(0) + get_local_id(0);
  for(int i = local_linid; i < (get_local_size(0)+2)*(get_local_size(1)+2); i += get_local_size(0)*get_local_size(1))
  {
    int2 global_pos = (int2)(blockid.x, blockid.y) + (int2)(i%(get_local_size(0)+2),i/(get_local_size(0)+2)) - (int2)(1,1);

    float incost = 0.01f*MAXFLOAT;
    if(global_pos.x > 0 && global_pos.x < dim.x &&
       global_pos.y > 0 && global_pos.y < dim.y)
      incost = routecost[dim.x*global_pos.y + global_pos.x + dim.x*dim.y*blockid.z];
    l_costs[i] = incost;
  }
}

void copyOutLocalData(global float* routecost,
           int3 blockid,
           local float* l_costs,
           int2 dim)
{
  //copy cost to global
  int2 global_pos = (int2)(get_local_size(0)*blockid.x + get_local_id(0), get_local_size(1)*blockid.y + get_local_id(1));
  if(global_pos.x < dim.x && global_pos.y < dim.y)
    routecost[dim.x*global_pos.y + global_pos.x + dim.x*dim.y*blockid.z] = *accessLocalCost(l_costs, (int2)(get_local_id(0), get_local_id(1)));
}

bool route(read_only image2d_t costmap,
           global float* routecost,
           int3 blockid,
           local float* l_costs,
           int2 dim)
{
  copyInLocalData(routecost, blockid, l_costs, dim);

  //route as long as there is change
  local bool change;
  change = true;
  barrier(CLK_LOCAL_MEM_FENCE);

  int2 localid = (int2)(get_local_id(0),get_local_id(1));
  int2 globalid = (int2)(blockid.x*get_local_size(0)+get_local_id(0),blockid.y*get_local_size(1)+get_local_id(1));

  //iterate over it
  float myPenalty = getPenalty(costmap, globalid);
  float local * myval = accessLocalCost(l_costs, localid);
  int counter = -1;
  while(change)
  {
    change = false;
    barrier(CLK_LOCAL_MEM_FENCE);

    float lastmyval = *myval;

    int2 offset;
    for(offset.y = -1; offset.y  <= 1; ++offset.y)
      for(offset.x = -1; offset.x <= 1; ++offset.x)
      {
        //TODO d is either 1 or M_SQRT2 - maybe it is faster to substitute
        float d = native_sqrt((float)(offset.x*offset.x+offset.y*offset.y));
        float penalty = 0.5f*(myPenalty + getPenalty(costmap, globalid + offset));
        float c_other = *accessLocalCost(l_costs, localid + offset);
        //TODO use custom params here
        *myval = min(*myval, c_other + 0.01f*d + d*penalty);
      }

    if(*myval != lastmyval)
      change = true;

    barrier(CLK_LOCAL_MEM_FENCE);
    ++counter;
  }

  copyOutLocalData(routecost, blockid, l_costs, dim);
  return counter > 0;
}

void minBorderCosts(local float* l_costs,
                    local float borderCosts[4])
{
  if(get_local_id(0) < 4)
    borderCosts[get_local_id(0)] = MAXFLOAT;
  barrier(CLK_LOCAL_MEM_FENCE);
  float myCost = *accessLocalCost( l_costs, (int2)(get_local_id(0), get_local_id(1)) );
  int target = -1;
  if(get_local_id(0) == 0)
    atomic_min((local uint*)(&borderCosts[0]), as_uint(myCost));
  else if(get_local_id(0) == get_local_size(0) - 1)
    atomic_min((local uint*)(&borderCosts[2]), as_uint(myCost));
  if(get_local_id(1) == 0)
    atomic_min((local uint*)(&borderCosts[1]), as_uint(myCost));
  else if(get_local_id(1) == get_local_size(1) - 1)
    atomic_min((local uint*)(&borderCosts[3]), as_uint(myCost));
  barrier(CLK_LOCAL_MEM_FENCE);
}

void routing_worker(read_only image2d_t costmap,
                      global float* routecost,
                      volatile global QueueElement* queue,
                      volatile global float* queuePriority,
                      volatile global QueueGlobal* queueInfo,
                      uint queueSize,
                      const int2 dim,
                      const int2 numBlocks,
                      const uint processedBlocksThreshold,
                      local float* l_costs)
{
  while(queueInfo->activeBlocks > 0 && queueInfo->processedBlocks < processedBlocksThreshold)
  {
    local int3 blockId;
    if(get_local_id(0) == 0 && get_local_id(1) == 0)
    {
      int3 element;
      if(Dequeue(queueInfo, queue, queuePriority, &element, numBlocks, queueSize))
        blockId = element;
      else
        blockId.x = -1;
    }
    barrier(CLK_LOCAL_MEM_FENCE);

    if(blockId.x > -1)
    {
      //process it
      bool change = route(costmap, routecost, blockId, l_costs, dim);

      //add surrounding ones
      if(change)
      {
        local float borderCosts[4];
        minBorderCosts(l_costs, borderCosts);
        if(get_local_id(0) == 0 && get_local_id(1) == 0)
        {
          int3 block = blockId;
          if(blockId.x - 1 >= 0)
          {
            block.x = blockId.x - 1;
            Enqueue(queueInfo, queue, queuePriority, block, borderCosts[0], numBlocks, queueSize);
            block.x = blockId.x;
          }
          if(blockId.x + 1 < numBlocks.x)
          {
            block.x = blockId.x + 1;
            Enqueue(queueInfo, queue, queuePriority, block, borderCosts[2], numBlocks, queueSize);
            block.x = blockId.x;
          }
          if(blockId.y - 1 >= 0)
          {
            block.y = blockId.y - 1;
            Enqueue(queueInfo, queue, queuePriority, block, borderCosts[1], numBlocks, queueSize);
          }
          if(blockId.y + 1 < numBlocks.y)
          {
            block.y = blockId.y + 1;
            Enqueue(queueInfo, queue, queuePriority, block, borderCosts[3], numBlocks, queueSize);
          }
        }
      }
      //else if(get_local_id(0) == 0 && get_local_id(1) == 0)
      //  atomic_inc(&queueInfo->debug);

      //update counter
      if(get_local_id(0) == 0 && get_local_id(1) == 0)
      {
        atomic_dec(&queueInfo->activeBlocks);
        atomic_inc(&queueInfo->processedBlocks);
      }

    }
    barrier(CLK_LOCAL_MEM_FENCE);
  }
}

uint wrappedAroundQueueId(uint id, uint queueSize)
{
  return min(id, id + queueSize);
}
void sortQueue(volatile global QueueElement* queue,
               volatile global QueueGlobal* queueInfo,
               volatile global float* queuePriorities,
               uint queueSize,
               const int2 numBlocks,
               const uint processedBlocksThreshold,
               local float* l_priorites,
               local uint* l_ids)
{
  local bool ok;
  uint nextsort = queueSize + 1;
  uint linid = get_local_id(1) * get_local_size(0) + get_local_id(0);
  uint threads = get_local_size(0)*get_local_size(1);
  uint sortMargin =  4*threads;

  //prepare local ids
  l_ids[linid] = linid;
  l_ids[threads + linid] = threads + linid;

  while(queueInfo->activeBlocks > 0 && queueInfo->processedBlocks < processedBlocksThreshold)
  {
    ok = true;
    barrier(CLK_LOCAL_MEM_FENCE);

    //restart at back?
    if(nextsort > queueSize)
      nextsort = queueInfo->back % queueSize; //the back might just be adding

    //is the distance bigger than the margin? (ring buffer)
    uint margin = wrappedAroundQueueId(nextsort - (queueInfo->front%queueSize), queueSize);
    if(margin < sortMargin)
    {
      //margin is too small, dont even bother
      nextsort = queueSize + 1;
      continue;
    }

    //copy the data in
    uint in_out_id0 = wrappedAroundQueueId(nextsort - 2*threads + linid, queueSize);
    //make sure it is ready
    int err_counter = 100;
    while(getQueueState(queue + in_out_id0) != QueueElementReady && --err_counter);
    if(!err_counter) ok = false;
    int3 blockId0 = getBlockId(queue + in_out_id0);
    l_priorites[linid] = queuePriorities[linAccess3D(blockId0, numBlocks)];

    uint in_out_id1 = wrappedAroundQueueId(nextsort - threads + linid, queueSize);
    //make sure it is ready
    err_counter = 100;
    while(getQueueState(queue + in_out_id1) != QueueElementReady && --err_counter);
    if(!err_counter) ok = false;
    int3 blockId1 = getBlockId(queue + in_out_id1);
    l_priorites[threads +  linid] = queuePriorities[linAccess3D(blockId1, numBlocks)];

    barrier(CLK_LOCAL_MEM_FENCE);

    if(!ok) //loading did not work
      continue;

    //sort
    bitonicSort(linid,l_priorites,l_ids,2*get_local_size(0),true);

    uint target0 = l_ids[linid];
    uint target1 = l_ids[threads + linid];

    //copy blockids around
    #define copyBlockIds(COMPONENT) \
      barrier(CLK_LOCAL_MEM_FENCE); \
      l_ids[target0] = blockId0.COMPONENT; \
      l_ids[target1] = blockId1.COMPONENT; \
      barrier(CLK_LOCAL_MEM_FENCE); \
      blockId0.COMPONENT = l_ids[linid]; \
      blockId1.COMPONENT = l_ids[threads + linid];

    copyBlockIds(x);
    copyBlockIds(y);
    copyBlockIds(z);

    #undef copyBlockIds

    //check if we can write it back
    margin = wrappedAroundQueueId(nextsort - (queueInfo->front%queueSize), queueSize);
    if(margin > 3*threads)
    {
      //set the barrier
      queueInfo->sortingBarrier =  wrappedAroundQueueId(nextsort - 2*threads, queueSize);
      write_mem_fence(CLK_GLOBAL_MEM_FENCE);

      //check if it is still ok
      margin = wrappedAroundQueueId(nextsort - (queueInfo->front%queueSize), queueSize);
      if(margin > 2*threads)
      {
        //write back
        setBlockId(queue + in_out_id0, blockId0);
        setBlockId(queue + in_out_id1, blockId1);
      }
      //remove barrier
      queueInfo->sortingBarrier = queueSize + 1;
      write_mem_fence(CLK_GLOBAL_MEM_FENCE);
      queueInfo->debug++;
    }
    else
    {
      //increase sort margin
      sortMargin += threads;
      //restart sort at the beginning
      nextsort = queueSize + 1;
      queueInfo->debug+= 1000;
    }

    //reset local ids
    l_ids[linid] = linid;
    l_ids[threads + linid] = threads + linid;
  }
}

__kernel void routing(read_only image2d_t costmap,
                      global float* routecost,
                      volatile global QueueElement* queue,
                      volatile global float* queuePriority,
                      volatile global QueueGlobal* queueInfo,
                      uint queueSize,
                      const int2 dim,
                      const int2 numBlocks,
                      const uint processedBlocksThreshold,
                      local float* l_data)
{
  //if(get_group_id(0) + get_group_id(1) + get_group_id(2) == 0)
  ////sorter
  //  sortQueue(queue,
  //            queueInfo,
  //            queuePriority,
  //            queueSize,
  //            numBlocks,
  //            processedBlocksThreshold,
  //            l_data,
  //            (local uint*)( l_data+2*get_local_size(0)*get_local_size(1) ) );
  //else
    //worker
     routing_worker(costmap,
                    routecost,
                    queue,
                    queuePriority,
                    queueInfo,
                    queueSize,
                    dim,
                    numBlocks,
                    processedBlocksThreshold,
                    l_data);
}

__kernel void getMinimum(global const float* routecost,
                         volatile global QueueGlobal* queueInfo,
                         volatile global ushort2* coordinates,
                         const int2 dim,
                         int targets)
{
  float cost = 0;
  for(int i = 0; i < targets; ++i)
    cost += routecost[dim.x*get_global_id(1) + get_global_id(0) + dim.x*dim.y*i];

  __local uint localmin;
  __local uint2 coords;
  localmin = as_uint(MAXFLOAT);

  barrier(CLK_LOCAL_MEM_FENCE);

  atomic_min(&localmin, as_uint(cost));

  barrier(CLK_LOCAL_MEM_FENCE);

  if(localmin == as_uint(cost))
    coords = (uint2)(get_global_id(0), get_global_id(1));

  barrier(CLK_LOCAL_MEM_FENCE);

  if( (get_local_id(0) + get_local_id(1)) == 0 )
  {
    uint minv = as_uint(localmin);
    //compress it
    minv = (minv << 1) & 0xFFFFFF00;
    //combine with block id
    uint groupv = (get_group_id(0) + get_group_id(1)*get_num_groups(0)) & 0xFF;
    uint combined = minv | groupv;
    uint old = atomic_min(&queueInfo->mincost, combined);
    if(old > combined)
    {
      //write coords to array entry
      coordinates[groupv] = (ushort2)(coords.x,coords.y);
    }
  }
}

uint borderId(int2 lid, int2 lsize)
{
  uint id = lid.x;
  if(lid.x==lsize.x-1)
    id += lid.y;
  else if(lid.y==lsize.y-1)
    id = lsize.x*2 + lsize.y - 3 - lid.x;
  else if(lid.x == 0 && lid.y > 0)
    id = 2*(lsize.x + lsize.y - 2) - lid.y;
  return id;
}

__kernel void calcInOut(global const float* routecost,
                        global uint* blockroutes,
                        local int* l_cost,
                        local int* l_origin,
                        const int2 start,
                        const int2 dim)
{
  int3 coord = (int3)(get_global_id(0), get_global_id(1), get_global_id(2));
  float mincost = MAXFLOAT;
  if(coord.x >= 0 && coord.x < dim.x &&
     coord.y >= 0 && coord.y < dim.y)
    mincost =  routecost[coord.x + dim.x*coord.y + dim.x*dim.y*coord.z];

  int2 dir = (int2)(0,0);
  for(int y = -1; y <= 1; ++y)
    for(int x = -1; x <= 1; ++x)
    {
      coord = (int3)(get_global_id(0)+x, get_global_id(1)+y, get_global_id(2));
      if(coord.x >= 0 && coord.x < dim.x &&
         coord.y >= 0 && coord.y < dim.y)
      {
        float tcost =  routecost[coord.x + dim.x*coord.y + dim.x*dim.y*coord.z];
        if(tcost < mincost)
        {
          mincost = tcost;
          dir = (int2)(x,y);
        }
      }
    }
  //store dir and cost
  int2 lid = (int2)(get_local_id(0), get_local_id(1));
  int2 fromid = lid + dir;

  bool outside = fromid.x < 0 ||
                fromid.y < 0 ||
                fromid.x >= get_local_size(0) ||
                fromid.y >= get_local_size(1);

  int cost = (dir.x == 0 && dir.y == 0) ? 0 :
             (outside ? 1 : (2*get_local_size(0)*get_local_size(1)));

  int origin = (dir.x == 0 && dir.y == 0) ? ( ( (1 + lid.y) << 8 ) | (lid.x + 1)) :
               (outside ? ( ( (1 + fromid.y) << 8 ) | (1 + fromid.x ) ) : -1);

  int loffset = lid.x + lid.y*(int)get_local_size(0);
  int lfrom = fromid.x + fromid.y*(int)get_local_size(0);
  l_cost[loffset] = cost;
  l_origin[loffset] = origin;

  __local bool changed;
  uint count = 0;
  do
  {
    changed = false;
    barrier(CLK_LOCAL_MEM_FENCE);
    if(origin == -1)
    {
      if(l_cost[loffset] != l_cost[lfrom]+1)
      {
        l_cost[loffset] = l_cost[lfrom]+1;
        l_origin[loffset] = l_origin[lfrom];
        changed = true;
      }
    }
    barrier(CLK_LOCAL_MEM_FENCE);
  }
  while(changed);

  bool border =  get_local_id(0) == 0 ||
                 get_local_id(1) == 0 ||
                 get_local_id(0) == get_local_size(0)-1 ||
                 get_local_id(1) == get_local_size(1)-1;

  uint info = (l_cost[loffset] << 16) | (l_origin[loffset] &0xFFFF);

  if(border)
  {
    //store the info
    uint localoffset = borderId((int2)(get_local_id(0),get_local_id(1)), (int2)(get_local_size(0),get_local_size(1)));
    uint groupid = get_group_id(0)+get_group_id(1)*get_num_groups(0) + get_group_id(2)*get_num_groups(0)*get_num_groups(1);
    uint elements_per_group = 2*(get_local_size(0) + get_local_size(1) - 2);
    //blockroutes[0] = info;
    blockroutes[ get_global_size(2) + groupid * elements_per_group + localoffset] =  info;
  }

  if( start.x == get_global_id(0) && start.y == get_global_id(1) )
  {
    //this is the starting point, we also need to save this info
    blockroutes[ get_global_id(2) ] =  info;
  }

}

uint blockInOutCost(uint data)
{
  return data >> 16;
}
int2 blockInOutOrigin(uint data)
{
  int x = data & 0xFF;
  int y = (data >> 8) & 0xFF;
  return (int2)(x-1,y-1);
}
uint pack(int2 d)
{
  return (d.x << 16) | d.y;
}
int2 unpack(uint d)
{
  return (int2)(d >> 16, d & 0xFFFF);
}

__kernel void calcInterBlockRoute(global const uint* blockroutes,
                                  global uint2* interblockroutes,
                                  const int2 start,
                                  const int2 blocks,
                                  const int2 blocksize)
{
  uint target = get_global_id(0);
  uint startinfo = blockroutes[target];
  uint elements_per_group = 2*(blocksize.x + blocksize.y - 2);
  uint inoffset = get_global_size(0) + target*blocks.x*blocks.y*elements_per_group;
  uint outoffset = blocks.x*blocks.y*target + get_global_size(0);
  uint blockcount = 0;

  uint newcost = blockInOutCost(startinfo);
  int2 localorigin = blockInOutOrigin(startinfo);
  interblockroutes[outoffset++] = (uint2)(0,pack(start));

  int2 block = (int2)(start.x/blocksize.x, start.y/blocksize.y);
  uint sumcost = 0;
  int2 blockdiff = (int2)(0,0);

  do
  {
    sumcost += newcost;
    int2 currentpos = block*blocksize + localorigin;
    blockdiff = (int2)(localorigin.x < 0 ? -1 : (localorigin.x >= blocksize.x ? 1 : 0),
                       localorigin.y < 0 ? -1 : (localorigin.y >= blocksize.y ? 1 : 0));
    block.x += blockdiff.x;
    block.y += blockdiff.y;
    int2 localpos = currentpos - block*blocksize;

    int localoffset = borderId(localpos, blocksize);
    interblockroutes[outoffset++] = (uint2)(sumcost,pack(currentpos));

    uint currentinfo = blockroutes[inoffset + (block.y*blocks.x + block.x)*elements_per_group + localoffset];
    newcost = blockInOutCost(currentinfo);
    localorigin = blockInOutOrigin(currentinfo);
    ++blockcount;

  } while(blockdiff.x != 0 || blockdiff.y != 0);

  interblockroutes[target].x = blockcount;
  interblockroutes[target].y = sumcost+1;
}

__kernel void constructRoute(global const float* routecost,
                             global const uint2* interblockroutes,
                             global int2* route,
                             const int maxblocks_pertarget,
                             const int maxpoints_pertarget,
                             const int numtargets,
                             const int2 dim,
                             const int2 blocks)
{
  uint target = get_global_id(0)/maxblocks_pertarget;
  uint element = get_global_id(0)%maxblocks_pertarget;
  if(element >= interblockroutes[target].x)
    return;

  int offset = maxpoints_pertarget*target + interblockroutes[blocks.x*blocks.y*target + numtargets + element].x;
  int endoffset = maxpoints_pertarget*target + interblockroutes[blocks.x*blocks.y*target + numtargets + element + 1].x;
  int2 pos = unpack( interblockroutes[blocks.x*blocks.y*target + numtargets + element].y );

  route[offset++] = pos;
  //follow the route
  while(offset <= endoffset)
  {
    float mincost = MAXFLOAT;
    int2 next = pos;
    for(int y = -1; y <= 1; ++y)
      for(int x = -1; x <= 1; ++x)
      {
        int2 coord = (int2)(pos.x+x, pos.y+y);
        if(coord.x >= 0 && coord.x < dim.x &&
           coord.y >= 0 && coord.y < dim.y)
        {
          float tcost =  routecost[coord.x + dim.x*coord.y + dim.x*dim.y*target];
          if(mincost > tcost)
          {
            mincost = tcost;
            next = coord;
          }
        }
      }
    pos = next;
    route[offset++] = pos;
  }
}

#if 0
  int3 blockid = {get_group_id(0), get_group_id(1), get_global_id(2)};
  route(costmap, routecost, blockid, l_costs, dim);
  return;

  int3 id = {get_global_id(0), get_global_id(1), get_global_id(2)};

  //copy cost to local
  int local_linid = get_local_id(1)*get_local_size(0) + get_local_id(0);
  for(int i = local_linid; i < (get_local_size(0)+2)*(get_local_size(1)+2); i += get_local_size(0)*get_local_size(1))
  {
    int2 global_pos = (int2)(get_group_id(0)*get_local_size(0),get_group_id(1)*get_local_size(1));
    global_pos += (int2)(i%(get_local_size(0)+2),i/(get_local_size(0)+2)) - (int2)(1,1);

    float incost = 0.01f*MAXFLOAT;
    if(global_pos.x > 0 && global_pos.x < dim.x &&
       global_pos.y > 0 && global_pos.y < dim.y)
      incost = routecost[dim.x*global_pos.y + global_pos.x + dim.x*dim.y*id.z];
    l_costs[i] = incost;
  }

  local bool change;
  if(local_linid == 0)
    change = true;
  barrier(CLK_LOCAL_MEM_FENCE);

  int2 localid = (int2)(get_local_id(0),get_local_id(1));
  int2 globalid = (int2)(get_global_id(0),get_global_id(1));

  //iterate over it
  float myPenalty = getPenalty(costmap, globalid);
  float local * myval = accessLocalCost(l_costs, localid);
  while(change)
  {
    if(local_linid == 0)
      change = false;
    barrier(CLK_LOCAL_MEM_FENCE);

    float lastmyval = *myval;

    int2 offset;
    for(offset.y = -1; offset.y  <= 1; ++offset.y)
      for(offset.x = -1; offset.x <= 1; ++offset.x)
      {

        float d = native_sqrt((float)(offset.x*offset.x+offset.y*offset.y));
        float penalty = 0.5f*(myPenalty + getPenalty(costmap, globalid + offset));
        float c_other = *accessLocalCost(l_costs, localid + offset);
        *myval = min(*myval, c_other + 0.01f*d + d*penalty);
      }

    if(*myval != lastmyval)
      change = true;

    barrier(CLK_LOCAL_MEM_FENCE);
  }

  //write back
  routecost[dim.x*globalid.y + globalid.x + dim.x*dim.y*id.z] = *myval;
}
#endif

#if 0

/*
 The values stored in the resulting map contain the cost the reach each node and
 which is its predecessor on the path there. It contains 32 Bits for each node
 where the first two bits represent the direction of the predecessor and the
 remaining bits form the actual cost of reaching the node.
 */

constant uint MASK_VALUE        = 0x3fffffff;
constant uint MASK_DIRECTION    = 0xc0000000;
constant uint VALUE_NOT_VISITED = 0xffffffff;

//------------------------------------------------------------------------------
// Heap
//------------------------------------------------------------------------------
typedef struct
{
  global uint *values; //!< The values to be sorted
  global uint *heap;   //!< Sorted indices (by values)
  uint size;           //!< Current heap size
  int2 dim;            //!< Dimensions of values
} data_t;

#define _value(index) (d->values[ d->heap[index] ] & MASK_VALUE)
#define _parent(index) (index / 2)
#define _child_left(index) (2 * index + 1)
#define _child_right(index) (2 * index + 2)

/**
 * @param index The index of the node to insert
 */
void heap_insert( data_t *d,
                  const uint index )
{
  uint cur = d->size++;
  uint val = _value(cur);
  uint parent = _parent(cur);

  // move up
  while( cur && val < _value(parent) )
  {
    d->heap[ cur ] = d->heap[ parent ];

    cur = parent;
    parent = _parent(cur);
  }

  d->heap[ cur ] = index;
}

/**
 *
 * @return Index of the minimum node
 */
uint heap_removeMin( data_t *d )
{
  // Remove first element
  uint cur = 0,
       front = d->heap[ cur ],
       child = _child_left(cur);

  // Propagate hole down the tree
  while( child < d->size )
  {
    // Change to right child if it exists and is smaller than the left one
    if( child + 1 < d->size && _value(child + 1) < _value(child) )
      child += 1;

    d->heap[ cur ] = d->heap[ child ];
    cur = child;
    child = _child_left(cur);
  }

  // Put last element into hole...
  uint index = d->heap[ --d->size ];
  uint val = _value(index);
  uint parent = _parent(cur);

  // ...and let it bubble up
  while( cur && val < _value(parent) )
  {
    d->heap[ cur ] = d->heap[ parent ];

    cur = parent;
    parent = _parent(cur);
  }

  d->heap[ cur ] = index;
  return front;
}

//------------------------------------------------------------------------------
// Dijkstra
//------------------------------------------------------------------------------
float getPenalty(read_only image2d_t costmap, int x, int y)
{
  const sampler_t sampler = CLK_FILTER_NEAREST
                          | CLK_NORMALIZED_COORDS_FALSE
                          | CLK_ADDRESS_CLAMP_TO_EDGE;

  return read_imagef(costmap, sampler, (int2)(x, y)).x;
}

//------------------------------------------------------------------------------
// Kernel
//------------------------------------------------------------------------------

#define makeIndex(pos, dim) (pos.y * dim.x + pos.x)

__kernel void test_kernel( read_only image2d_t costmap,
                           global uint* nodes,
                           global uint* queue,
                           const int2 dim,
                           const int2 start,
                           const int2 target )
{
  data_t data = {
    .values = nodes,
    .heap = queue,
    .size = 0,
    .dim = dim
  };

  const float link_width = 4,
              alpha_L = 0.01,
              alpha_P = 2.99,
              // simplify and scale the cost calculation a bit...
              factor = 100 * (0.5/alpha_L * alpha_P * link_width + 1);

  // Initialize all to "Not visited" and startnode to zero
  for(uint i = 0; i < dim.x * dim.y; ++i)
    nodes[i] = VALUE_NOT_VISITED;

  uint start_index = makeIndex(start, dim);
  nodes[ start_index ] = 0;
  heap_insert(&data, start_index);
  int count = 10;
  uint max_size = 0;
  // Start with Dijkstra
  do
  {
    max_size = max(data.size, max_size);
    uint index = heap_removeMin(&data);
    int px = index % dim.x, // DON'T use int2 -> don't know why but % doesn't work...
        py = index / dim.x;

    // Update all 8 neighbours
    for( int x = max(px - 1, 0); x <= min(px + 1, dim.x); ++x )
      for( int y = max(py - 1, 0); y <= min(py + 1, dim.y); ++y )
      {
        if( px == x && py == y )
          continue;

        int index_n = makeIndex((int2)(x, y), dim);

        if( nodes[index_n] == VALUE_NOT_VISITED )
          heap_insert(&data, index_n);

        float len = (x == px || y == py) ? 1 : 1.4f;
        float cost = factor
                   * len
                   * (getPenalty(costmap, x, y) + getPenalty(costmap, px, py));

        int total_cost = nodes[index] + (int)cost;

        // update/relax
        if( total_cost < nodes[index_n] )
        {
          nodes[index_n] = total_cost;
        }
      }
  } while( data.size && count-- );

  nodes[ 0 ] = max_size;
}

#endif


updated reference kernel

updated reference kernel

kernel void calculate(__global double* var2_global, int var2_rows, int var2_view, __global double* var3_global, int var3_rows, int var3_view, const int rows, const int cols){
const int gid_i = get_global_id(0);
const int gid_j = get_global_id(1);
const int lid_i = get_local_id(0);
const int lsize_i = get_local_size(0);
const int gsize_i = get_global_size(0);
const int gsize_j = get_global_size(1);
const int wg_id_i = get_group_id(0);
const int wg_id_j = get_group_id(1);
const int n_groups_i = get_num_groups(0);
__local double var1_local[LOCAL_SIZE_];
double var1;
for(int j = gid_j; j < cols; j+=gsize_j){
var1 = 0;
for(int i = gid_i; i < rows; i+=gsize_i){
double var2 = 0; if (!((!contains_nonzero(var2_view, LOWER) && j < i) || (!contains_nonzero(var2_view, UPPER) && j > i))) {var2 = var2_global[i + var2_rows * j];}
var1 = var1 + var2;
}
var1_local[lid_i] = var1;
barrier(CLK_LOCAL_MEM_FENCE);
for (int step = lsize_i / REDUCTION_STEP_SIZE; step > 0; step /= REDUCTION_STEP_SIZE) {
  if (lid_i < step) {
    for (int i = 1; i < REDUCTION_STEP_SIZE; i++) {
      var1_local[lid_i] = var1_local[lid_i] + var1_local[lid_i + step * i];
    }
  }
  barrier(CLK_LOCAL_MEM_FENCE);
}
if (lid_i == 0) {
var3_global[j * n_groups_i + wg_id_i] = var1_local[0];
}
}
}

kernel void calculate(__global double* var2_global, int var2_rows, int var2_view, __global double* var3_global, int var3_rows, int var3_view, const int rows, const int cols){
const int gid_i = get_global_id(0);
const int gid_j = get_global_id(1);
const int lid_i = get_local_id(0);
const int lsize_i = get_local_size(0);
const int gsize_i = get_global_size(0);
const int gsize_j = get_global_size(1);
const int wg_id_i = get_group_id(0);
const int n_groups_i = get_num_groups(0);
__local double var1_local[LOCAL_SIZE_];
double var1;
for(int j = gid_j; j < cols; j+=gsize_j){
var1 = 0;
for(int i = gid_i; i < rows; i+=gsize_i){
double var2 = 0; if (!((!contains_nonzero(var2_view, LOWER) && j < i) || (!contains_nonzero(var2_view, UPPER) && j > i))) {var2 = var2_global[i + var2_rows * j];}
var1 = var1 + var2;
}
var1_local[lid_i] = var1;
barrier(CLK_LOCAL_MEM_FENCE);
for (int step = lsize_i / REDUCTION_STEP_SIZE; step > 0; step /= REDUCTION_STEP_SIZE) {
  if (lid_i < step) {
    for (int i = 1; i < REDUCTION_STEP_SIZE; i++) {
      var1_local[lid_i] = var1_local[lid_i] + var1_local[lid_i + step * i];
    }
  }
  barrier(CLK_LOCAL_MEM_FENCE);
}
if (lid_i == 0) {
var3_global[j * n_groups_i + wg_id_i] = var1_local[0];
}
}
}


ambient lighting tweak

ambient lighting tweak

#include "cl/mwc64x/cl/mwc64x.cl"

// Doesn't return 0 so that rays can bounce without bumping.
float sphere_toi(
  const float3 eye,
  const float3 look,

  const float3 center,
  const float radius)
{
  // quadratic coefficients
  float a = dot(look, look);
  float b = 2 * dot(eye - center, look);
  float3 to_center = eye - center;
  float c = dot(to_center, to_center) - radius*radius;

  // discriminant
  float d = b*b - 4*a*c;

  if (d < 0) {
    return HUGE_VALF;
  }

  float s1 = (sqrt(d) - b) / (2 * a);
  if (s1 <= 0) {
    s1 = HUGE_VALF;
  }

  float s2 = (-sqrt(d) - b) / (2 * a);
  if (s1 < s2 || s2 <= 0) {
    return s1;
  }

  return s2;
}

typedef struct {
  float4 row1;
  float4 row2;
  float4 row3;
  float4 row4;
} mat4;

float4 vmult(mat4 m, float4 v) {
  return
    (float4)(
      dot(m.row1, v),
      dot(m.row2, v),
      dot(m.row3, v),
      dot(m.row4, v)
    );
}

mat4 screen_to_view(uint w, uint h, float fovy) {
  float aspect = (float)w / (float)h;

  float b = tan(fovy / 2);
  float a = 2 * b / h;
  b = -b;

  mat4 r;
  // We divide both x and y by h, to take aspect ratio into account.
  // An increase in image width will cause wider rays to be shot.
  r.row1 = (float4)(a, 0, 0, b * aspect);
  r.row2 = (float4)(0, a, 0, b);
  r.row3 = (float4)(0, 0, 1, 0);
  r.row4 = (float4)(0, 0, 0, 1);

  return r;
}

mat4 view_to_world(float3 eye, float3 look, float3 up) {
  float4 x = (float4)(cross(look, up), 0);
  float4 y = (float4)(up, 0);
  float4 z = (float4)(look, 0);
  float4 w = (float4)(eye, 1);

  // Change of basis

  mat4 r;
  r.row1.x = x.x;
  r.row2.x = x.y;
  r.row3.x = x.z;
  r.row4.x = x.w;

  r.row1.y = y.x;
  r.row2.y = y.y;
  r.row3.y = y.z;
  r.row4.y = y.w;

  r.row1.z = z.x;
  r.row2.z = z.y;
  r.row3.z = z.z;
  r.row4.z = z.w;

  r.row1.w = w.x;
  r.row2.w = w.y;
  r.row3.w = w.z;
  r.row4.w = w.w;

  return r;
}

typedef struct {
  float r;
  float g;
  float b;
} RGB;

RGB rgb(float3 xyz) {
  RGB r;
  r.r = xyz.x;
  r.g = xyz.y;
  r.b = xyz.z;
  return r;
}

typedef struct { float x, y, z } float3_parse;

float3 pack_float3(float3_parse f) {
  return (float3)(f.x, f.y, f.z);
}

typedef struct {
  float3_parse center;
  float radius;
  float3_parse color;
  float diffuseness;
  float emittance;
  float reflectance;
  float transmittance;
} Object;

float parse_float(__global const float** data) {
  float r = **data;
  ++*data;
  return r;
}

float3_parse parse_float3(__global const float** data) {
  float3_parse r;
  r.x = parse_float(data);
  r.y = parse_float(data);
  r.z = parse_float(data);
  return r;
}

Object parse_object(__global const float* data) {
  Object r;
  r.center        = parse_float3(&data);
  r.radius        = parse_float(&data);
  r.color         = parse_float3(&data);
  r.diffuseness   = parse_float(&data);
  r.emittance     = parse_float(&data);
  r.reflectance   = parse_float(&data);
  r.transmittance = parse_float(&data);
  return r;
}

typedef struct {
  float3 origin;
  float3 direction;
} Ray;

void raycast(
  Ray ray,

  __global const float* objects,
  const uint num_objects,

  float* toi,
  Object* collision
) {
  *toi = HUGE_VALF;

  for (uint i = 0; i < num_objects; ++i) {
    Object object = parse_object((Object*)objects + i);
    float this_toi = sphere_toi(ray.origin, ray.direction, pack_float3(object.center), object.radius);

    if (this_toi >= *toi) {
      continue;
    }

    *toi = this_toi;
    *collision = object;
  }
}

float rand(mwc64x_state_t* rand_state) {
  return (float)MWC64X_NextUint(rand_state) / (float)UINT_MAX;
}

float3 perturb_frame(mwc64x_state_t* rand_state, const float max_angle, float3 x, float3 y, float3 z) {
  float3 coeffs;
  const float c = cos(max_angle);
  coeffs.y = rand(rand_state) * (1 - c) + c;
  const float xz = sqrt(1 - coeffs.y * coeffs.y);
  const float azimuth = rand(rand_state) * 2 * 3.14;
  coeffs.x = cos(azimuth);
  coeffs.z = sin(azimuth);
  coeffs.x *= xz;
  coeffs.z *= xz;
  return coeffs.x*x + coeffs.y*y + coeffs.z*z;
}

float3 perturb(mwc64x_state_t* rand_state, const float3 unperturbed, const float3 normal, const float max_angle) {
  const float3 y = unperturbed;
  // TODO: find z/x better when normal ~= unperturbed
  const float3 z = normalize(cross(normal, y));
  const float3 x = normalize(cross(z, y));

  for (int i = 0; i < 4; ++i) {
    const float3 r = perturb_frame(rand_state, max_angle, x, y, z);
    if (dot(r, normal) >= 0) {
      return r;
    }
  }

  // If we failed several times, we're probably almost perpendicular to the normal.
  // I think that's a pretty small area, so we can just forget the perturb here.
  return unperturbed;
}

float3 pathtrace(
  Ray ray,
  uint max_depth,
  float3 ambient,
  mwc64x_state_t* rand_state,
  __global const float* objects,
  const uint num_objects
) {
  if (max_depth == 0) {
    return ambient;
  }

  float toi;
  Object collided_object;

  raycast(ray, objects, num_objects, &toi, &collided_object);

  if (toi == HUGE_VALF) {
    return ambient;
  }

  float3 light = ambient * (collided_object.reflectance + collided_object.transmittance);

  light += (float3)(collided_object.emittance);

  const float3 collision_point = ray.origin + toi*ray.direction;
  const float3 normal = (collision_point - pack_float3(collided_object.center)) / collided_object.radius;
  const float max_scatter_angle = 3.14 * collided_object.diffuseness;

  float r = rand(rand_state);

  // TODO: loop + accumulators instead of recursion
  // TODO: consider removing ambient light and adding a large light "around" the whole world.

  r -= collided_object.transmittance;
  if (r <= 0) {
    // TODO: cos_theta < 0?
    Ray transmitted_ray;
    const float3 unperturbed = ray.direction;
    transmitted_ray.direction = perturb(rand_state, unperturbed, -normal, max_scatter_angle);
    transmitted_ray.origin = collision_point + 0.1f * transmitted_ray.direction;

    light +=
      pathtrace(
        transmitted_ray,
        max_depth - 1,
        ambient,
        rand_state,
        objects,
        num_objects
      );
  } else {
    r -= collided_object.reflectance;
    if (r <= 0) {
      // TODO: cos_theta < 0?
      Ray reflected_ray;
      const float cos_theta = dot(ray.direction, normal);
      const float3 unperturbed = ray.direction - 2*cos_theta*normal;
      reflected_ray.direction = perturb(rand_state, unperturbed, normal, max_scatter_angle);
      reflected_ray.origin = collision_point + 0.1f * reflected_ray.direction;

      light +=
        pathtrace(
          reflected_ray,
          max_depth - 1,
          ambient,
          rand_state,
          objects,
          num_objects
        );
    }
  }

  return pack_float3(collided_object.color) * light;
}

mwc64x_state_t init_rand_state(ulong random_seed) {
  mwc64x_state_t rand_state;
  rand_state.x = (uint)(random_seed & 0xFFFFFFFF);
  rand_state.c = (random_seed & 0xFFFFFFFF00000000) >> 32;
  return rand_state;
}

__kernel void render(
  const uint image_width,
  const uint image_height,

  const float fovy,
  float3 eye,
  const float3 look,
  const float3 up,

  ulong random_seed,
  float3 ambient_light,

  __global const float* objects,
  const uint num_objects,

  __global RGB * output)
{
  int id = get_global_id(0);

  const int x_pix = id % image_width;
  const int y_pix = id / image_width;

  float4 world_pos =
    vmult(view_to_world(eye, look, up),
    vmult(screen_to_view(image_width, image_height, fovy),
    (float4)(x_pix, y_pix, 1, 1)
  ));

  Ray ray;
  ray.origin = eye;
  ray.direction = normalize((world_pos / world_pos.w).xyz - eye);

  random_seed = random_seed * id * id;
  mwc64x_state_t rand_state = init_rand_state(random_seed);
  MWC64X_Skip(&rand_state, 20);

  output[id] = rgb(pathtrace(ray, 8, ambient_light, &rand_state, objects, num_objects));
}

#include "cl/mwc64x/cl/mwc64x.cl"

// Doesn't return 0 so that rays can bounce without bumping.
float sphere_toi(
  const float3 eye,
  const float3 look,

  const float3 center,
  const float radius)
{
  // quadratic coefficients
  float a = dot(look, look);
  float b = 2 * dot(eye - center, look);
  float3 to_center = eye - center;
  float c = dot(to_center, to_center) - radius*radius;

  // discriminant
  float d = b*b - 4*a*c;

  if (d < 0) {
    return HUGE_VALF;
  }

  float s1 = (sqrt(d) - b) / (2 * a);
  if (s1 <= 0) {
    s1 = HUGE_VALF;
  }

  float s2 = (-sqrt(d) - b) / (2 * a);
  if (s1 < s2 || s2 <= 0) {
    return s1;
  }

  return s2;
}

typedef struct {
  float4 row1;
  float4 row2;
  float4 row3;
  float4 row4;
} mat4;

float4 vmult(mat4 m, float4 v) {
  return
    (float4)(
      dot(m.row1, v),
      dot(m.row2, v),
      dot(m.row3, v),
      dot(m.row4, v)
    );
}

mat4 screen_to_view(uint w, uint h, float fovy) {
  float aspect = (float)w / (float)h;

  float b = tan(fovy / 2);
  float a = 2 * b / h;
  b = -b;

  mat4 r;
  // We divide both x and y by h, to take aspect ratio into account.
  // An increase in image width will cause wider rays to be shot.
  r.row1 = (float4)(a, 0, 0, b * aspect);
  r.row2 = (float4)(0, a, 0, b);
  r.row3 = (float4)(0, 0, 1, 0);
  r.row4 = (float4)(0, 0, 0, 1);

  return r;
}

mat4 view_to_world(float3 eye, float3 look, float3 up) {
  float4 x = (float4)(cross(look, up), 0);
  float4 y = (float4)(up, 0);
  float4 z = (float4)(look, 0);
  float4 w = (float4)(eye, 1);

  // Change of basis

  mat4 r;
  r.row1.x = x.x;
  r.row2.x = x.y;
  r.row3.x = x.z;
  r.row4.x = x.w;

  r.row1.y = y.x;
  r.row2.y = y.y;
  r.row3.y = y.z;
  r.row4.y = y.w;

  r.row1.z = z.x;
  r.row2.z = z.y;
  r.row3.z = z.z;
  r.row4.z = z.w;

  r.row1.w = w.x;
  r.row2.w = w.y;
  r.row3.w = w.z;
  r.row4.w = w.w;

  return r;
}

typedef struct {
  float r;
  float g;
  float b;
} RGB;

RGB rgb(float3 xyz) {
  RGB r;
  r.r = xyz.x;
  r.g = xyz.y;
  r.b = xyz.z;
  return r;
}

typedef struct { float x, y, z } float3_parse;

float3 pack_float3(float3_parse f) {
  return (float3)(f.x, f.y, f.z);
}

typedef struct {
  float3_parse center;
  float radius;
  float3_parse color;
  float diffuseness;
  float emittance;
  float reflectance;
  float transmittance;
} Object;

float parse_float(__global const float** data) {
  float r = **data;
  ++*data;
  return r;
}

float3_parse parse_float3(__global const float** data) {
  float3_parse r;
  r.x = parse_float(data);
  r.y = parse_float(data);
  r.z = parse_float(data);
  return r;
}

Object parse_object(__global const float* data) {
  Object r;
  r.center        = parse_float3(&data);
  r.radius        = parse_float(&data);
  r.color         = parse_float3(&data);
  r.diffuseness   = parse_float(&data);
  r.emittance     = parse_float(&data);
  r.reflectance   = parse_float(&data);
  r.transmittance = parse_float(&data);
  return r;
}

typedef struct {
  float3 origin;
  float3 direction;
} Ray;

void raycast(
  Ray ray,

  __global const float* objects,
  const uint num_objects,

  float* toi,
  Object* collision
) {
  *toi = HUGE_VALF;

  for (uint i = 0; i < num_objects; ++i) {
    Object object = parse_object((Object*)objects + i);
    float this_toi = sphere_toi(ray.origin, ray.direction, pack_float3(object.center), object.radius);

    if (this_toi >= *toi) {
      continue;
    }

    *toi = this_toi;
    *collision = object;
  }
}

float rand(mwc64x_state_t* rand_state) {
  return (float)MWC64X_NextUint(rand_state) / (float)UINT_MAX;
}

float3 perturb_frame(mwc64x_state_t* rand_state, const float max_angle, float3 x, float3 y, float3 z) {
  float3 coeffs;
  const float c = cos(max_angle);
  coeffs.y = rand(rand_state) * (1 - c) + c;
  const float xz = sqrt(1 - coeffs.y * coeffs.y);
  const float azimuth = rand(rand_state) * 2 * 3.14;
  coeffs.x = cos(azimuth);
  coeffs.z = sin(azimuth);
  coeffs.x *= xz;
  coeffs.z *= xz;
  return coeffs.x*x + coeffs.y*y + coeffs.z*z;
}

float3 perturb(mwc64x_state_t* rand_state, const float3 unperturbed, const float3 normal, const float max_angle) {
  const float3 y = unperturbed;
  // TODO: find z/x better when normal ~= unperturbed
  const float3 z = normalize(cross(normal, y));
  const float3 x = normalize(cross(z, y));

  for (int i = 0; i < 4; ++i) {
    const float3 r = perturb_frame(rand_state, max_angle, x, y, z);
    if (dot(r, normal) >= 0) {
      return r;
    }
  }

  // If we failed several times, we're probably almost perpendicular to the normal.
  // I think that's a pretty small area, so we can just forget the perturb here.
  return unperturbed;
}

float3 pathtrace(
  Ray ray,
  uint max_depth,
  float3 ambient,
  mwc64x_state_t* rand_state,
  __global const float* objects,
  const uint num_objects
) {
  if (max_depth == 0) {
    return ambient;
  }

  float toi;
  Object collided_object;

  raycast(ray, objects, num_objects, &toi, &collided_object);

  if (toi == HUGE_VALF) {
    return ambient;
  }

  float3 light = ambient;

  light += (float3)(collided_object.emittance);

  const float3 collision_point = ray.origin + toi*ray.direction;
  const float3 normal = (collision_point - pack_float3(collided_object.center)) / collided_object.radius;
  const float max_scatter_angle = 3.14 * collided_object.diffuseness;

  float r = rand(rand_state);

  // TODO: loop + accumulators instead of recursion
  // TODO: consider removing ambient light and adding a large light "around" the whole world.

  r -= collided_object.transmittance;
  if (r <= 0) {
    // TODO: cos_theta < 0?
    Ray transmitted_ray;
    const float3 unperturbed = ray.direction;
    transmitted_ray.direction = perturb(rand_state, unperturbed, -normal, max_scatter_angle);
    transmitted_ray.origin = collision_point + 0.1f * transmitted_ray.direction;

    light +=
      pathtrace(
        transmitted_ray,
        max_depth - 1,
        ambient,
        rand_state,
        objects,
        num_objects
      );
  } else {
    r -= collided_object.reflectance;
    if (r <= 0) {
      // TODO: cos_theta < 0?
      Ray reflected_ray;
      const float cos_theta = dot(ray.direction, normal);
      const float3 unperturbed = ray.direction - 2*cos_theta*normal;
      reflected_ray.direction = perturb(rand_state, unperturbed, normal, max_scatter_angle);
      reflected_ray.origin = collision_point + 0.1f * reflected_ray.direction;

      light +=
        pathtrace(
          reflected_ray,
          max_depth - 1,
          ambient,
          rand_state,
          objects,
          num_objects
        );
    }
  }

  return pack_float3(collided_object.color) * light;
}

mwc64x_state_t init_rand_state(ulong random_seed) {
  mwc64x_state_t rand_state;
  rand_state.x = (uint)(random_seed & 0xFFFFFFFF);
  rand_state.c = (random_seed & 0xFFFFFFFF00000000) >> 32;
  return rand_state;
}

__kernel void render(
  const uint image_width,
  const uint image_height,

  const float fovy,
  float3 eye,
  const float3 look,
  const float3 up,

  ulong random_seed,
  float3 ambient_light,

  __global const float* objects,
  const uint num_objects,

  __global RGB * output)
{
  int id = get_global_id(0);

  const int x_pix = id % image_width;
  const int y_pix = id / image_width;

  float4 world_pos =
    vmult(view_to_world(eye, look, up),
    vmult(screen_to_view(image_width, image_height, fovy),
    (float4)(x_pix, y_pix, 1, 1)
  ));

  Ray ray;
  ray.origin = eye;
  ray.direction = normalize((world_pos / world_pos.w).xyz - eye);

  random_seed = random_seed * id * id;
  mwc64x_state_t rand_state = init_rand_state(random_seed);
  MWC64X_Skip(&rand_state, 20);

  output[id] = rgb(pathtrace(ray, 8, ambient_light, &rand_state, objects, num_objects));
}


Add -no-canonical-prefixes to allow different build modes.

git-svn-id: ffe668792ed300d6c2daa1f6eba2e0aa28d7ec6c@325236 91177308-0d34-0410-b5e6-96231b3b80d8

// RUN: %clang -no-canonical-prefixes -### -target amdgcn-- -mcpu=gfx803 %s 2>&1 | FileCheck %s
// RUN: %clang -no-canonical-prefixes -### -target amdgcn-amd- -mcpu=gfx803 %s 2>&1 | FileCheck %s
// RUN: %clang -no-canonical-prefixes -### -target amdgcn-amd-amdhsa -mcpu=gfx803 %s 2>&1 | FileCheck %s
// RUN: %clang -no-canonical-prefixes -### -target amdgcn-amd-amdpal -mcpu=gfx803 %s 2>&1 | FileCheck %s
// RUN: %clang -no-canonical-prefixes -### -target amdgcn-amd-mesa3d -mcpu=gfx803 %s 2>&1 | FileCheck %s

// CHECK: clang{{.*}} "-mrelocation-model" "pic" "-pic-level" "1"

// RUN: %clang -### -target amdgcn-- -mcpu=gfx803 %s 2>&1 | FileCheck %s
// RUN: %clang -### -target amdgcn-amd- -mcpu=gfx803 %s 2>&1 | FileCheck %s
// RUN: %clang -### -target amdgcn-amd-amdhsa -mcpu=gfx803 %s 2>&1 | FileCheck %s
// RUN: %clang -### -target amdgcn-amd-amdpal -mcpu=gfx803 %s 2>&1 | FileCheck %s
// RUN: %clang -### -target amdgcn-amd-mesa3d -mcpu=gfx803 %s 2>&1 | FileCheck %s

// CHECK: clang{{.*}} "-mrelocation-model" "pic" "-pic-level" "1"


removed the swat optimized kernel, does not work due to mismatch in arguments

#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics: enable

#define __THREAD_FENCE_USED__

typedef struct {
    int nposi, nposj;
    int nmaxpos;
    float fmaxscore;
    int noutputlen;
}   MAX_INFO;

#define PATH_END 0
#define COALESCED_OFFSET 32

//Simple GPU barrier, with atomicAdd used
//Input: Local thread idx in a block, goal value
void __barrier_opencl_lock_based(int localID, int goalValue, volatile __global int *g_mutexOpencl)
{
    int tid = localID;
    barrier(CLK_LOCAL_MEM_FENCE | CLK_GLOBAL_MEM_FENCE);

#ifdef __THREAD_FENCE_USED__
    write_mem_fence(CLK_LOCAL_MEM_FENCE | CLK_GLOBAL_MEM_FENCE);
    //other options
    //read_mem_fence(CLK_LOCAL_MEM_FENCE | CLK_GLOBAL_MEM_FENCE);
    //mem_fence(CLK_LOCAL_MEM_FENCE | CLK_GLOBAL_MEM_FENCE);
#endif

    if (tid == 0)
    {
        atom_add(g_mutexOpencl, 1);
        mem_fence(CLK_LOCAL_MEM_FENCE | CLK_GLOBAL_MEM_FENCE);
        while (atom_add(g_mutexOpencl,0) < goalValue)
        { }
    }

    barrier(CLK_LOCAL_MEM_FENCE | CLK_GLOBAL_MEM_FENCE);
}

__kernel void  MatchStringGPUSync(__global char  *pathFlag,
								  __global char  *extFlag,
								  __global float *nGapDist,
								  __global float *hGapDist,
								  __global float *vGapDist,
								  __global int   *diffPos,
								  __global int   *threadNum,
								  int            rowNum,
								  int            columnNum,
								  __global char  *seq1,
								  __global char  *seq2,
								  int            blosumWidth,
								  float          openPenalty,
								  float          extensionPenalty,
								  __global MAX_INFO *maxInfo,
								  __global float *blosum62D,
								  volatile __global int *mutexMem,
								  __local int* temp)
{
	int npos, ntablepos, tid;
	int npreposngap, npreposhgap, npreposvgap;
	int nLocalID = get_local_id(0);
	int blockNum = get_num_groups(0);
	int blockSize = get_local_size(0);
	int totalThreadNum = blockSize * blockNum;
	int threadid = get_global_id(0);

	int launchNo;
	int launchNum = rowNum + columnNum - 1;
	int indexi1 = -1;
	int indexj1 = 0;
	int indexi, indexj;
	int startPos = 2 * COALESCED_OFFSET;
	int noffset = 0;

	float fdist;
	float fdistngap, fdisthgap, fdistvgap;
	float ext_dist;
	float fmaxdist;

	//__local float temp[5000];
	temp[0] = 0.0f;

	for (launchNo = 2; launchNo < launchNum; launchNo++)
	{
		if (launchNo < rowNum)
		{
			indexi1++;
		}
		else if (launchNo == rowNum)
		{
			indexi1++;
			noffset = 1;
		}
		else
		{
			indexj1++;
		}

		for (tid = threadid; tid < threadNum[launchNo]; tid += totalThreadNum)
		{
			indexi = indexi1 - tid;
			indexj = indexj1 + tid;

			npos = startPos + tid;

			npreposhgap = npos - diffPos[launchNo];
			npreposvgap = npreposhgap - 1;
			npreposngap = npreposvgap - diffPos[launchNo - 1];

			ntablepos = seq1[indexi] * blosumWidth + seq2[indexj];
			//ntablepos = seq1C[indexi] * blosumWidth + seq2C[indexj];
			fdist = blosum62D[ntablepos];

			fmaxdist = nGapDist[npreposngap];
			fdistngap = fmaxdist + fdist;

			ext_dist  = hGapDist[npreposhgap] - extensionPenalty;
			fdisthgap = nGapDist[npreposhgap] - openPenalty;

			if (fdisthgap <= ext_dist)
			{
				fdisthgap = ext_dist;
				extFlag[npreposhgap] = 1;
			}

			ext_dist  = vGapDist[npreposvgap] - extensionPenalty;
			fdistvgap = nGapDist[npreposvgap] - openPenalty;

			if (fdistvgap <= ext_dist)
			{
				fdistvgap = ext_dist;
				pathFlag[npreposvgap] += 8;
			}

			fdistngap = (fdistngap < 0.0f) ? 0.0f : fdistngap;
			fdisthgap = (fdisthgap < 0.0f) ? 0.0f : fdisthgap;
			fdistvgap = (fdistvgap < 0.0f) ? 0.0f : fdistvgap;

			hGapDist[npos] = fdisthgap;
			vGapDist[npos] = fdistvgap;

			//priority 00, 01, 10
			if (fdistngap >= fdisthgap && fdistngap >= fdistvgap)
			{
				fmaxdist = fdistngap;
				pathFlag[npos] = 2;
			}
			else if (fdisthgap >= fdistngap && fdisthgap >= fdistvgap)
			{
				fmaxdist = fdisthgap;
				pathFlag[npos] = 1;
			}
			else //fdistvgap >= fdistngap && fdistvgap >= fdisthgap
			{
				fmaxdist = fdistvgap;
				pathFlag[npos] = 3;
			}

			nGapDist[npos] = fmaxdist;

			//Here, the maximum match distance is 0, which means
			//previous alignment is useless
			if (fmaxdist <= 0.00000001f)
			{
				pathFlag[npos] = PATH_END;
			}

			if (maxInfo[threadid].fmaxscore < fmaxdist)
			{
				maxInfo[threadid].nposi = indexi + 1;
				maxInfo[threadid].nposj = indexj + 1;
				maxInfo[threadid].nmaxpos = npos;
				maxInfo[threadid].fmaxscore = fmaxdist;
			}
		}

		//GPU synchronization
		__barrier_opencl_lock_based(nLocalID, (launchNo - 1) * blockNum, mutexMem);
		startPos += diffPos[launchNo + 1] + noffset;
	}
}

__kernel void trace_back2(__global char *str_npathflagp,
						  __global char *str_nExtFlagp,
						  __global int  *ndiffpos,
						  __global char *instr1D,
						  __global char *instr2D,
						  __global char *outstr1,
						  __global char *outstr2,
						  __global MAX_INFO * strMaxInfop,
						  int mfThreadNum)
{
	int i, j;
	int npos, maxPos, nlen;
	int npathflag;
	int nlaunchno;
	float maxScore;

	maxPos = 0;
	maxScore = strMaxInfop[0].fmaxscore;
	for (i = 1; i < mfThreadNum; i++)
	{
		if (maxScore < strMaxInfop[i].fmaxscore)
		{
			maxPos = i;
			maxScore = strMaxInfop[i].fmaxscore;
		}
	}

	npos = strMaxInfop[maxPos].nmaxpos;
	npathflag = str_npathflagp[npos] & 0x3;
	nlen = 0;

	i = strMaxInfop[maxPos].nposi;
	j = strMaxInfop[maxPos].nposj;
	nlaunchno = i + j;

	while (1)
	{
		if (npathflag == 3)
		{
			outstr1[nlen] = 23;
			outstr2[nlen] = instr2D[j - 1];
			nlen++;
			j--;

			//position in the transformed matrix
			npos = npos - ndiffpos[nlaunchno] - 1;
			nlaunchno--;
		}
		else if (npathflag == 1)
		{
			outstr1[nlen] = instr1D[i - 1];
			outstr2[nlen] = 23;
			nlen++;
			i--;

			//position in the transformed matrix
			npos = npos - ndiffpos[nlaunchno];
			nlaunchno--;
		}
		else if (npathflag == 2)
		{
			outstr1[nlen] = instr1D[i - 1];
			outstr2[nlen] = instr2D[j - 1];
			nlen++;
			i--;
			j--;

			//position in the transformed matrix
			npos = npos - ndiffpos[nlaunchno] - ndiffpos[nlaunchno - 1] - 1;
			nlaunchno = nlaunchno - 2;
		}
		else
		{
			//printf("npathflag = %d, npos = %d\n", npathflag, npos);
			//printf("find path error!\n");
			return;
		}

		//only if it is not an extension gap, will the path direction change
		//otherwise, back on the same direction
		int nExtFlag = str_npathflagp[npos] / 4;
		if (npathflag == 3 && (nExtFlag == 2 || nExtFlag == 3))
		{
			npathflag = 3;
		}
		//else if (npathflag == 1 && (nExtFlag == 1 || nExtFlag == 3))
		else if (npathflag == 1 && str_nExtFlagp[npos] == 1)
		{
			npathflag = 1;
		}
		else
		{
			npathflag = str_npathflagp[npos] & 0x3;
		}

		if (i == 0 || j == 0)
		{
			break;
		}

		if (npathflag == PATH_END)
		{
			break;
		}
	}

	i--;
	j--;

	while(i >= 0)
	{
		outstr1[nlen] = instr1D[i];
		outstr2[nlen] = 23;
		nlen++;
		i--;
	}

	while(j >= 0)
	{
		outstr1[nlen] = 23;
		outstr2[nlen] = instr2D[j];
		nlen++;
		j--;
	}

	strMaxInfop[0] = strMaxInfop[maxPos];
	strMaxInfop[0].noutputlen = nlen;

	return;
}

__kernel void setZero(__global char *a,
                      int arraySize)
{
    unsigned int index = get_global_id(0);
    if (index < arraySize)
    {
        a[index] = 0;
    }
}