bool socket_receive_fds(int fd, std::vector<int> *fds)
{
    char dummy;

    struct iovec iov;
    iov.iov_base = &dummy;
    iov.iov_len = 1;

    std::size_t n_fds = fds->size();
    std::vector<char> control(sizeof(struct cmsghdr) + sizeof(int) * n_fds);

    struct msghdr msg;
    msg.msg_name = nullptr;
    msg.msg_namelen = 0;
    msg.msg_iov = &iov;
    msg.msg_iovlen = 1;
    msg.msg_flags = 0;
    msg.msg_control = control.data();
    msg.msg_controllen = control.size();

    struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg);
    cmsg->cmsg_len = msg.msg_controllen;
    cmsg->cmsg_level = SOL_SOCKET;
    cmsg->cmsg_type = SCM_RIGHTS;

    if (recvmsg(fd, &msg, 0) < 0) {
        return false;
    }

    int *data = reinterpret_cast<int *>(CMSG_DATA(cmsg));
    n_fds = (cmsg->cmsg_len - sizeof(struct cmsghdr)) / sizeof(int);
    if (n_fds != fds->size()) {
        // Did not receive correct amount of file descriptors
        return false;
    }

    for (std::size_t i = 0; i < n_fds; ++i) {
        (*fds)[i] = data[i];
    }

    return true;
}

void
process(Tokenrow *trp)
{
	int anymacros = 0;

	for (;;) {
		if (trp->tp >= trp->lp) {
			trp->tp = trp->lp = trp->bp;
			outp = outbuf;
			anymacros |= gettokens(trp, 1);
			trp->tp = trp->bp;
		}
		if (trp->tp->type == END) {
			if (--incdepth>=0) {
				if (cursource->ifdepth)
					error(ERROR,
					 "Unterminated conditional in #include");
				unsetsource();
				cursource->line += cursource->lineinc;
				trp->tp = trp->lp;
				genline();
				continue;
			}
			if (ifdepth)
				error(ERROR, "Unterminated #if/#ifdef/#ifndef");
			break;
		}
		if (trp->tp->type==SHARP) {
			trp->tp += 1;
			control(trp);
		} else if (!skipping && anymacros)
			expandrow(trp, NULL);
		if (skipping)
			setempty(trp);
		puttokens(trp);
		anymacros = 0;
		cursource->line += cursource->lineinc;
		if (cursource->lineinc>1) {
			genline();
		}
	}
}

// Expand simple expressions like new int[3][5] and new Object[2][nonConLen].
// Also handle the degenerate 1-dimensional case of anewarray.
Node* Parse::expand_multianewarray(ciArrayKlass* array_klass, Node* *lengths, int ndimensions, int nargs) {
  Node* length = lengths[0];
  assert(length != NULL, "");
  Node* array = new_array(makecon(TypeKlassPtr::make(array_klass)), length, nargs);
  if (ndimensions > 1) {
    jint length_con = find_int_con(length, -1);
    guarantee(length_con >= 0, "non-constant multianewarray");
    ciArrayKlass* array_klass_1 = array_klass->as_obj_array_klass()->element_klass()->as_array_klass();
    const TypePtr* adr_type = TypeAryPtr::OOPS;
    const TypeOopPtr*    elemtype = _gvn.type(array)->is_aryptr()->elem()->make_oopptr();
    const intptr_t header   = arrayOopDesc::base_offset_in_bytes(T_OBJECT);
    for (jint i = 0; i < length_con; i++) {
      Node*    elem   = expand_multianewarray(array_klass_1, &lengths[1], ndimensions-1, nargs);
      intptr_t offset = header + ((intptr_t)i << LogBytesPerHeapOop);
      Node*    eaddr  = basic_plus_adr(array, offset);
      store_oop_to_array(control(), array, eaddr, adr_type, elem, elemtype, T_OBJECT);
    }
  }
  return array;
}

int Jump(int stgnum,Agent_status *agent, node_t *trees,int auto_or_mamual,Girl *Girl_status)
{
    //慣性を監視
    //    mvprintw(0,0,"intertia = %d",agent->INTERTIA);

    int highest = 5;

    for(;highest > 0;highest--)
    {
        if(mvinch(agent->Y-3,agent->X) != 'I')
        {
            agent->Y -= 1;
            agent->Jumpflag = 1;
        }
        moveRoL(agent);
        control(stgnum,agent,trees,auto_or_mamual,Girl_status);
        //            usleep(50000);
    }
    return 0;
}

void AgentCore::algorithmCallback(const ros::TimerEvent &timer_event) {
  consensus();  // also clears the received statistics container
  control();  // also publishes virtual agent pose and path
  guidance();
  dynamics();  // also publishes agent pose and path

  waitForSlotTDMA(slot_tdma_*agent_id_);  // sync to the next transmission TDMA slot (agent dependent)

  // publishes the last estimated statistics in the proper TDMA slot
  formation_control::FormationStatisticsStamped msg;
  msg.header.frame_id = agent_virtual_frame_;
  msg.header.stamp = ros::Time::now();
  msg.agent_id = agent_id_;
  msg.stats = estimated_statistics_;
  stats_publisher_.publish(msg);

  std::stringstream s;
  s << "Estimated statistics published.";
  console(__func__, s, DEBUG);
}

void casede(void)
{
	int i, req;
	Offset savoff;

	req = '.';
	lgf++;
	skip();
	if ((i = getrq()) == 0)
		goto de1;
	if ((offset = finds(i)) == 0)
		goto de1;
	if (newmn)
		savslot = newmn;
	else
		savslot = findmn(i);
	savname = i;
	if (ds)
		copys();
	else
		req = copyb();
	clrmn(oldmn);
	if (newmn) {
		if (contabp[newmn].rq)
			munhash(&contabp[newmn]);
		contabp[newmn].rq = i;
		maddhash(&contabp[newmn]);

	}
	if (apptr) {
		savoff = offset;
		offset = apptr;
		wbf((Tchar) IMP);
		offset = savoff;
	}
	offset = dip->op;
	if (req != '.')
		control(req, 1);
de1:
	ds = app = 0;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, ROS_PACKAGE_NAME);
  ros::NodeHandle node;

  ros::Publisher cmd_pub = node.advertise<ackermann_msgs::AckermannDriveStamped>(
      "ackermann_cmd",
      1000
    );
  ros::Publisher cte_pub = node.advertise<std_msgs::Float64>(
      "cte",
      1000
    );
  aav_control::QuinticControl control(&cmd_pub, &cte_pub);
  /*
  ros::Subscriber sub = node.subscribe(
      "odometry/filtered",
      1000,
      &aav_control::QuinticControl::updateOdometry,
      &control
    );
  */
  aav_control::GazeboStateForwarder forwarder(control, "car");
  ros::Subscriber sub = node.subscribe(
      "/gazebo/model_states",
      1000,
      &aav_control::GazeboStateForwarder::forwardState,
      &forwarder
    );

  actionlib::SimpleActionServer<aav_msgs::DoQuinticPathAction> server(
      node,
      "control",
      boost::bind(&aav_control::QuinticControl::updateGoal, &control, _1),
      false
    );
  server.start();

  ros::spin();
  return 0;
}

int main (void)
{
  int fd = initMW();

  ThinkGearStreamParser ctx;
  initParser(&ctx);

#ifdef OUTPUT_UDP
  initUDP();
#endif

  int res, i, n;
  unsigned char bytes[BUFSIZE];
  while (1) {
    n = read (fd, bytes, BUFSIZE); // read up to BUFSIZE bytes from the device

    if (n > 0) {
#ifdef __DEBUG
      fprintf (stderr, "%i bytes read\n", n);
#endif
      for (i = 0; i < n; i++) {
        res = THINKGEAR_parseByte(&ctx, bytes[i]);
        if (res < 0) {
          fprintf (stderr, "error parsing byte: %i\n", res);
          initParser(&ctx);
          continue;
        }
        control();  // output robot control char
      }
    } else
    if (n < 0) {
      fprintf (stderr, "error %d reading %s: %s\n", errno, MINDWAVEPORT,
        strerror (errno));
      return (1);
    }

    usleep ((n * 100) + 1000);  // sleep approx 100 uS per char transmit + 1kuS
  }

  return (0);
}

void *elfloader(char *path)
{
	
	/*Load the program into memory first*/

	int fd = open(RFILESYS,path, "or");
	if (fd == SYSERR)
	{
		kprintf("Could not open the file\n");
		return (void *)SYSERR;
	}
	
	int32 filesize = control(RFILESYS, RFS_CTL_SIZE, fd, 0);

	char *filestart;
	
	filestart = getmem(filesize);

	if(filestart == (char *)SYSERR)
	{
		kprintf("Not enough memory to load the entire file\n");
		return (void *)SYSERR;
	}


	int rc = read(fd, filestart, filesize);

	if(rc == filesize)
	{
		kprintf("successfully read the file into memory at %u\n");
	}
	else
	{
		kprintf("Error reading file into memory\n");
		close(fd);
		return (void *)SYSERR;
	}

	close(fd);
return (void *)filestart;
}

/* MotionController::advance: advance motion controller by the given frame, return true when reached end */
bool MotionController::advance(double deltaFrame)
{
   if (m_boneCtrlList == NULL && m_faceCtrlList == NULL)
      return false;

   /* apply motion at current frame to bones and faces */
   control((float) m_currentFrame);

   /* advance the current frame count */
   /* store the last frame to m_previousFrame */
   m_previousFrame = m_currentFrame;
   m_currentFrame += deltaFrame;
   if (m_currentFrame >= m_maxFrame) {
      /* we have reached the last key frame of this motion */
      /* clamp the frame to the maximum */
      m_currentFrame = m_maxFrame;
      /* return finished status */
      return true;
   }
   return false;
}

void startup(void)
{
	printf("System is Starting...\n");

	printf("Initializing Serial Port...\n");
	if(roboLinkInit()>0)
	{
		printf("RoboLink is Ready.\n");
	}
	else
	{
		printf("RoboLink Init Failed.\n");
	}
	boost::thread udp(&udpServerTask);
	printf("UDP Server is Running!\n");
	boost::thread tcp(&tcpServerTask);
	printf("TCP Server is Running!\n");
	boost::thread control(&roboControlLoop);
	printf("Robot Control Loop is Running!!");
	triZero();
}

TEST_F(LearningUtilsTest, CC7BitKnob) {
    // Standard CC 7-bit knobs show up as a MIDI_CC message, single channel,
    // single control and a variety of values in the range of 0x00 to 0x7F.
    // Status: 0x81 Control: 0x01
    addMessage(MIDI_CC | 0x01, 0x10, 0x7F);
    addMessage(MIDI_CC | 0x01, 0x10, 0x70);
    addMessage(MIDI_CC | 0x01, 0x10, 0x60);
    addMessage(MIDI_CC | 0x01, 0x10, 0x50);
    addMessage(MIDI_CC | 0x01, 0x10, 0x60);
    addMessage(MIDI_CC | 0x01, 0x10, 0x50);
    addMessage(MIDI_CC | 0x01, 0x10, 0x40);

    ConfigKey control("[Test]", "SomeControl");
    MidiInputMappings mappings =
            LearningUtils::guessMidiInputMappings(control, m_messages);

    ASSERT_EQ(1, mappings.size());
    EXPECT_EQ(MidiInputMapping(MidiKey(MIDI_CC | 0x01, 0x10),
                               MidiOptions(), control),
              mappings.at(0));
}

int main(int argc, char *argv[])
{
	QApplication app(argc, argv);

	control();

	ControlDlg *controlDlg = new ControlDlg;
	StatusDlg *statusDlg = new StatusDlg(NULL, controlDlg);

	QTimer screenUpdate;
	QObject::connect(&screenUpdate, SIGNAL( timeout(void) ), controlDlg, SLOT ( updateScreen(void) ));
	QObject::connect(&screenUpdate, SIGNAL( timeout(void) ), statusDlg, SLOT ( updateScreen(void) ));

	screenUpdate.setSingleShot(false);
	screenUpdate.start(100);

	statusDlg->show();
	return app.exec();

	return 0;
}

void or(char *rs,char *rt,char *rd)
{
  int j = 0;
  char address_buffer[33];
  opcode = "000000";
  shamt = "00000";
  function = "100101";

  sscanf(rs, "%d", &j);
  to_Binary(j,5,"rs");

  sscanf(rt, "%d", &j);
  to_Binary(j,5,"rt");
  
  sscanf(rd, "%d", &j);
  to_Binary(j,5,"rd");

  snprintf( address_buffer, sizeof( address_buffer ), "%s%s%s%s%s%s", opcode, rs_out, rt_out, rd_out, shamt, function);
  strcpy(address, address_buffer);
  control();
}

void
CameraTrigger::start()
{
	// enable immediate if configured that way
	if (_mode == 2) {
		control(true);
	}

	// Prevent camera from sleeping, if triggering is enabled
	if (_mode > 0 && _mode < 4) {
		turnOnOff();
		keepAlive(true);

	} else {
		keepAlive(false);
	}

	// start to monitor at high rate for trigger enable command
	work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline, this, USEC2TICK(1));

}

/***** The next 3 functions are the worker threads
 */
void * lockthr(void * arg)
{
	int ret;
	char loc; /* Local value for control */
	cell_t * c = (cell_t *)arg;
	
	/* Set the thread local data key value (used in the signal handler) */
	ret = pthread_setspecific(_c, arg);
	if (ret != 0)
	{  UNRESOLVED(ret, "Unable to assign the thread-local-data key");  }
	
	/* Signal we're started */
	ret = pthread_mutex_lock(&(c->tmtx));
	if (ret != 0)  {  UNRESOLVED(ret, "Failed to lock the mutex");  }
	c->tcnt += 1;
	ret = pthread_mutex_unlock(&(c->tmtx));
	if (ret != 0)  {  UNRESOLVED(ret, "Failed to unlock the mutex");  }
	
	do
	{
		/* Lock, control, then unlock */
		ret = pthread_mutex_lock(&(c->mtx));
		if (ret != 0)
		{  UNRESOLVED(ret, "Mutex lock failed in worker thread");  }

		control(c, &loc);
			
		ret = pthread_mutex_unlock(&(c->mtx));
		if (ret != 0)
		{  UNRESOLVED(ret, "Mutex unlock failed in worker thread");  }
		
		/* Increment the operation counter */
		c->opcnt++;
	}
	while (do_it);
	
	/* Wait for the signal thread to terminate before we can exit */
	waitsigend(c);
	return NULL;
}

bool ChangeLocationForm::handleEvent(EventType& event)
{
    bool handled=false;
    switch (event.eType)
    {
        case ctlSelectEvent:
            handleControlSelect(event);
            handled=true;
            break;
            
        case keyDownEvent:
            if (chrCarriageReturn==event.data.keyDown.chr || chrLineFeed==event.data.keyDown.chr)
            {
                Control control(*this, okButton);
                control.hit();
            }
            break;
            
        case MoriartyApplication::appSetWeatherEvent:
            //change form to be weatherSelectLocationForm
            whenOk_ = weatherMode;            
            Preferences& prefs=application().preferences();
            const String& location = prefs.weatherPreferences.weatherLocation;
            if (!prefs.weatherPreferences.weatherLocation.empty())
            {
                locationField_.setEditableText(prefs.weatherPreferences.weatherLocation);
                locationField_.select();
            }
            else
            {
                locationField_.setEditableText(_T(""));
            }
            handled = true;
            break;
    
        default:
            handled=MoriartyForm::handleEvent(event);
    }
    return handled;
}

int32 main()
{
	const int32 sampleMinute = 60000 / 10000;
	const int32 numberOfDays = 3;


	// Initialize class HeathRegulationControl 
	HeathRegulationControl control(sampleMinute, numberOfDays);				

	// read adress and port for ADAMS
	control.readInputFile();
	
	// make connection to the ADAMS
	control.makeConnection();
	
	// Initialize start values
	control.startUpMode();										
	
	control.closeConnection();
	//  wait 30 seconds
	Sleep(30000);
	


	// loop for regulation of the temperatures in the cabinets
	while(1) 
	{

		// control the temperatures of the Cabinets
		control.controlsTemperatures();
		
		//  wait 10 seconds (an minute  is = 60000)
		Sleep (10000);
	}

		
	control.~HeathRegulationControl();

	return 0;
}

//**********************************************
////////////////////////////////////////////////
////////////////////////////////////////////////
//*******************MAIN FUNCTION**************
int main()
{
    pc.baud(115200);
 
	Control control(0x07, 0x08, 0x09);
    while(true)
    {
        if(pc.readable()){
            uint8_t command = pc.getc();
            switch(command)
            {
                case '1':                    
                    stepTimer.reset();
                    stepTimer.start();
                    isWalking = true;
                    startStopLED = 1;
                    break;
                case '2':                    
                    stepTimer.stop();
                    isWalking = false;
                    startStopLED = 0;
                    stepLED = 0;
                    mx28.SetEnableTorque(0x07,0);
                    mx28.SetEnableTorque(0x08,0);
                    mx28.SetEnableTorque(0x09,0);
                    break;
            }
        }
	
        if(isWalking){
            int timerValue = stepTimer.read_us();
            if(timerValue > updateInterval){
                stepLED = !stepLED;
				control.Control_all(0x0020);
                stepTimer.reset();
            }
        }
        
    }
}

void timestuff(int rate, void (*control) (int), void (*draw) (void), int maxtime)
{
    int waitmode = 0, t;
    tl_timer *timer;
    bbupdate();
    /*starttime = TIME; */
    endtime = starttime + maxtime;
    timer = tl_create_timer();
    if (control == NULL) {
	rate = -40;
    }
    if (rate < 0) {
	waitmode = 1, rate = -rate;
	control1 = control;
	tl_set_multihandler(timer, mycontrol, NULL);
    }
    else
	tl_set_multihandler(timer, mycontrol2, control);
    tl_set_interval(timer, 1000000 / rate);
    tl_add_timer(syncgroup, timer);
    tl_reset_timer(timer);
    tl_slowdown_timer (timer, starttime - TIME);
    if (control != NULL)
	control(1);
    while (!finish_stuff && TIME < endtime) {
	called = 0;
	bbupdate();
	t = tl_process_group(syncgroup, NULL);
	if (TIME > endtime)
	    break;
	if (!called && waitmode)
	    tl_sleep(t);
	else {
	    if (draw != NULL)
		draw();
	}
    }
    starttime = endtime;
    tl_free_timer(timer);
}