From 01a2b59bd82919d7ce87eeeda3783283e52ed90e Mon Sep 17 00:00:00 2001
From: Petr Silhavik <silhavik.p@gmail.com>
Date: Sat, 22 Dec 2012 20:54:26 +0100
Subject: [PATCH] robofsm/test: Transform rectangle to C++

---
 src/robofsm/test/rectangle.cc | 123 ++++++++++++++--------------------
 1 file changed, 50 insertions(+), 73 deletions(-)

diff --git a/src/robofsm/test/rectangle.cc b/src/robofsm/test/rectangle.cc
index 53e55a5c..a6670b5f 100644
--- a/src/robofsm/test/rectangle.cc
+++ b/src/robofsm/test/rectangle.cc
@@ -8,7 +8,7 @@
  * License: GNU GPL v.2
  */
 
-#define FSM_MAIN
+//#define FSM_MAIN
 #include <movehelper.h>
 #include <robot.h>
 #include <trgen.h>
@@ -16,26 +16,36 @@
 #include <string.h>
 #include <error.h>
 #include <ul_log.h>
+#include <events.h>
+#include <boost/statechart/asynchronous_state_machine.hpp>
+#include <boost/statechart/custom_reaction.hpp>
+#include <boost/statechart/transition.hpp>
+#include "../timedFSM.h"
+#include "../robot.h"
 
 UL_LOG_CUST(ulogd_rectangle); /* Log domain name = ulogd + name of the file */
 
-FSM_STATE_DECL(rectangle);
-FSM_STATE_DECL(turn);
-FSM_STATE_DECL(wait);
-
-FSM_STATE(init) {
-	/* Where we are at the begining? */
-	switch (FSM_EVENT) {
-		case EV_ENTRY:
-			FSM_TIMER(10);
-			break;
-		case EV_TIMER:
-			FSM_TRANSITION(rectangle);
-			robot_set_est_pos_trans(0.5, 0.5, DEG2RAD(0));
-			break;
-		default:;
+struct rectangle;
+struct Wait;
+struct init;
+
+struct FSMMain : boost::statechart::asynchronous_state_machine<FSMMain, init, Scheduler> {
+	FSMMain(my_context ctx) : my_base(ctx) {
+		printf("%s\n", __FUNCTION__);
 	}
-}
+};
+
+struct init : TimedState<init, FSMMain> {
+	Timer timeTest;
+	init(my_context ctx) : base_state(ctx) {
+		runTimer(timeTest, 10, new EV_TIMER());
+	}
+	sc::result react (const EV_TIMER &) {
+		robot.set_est_pos_trans(0.5, 0.5, DEG2RAD(0));
+		return transit<rectangle>();
+	}
+	typedef sc::custom_reaction<EV_TIMER> reactions;
+};
 
 
 void follow_rectangle()
@@ -53,92 +63,59 @@ void follow_rectangle()
 
 	/* Allocate new trajectory */
 	if (!backward)
-		robot_trajectory_new(&tc);
+		robot.move_helper.trajectory_new(&tc);
 	else
-		robot_trajectory_new_backward(&tc);
+		robot.move_helper.trajectory_new_backward(&tc);
 
 	unsigned i, j;
 	for (i=0; i<sizeof(rectangle)/sizeof(rectangle[0]); i++) {
 		if (!backward) j=(i+1)%4;
 		else j=(3-i);
 		if (i<3) {
-			robot_trajectory_add_point_trans(rectangle[j][0], rectangle[j][1]);
+			robot.move_helper.add_point_trans(rectangle[j][0], rectangle[j][1]);
 		} else {
-			robot_trajectory_add_final_point_trans(rectangle[j][0], rectangle[j][1], TURN(0));
+			robot.move_helper.add_final_point_notrans(rectangle[j][0], rectangle[j][1], TURN(0));
 		}
 	}
 	//backward = !backward;
 }
 
-FSM_STATE(rectangle)
+struct rectangle : TimedSimpleState<rectangle, FSMMain>
 {
-	switch (FSM_EVENT) {
-		case EV_ENTRY:
-			follow_rectangle();
-			break;
-		case EV_MOTION_DONE:
- 			//FSM_TRANSITION(turn);
-			FSM_TRANSITION(wait);
-			break;
-		default:
-			break;
+	rectangle() {
+		follow_rectangle();
 	}
-}
+	typedef sc::transition<EV_MOTION_DONE, Wait> reactions;
+};
 
-FSM_STATE(turn)
+struct Wait : TimedState<Wait, FSMMain>
 {
-	switch (FSM_EVENT) {
-		case EV_ENTRY:
-			robot_trajectory_new(NULL);
-			robot_trajectory_add_final_point_trans(1, 0.5,
-						TURN_CCW(DEG2RAD(0)));
-			break;
-		case EV_MOTION_DONE:
-			FSM_TRANSITION(wait);
-			break;
-		default:
-			break;
+	Timer waitTout;
+	Wait(my_context ctx) : base_state(ctx) {
+		runTimer(waitTout, 1000, new EV_TIMER());
 	}
-}
+	typedef sc::transition<EV_TIMER, rectangle> reactions;
+};
 
-
-FSM_STATE(wait)
-{
-	FSM_TIMER(1000);
-	switch (FSM_EVENT) {
-		case EV_TIMER:
-			FSM_TRANSITION(rectangle);
-			break;
-		default:
-			break;
-	}
-}
-
-void transition_callback(struct robo_fsm *fsm)
+/*void transition_callback(struct robo_fsm *fsm)
 {
     ul_loginf("fsmcb %s: %s(%s)\n", fsm->debug_name, fsm->state_name, fsm_event_str(fsm->events[fsm->ev_head]));
-}
-
-
+}*/
 
 int main()
 {
 	int rv;
 
-	rv = robot_init();
+	rv = robot.init();
 	if (rv) error(1, errno, "robot_init() returned %d\n", rv);
 
- 	//robot.fsm.main.transition_callback = transition_callback;
- 	//robot.fsm.motion.transition_callback = transition_callback;
- 	//robot.fsm.main.debug_states = 1;
- 	robot.fsm.motion.debug_states = 1;
-
-	robot.fsm.main.state = &fsm_state_main_init;
-
-        rv = robot_start();
+	//robot.fsm.main.transition_callback = transition_callback;
+	//robot.fsm.motion.transition_callback = transition_callback;
+	//robot.fsm.main.debug_states = 1;
+        rv = robot.start();
 	if (rv) error(1, errno, "robot_start() returned %d\n", rv);
 
-	robot_destroy();
+	robot.destroy();
 
 	return 0;
 }
-- 
2.39.2