void rbarray(double *a) {
	for (int i = 0; i < sz; i++) {
		if (RB(i) < i) {
			double t1 = a[i*2], t2 = a[i*2+1];
			a[i*2] = a[RB(i)*2];
			a[i*2+1] = a[RB(i)*2+1];
			a[RB(i)*2] = t1;
			a[RB(i)*2+1] = t2;
		}
	}
}

SSIZE_T parse( BYTE *data, SIZE_T len, Descriptors &descriptors ) {
	SSIZE_T offset = 0;

	WORD compatibilityLen = RW(data,offset);
	if (!compatibilityLen) {
		//	Ignore if not present
		return 2;
	}
	WORD descriptorCount  = RW(data,offset);

	printf( "[dsmcc::compatiblity] Compatibility descriptor: dataLen=%ld, descLen=%d, count=%d\n",
	 	len, compatibilityLen, descriptorCount );

	//	Check if len is ok
	if (len < compatibilityLen) {
		printf( "[dsmcc::compatiblity] No data available to parse Compatibility descriptor: descLen=%ld, available=%d\n",
			len, compatibilityLen );
		return compatibilityLen+2;
	}

	//	Parse descriptors
	for (WORD desc=0; desc<descriptorCount; desc++) {
		Descriptor desc;

		desc.type      = RB(data,offset);
		
		//BYTE dLen      = RB(data,offset);
		offset += 1;
		
		desc.specifier = RDW(data,offset);
		desc.model     = RW(data,offset);
		desc.version   = RW(data,offset);

		//	Parse sub descriptors
		BYTE subCount = RB(data,offset);		
		for (BYTE sub=0; sub<subCount; sub++) {
			BYTE subType = RB(data,offset);
			BYTE subLen  = RB(data,offset);
			//	AdditionalInformation
			offset += subLen;
			printf( "[dsmcc::compatibility] Warning, subdescriptor not parsed: count=%d, type=%x, len=%x\n", subCount, subType, subLen );
		}

		descriptors.push_back( desc );
	}
	
	return compatibilityLen+2; 
}

/*******************************************************************************
 *
 * Render backend context functions.
 *
 ******************************************************************************/
int
rb_create_context
  (struct mem_allocator* specific_allocator,
   struct rb_context** out_ctxt)
{
  struct mem_allocator* allocator = NULL;
  struct rb_context* ctxt = NULL;
  int err = 0;

  if(!out_ctxt)
    goto error;

  allocator = specific_allocator ? specific_allocator : &mem_default_allocator;
  ctxt = MEM_CALLOC(allocator, 1, sizeof(struct rb_context));
  if(!ctxt)
    goto error;
  ctxt->allocator = allocator;
  ref_init(&ctxt->ref);

  setup_config(&ctxt->config);

exit:
  if(ctxt)
    *out_ctxt = ctxt;
  return err;

error:
  if(ctxt) {
    RB(context_ref_put(ctxt));
    ctxt = NULL;
  }
  err = -1;
  goto exit;
}

static void
release_vertex_array(struct ref* ref)
{
  struct rb_context* ctxt = NULL;
  struct rb_vertex_array* varray = NULL;
  assert(ref);

  varray = CONTAINER_OF(ref, struct rb_vertex_array, ref);
  ctxt = varray->ctxt;

  if(ctxt->state_cache.vertex_array_binding == varray->name)
    RB(bind_vertex_array(ctxt, NULL));

  OGL(DeleteVertexArrays(1, &varray->name));
  MEM_FREE(ctxt->allocator, varray);
  RB(context_ref_put(ctxt));
}

static void
release_uniform(struct ref* ref)
{
  struct rb_context* ctxt = NULL;
  struct rb_uniform* uniform = NULL;
  assert(ref);

  uniform = CONTAINER_OF(ref, struct rb_uniform, ref);
  ctxt = uniform->ctxt;

  if(uniform->program)
    RB(program_ref_put(uniform->program));
  if(uniform->name)
    MEM_FREE(ctxt->allocator, uniform->name);
  MEM_FREE(ctxt->allocator, uniform);
  RB(context_ref_put(ctxt));
}

void PocketPCLandscapeAspect(const uint8 *srcPtr, uint32 srcPitch, uint8 *dstPtr, uint32 dstPitch, int width, int height) {

#define RB(x) ((x & redblueMasks[maskUsed])<<8)
#define G(x)  ((x & greenMasks[maskUsed])<<3)

#define P20(x) (((x)>>2)-((x)>>4))
#define P40(x) (((x)>>1)-((x)>>3))
#define P60(x) (((x)>>1)+((x)>>3))
#define P80(x) (((x)>>1)+((x)>>2)+((x)>>4))

#define MAKEPIXEL(rb,g) ((((rb)>>8) & redblueMasks[maskUsed] | ((g)>>3) & greenMasks[maskUsed]))

	int i,j;
	unsigned int p1;
	unsigned int p2;
	uint16 * inbuf;
	uint16 * outbuf;
	inbuf=(uint16 *)srcPtr;
	outbuf=(uint16 *)dstPtr;

	uint16 srcPitch16 = (uint16)(srcPitch / sizeof(uint16));
	uint16 dstPitch16 = (uint16)(dstPitch / sizeof(uint16));

	for (i=0; i<((height)/6); i++) {
        for (j=0; j<width; j++) {
			p1=*((uint16*)inbuf+j);
			inbuf+=srcPitch16;
			*((uint16*)outbuf+j)=p1;
			outbuf+=dstPitch16;
			p2=*((uint16*)inbuf+j);
			inbuf+=srcPitch16;
			*((uint16*)outbuf+j)=MAKEPIXEL(P20(RB(p1))+P80(RB(p2)),P20(G(p1))+P80(G(p2)));  
			outbuf+=dstPitch16;
			p1=p2; 
			p2=*((uint16*)inbuf+j);
			inbuf+=srcPitch16;
			*((uint16*)outbuf+j)=MAKEPIXEL(P40(RB(p1))+P60(RB(p2)),P40(G(p1))+P60(G(p2)));  
			outbuf+=dstPitch16;
			p1=p2;
			p2=*((uint16*)inbuf+j);
			inbuf+=srcPitch16;
			*((uint16*)outbuf+j)=MAKEPIXEL(P60(RB(p1))+P40(RB(p2)),P60(G(p1))+P40(G(p2)));  
			outbuf+=dstPitch16;
			p1=p2;
			p2=*((uint16*)inbuf+j);
			*((uint16*)outbuf+j)=MAKEPIXEL(P80(RB(p1))+P20(RB(p2)),P80(G(p1))+P20(G(p2)));  
			outbuf+=dstPitch16;
			*((uint16*)outbuf+j)=p2;
			inbuf=inbuf-srcPitch16*4;   
			outbuf=outbuf-dstPitch16*5; 
		}      
        inbuf=inbuf+srcPitch16*5;
        outbuf=outbuf+dstPitch16*6;
	}
}

bool b2WheelJoint::SolvePositionConstraints(float32 baumgarte)
{
	B2_NOT_USED(baumgarte);

	b2Body* bA = m_bodyA;
	b2Body* bB = m_bodyB;

	b2Vec2 xA = bA->m_sweep.c;
	float32 angleA = bA->m_sweep.a;

	b2Vec2 xB = bB->m_sweep.c;
	float32 angleB = bB->m_sweep.a;

	b2Mat22 RA(angleA), RB(angleB);

	b2Vec2 rA = b2Mul(RA, m_localAnchorA - m_localCenterA);
	b2Vec2 rB = b2Mul(RB, m_localAnchorB - m_localCenterB);
	b2Vec2 d = xB + rB - xA - rA;

	b2Vec2 ay = b2Mul(RA, m_localYAxisA);

	float32 sAy = b2Cross(d + rA, ay);
	float32 sBy = b2Cross(rB, ay);

	float32 C = b2Dot(d, ay);

	float32 k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy;

	float32 impulse;
	if (k != 0.0f)
	{
		impulse = - C / k;
	}
	else
	{
		impulse = 0.0f;
	}

	b2Vec2 P = impulse * ay;
	float32 LA = impulse * sAy;
	float32 LB = impulse * sBy;

	xA -= m_invMassA * P;
	angleA -= m_invIA * LA;
	xB += m_invMassB * P;
	angleB += m_invIB * LB;

	// TODO_ERIN remove need for this.
	bA->m_sweep.c = xA;
	bA->m_sweep.a = angleA;
	bB->m_sweep.c = xB;
	bB->m_sweep.a = angleB;
	bA->SynchronizeTransform();
	bB->SynchronizeTransform();

	return b2Abs(C) <= b2_linearSlop;
}

void packet_print(){
	short x = (X_SIGN(packet[0]) ? (packet[1] | (0xff << 8)) : packet[1]);
	short y = (Y_SIGN(packet[0]) ? (packet[2] | (0xff << 8)) : packet[2]);
	printf("B1=0x%x B2=0x%x B3=0x%x LB=%d MB=%d RB=%d XOV=%d YOV=%d X=%d Y=%d \n",
			packet[0], packet[1], packet[2],
			LB(packet[0]), MB(packet[0]), RB(packet[0]),
			X_OVF(packet[0]), Y_OVF(packet[0]),
			x, y);
	counter = 0;
}

    vector<Point2F> Square::GetCorners() const {
        vector<Point2F> v;
        v.reserve(4);

        v.push_back(LT());
        v.push_back(LB());
        v.push_back(RT());
        v.push_back(RB());

        return v;
    }

	/* Extract quantization threshold from preamble sequence */
	int32_t extractThreshold(void)
	{
		int32_t threshold = 0;
		int c;
		for (c = 0; c < 16 /* 2 * srate */; c++) {
			//XXX: threshold += (int32_t)_ringbuffer.get(c);
			threshold += (int32_t)RB(c);
		}

		return (int32_t)threshold / 16; // 8 * g_srate
	}

int main()
{
	double *a = calloc(sz*2, sizeof(double));
	for (int i = 0; i < 4; i++) {
		scanf("%lf",&a[RB(i)*2]);
		a[RB(i)*2+1] = 0;
	}
	dft(a, false);
	for (int i = 0; i < sz; i++) {
		double rr = a[i*2], ii = a[i*2+1];
		a[i*2] = rr*rr-ii*ii;
		a[i*2+1] = rr*ii+rr*ii;
	}

	rbarray(a);
	dft(a, true);

	for (int i = 0; i < 7; i++) {
		printf("%.6f\n", a[i*2] / sz);
	}
	free(a);
	return 0;
}

	bool feedOne(const uint16_t sample)
	{
		//XXX: _ringbuffer.increment();
		//_ringbuffer.put((int)sample);
		RB_inc();
		RB(0) = (int)sample;

		if (--_skip < 1) {
			if (decodePacket(++_samples)) {
				_skip = 20;
				return true;
			}
		}
		return false;
	}

/*******************************************************************************
 *
 * Uniform implementation.
 *
 ******************************************************************************/
int
rb_get_named_uniform
  (struct rb_context* ctxt,
   struct rb_program* program,
   const char* name,
   struct rb_uniform** out_uniform)
{
  struct rb_uniform* uniform = NULL;
  GLuint uniform_index = GL_INVALID_INDEX;
  size_t name_len = 0;
  int err = 0;

  if(!ctxt || !program || !name || !out_uniform)
    goto error;

  if(!program->is_linked)
    goto error;

  OGL(GetUniformIndices(program->name, 1, &name, &uniform_index));
  if(uniform_index == GL_INVALID_INDEX)
    goto error;

  err = get_active_uniform(ctxt, program, uniform_index, 0, NULL, &uniform);
  if(err != 0)
    goto error;

  name_len = strlen(name) + 1;
  uniform->name = MEM_ALLOC(ctxt->allocator, sizeof(char) * name_len);
  if(!uniform->name)
    goto error;

  uniform->name = strncpy(uniform->name, name, name_len);
  uniform->location = OGL(GetUniformLocation(program->name, uniform->name));

exit:
  *out_uniform = uniform;
  return err;

error:
  if(uniform) {
    RB(uniform_ref_put(uniform));
    uniform = NULL;
  }
  err = -1;
  goto exit;
}

static void
release_buffer(struct ref* ref)
{
  struct rb_buffer* buffer = NULL;
  struct rb_context* ctxt = NULL;
  ASSERT(ref);

  buffer = CONTAINER_OF(ref, struct rb_buffer, ref);
  ctxt = buffer->ctxt;

  if(buffer->name == ctxt->state_cache.buffer_binding[buffer->binding])
    OGL(BindBuffer(buffer->target, 0));
 
  OGL(DeleteBuffers(1, &buffer->name));
  MEM_FREE(ctxt->allocator, buffer);
  RB(context_ref_put(ctxt));
}

void vm_mini_vm(lua_State *L, LClosure *cl, int count, int pseudo_ops_offset) {
  const Instruction *pc;
  StkId base;
  TValue *k;

  k = cl->p->k;
  pc = cl->p->code + pseudo_ops_offset;
  base = L->base;
  /* process next 'count' ops */
  for (; count > 0; count--) {
    const Instruction i = *pc++;
    StkId ra = RA(i);
    lua_assert(base == L->base && L->base == L->ci->base);
    lua_assert(base <= L->top && L->top <= L->stack + L->stacksize);
    lua_assert(L->top == L->ci->top || luaG_checkopenop(i));
    switch (GET_OPCODE(i)) {
      case OP_MOVE: {
        setobjs2s(L, ra, RB(i));
        continue;
      }
      case OP_LOADK: {
        setobj2s(L, ra, KBx(i));
        continue;
      }
      case OP_GETUPVAL: {
        int b = GETARG_B(i);
        setobj2s(L, ra, cl->upvals[b]->v);
        continue;
      }
      case OP_SETUPVAL: {
        UpVal *uv = cl->upvals[GETARG_B(i)];
        setobj(L, uv->v, ra);
        luaC_barrier(L, uv, ra);
        continue;
      }
      case OP_SETTABLE: {
        Protect(luaV_settable(L, ra, RKB(i), RKC(i)));
        continue;
      }
      default: {
        luaG_runerror(L, "Bad opcode: opcode=%d", GET_OPCODE(i));
        continue;
      }
    }
  }
}

SSIZE_T Object::objectKey( const BYTE *data, SIZE_T len, ObjectKeyType &key ) {
    SSIZE_T off=0;

    //  Object key len
    BYTE objectKeyLen = RB(data,off);
    if (objectKeyLen > 4) {
        throw std::bad_cast();
    }
    assert(len >= SIZE_T(1+objectKeyLen));
	
    //  Object key
	key = RDW( data, off );

	//printf( "[dsmcc::Object] Parsing object key: len=%08lx, key=%08lx\n", len, key );

    return off;
}

void luaV_execute (lua_State *L) {
  CallInfo *ci = L->ci;
  LClosure *cl;
  TValue *k;
  StkId base;
 newframe:  /* reentry point when frame changes (call/return) */
  lua_assert(ci == L->ci);
  cl = clLvalue(ci->func);
  k = cl->p->k;
  base = ci->u.l.base;

  //printf( "s:%p\n", ci->u.l.savedpc );
  /* main loop of interpreter */
  for (;;) {

    Instruction i = *(ci->u.l.savedpc++);
    StkId ra;
    if ((L->hookmask & (LUA_MASKLINE | LUA_MASKCOUNT)) &&
        (--L->hookcount == 0 || L->hookmask & LUA_MASKLINE)) {
      Protect(traceexec(L));
    }
    /* warning!! several calls may realloc the stack and invalidate `ra' */
    ra = RA(i);
    lua_assert(base == ci->u.l.base);
    lua_assert(base <= L->top && L->top < L->stack + L->stacksize);


    // 命令出力
    //printInst( ci->u.l.savedpc - 1 );


    vmdispatch (GET_OPCODE(i)) {
      vmcase(OP_MOVE,
        setobjs2s(L, ra, RB(i));
      )
      vmcase(OP_LOADK,
        TValue *rb = k + GETARG_Bx(i);
        setobj2s(L, ra, rb);
      )
      vmcase(OP_LOADKX,
        TValue *rb;
        lua_assert(GET_OPCODE(*ci->u.l.savedpc) == OP_EXTRAARG);
        rb = k + GETARG_Ax(*ci->u.l.savedpc++);
        setobj2s(L, ra, rb);
      )

int test_config(void) {
	unsigned long byte;
	subscribe_mouse();
	mouse_write_byte(DISABLE_DATA_PACKETS);
	mouse_write_byte(STATUS_REQUEST);
	byte = mouse_read();
	if(byte == -1) return 1;
	printf("byte 1: 0x%X\n", byte);
	printf("Scaling: ");
	if(!SCALING(byte))
		printf("1:1  ");
	else
		printf("2:1  ");
	printf("Data Reporting: ");
	if(!DATA_REPORTING(byte))
		printf("disable  ");
	else
		printf("enable   ");
	printf("Mode: ");
	if(!MODE(byte))
		printf("remote mode\n\n");
	else
		printf("stream mode\n\n");
	if(LB(byte))
		printf("LB: pressed ");
	else
		printf("LB: not pressed ");
	if(RB(byte))
		printf("RB: pressed ");
	else
		printf("RB: not pressed ");
	if(MB(byte))
		printf("MB: pressed ");
	else
		printf("MB: not pressed ");
	byte = mouse_read();
	if(byte == -1) return 1;
	printf("\n byte 2: 0x%X\n", byte);
	printf("Resolution: %d\n\n", byte);
	byte = mouse_read();
	if(byte == -1) return 1;
	printf("byte 3: 0x%X\n", byte);
	printf("Sample Rate: %d\n\n", byte);
}

/*******************************************************************************
 *
 * Vertex array functions.
 *
 ******************************************************************************/
int
rb_create_vertex_array
  (struct rb_context* ctxt,
   struct rb_vertex_array** out_array)
{
  struct rb_vertex_array* array = NULL;

  if(!ctxt || !out_array)
    return -1;

  array = MEM_ALLOC(ctxt->allocator, sizeof(struct rb_vertex_array));
  if(!array)
    return -1;
  ref_init(&array->ref);
  RB(context_ref_get(ctxt));
  array->ctxt = ctxt;

  OGL(GenVertexArrays(1, &array->name));
  *out_array = array;
  return 0;
}

/*******************************************************************************
 *
 * Private functions.
 *
 ******************************************************************************/
int
rb_ogl3_create_buffer
  (struct rb_context* ctxt,
   const struct rb_ogl3_buffer_desc* desc,
   const void* init_data,
   struct rb_buffer** out_buffer)
{
  struct rb_buffer* buffer = NULL;

  if(!ctxt
  || !desc
  || !out_buffer
  || (desc->target == RB_OGL3_NB_BUFFER_TARGETS)
  || (desc->usage == RB_USAGE_IMMUTABLE && init_data == NULL))
    return -1;

  buffer = MEM_ALLOC(ctxt->allocator, sizeof(struct rb_buffer));
  if(!buffer)
    return -1;
  ref_init(&buffer->ref);
  RB(context_ref_get(ctxt));
  buffer->ctxt = ctxt;

  buffer->target = rb_to_ogl3_buffer_target(desc->target);
  buffer->usage = rb_to_ogl3_usage(desc->usage);
  buffer->size = (GLsizei)desc->size;
  buffer->binding = desc->target;

  OGL(GenBuffers(1, &buffer->name));
  OGL(BindBuffer(buffer->target, buffer->name));
  OGL(BufferData(buffer->target, buffer->size, init_data, buffer->usage));
  OGL(BindBuffer
    (buffer->target, 
     ctxt->state_cache.buffer_binding[buffer->binding]));

  *out_buffer = buffer;
  return 0;
}