inline void motors_init (void) {
    set_output(MOTOR0_DDR, MOTOR0_STEP_DD);
    set_output(MOTOR0_DDR, MOTOR0_DIR_DD);
    output_low(MOTOR0_PORT, MOTOR0_STEP);
    output_low(MOTOR0_PORT, MOTOR0_DIR);

    set_output(MOTOR1_DDR, MOTOR1_STEP_DD);
    set_output(MOTOR1_DDR, MOTOR1_DIR_DD);
    output_low(MOTOR1_PORT, MOTOR1_STEP);
    output_low(MOTOR1_PORT, MOTOR1_DIR);

    set_output(MOTOR2_DDR, MOTOR2_STEP_DD);
    set_output(MOTOR2_DDR, MOTOR2_DIR_DD);
    output_low(MOTOR2_PORT, MOTOR2_STEP);
    output_low(MOTOR2_PORT, MOTOR2_DIR);

    set_output(MOTOR3_DDR, MOTOR3_STEP_DD);
    set_output(MOTOR3_DDR, MOTOR3_DIR_DD);
    output_low(MOTOR3_PORT, MOTOR3_STEP);
    output_low(MOTOR3_PORT, MOTOR3_DIR);

    set_output(MOTOR4_DDR, MOTOR4_STEP_DD);
    set_output(MOTOR4_DDR, MOTOR4_DIR_DD);
    output_low(MOTOR4_PORT, MOTOR4_STEP);
    output_low(MOTOR4_PORT, MOTOR4_DIR);

    return;
}

static void timer_handler(int sig, siginfo_t *si, void *uc) {
    struct node_data *data = si->si_value.sival_ptr;
    struct timeval  tv;
    static unsigned long lasttime = 0;
    static unsigned long cumulative = 0;
    unsigned long newtime;
    char percent;
    int i;

    gettimeofday(&tv, NULL);
    newtime = (tv.tv_sec) * 1000 + (tv.tv_usec) / 1000;

    /* calculate how far into the period we are */
    cumulative += (newtime - lasttime);
    lasttime = newtime;
    if (cumulative >= PWM_PERIOD)
        cumulative = 0;
    percent = 100 * cumulative / PWM_PERIOD;

    DEBUG("Output timer sig=%d: newtime=%ld cumulative=%ld percent=%d\n", sig, newtime, cumulative, percent);
    for (i=0; i< NUM_NODES; i++){
        if (data[i].output.power > percent && ! data[i].output.state)
            set_output(&data[i], ON);
        else if (data[i].output.power < percent && data[i].output.state)
            set_output(&data[i], OFF);
    }
}

int main(void) {
  // initialize the direction of PORTD #6 to be an output
  set_output(DDRC, LED1);  
  set_output(DDRC, LED2);
  set_output(DDRC, LED3);
  set_output(DDRD, LED4);

  while (1) {
    output_high(PORTC, LED1);
    delay_ms(200);
    output_high(PORTC, LED2);
    delay_ms(200);
    output_high(PORTC, LED3);
    delay_ms(200);
    output_high(PORTD, LED4);
    delay_ms(200);

    output_low(PORTC, LED1);
    delay_ms(200);
    output_low(PORTC, LED2);
    delay_ms(200);
    output_low(PORTC, LED3);
    delay_ms(200);
    output_low(PORTD, LED4);
    delay_ms(200);
  }
}

static inline void mode_strobe(void) {
  while(1) {
    set_output(STROBE_ON_OUT, 0);
    delay_5ms(strobe_delay);
    set_output(STROBE_OFF_OUT, 0);
    delay_5ms(strobe_delay);
  }
}

//Target side: true for side 1, false for side 2
void pick_side(bool targetSide){
	if(targetSide){
		set_output(false,SIDE_1_ENABLE);
		set_output(true,SIDE_2_ENABLE);
	}else{
		set_output(false,SIDE_2_ENABLE);
		set_output(true,SIDE_1_ENABLE);
	}
}

void pick_input(uint8_t target){
	if(target <= 3){
		bool bit_low = target & 0x01;
		
		bool bit_high = target & 0x02;
	
		set_output(bit_low, BIT_0_CONTROL);
		set_output(bit_high, BIT_1_CONTROL);
	}
}

int main(void)
{
    // Lights, backlights, and fan initialization
    set_output(F1DD,F1PIN);
    set_output(F2DD,F2PIN);
    set_output(LDD,LPIN);
    set_output(BLDD,BLPIN);
    set_high(LPORT,BLPIN);

    TCCR0A = 0xA3;					// Timer0, channels A and B to Fast PWM Mode
    TCCR2A = 0x23;					// Timer2, channel B to Fast PWM Mode
    TCCR0B = 0x05;					// Prescaler is 1024 to give MOSFETS plenty of time
    TCCR2B = 0x07;
    OCR0A = 0xFF;					// Rev up fans for 2s to avoid stalling
    OCR0B = 0xFF;
    _delay_ms(2000);
    OCR0A = 0x7F;					// Reduce fans to 50% power (127)
    OCR0B = 0x7F;
    uchar i;
    for (i=0; i<3; i++) {				// Blink lights to signal life
        OCR2B = 0xFF;
        _delay_ms(200);
        OCR2B = 0x00;
        _delay_ms(200);
    }
    OCR2B = 0x14;					// Init lights at low brightness (20)


    // LCD initialization
    _delay_ms(200);
    CHA_LCDinit();
    CHA_LCDclr();
    CHB_LCDinit();
    CHB_LCDclr();
    CHA_LCDstringLine(lcdinit,0);	// LCD ready to go!

    // USB initialization
    wdt_enable(WDTO_1S); 			// Enable 1s watchdog timer
    usbInit();
    usbDeviceDisconnect(); 			// Enforce re-enumeration
    for(i = 0; i<200; i++) { 		// Wait 200 ms
        wdt_reset();				// Keep the watchdog happy
        _delay_ms(1);
    }
    usbDeviceConnect();
    sei(); 							// Enable interrupts after re-enumeration
    CHB_LCDstringLine(usbinit,0);	// USB ready to go!

    while(1) {						// Feed watchdog and poll USB forever
        wdt_reset();
        usbPoll();
    }

    return 0;
}

void javax_wukong_wknode_WuNodeLeds_void__init()
{
	set_output(DDRK, 0);
	set_output(DDRK, 1);
	set_output(DDRK, 2);
	set_output(DDRK, 3);
	output_high(PORTK, 0);
	output_high(PORTK, 1);
	output_high(PORTK, 2);
	output_high(PORTK, 3);
}

/* Init on board LEDs */
void led_init() {
	drive(LED1);
	drive(LED2);

	set_output(LED1);
	set_output(LED2);

	delay_1s();

	clr_output(LED1);
	clr_output(LED2);
}

  bool extract_boundary::run(viennamesh::algorithm_handle &)
  {
    mesh_handle input_mesh = get_required_input<mesh_handle>("mesh");
    mesh_handle output_mesh = make_data<mesh_handle>();

    point_container hole_points;
    seed_point_container seed_points;

    std::cerr << "Extract boundary of " << input_mesh.size() << " partitions" << std::endl;
    //MY IMPLEMENTATION
    output_mesh.resize(input_mesh.size());
    for (size_t i = 0; i < input_mesh.size(); ++i)
    {

      hole_points.clear();
      seed_points.clear();

      input_mesh(i).set_full_layout();
      viennagrid::extract_boundary( input_mesh(i), output_mesh(i), viennagrid::facet_dimension(input_mesh(i)) );
      viennagrid::extract_seed_points( input_mesh(i), seed_points );
  //     viennagrid::extract_hole_points( input_mesh(), hole_points );

     // set_output( "mesh", output_mesh ); //MY IMPLEMENTATION

  //     if (!hole_points.empty())
  //     {
  //       info(1) << "Extracted " << hole_points.size() << " hole points" << std::endl;
  //       for (point_container::const_iterator it = hole_points.begin(); it != hole_points.end(); ++it)
  //         info(1) << "   " << *it << std::endl;
  //
  //       point_handle output_hole_points = make_data<point>();
  //       output_hole_points.set( hole_points );
  //       set_output( "hole_points", output_hole_points );
  //     }

      if (!seed_points.empty())
      {
        info(1) << "Extracted " << seed_points.size() << " seed points" << std::endl;
        for (seed_point_container::const_iterator it = seed_points.begin(); it != seed_points.end(); ++it)
          info(1) << "   " << (*it).first << " -> " << (*it).second << std::endl;


        seed_point_handle output_seed_points = make_data<seed_point>();
        output_seed_points.set( seed_points );
        set_output( "seed_points", output_seed_points );
      }
    }//end of for over input_mesh.size()


    set_output( "mesh", output_mesh );
    return true;
  }

irom static void trigger_timer(gpio_t *gpio, bool_t onoff)
{
	const gpio_config_entry_t *cfg = get_config(gpio);

	if(onoff)
	{
		set_output(gpio, cfg->timer.direction == gpio_up ? 1 : 0);
		gpio->timer.delay = cfg->timer.delay;
	}
	else
	{
		set_output(gpio, cfg->timer.direction == gpio_up ? 0 : 1);
		gpio->timer.delay = 0;
	}
}

/**
   @brief Called by the parser when it is in one of the UESC states.
   @param a Parser data.
   @param wc Character.
 */
static void json_string_uesc(struct parser_arg *a, wchar_t wc)
{
  if (wc == L'\0') {
    set_state(a, END);
    a->error = JSONERR_PREMATURE_EOF;
    a->textidx--;
  } else if (json_xdigit(wc) == 0xFF) {
    set_state(a, END);
    a->error = JSONERR_UNEXPECTED_TOKEN;
    a->textidx--;
  } else {
    a->curr = a->curr << 4;
    a->curr |= json_xdigit(wc);
    if (a->state < UESC3) {
      // continue reading all the input
      a->state += 1;
    } else {
      // time to "publish" our unicode escape
      if (a->prev == 0) {
        // if there was no "prev", that means this might be the start of a
        // surrogate pair.  Check for that!
        if (0xD800 <= a->curr && a->curr <= 0xDFFF) {
          // yup, it's a surrogate pair!
          a->prev = a->curr;
        } else {
          // nope, keep going
          set_output(a, a->curr);
        }
      } else {
        // there was a previous starting surrogate
        if (0xD800 <= a->curr && a->curr <= 0xDFFF) {
          // and this is also a surrogate
          a->curr &= 0x03FF; // clear upper bits; keep lower 10
          a->curr |= (a->prev & 0x03FF) << 10;
          a->curr += 0x10000; // apparently this needs to happen (?)
          a->prev = 0;
          set_output(a, a->curr);
        } else {
          // not a legal surrogate to match previous surrogate.
          a->state = END;
          a->error = JSONERR_INVALID_SURROGATE;
        }
      }
      set_state(a, INSTRING);
      a->curr = 0;
    }
  }
}

 bool hts_engine_call::execute()
 {
   set_input();
   set_output();
   engine_impl->synthesize();
   return !output.is_stopped();
 }

void DISCRETE_CLASS_FUNC(dss_input_stream, input_write)(int sub_node, UINT8 data )
{
	UINT8 new_data    = 0;

	new_data =  data;

	if (m_data != new_data)
	{
		if (m_is_buffered)
		{
			/* Bring the system up to now */
			m_buffer_stream->update();

			m_data = new_data;
		}
		else
		{
			/* Bring the system up to now */
			m_device->update_to_current_time();

			m_data = new_data;

			/* Update the node output here so we don't have to do it each step */
			set_output(0,  new_data * m_gain + m_offset);
		}
	}
}

static void start_pulse(const device_config *device)
{
	ttl74123_t *chip = get_safe_token(device);

	attotime duration = compute_duration(chip);

	if (timer_running(chip))
	{
		/* retriggering, but not if we are called to quickly */
		attotime delay_time = attotime_make(0, ATTOSECONDS_PER_SECOND * chip->intf->cap * 220);

		if (attotime_compare(timer_timeelapsed(chip->timer), delay_time) >= 0)
		{
			timer_adjust_oneshot(chip->timer, duration, 0);

			if (LOG) logerror("74123 %s:  Retriggering pulse.  Duration: %f\n", device->tag, attotime_to_double(duration));
		}
		else
		{
			if (LOG) logerror("74123 %s:  Retriggering failed.\n", device->tag);
		}
	}
	else
	{
		/* starting */
		timer_adjust_oneshot(chip->timer, duration, 0);

		set_output(device);

		if (LOG) logerror("74123 %s:  Starting pulse.  Duration: %f\n", device->tag, attotime_to_double(duration));
	}
}

//! set parameter 
void sci_psk_dem::set(int param, var* p_v)
{
 int iv;
 double v;
 var_vec *p_var_vec;
 var_ivec *p_var_ivec;

	p_var_vec = NULL;
	p_var_ivec = NULL;
	// p_v might be either var_vec, var_ivec or var_bvec
	p_var_vec = dynamic_cast<var_vec *> (p_v);
	if (p_var_vec ==  NULL) {
			p_var_ivec = dynamic_cast<var_ivec *> (p_v);
			if (p_var_ivec == NULL) {
				throw sci_exception ("sci_psk_dem::set - p_v -  bad cast");
			}
	}

	switch (param)
	{		
	case SCI_SIZE:		// size constellation
		if (p_var_vec) {	
			iv = (int)p_var_vec->v[0];
		}
		else if (p_var_ivec) {			
			iv = p_var_ivec->v[0];
		}			
		else {
			throw sci_exception ("sci_psk_dem::set - SCI_SIZE -  bad cast");
		}
		set_size(iv);
	break;

	case SCI_SCALE:
		if (p_var_vec) {	
			v = p_var_vec ->v[0];
		} else {
			throw sci_exception ("sci_psk_dem::set - SCI_SCALE - bad cast");
		}
		set_scale(v);
	break;

	case SCI_OUTPUT:
		if (p_var_vec) {	
			iv = (int)p_var_vec->v[0];
		}
		else if (p_var_ivec) {			
			iv = p_var_ivec->v[0];
		}			
		else {
			throw sci_exception ("sci_psk_dem::set -  SCI_OUTPUT - bad cast");
		}
		set_output( iv );					
	break;

	default:
		throw sci_exception ("sci_psk_mod::set - unknown param");
	}
	return;
};

int main(const int argc, const char* const argv[]) {
  if (argc == 1) show_usage();
  const std::string filename = argv[1];
  if (filename == "-v" or filename == "--version") version_information();
  if (argc >= 3 and filename == std::string(argv[2])) {
      errout << "Output filename: \"" << argv[2]  << "\" identical with input filename.";
      std::exit(code(Error::file_writing));
  }
  if (argc >= 4 and filename == std::string(argv[3])) {
      errout << "Log filename: \"" << argv[3]  << "\" identical with input filename.";
      std::exit(code(Error::file_writing));
  }
  const InOut::ConformityLevel conlev = (argc >= 5) ? InOut::s2conlev(argv[4]) : InOut::ConformityLevel::normal;
  const LogLevel loglev = (argc >= 6) ? s2loglev(argv[5]) : LogLevel::normal;

  set_output(argc, argv, solout, logout, errout);
  const InOut::InputStream in(filename, errout);
  Input::ReadDimacs rd(*in, errout, conlev);
  const ClauseSets::DClauseSet& F = rd();

  const Encodings::Encoding enc(F);

  const Translations::Translation trans(F,enc);
  const ClauseSets::CLS G = trans();

  output(filename, conlev, F, enc, trans, G, loglev);
}

void main(void) 
{
    struct puzzleStruct puzzle;
    struct buttonStruct buttons;
    bool result = true;
    //init code here
    setup_int();
    init_puzzle(&puzzle);
    init_buttons(&buttons);
    init_inputs();

    //Enter main loop
    for(;;) {
        //Draw GUI
        build_gui(&puzzle);
        //Handle USB
        update_puzzle(&puzzle,&buttons);
        //Set output
        set_output(&puzzle);
        //Check inputs
        update_buttons(&buttons);
        //Read result
        result = check_result(&puzzle);
    }
}

Data_Exception::Data_Exception(const string &comment) {
	_min = 0;
	_max = 0;
	_value = 0;
	_comment = string(comment);
	set_output();
}

static msg_t uart_thread(void *arg) {
	(void)arg;

	chRegSetThreadName("UART");

	uartStart(&HW_UART_DEV, &uart_cfg);
	palSetPadMode(HW_UART_TX_PORT, HW_UART_TX_PIN, PAL_MODE_ALTERNATE(HW_UART_GPIO_AF) |
			PAL_STM32_OSPEED_HIGHEST |
			PAL_STM32_PUDR_PULLUP);
	palSetPadMode(HW_UART_RX_PORT, HW_UART_RX_PIN, PAL_MODE_ALTERNATE(HW_UART_GPIO_AF) |
			PAL_STM32_OSPEED_HIGHEST |
			PAL_STM32_PUDR_PULLUP);

	systime_t time = chTimeNow();

	for(;;) {
		time += MS2ST(1);

		if ((systime_t) ((float) chTimeElapsedSince(last_uart_update_time)
				/ ((float) CH_FREQUENCY / 1000.0)) > (float)TIMEOUT) {
			mcpwm_set_brake_current(-10.0);
		} else {
			set_output(out_received);
		}

		chThdSleepUntil(time);
	}

	return 0;
}