//---------------------------------------------------
// Helm Behavior file 

initialize   DEPLOY  = false
initialize   MUSTER  = false
initialize   RETURN  = false
initialize   STATION = false
initialize   LOITER  = false
initialize   AVOID   = true
initialize   LEADER   = false
initialize   COMMS_POLICY = lean

set MODE = ACTIVE {
  DEPLOY = true
} INACTIVE

set MODE = STATIONING {
//  MODE = ACTIVE
  STATION = true
}

set MODE = RETURNING {
  MODE = ACTIVE
  RETURN = true
} 

set MODE = MUSTERING {
  MODE = ACTIVE
  MUSTER = true
}                       

set MODE = IN_FORMATION {
  MODE = ACTIVE
  FORMATION_ACTIVE = true
}

//----------------------------------------------
Behavior = BHV_FormationCtrl
{
  name      = FormationCtrl
  pwt       = 200
  debug = true 

  condition = DEPLOY==true
  condition = RETURN==false

  // Position index - set "auto" or -1 for external assignment. Set 0 to force the leader

  pos_idx = auto
  
  // Formation options
  formation_type = vee : vee_angle=45, min_spacing=15
  //formation_type = circle : min_spacing = 20, leader_centered = true 
  //formation_type = echelon : echelon_side=right, echelon_angle=45, fingered = false, min_spacing = 13
  //formation_type = ncolumn : col_spacing = 10, row_spacing = 10, num_cols = 3
  //formation_type = wedge : horiz_separation = 5, vert_separation = 5
  //formation_type = arrowhead : angle = 45, min_spacing = 5 // not implemented yet
  //formation_type = diamond : angle = 45, min_spacing = 5 // not implemented yet
  //formation_type = convoy : ideal_follow_range = 10

  // Control settings
  desired_formation_speed = 1

  // Control point queue settings, and point capture
  max_queue_length = 8
  min_queue_point_sep = 0.5
  capture_radius = 1
  slip_radius = 8
  max_speed = 1.8

  // Control settings for form: speed = nominal_speed + err*(Kp_1 + Kp_2*err) + err_dot*(Kd_1 + Kd_2*err_dot) + coupling_terms

  // Local control settings
  Kp_spd_1 = 0.5
  Kp_spd_2 = 0.1

  Kd_spd_1 = 0
  Kd_spd_2 = 0

  Kp_hdg_1 = 0.5
  Kp_hdg_2 = 0.2

  Kd_hdg_1 = 0.5
  Kd_hdg_2 = 0.2

  // Coupling terms, for form: 
  // coupling_terms = sat(err)*(Kp_c*sum(agent_errors) + Kd_c*sum(agent_errors_dot))
  // and sat(err) = 1 if err < r_sat else exp(-lambda*(err-r_sat))
  // we incorporate saturation so agents synchronize when they are on/near trajectory only

  sat_spd_range = 0.25 // Saturation radius - from the ideal point, when 
  sat_spd_lambda = 1 // Exponential decay for sat function
  
  sat_hdg_lambda = 0.25 
  sat_hdg_range = 1

  kpc_spd = 0.5
  kdc_spd = 0.25

  kpc_hdg = 0
  kdc_hdg = 0

}

Behavior = BHV_Waypoint
 {
   // General Behavior Parameters
   // ---------------------------
   name         = circuit               // example
   pwt          = 100                   // default
   condition    = MODE==ACTIVE
   condition    = DEPLOY==true
   condition    = RETURN==false
   condition    = LEADER==true      // example
   updates      = TRANSIT_UPDATES       // example

   // Parameters specific to this behavior
   // ------------------------------------
      capture_radius = 3                       // default
        capture_line = false                   // default
                lead = 0                      // default
       lead_to_start = false                   // default
               order = normal                  // default 
              //Define polygon elsewhere

              #include plug_circuit.moos 

              repeat = 3
         slip_radius = 15
               speed = 1
             endflag = RETURN_ALL=true
             endflag = QUIT_MISSION=true
          
        visual_hints = vertex_size  = 3             // default
        visual_hints = edge_size    = 1             // default
        visual_hints = vertex_color = dodger_blue   // default
        visual_hints = edge_color   = white         // default
        visual_hints = nextpt_color = yellow        // default
        visual_hints = nextpt_lcolor = aqua         // default
        visual_hints = nextpt_vertex_size = 5       // default
 }

 Behavior = BHV_TaskFormationLeader
{ 
       name = formation_leader
    updates = TASK_FORMATION_LEADER
 templating = spawn
    condition = DEPLOY==true

         type = formation_leader
 max_bid_wins = false
 
	// Typically set/overridden upon spawning event
	waypt   = $(LEADIN_POS)
  turn_radius = 5

  bidwonflag = TRANSIT = true
  bidwonflag = STATION = false
  bidwonflag = RETURN  = false
  bidwonflag = FORMATION_ACTIVE  = true
  bidwonflag = LEADER  = true
  bidwonflag = FORMATION_IDX  = 0
  bidwonflag = TASK_FORMATION_LEADER  = waypt=$(LEADIN_POS)
	xbidwonflag = MISSION_TASK = type=formation_follower,id=1,contact=$[OWNSHIP],exempt=$[OWNSHIP],task_time=$[UTC],pos_idx=1
  xbidwonflag = FORMATION_LEADER = $[OWNSHIP]
}

//----------------------------------------------
Behavior = BHV_TaskFormationFollower
{ 
       name = formation_follower
    updates = TASK_FORMATION
 templating = spawn

         type = formation_follower
 max_bid_wins = false

	team_by_group = true
  turn_radius = 5
	
  bidwonflag = FORMATION_ACTIVE  = true
  bidwonflag = TRANSIT = false
  bidwonflag = STATION = false
  //bidwonflag = UP_CONVOY = contact=$[CONTACT]
  //bidwonflag = FORMATION_IDX = $[ID]
	xbidwonflag = MISSION_TASK = type=formation_follower,id=$[ID+1],contact=$[OWNSHIP],exempt=$[OWNSHIP]:$[EXEMPT],task_time=$[UTC],pos_idx=$[POS_IDX]
}

#include plugs/bhvs/std_bhvs/plug_stationkeep.bhv
//#include plugs/bhvs/std_bhvs/plug_opregion.bhv
#include plugs/bhvs/std_bhvs/plug_returnhome.bhv
#include plugs/bhvs/std_bhvs/plug_colregs.bhv