let make_twist_message v omega=
  let open Geometry_msgs in
  let mkvector x y z =  { vector3_x = x; vector3_y = y; vector3_z = z   } in 
  Twist { twist_linear  = mkvector v 0 0 ; twist_angular = mkvector 0 0 omega }

let process_clock_message state =
  match state.mode with 
  | Driving -> { state with outgoing = Some (make_twist_message 10000 0) } 
  | Turning -> begin
  match state.direction with 
    | None
    | Some ( CW ) -> { state with outgoing = Some (make_twist_message 0   10000) } 
    | Some (CCW ) -> { state with outgoing = Some (make_twist_message 0 (-10000))} 
  end

let get_min_and_direction msg =
  let max = msg.Sensor_msgs.laserScan_range_max in
  let lst = msg.Sensor_msgs.laserScan_ranges in
  let min_range = get_min_range max lst in
  let mini = foldi ~base:(max, 0)
    (fun i a b -> if a < fst b then (a,i) else b) in
  let _ , idx = mini lst in
  if idx < List.length lst / 2 then min_range, CW else min_range, CCW

let process_sensor_message state min_range min_direction =
  let driving_state = 
    { state with mode = Driving; min_range = None; direction = None } in
  let turning_state = 
    { state with 
      mode      = Turning
    ; direction = Some min_direction
    ; min_range = Some min_range 
    } in  
  match state.mode , state.min_range with 
  | Driving , _    -> if min_range < 20000 then turning_state else driving_state
  | Turning , None -> if min_range > 25000 then driving_state else turning_state
  | Turning , Some old_range -> 
    if min_range > 25000 then driving_state
    else if min_range > old_range then state else turning_state

let one_step state =
  match state.incoming with None -> state | Some in_msg ->
  let state = { state with incoming = None; outgoing = None } in
  match in_msg with 
  | Sensor laserScan -> 
    let min_range, min_direction = get_min_and_direction laserScan in
    process_sensor_message state min_range min_direction
  | Clock  _ -> process_clock_message state

let get_min_range max lst =
  List.fold_right ~base:max
    (fun x a -> if x < a then x else a) lst

let rec foldi ~base ?(i=0) f l =
  match l with
  | [] -> base
  | x :: tail -> f i x ( foldi f ~base ~i:(i+1) tail )

type incoming_msg = 
  | Clock  of Rosgraph_msgs.clock
  | Sensor of Sensor_msgs.laserScan
  
type outgoing_msg =
  | Twist of Geometry_msgs.twist

type direction = CW | CCW 

type mode = Driving | Turning

type state =
  { mode : mode
  ; min_range : int option
  ; direction : direction option
  ; incoming  : incoming_msg option
  ; outgoing  : outgoing_msg option 
  }

module Sensor_msgs = struct
  type laserScan = 
    { laserScan_range_min : int 
    ; laserScan_range_max : int 
    ; laserScan_ranges : int list 
    }
end
module Rosgraph_msgs = struct
  type time = 
    { seconds     : int
    ; nanoseconds : int
    }
  type clock = { clock : time }
end

module Geometry_msgs = struct
  type vector3 = 
    { vector3_x : int
    ; vector3_y : int
    ; vector3_z : int
    }
  type twist = 
    { twist_linear  : vector3
    ; twist_angular : vector3
    }    
end

let state = {
...
Fix_engine_state.incoming_fix_msg =
  (Some
    (Full_messages.ValidMsg
      ({Full_messages.full_msg_header =
          {Full_messages.h_begin_string = "FIX.4.4";
           Full_messages.h_poss_dup_flag = (Some true);
...
}

incoming_fix_msg : full_top_level_msg option;    
outgoing_fix_msg : full_top_level_msg option;

incoming_int_msg : fix_engine_int_inc_msg option;    
outgoing_int_msg : fix_engine_int_out_msg option;