static int 
array_offset(int* arrayinfo, ...)
{
    int i = 0;
    int s = 0;
    int loc = 0;
    int space = 1;
    va_list list;

    va_start(list, arrayinfo);
    for (i = 1; i <= arrayinfo[0]; i++)
    {
        s = va_arg(list, int);

        if (s < arrayinfo[LOW(i)] || s > arrayinfo[HIGH(i)])
        {
            va_end(list);

            return -1;
        }

        space *= (arrayinfo[HIGH(i-1)]-arrayinfo[LOW(i-1)]+1);
        loc += (s-arrayinfo[LOW(i)])*space;
    }
    va_end(list);

    return loc;
}

static void insert_level(int levToInsert) {
	int n;
    for (n = 2; n < bddnodesize; n++) {
        int lev, lo, hi, newLev, hash, r, r2, NEXT_r;
        if (LOW(n) == INVALID_BDD) continue;
        lev = LEVEL(n);
        if (lev <= levToInsert || lev == bddvarnum-1) {
            continue;
        }
        lo = LOW(n);
        hi = HIGH(n);
        newLev = lev+1;
        hash = NODEHASH(lev, lo, hi);
        r = HASH(hash);
        r2 = 0;
        while (r != n && r != 0) {
            r2 = r;
            r = NEXT(r);
        }
        NEXT_r = NEXT(r);
        if (r2 == 0) {
            SETHASH(hash, NEXT_r);
        } else {
            SETNEXT(r2, NEXT_r);
        }
        SETLEVEL(n, newLev);
        lo = LOW(n); hi = HIGH(n);
        hash = NODEHASH(newLev, lo, hi);
        r = HASH(hash);
        SETHASH(hash, n);
        SETNEXT(n, r);
    }
}

/**
 * Get ICC status
 *
 * @param ctx Reader context
 * @param lr Length of response
 * @param rsp Response buffer
 * @return \ref OK, \ref ERR_CT, \ref ERR_MEMORY
 */
int GetICCStatus(struct scr *ctx, unsigned int *lr, unsigned char *rsp)
{
	int status;

	status = PC_to_RDR_GetSlotStatus(ctx);

	if (status < 0) {
		rsp[0] = HIGH(NOT_SUCCESSFUL);
		rsp[1] = LOW(NOT_SUCCESSFUL);
		*lr = 2;
		return ERR_CT;
	}

	if (*lr < 5) {
		return ERR_MEMORY;
	}

	rsp[0] = 0x80;
	rsp[1] = 0x01;
	rsp[2] = 0x00; /* Set ICC Status DO - default is no ICC present */

	if (status == ICC_PRESENT_AND_INACTIVE) {
		rsp[2] |= 0x03; /* card in, no CVCC                  */
	}

	if (status == ICC_PRESENT_AND_ACTIVE) {
		rsp[2] |= 0x05; /* card in, CVCC on                  */
	}

	rsp[3] = HIGH(SMARTCARD_SUCCESS);
	rsp[4] = LOW(SMARTCARD_SUCCESS);
	*lr = 5;

	return OK;
}

/*
NAME    {* bdd\_scanset *}
SECTION {* kernel *}
SHORT   {* returns an integer representation of a variable set *}
PROTO   {* int bdd_scanset(BDD r, int **v, int *n) *}
DESCR   {* Scans a variable set {\tt r} and copies the stored variables into
           an integer array of variable numbers. The argument {\tt v} is
	   the address of an integer pointer where the array is stored and
	   {\tt n} is a pointer to an integer where the number of elements
	   are stored. It is the users responsibility to make sure the
	   array is deallocated by a call to {\tt free(v)}. The numbers
	   returned are guaranteed to be in ascending order. *}
ALSO    {* bdd\_makeset *}
RETURN  {* Zero on success, otherwise a negative error code. *}
*/
int bdd_scanset(BDD r, int **varset, int *varnum)
{
   int n, num;

   CHECK(r);
   if (r < 2)
   {
      *varnum = 0;
      *varset = NULL;
      return 0;
   }
   
   for (n=r, num=0 ; n > 1 ; n=HIGH(n))
      num++;

   if (((*varset) = (int *)malloc(sizeof(int)*num)) == NULL)
      return bdd_error(BDD_MEMORY);
   
   for (n=r, num=0 ; n > 1 ; n=HIGH(n))
      (*varset)[num++] = bddlevel2var[LEVEL(n)];

   *varnum = num;

   return 0;
}

void right_rotate(Node* &T, Node *x)
{
	if (NIL == LEFT(x))
		return;

	Node *y = LEFT(x);

	// y的右孩子挂到x的左孩子
	LEFT(x) = RIGHT(y);
	if (NIL != RIGHT(y))
		PARENT(RIGHT(y)) = x;
	
	// x的父亲作为y的父亲
	PARENT(y) = PARENT(x);

	// x是根节点，则y是新的根；否则，把y挂到x的父节点下
	if (NIL == PARENT(x)) {
		T = y;
	} else if (IS_LEFT(x)) {
		LEFT(PARENT(x)) = y;
	} else {
		RIGHT(PARENT(x)) = y;
	}

	// 把x挂到y的右孩子
	RIGHT(y) = x;
	PARENT(x) = y;

	// 重新计算x和y的max值
	MAX(x) = max(MAX(LEFT(x)), MAX(RIGHT(x)), HIGH(KEY(x)));
	MAX(y) = max(MAX(LEFT(y)), MAX(RIGHT(y)), HIGH(KEY(y)));
}

/**
 * CT-BCS Get Status command
 *
 * @param ctx Reader context
 * @param cmd Command
 * @param lr Length of response
 * @param rsp Response buffer
 * @return \ref OK, \ref ERR_CT, \ref ERR_MEMORY
 */
int GetStatus(struct scr *ctx, unsigned char *cmd, unsigned int *lr,
			  unsigned char *rsp)
{
	int response;
	unsigned char func_unit = cmd[2];
	unsigned char what = cmd[3];

#ifdef DEBUG
	ctccid_debug("GetStatus(FU=%02x,%02x)\n", func_unit, what);
#endif

	if (func_unit == 0x00) {

		switch (what) {

		case 0x46: /* Card Manufacturer DO */

			if (*lr < 19) {
				return ERR_MEMORY;
			}

			memcpy(rsp, "\x46\x0F" "DESCMSCR3X00000", 17);
			rsp[17] = HIGH(SMARTCARD_SUCCESS);
			rsp[18] = LOW(SMARTCARD_SUCCESS);
			*lr = 17 + 2;
			break;

		case 0x80: /* ICC Status DO */

			if ((response = GetICCStatus(ctx, lr, rsp)) < 0) {
				return response;
			}

			break;

		case 0x81: /* Functional Unit DO */

			if (*lr < 5) {
				return ERR_MEMORY;
			}

			rsp[0] = 0x81; /* TAG */
			rsp[1] = 0x01; /* Length of following data */
			rsp[2] = 0x01; /* Status for CT/ICC-Interface1 */
			rsp[3] = HIGH(SMARTCARD_SUCCESS);
			rsp[4] = LOW(SMARTCARD_SUCCESS);
			*lr = 5;
			break;
		}

	} else {
		if ((response = GetICCStatus(ctx, lr, rsp)) < 0) {
			return response;
		}
	}

	return OK;
}

void lcd_dummy_read(uint8_t chip)
{
	wait_while_busy(chip);
	HIGH(RW_PORT, RW_PIN);//read
	HIGH(RS_PORT, RS_PIN);//data
	HIGH(EN_PORT, EN_PIN);
	_delay_us(2);//>1000ns
	LOW(EN_PORT, EN_PIN);
}

uint8_t lcd_data_read(uint8_t chip)
{
	wait_while_busy(chip);
	uint8_t data;
	HIGH(RW_PORT, RW_PIN);//read
	HIGH(RS_PORT, RS_PIN);//data
	data = lcd_read();
	return data;
}

/**
 * CT-BCS Request ICC command
 *
 * @param ctx Reader context
 * @param lc Length of command
 * @param cmd Command
 * @param lr Length of response
 * @param rsp Response buffer
 * @return \ref OK, \ref ERR_CT, \ref ERR_MEMORY
 */
int RequestICC(struct scr *ctx, unsigned int lc, unsigned char *cmd,
			   unsigned int *lr, unsigned char *rsp)
{
	int status, timeout;

	if ((lc > 4) && (cmd[4] == 1)) {
		timeout = cmd[5];
	} else {
		timeout = 0;
	}

	status = PC_to_RDR_GetSlotStatus(ctx);

	if (status < 0) {
		rsp[0] = HIGH(NOT_SUCCESSFUL);
		rsp[1] = LOW(NOT_SUCCESSFUL);
		*lr = 2;
		return ERR_CT;
	}

	timeout *= 4;

	do {

		status = PC_to_RDR_GetSlotStatus(ctx);

		if (status < 0) {
			rsp[0] = HIGH(NOT_SUCCESSFUL);
			rsp[1] = LOW(NOT_SUCCESSFUL);
			*lr = 2;
			return ERR_CT;
		}

		if ((status == ICC_PRESENT_AND_INACTIVE) || !timeout) {
			break;
		}

		usleep(250000);
		timeout--;
	} while (timeout);

	if (!timeout && (status == NO_ICC_PRESENT)) {
		rsp[0] = HIGH(W_NO_CARD_PRESENTED);
		rsp[1] = LOW(W_NO_CARD_PRESENTED);
		*lr = 2;
		return OK;
	}

	if ((status = ResetCard(ctx, lc, cmd, lr, rsp)) < 0) {
		return status;
	}

	return OK;
}

static ssize_t
push (gnutls_transport_ptr_t tr, const void *data, size_t len)
{
int fd = (long int)tr;

  if (to_send >= 0 && len < HIGH(MAX_BUF))
    {
      fail("Sent data (%u) are lower than expected (%u)\n", (unsigned)len, (unsigned)HIGH(MAX_BUF));
      terminate();
    }
  return send(fd, data, len, 0);
}

void wait_while_busy(uint8_t chip)
{
	lcd_select_chip(chip);
	HIGH(RW_PORT, RW_PIN);
	LOW(RS_PORT, RS_PIN);
	DATA_PORT = 0x00;
	DATA_DDR = 0x00;
	HIGH(EN_PORT, EN_PIN);
	_delay_us(2);//>1000ns
	LOW(EN_PORT, EN_PIN);
	while (LCD_BUSY & DATA_PIN);
	DATA_DDR = 0xFF;
}

uint32_t i2clcd_init( unsigned char contrast)
{
	OUTPUT(1, I2C_LCD_RST);

	LOW(1, I2C_LCD_RST);
	wait_ms(10);
	HIGH(1, I2C_LCD_RST);

	wait_ms(40);
	i2c_cmd(0x38);//0b00111000); // function set
	i2c_cmd(0x39);//0b00111001); // function set
	i2c_cmd(0x14);//0b00010100); // interval osc
	i2c_cmd(0x70 | (contrast & 0xF)); // contrast Low
        
	//i2c_cmd(0b01011100 | ((contrast >> 4) & 0x3)); // contast High/icon/power
	i2c_cmd(0x5C | ((contrast >> 4) & 0x3)); // contast High/icon/power
	i2c_cmd(0x6C); // follower control
	wait_ms(300);

	i2c_cmd(0x38);//0b00111000); // function set
	i2c_cmd(0x0c);//0b00001100); // Display On
	
	i2c_cmd(0x01);//0b00000001); // Clear Display
	wait_ms(2);			 // Clear Display�͒lj��E�F�C�g���K�v
  
  	return 0;
}

dht11_status read_dht11(dht11_data *data) {
  OUTPUT(DHT_PIN);
  LOW(DHT_PIN); // send DHT wakeup signal
  _delay_ms(18);
  HIGH(DHT_PIN);
  _delay_us(40);
  INPUT(DHT_PIN);

  if(!listen_edge(false, 0, 100)) // hold for DHT ack signal, 80us
    return bad_ack;

  _delay_us(5);
  if(!listen_edge(true, 0, 100)) // pre-transmission pause from DHT
    return bad_post_ack;

  union _dht_output raw = { .bytes={0} };
  for(uint8_t byte = 0; byte < sizeof(raw); byte++) {
    for(int8_t bit = 7; bit >= 0; bit--) {
      if(!listen_edge(false, 20, 80)) // 50us bit prelude
        return bad_bit_prelude;
      uint8_t bit_length = listen_edge(true, 10, 90); // bit signal - 26-28us for 0 70us for 1
      if(!bit_length)
        return bad_bit_data_pause; // invalid data pulse
      if(bit_length > 30)
        raw.bytes[byte] |= _BV(bit);
    }
  }

  if(raw.data.checksum != (uint8_t)(raw.bytes[0]+raw.bytes[1]+raw.bytes[2]+raw.bytes[3]))
    return bad_checksum;

  memcpy(data, &raw.data.inner, sizeof(dht11_data));
  return success;
}

void lcd_data_write(uint8_t data, uint8_t chip)
{
	wait_while_busy(chip);
	LOW(RW_PORT, RW_PIN);//write
	HIGH(RS_PORT, RS_PIN);//data
	lcd_write(data, chip);
}

///////////////////////////////////////////////////////////////////////////////
// write block to eeprom
void drvEEPROMWriteBlock( dosWord in_address, dosByte* in_buffer, dosWord in_length)
{
	dosWord i;
	
	// erase sector
	drvEEPROMSectorErase( in_address );
			
	// Set the Write Enable Latch
	drvEEPROMWriteEnable();
	
	// perform the write sequence
	EEPROMCS(PIN_LOW);												// select the Serial EEPROM
	EEPROMSendReceiveByte(SEE_WRITE);					// write command
	EEPROMSendReceiveByte( l_address_highest );	// address highest first
	EEPROMSendReceiveByte(HIGH(in_address));	// address MSB first
	EEPROMSendReceiveByte(LOW(in_address));		// address LSB (word aligned)

	// read block data
	for( i = 0; i < in_length; i++ )
	{
		EEPROMSendReceiveByte(in_buffer[i]); // read dummy, write byte
	}

	// relase CS
	EEPROMCS(PIN_HIGH);										// deselect terminate command

	// wait for busy	
	drvEEPROMWaitForBusy( 20 );
}

int main (int argc, char **argv)
{
  /* Disable watchdog if enabled by bootloader/fuses */
  MCUSR &= ~(1 << WDRF);
  wdt_disable();

  // Assert pshold to stay powered on
  OUTPUT(pshold);
  HIGH(pshold);

  // En fast batt charging
  OUTPUT(usb_i_sel);
  LOW(usb_i_sel);

  // Init UI
  ui_setup();

  // do nothing
  while(1) {
    // Usb processing
    //ser.process(); done on timer

    // Update ui
    ui_process();
  }
}

/*
NAME    {* bdd\_high *}
SECTION {* info *}
SHORT   {* gets the true branch of a bdd  *}
PROTO   {* BDD bdd_high(BDD r) *}
DESCR   {* Gets the true branch of the bdd {\tt r}.  *}
RETURN  {* The bdd of the true branch *}
ALSO    {* bdd\_low *}
*/
BDD bdd_high(BDD root)
{
   CHECK(root);
   if (root < 2)
      return bdd_error(BDD_ILLBDD);

   return (HIGH(root));
}

void ngc_poll() {
    portc_mask = 0b00000001;
    LATC &= ~portc_mask; // pull down - always call this before CLR() calls
    CLR(); // set data pin to output, making the pin low
    // send 01000000
    //      00000011    
    //      00000010
    LOW(); HIGH(); LOW(); LOW(); LOW(); LOW(); LOW(); LOW(); 
    LOW(); LOW(); LOW(); LOW(); LOW(); LOW(); HIGH(); HIGH(); 
    LOW(); LOW(); LOW(); LOW(); LOW(); LOW(); LOW(); LOW(); 
    
    // stop bit, 2 us
    CLR(); 
    _delay(22);
    SET();// back set to open collector input with pull up
    LATC |= portc_mask; // reset pull up
}

void lcd_write(uint8_t value, uint8_t chip)
{
	DATA_PORT = value;
	HIGH(EN_PORT, EN_PIN);
	_delay_us(2); //>1000ns
	LOW(EN_PORT,EN_PIN);
	DATA_PORT = 0x00;
}

void display_pulseEnable(void) {
    LOW(_enable_pin);
    _delay_us(1);    
    HIGH(_enable_pin);
    _delay_us(1);    // enable pulse must be >450ns
    LOW(_enable_pin);
    _delay_us(100);   // display_commands need > 37us to settle
}