int run_node(node_t* n, uint64_t lg2_pin_size, uint64_t pin_offs, void* pins) {
#define PIN(x) ((((x)+pin_offs)<<lg2_pin_size)+pins)
  const uint64_t pin_size = 1<<lg2_pin_size;
  int i;
  if(n->output_constant) {
    for(i=n->output0;i<n->outputN;i++) {
      memcpy(PIN(i),n->output_constant,pin_size);
    }
    return 0;
  } else if (n->function) {
    void* inputs  = PIN(n->input0);
    void* outputs = PIN(n->output0);
    n->function(inputs,outputs);
  } else if (n->subnodes) {
    pin_offs+=n->input0;
    node_t* subnode;
    for(i=0, subnode=n->subnodes; i<n->k; i++, subnode++) {
      run_node(subnode,lg2_pin_size,pin_offs,pins);
    }
    struct connection* cxn;
    for(i=0, cxn=n->cxn; i<n->ncxn; i++, cxn++)  {
      memcpy(PIN(cxn->to_pin),PIN(cxn->from_pin),pin_size);
    }
  } else {
    return -2;
  }
#undef PIN
}

u16 REMOTE_get(void)
{
  u08 i, tmp = 0;
  u08 time;
  u08 T2,T4;
  union u16convert code;

  loop_until_bit_is_set(PIN(REMOTE_PORT), REMOTE_BIT);  // skip leading signal

  TCCR0=4;        //update every 32us
  TCNT0 = 1;

  while (bit_is_set(PIN(REMOTE_PORT), REMOTE_BIT))
  {
    T2 = TCNT0;
    if (T2 >= 100)    // max wait time
      return 0;
  }
  
  // measure time T

  TCNT0 = 1;
  loop_until_bit_is_set(PIN(REMOTE_PORT), REMOTE_BIT);
  
  T2 = TCNT0;    // T is normally around 0E-10 hex = 15 -> 480 uS
  T2 = T2 * 2;
  // max time is 4T
  T4 = T2 * 2;    

  for (i = 0; i < 48; i++)
  {
    TCNT0 = 1;
    while(1)
    {
      time = TCNT0;
      if (time > T4)
        return 0;
      
      // measure time on the lo flank
      if (bit_is_clear(PIN(REMOTE_PORT), REMOTE_BIT))
      {
        tmp <<= 1;
        if (time >= T2)
          tmp++;
        break;
      }
    }

    // save command data as we go
    if( i == 39)
      code.bytes.low = tmp;

    if( i == 47)
      code.bytes.high = tmp;
    
    // syncronize - wait for next hi flank
    loop_until_bit_is_set(REMOTE_PORT - 2, REMOTE_BIT);
  }
  return code.value;
}

void target_early_init(void)
{
	// UART1 on P6.4 (TX) and P2.1 (RX)
	// LpcXpresso4337 P4 FTDI header
	pin_config(PIN(6,4), PIN_MODE(2) | PIN_PLAIN);
	pin_config(PIN(2,1), PIN_MODE(1) | PIN_PLAIN | PIN_INPUT);
}

		CurrentSensor():
			current_adc(
				AD7685<2>(PIN(F, 12), SPI_CHANNEL4, PIN(F, 12), // Convert & CS are same pin
				AD7685<2>::CHAIN_MODE_NO_BUSY)),
			currents(0)
		{
		}

		BPS(): Nu32(Nu32::V2011),
		// Initialize hardware
			nu32			(Nu32::V2011),
			ground_relay	(PIN(D, 1), false),
			power_relay		(PIN(D, 2), false),
			motor_relay		(PIN(D, 3), false),
			array_relay		(PIN(D, 4), false),
			bypass_button	(PIN(B, 0)), // thru B5
			common_can		(CAN1),
			common_can_in	(common_can, CAN_CHANNEL0),
			common_can_out	(common_can, CAN_CHANNEL1),
			common_can_err	(common_can, CAN_CHANNEL2),
			// lcd1			(PIN(G, 9), SPI_CHANNEL2, PIN(A, 9),  PIN(E, 9)),
			// lcd2			(PIN(E, 8), SPI_CHANNEL2, PIN(A, 10), PIN(E, 9)),
			voltage_sensor	(),
			current_sensor	(),
			//temp_sensor		(),
		// initialize timers
			dc_can_timeout		(NU_BPS_DC_TIMEOUT_INT_MS,	Timer::ms,	true),
			precharge_timer		(NU_BPS_PRECHARGE_TIME_MS,	Timer::ms,	false),
			can_timer			(NU_BPS_CAN_TIMER_MS,		Timer::ms,	true),
			lcd_timer			(1,							Timer::s,	false),
			can_module_i(0),
		// and data
			state()
		{
		}

/******************************* Main Program Code *************************/
int main(void)
{
// enable motor#1
    DDRA = 0b00001100;
    
// initialize the LCD controller as determined by the defines (LCD instructions)
    lcd_init_4d();                                  // initialize the LCD display for a 4-bit interface

// endless loop
    while(1) {
		lcd_clear_line_4d(lcd_LineOne);
		_delay_ms(500);
    	// display the first line of information
		lcd_write_string_line_4d("Motor Clockwise", lcd_LineOne);
		// rotate motor
	    UNSET(PORTA, PIN(2));
	    SET(PORTA, PIN(3));
	    // clear LCD
		lcd_clear_line_4d(lcd_LineTwo);
		_delay_ms(500);
    	// display the first line of information
		lcd_write_string_line_4d("Counter Clockwiz", lcd_LineTwo);
		// rotate motor
	    SET(PORTA, PIN(2));
	    UNSET(PORTA, PIN(3));
    }
    return 0;
}

static inline base_t LGetValue (void)
{
	base_t Value;

	Value = PIN(LEDS_PORT_HIGH) & (LED4|LED5|LED6|LED7);
	Value |= ((PIN(LEDS_PORT_LOW) & (LED0|LED1|LED2|LED3))>>4);
	return Value;
}

bool GPS_Init(void) {
	// Initialize pins
	TRACE_INFO("GPS  > Init pins");
	palSetPadMode(PORT(GPS_RESET), PIN(GPS_RESET), PAL_MODE_OUTPUT_PUSHPULL);	// GPS reset
	palSetPadMode(PORT(GPS_EN), PIN(GPS_EN), PAL_MODE_OUTPUT_PUSHPULL);			// GPS off
	palSetPadMode(PORT(GPS_TIMEPULSE), PIN(GPS_TIMEPULSE), PAL_MODE_INPUT);		// GPS timepulse

	// Switch MOSFET
	TRACE_INFO("GPS  > Switch on");
	palSetPad(PORT(GPS_RESET), PIN(GPS_RESET));	// Pull up GPS reset
	palSetPad(PORT(GPS_EN), PIN(GPS_EN));		// Switch on GPS
	
	// Wait for GPS startup
	chThdSleepMilliseconds(3000);

	uint8_t status = 1;

	// Configure GPS
	TRACE_INFO("GPS  > Initialize GPS");
	if(gps_disable_nmea_output()) {
		TRACE_INFO("GPS  > Disable NMEA output OK");
	} else {
		TRACE_ERROR("GPS  > Disable NMEA output FAILED");
		status = 0;
	}

	#if GPS_TYPE == MAX7 || GPS_TYPE == MAX8
	// MAX6 does not support anything else than GPS
	if(gps_set_gps_only()) {
		TRACE_INFO("GPS  > Set GPS only OK");
	} else {
		TRACE_ERROR("GPS  > Set GPS only FAILED");
		status = 0;
	}
	#endif

	if(gps_set_airborne_model()) {
		TRACE_INFO("GPS  > Set airborne model OK");
	} else {
		TRACE_ERROR("GPS  > Set airborne model FAILED");
		status = 0;
	}
	if(gps_set_power_save()) {
		TRACE_INFO("GPS  > Configure power save OK");
	} else {
		TRACE_ERROR("GPS  > Configure power save FAILED");
		status = 0;
	}
	if(gps_power_save(0)) {
		TRACE_INFO("GPS  > Disable power save OK");
	} else {
		TRACE_ERROR("GPS  > Disable power save FAILED");
		status = 0;
	}

	return status;
}

void SystemSetupStuff(void)
{
    LPC_SC->PCONP |= PCONP_PCGPIO;              // power up GPIO
    LPC_GPIO1->FIODIR |= PIN(25);               // p1.25 Pwr on set output
    LPC_GPIO1->FIOPIN |= PIN(25);               // p1.25 Keep pwr on

    // Configure Power button
    LPC_PINCON->PINMODE3 |= (PINMODE_PULLDOWN << 24);      // Pullup
}

//----------------------------------------------------------------------------------------
//
//		 Odczyt po��wki bajtu z LCD (D4..D7)
//
//----------------------------------------------------------------------------------------
static inline uint8_t lcd_readHalf(void)
{
	uint8_t result=0;

	if(PIN(LCD_D4PORT)&(1<<LCD_D4)) result |= (1<<0);
	if(PIN(LCD_D5PORT)&(1<<LCD_D5)) result |= (1<<1);
	if(PIN(LCD_D6PORT)&(1<<LCD_D6)) result |= (1<<2);
	if(PIN(LCD_D7PORT)&(1<<LCD_D7)) result |= (1<<3);

	return result;
}

bool XML_ColorParser::HandleAttribute(const char *Tag, const char *Value)
{
	// Color value to be read in
	float CVal;
	
	if (NumColors > 0)
	{
	if (StringsEqual(Tag,"index"))
	{
		if (ReadBoundedInt16Value(Value,Index,0,NumColors-1))
		{
			IsPresent[3] = true;
			return true;
		}
		else return false;
	}
	}
	if (StringsEqual(Tag,"red"))
	{
		if (ReadFloatValue(Value,CVal))
		{
			IsPresent[0] = true;
			TempColor.red = uint16(PIN(65535*CVal+0.5,0,65535));
			return true;
		}
		else return false;
	}
	else if (StringsEqual(Tag,"green"))
	{
		if (ReadFloatValue(Value,CVal))
		{
			IsPresent[1] = true;
			TempColor.green = uint16(PIN(65535*CVal+0.5,0,65535));
			return true;
		}
		else return false;
	}
	else if (StringsEqual(Tag,"blue"))
	{
		float CVal;
		if (ReadFloatValue(Value,CVal))
		{
			IsPresent[2] = true;
			TempColor.blue = uint16(PIN(65535*CVal+0.5,0,65535));
			return true;
		}
		else return false;
	}
	UnrecognizedTag();
	return false;
}

int lassort(libmaus2::util::ArgParser const & arg, libmaus2::util::ArgInfo const &)
{
	std::string const outfilename = arg[0];
	std::vector<std::string> VI;
	for ( uint64_t i = 1 ; i < arg.size(); ++i )
		VI.push_back(arg[i]);
	int64_t const tspace = libmaus2::dazzler::align::AlignmentFile::getTSpace(VI);

	libmaus2::dazzler::align::AlignmentWriter::unique_ptr_type AW(
		new libmaus2::dazzler::align::AlignmentWriter(outfilename,tspace,false /* index */, 0 /* expt */)
	);

	libmaus2::dazzler::align::Overlap OVL;
	for ( uint64_t i = 0; i < VI.size(); ++i )
	{
		libmaus2::dazzler::align::AlignmentFileRegion::unique_ptr_type PIN(libmaus2::dazzler::align::OverlapIndexer::openAlignmentFileWithoutIndex(VI[i]));

		std::vector < libmaus2::dazzler::align::Overlap > VOVL;
		while ( PIN->getNextOverlap(OVL) )
		{
			if ( VOVL.size() && VOVL[0].aread != OVL.aread )
				handleVector(VOVL,*AW);
			VOVL.push_back(OVL);
		}
		handleVector(VOVL,*AW);
	}

	AW.reset();

	return EXIT_SUCCESS;
}

uint8_t s6b0108_inbyte(uint8_t rs)
{
	uint8_t x;

	if(rs) s6b0108_wait_ready();
	DDR(S6B0108_PDATA)=INPUT;
	PORT(S6B0108_PDATA)=0x00;
	_delay_us(0.3);
	if (rs)
	{	// reading data
		PORT(S6B0108_PCMD) |= (_BV(RW) | _BV(RS));
		_delay_us(0.2);
		//first access is to copy display data to display output register
		PORT(S6B0108_PCMD) |= _BV(E);
		_delay_us(2.0); 
		PORT(S6B0108_PCMD) &= ~_BV(E);
		_delay_us(3.0); // 0.5 should be enough but doesn't work
	}
	else
	{	// reading status
		PORT(S6B0108_PCMD) |= _BV(RW);
		PORT(S6B0108_PCMD) &= ~_BV(RS);
		_delay_us(0.2);
	}
	PORT(S6B0108_PCMD) |= _BV(E);
	_delay_us(2.0); // 0.32 should be enough but doesn't work
 	x = PIN(S6B0108_PDATA);
	_delay_us(0.2);
	PORT(S6B0108_PCMD) &= ~_BV(E);

	PORT(S6B0108_PCMD) &= ~( _BV(RS) | _BV(RW) | _BV(E) );
	return x;
}

uint8_t spi_rw_byte(uint8_t outgoing)
{
    uint8_t i, incoming;
    incoming = 0;

    //Send outgoing byte
    for(i = 0 ; i < 8 ; i++)
    {
        //send from MSB to LSB
        if(outgoing & 0b10000000)
	    PORT(MOSIPORT) |= BV(MOSIPIN);	   
        else
	    PORT(MOSIPORT) &= ~BV(MOSIPIN);
        
        PORT(SCKPORT) |= BV(SCKPIN);  //SPI_CLK = 1;
        _delay_us(RF_DELAY);

        //MISO bit is valid after clock goes going high
        incoming <<= 1;
        if( PIN(MISOPORT) & (1<<MISOPIN) ) incoming |= 0x01;

        PORT(SCKPORT) &= ~BV(SCKPIN);  //SPI_CLK = 0; 
        _delay_us(RF_DELAY);
        
        outgoing <<= 1;
    }

    return(incoming);
}

static void wait_ready(void)
{
    /*
    _delay_ms(1);
    return;
    */
    uint8_t flag;
    lcdPrepareRead();
    CMD_RS0();
    do
    {
        CMD_E_MARK();
        CMD_E_DELAY();
        flag = PIN(LCD_D7);
        CMD_E_RELEASE();
        CMD_E_DELAY();
        wdr();
    } while(flag);
    lcdPrepareWrite();
    /*
    uint16_t i;
    for(i=0; i<2000; i++)
        asm volatile ("nop");
    */
}

void interrupt_handler(IRQ_GPIO_EDGE1)
{
    uint8_t event_mask = 0; // This is a dummy var for compatible outline of call function.
    uint32_t inreg, old_inreg, mask, i;
    GPio_edge *TGpio = (GPio_edge*) SFRADR_GPIO_EDGE1;
    inreg       = TGpio->in;
    old_inreg   = TGpio->old_in;
    mask        = TGpio->mask;
    pin_id_t pin_id;

    //DPRINT ("INT in %02x old %02x", inreg, old_inreg);

    for(i=0; i<NUM_GPIOINT; i++)
    {
        if((gpio_callback[i] != 0x00) && (mask & 0x01))
        {
            if( (inreg&0x01) != (old_inreg&0x01) )
            {
                pin_id = PIN(SFRADR_GPIO_EDGE1, i);
                gpio_callback[i](pin_id, event_mask);
                break;
            }
        }

        inreg       >>= 1;
        old_inreg   >>= 1;
        mask        >>= 1;
    }
}

//20kHz
void TIM2_IRQHandler(void){
   TIM_ClearITPendingBit(TIM2,TIM_IT_Update);
   switch(hal.frt_state){
      case FRT_STOP:
         return;
      case FRT_CALC:
         hal.frt_state = FRT_STOP;
         hal.hal_state = FRT_TOO_LONG;
         hal.rt_state = RT_STOP;
         return;
      case FRT_SLEEP:
         if(hal.active_frt_func > -1){
            hal.frt_state = FRT_STOP;
            hal.hal_state = MISC_ERROR;
            hal.rt_state = RT_STOP;
            return;
         }
         hal.frt_state = FRT_CALC;
   }

   GPIO_SetBits(GPIOB,GPIO_Pin_9);

   static unsigned int last_start = 0;
   unsigned int start = SysTick->VAL;

   if(last_start < start){
     last_start += SysTick->LOAD;
   }

   float period = ((float)(last_start - start)) / RCC_Clocks.HCLK_Frequency;
   last_start = start;

   for(hal.active_frt_func = 0; hal.active_frt_func < hal.frt_func_count; hal.active_frt_func++){//run all fast realtime hal functions
      hal.frt[hal.active_frt_func](period);
   }
   hal.active_frt_func = -1;

   unsigned int end = SysTick->VAL;
   if(start < end){
     start += SysTick->LOAD;
   }
   PIN(frt_time) = ((float)(start - end)) / RCC_Clocks.HCLK_Frequency;
   PIN(frt_period_time) = period;

   hal.frt_state = FRT_SLEEP;
   GPIO_ResetBits(GPIOB,GPIO_Pin_9);
}

char Key_get_char(void){
	DDR(KPAD_PORT)=0b00001111;				//setting direction for output and input
	while((PIN(KPAD_PORT)&0xf0)==0xf0);		//wait until key is pressed
	_delay_ms(30);
	for (char i=0;i<4;i++){					//scanning for rows
		KPAD_PORT=~(1<<i);
		_delay_ms(10);
		for (char j=4;j<8;j++){				//now for columns 
			if (!(PIN(KPAD_PORT)&(1<<j))){	//checking for column key
				while((PIN(KPAD_PORT)&0xf0)!=0xf0);//waiting until key is released
				_delay_ms(10);
				KPAD_PORT=0;PIN(KPAD_PORT)=0xff;//initiating port and pin register for next scan
				return keys[i][j-4];			//returning values from array
			}
		}
	}
}

//=============================================================================
unsigned char debounce_sw_right(void)
{
  static uint16_t state3 = 0; //holds present state
  state3 = (state3 << 1) | (! bit_is_clear(PIN(BUTTON_RIGHT), BUTTON_RIGHT_LINE)) | 0xE000;
  if (state3 == 0xF000) return 1;
  if (state3 == 0xE000) return 2;
  return 0;
}

//=============================================================================
unsigned char debounce_sw_set(void)
{
  static uint16_t state2 = 0; //holds present state
  state2 = (state2 << 1) | (! bit_is_clear(PIN(BUTTON_SET), BUTTON_SET_LINE)) | 0xE000;
  if (state2 == 0xF000) return 1;
  if (state2 == 0xE000) return 2;
  return 0;
}