/* *******************************************************************
   balistic_walking.c - spring_flamingo3d

   Simple Walking Algorithm with mostly balistic swing phase.

   ******************************************************************* */
   
#include <stdio.h>
#include <math.h>
#include <ctype.h>
#include <sys/time.h>
#include <leggoal.h>
#include "creature.h"
#include "balistic_walking.h"
#include "M2.h"


double passive_ankle_torques(pos, vel)
     double pos, vel;
{
  if (pos < ls.ankle_limit_set)
    return (ls.ankle_limit_gain * (ls.ankle_limit_set - pos) * (ls.ankle_limit_set - pos));
  else return (0.0);
}


void calculate_fitness()
{
  /* ls.sim_id */
  			    
  ls.pow_lh = qd.lh_pitch * ls.ff_lh_pitch;
  ls.pow_lk = qd.lk * ls.ff_lk;
  ls.pow_la = qd.la_pitch * ls.act_la;

  ls.pow_rh = qd.rh_pitch * ls.ff_rh_pitch;
  ls.pow_rk = qd.rk * ls.ff_rk;
  ls.pow_ra = qd.ra_pitch * ls.act_ra;

  ls.power = fabs(ls.pow_lh) + fabs(ls.pow_lk) + fabs(ls.pow_la) + fabs(ls.pow_rh) + fabs(ls.pow_rk) + fabs(ls.pow_ra);

  if ((ls.t > 0.5) && (ls.fell_down < 0.5))
    {
      ls.torque_total = ls.torque_total + (fabs(ls.ff_rh_pitch) + fabs(ls.ff_rk) + fabs(ls.act_ra) + fabs(ls.ff_lh_pitch) + fabs(ls.ff_lk) + fabs(ls.act_la)) * ls.control_dt;
      ls.energy = ls.energy + ls.control_dt * ls.power;
      /*ls.fit1 = -(q.x+0.5)/ls.energy;*/  /* Efficiency */
      /*ls.fit1 = (q.x+0.5)*(q.x+0.5)/ls.energy;*/  /* Efficiency * Speed */
      /*ls.fit1 = -(q.x+0.5); */ /* Speed */

      /*ls.fit1 = (q.x-0.5)/ls.torque_total;*/

      ls.fit1 = (q.x - 0.5);

    }
  else ls.energy = 1.0;

  if (ls.fit1 < 0.00001) ls.fit1 = 0.00001;



  if ((q.z < 0.8) || (q.z > 1.05) || (ls.yd > 2.0) || (ls.yd < -2.0)
      || (ls.xd < 0.0) || (ls.xd > 1.75)) 
    {
      ls.fell_down = 1.0;
      if (ls.run > 0.02)
	ls.run = 0.01;
      /*ls.fit1 = 0.000001;*/
    }
	
}


void balistic_walking_state_machine()
{

  calculate_fitness();
  ls.vel = qd.x;

  ls.yd = qd.y * cos(q.yaw) - qd.x * sin(q.yaw);
  ls.xd = qd.x * cos(q.yaw) + qd.y * sin(q.yaw);
  
  if (fabs(ls.xd) > 0.01)
    ls.heading_direction = atan(ls.yd/ls.xd);
  else if (fabs(ls.yd) > 0.02) 
    ls.heading_direction = PI/2.0 + atan(ls.xd/ls.yd);
  else ls.heading_direction = 0.0;

  /* Center distance from foot to body */

  ls.l_ceny = -q.y + gc_lheel_in.at_y;
  ls.r_ceny = q.y - gc_rheel_in.at_y;
  
  /* Calculate forces on the feet */

  ls.force_alpha = ls.control_dt * ls.force_freq;

  ls.left_force = (1.0 - ls.force_alpha) * ls.left_force +
    ls.force_alpha * (gc_lheel_in.f_z + gc_ltoe_in.f_z + gc_lheel_out.f_z + gc_ltoe_out.f_z);
  if (ls.left_force > 5.0) 
    ls.left_cop = (gc_ltoe_in.f_z + gc_ltoe_out.f_z)/ls.left_force;
  else ls.left_cop = 0.5;


  ls.right_force = (1.0 - ls.force_alpha) * ls.right_force +
    ls.force_alpha * (gc_rheel_in.f_z + gc_rtoe_in.f_z + gc_rheel_out.f_z + gc_rtoe_out.f_z);
  if (ls.right_force > 5.0)
    ls.right_cop = (gc_rtoe_in.f_z + gc_rtoe_out.f_z)/ls.right_force;
  else ls.right_cop = 0.5;


  /* Actions in Each State */


  if (ls.left_state == SUPPORT)
    {
      ls.ff_la_roll_speed = ls.yd_ankle_gain * (-ls.flat_ankle_roll - ls.yd_gain * ls.yd - q.la_roll);
      ls.ff_la_roll = ls.ff_la_roll_speed + ls.ankle_k * (-ls.flat_ankle_roll - q.la_roll) - ls.ankle_b * qd.la_roll;

      /* Use hip to servo pitch */
      if (ls.left_force > 5.0)
	{
	  ls.ff_lh_pitch = -ls.t_gain * (ls.t_d - q.pitch) + ls.t_damp * qd.pitch;

	  ls.ff_hip_roll = (HIP_OFFSET_Y * cos(q.roll) 
			    + BODY_CG_Z * sin(q.roll)) * ls.ff_z;
	  ls.ff_lh_roll = ls.roll_gain * q.roll + ls.roll_damp * qd.roll +ls.ff_hip_roll;
	  ls.ff_lh_yaw = ls.yaw_gain * (q.yaw - q_d.yaw) + ls.yaw_damp * qd.yaw;
	}
      else
	{
	  ls.ff_lh_pitch = 0.0;
	  ls.ff_lh_roll = 0.0;
	  ls.ff_lh_yaw = 0.0;
	}

      /* Keep knee straight */
      ls.ff_lk = ls.knee_gain * (ls.knee_d - q.lk) - ls.knee_damp * qd.lk;
      
      /* Use ankle to servo speed, position */
      ls.act_la =  0.0;
      
      /* Ankle limit to go on toes and maintain cop */
      ls.pas_la = passive_ankle_torques(q.la_pitch, qd.la_pitch);
      
    }
  

  else if (ls.left_state == TOE_OFF)
    {
      /* Decay roll instead of setting straight to zero */
      ls.ff_la_roll_speed = (1.0 - ls.decay_freq * ls.control_dt) * ls.ff_la_roll_speed;
      ls.ff_la_roll = ls.ff_la_roll_speed + ls.ankle_k * (-ls.flat_ankle_roll - q.la_roll) - ls.ankle_b * qd.la_roll;

      ls.ff_hip_roll = (HIP_OFFSET_Y * cos(q.roll) 
			+ BODY_CG_Z * sin(q.roll)) * ls.ff_z;
      ls.ff_lh_roll = ls.roll_gain * q.roll + ls.roll_damp * qd.roll +ls.ff_hip_roll;
      if ((gc_ltoe_in.fs > 0.9) && (gc_ltoe_out.fs > 0.9))
	ls.ff_lh_yaw = ls.yaw_gain * (q.yaw - q_d.yaw) + ls.yaw_damp * qd.yaw;
      else (ls.ff_lh_yaw = 0.0);

      /* Use hip to servo pitch */
      ls.ff_lh_pitch = -ls.t_gain * (ls.t_d - q.pitch) + ls.t_damp * qd.pitch;
      
      /* Keep knee straight */
      ls.ff_lk = ls.knee_gain * (ls.knee_d - q.lk) - ls.knee_damp * qd.lk;
      
      /* Ankle limit to go on toes and maintain cop */
      ls.pas_la = passive_ankle_torques(q.la_pitch, qd.la_pitch);
      
      /* Ankle push off */
      ls.act_la = ls.push_gain * (ls.push_set - q.la_pitch) - ls.push_damp * qd.la_pitch;

}
  
  else if (ls.left_state == SWING)
    {
      ls.ramp_left = (ls.t - ls.left_switch_time)/ls.tran_time;
      if (ls.ramp_left > 1.0) ls.ramp_left = 1.0;

      q_d.lh_yaw = -q.yaw + ls.lateral_yaw_mul * ls.heading_direction;
      ls.k_lh_yaw = ls.ramp_left * ls.hip_k/10.0;
      ls.b_lh_yaw = ls.ramp_left * ls.hip_b/4.0;
      ls.ff_lh_yaw = ls.k_lh_yaw * (q_d.lh_yaw - q.lh_yaw) - ls.b_lh_yaw * qd.lh_yaw;
      
      if ((gc_ltoe_in.fs < 0.1) && (gc_ltoe_out.fs < 0.1))
	ls.k_lh_roll = ls.ramp_left * ls.hip_k;
      else ls.k_lh_roll = 0.0;
      ls.b_lh_roll = ls.ramp_left * ls.hip_b;
      ls.ff_lh_roll = ls.k_lh_roll * (-q.roll - q.lh_roll) - ls.b_lh_roll * qd.lh_roll;
      
      ls.k_la_roll = ls.ramp_left * ls.ankle_k;
      ls.b_la_roll = ls.ramp_left * ls.ankle_b;
      ls.ff_la_roll = ls.k_la_roll * (0.0 - q.la_roll) - ls.b_la_roll * qd.la_roll;

      /* Servo hip up */
      ls.left_hip_set = -ls.hip_d - q.pitch;
      ls.ff_lh_pitch = ls.hip_gain * (ls.left_hip_set - q.lh_pitch) - ls.hip_damp * qd.lh_pitch;
      if (ls.ff_lh_pitch > ls.max_hip_torque) ls.ff_lh_pitch = ls.max_hip_torque;
      if (ls.ff_lh_pitch < -ls.max_hip_torque) ls.ff_lh_pitch = -ls.max_hip_torque;

      /* Damp the knee */
      ls.ff_lk = -ls.swing_damp_knee1 * qd.lk;

      /* Continue toe off until toe leaves ground */
      if (gc_ltoe_in.fs > 0.1)
	{
	  ls.pas_la = passive_ankle_torques(q.la_pitch, qd.la_pitch);
	  ls.act_la = ls.push_gain * (ls.push_set - q.la_pitch) - ls.push_damp * qd.la_pitch;	  
	}
      else
	{
	  /* Servo ankle level to the ground */
	  ls.left_heel = -q.pitch - q.lh_pitch - q.lk - q.la_pitch;
	  ls.act_la = -ls.ankle_gain * (-ls.ankle_d - ls.left_heel) 
	    - ls.ankle_damp * qd.la_pitch;
	  /*ls.pas_la = passive_ankle_torques(q.la_pitch, qd.la_pitch);*/
	  ls.pas_la = 0.0;
	}
    }
  
  
  else if (ls.left_state == STRAIGHTEN)
    {
      q_d.lh_yaw = -q.yaw + ls.lateral_yaw_mul * ls.heading_direction; 
      ls.ff_lh_yaw = ls.hip_k/10.0 * (q_d.lh_yaw - q.lh_yaw) - ls.hip_b/4.0 * qd.lh_yaw;

      q_d.lh_roll = -q.roll + ls.swing_roll_off;
      q_d.lh_roll = q_d.lh_roll + ls.yd_gain * ls.yd + ls.ceny_gain * ls.r_ceny;
      ls.ff_lh_roll = ls.hip_k * (q_d.lh_roll - q.lh_roll) - ls.hip_b * qd.lh_roll;

      ls.ff_la_roll = ls.ankle_k * (0.0 - q.la_roll) - ls.ankle_b * qd.la_roll;      

      /* Servo hip to a more shallow angle */
      ls.left_hip_set = -ls.hip_hold - q.pitch;
      ls.ff_lh_pitch = ls.hip_gain * (ls.left_hip_set - q.lh_pitch) - ls.hip_damp * qd.lh_pitch;
      
      /* Keep knee straight */
      ls.ff_lk = ls.swing_gain_knee * (ls.knee_d - q.lk) - ls.swing_damp_knee * qd.lk;

      /* Couple knee torque to hip so that leg doesn't go swinging
	 forward when knee is stopped... */
      ls.ff_lh_pitch = ls.ff_lh_pitch + ls.ff_lk;
      

      /* Servo ankle level to the ground */
      ls.left_heel = -q.pitch - q.lh_pitch - q.lk - q.la_pitch;
      ls.act_la = -ls.ankle_gain * (-ls.ankle_d - ls.left_heel) 
	- ls.ankle_damp * qd.la_pitch;
      ls.pas_la = 0.0;
    }
  
  
  if (ls.right_state == SUPPORT)
    {
      ls.ff_ra_roll_speed = ls.yd_ankle_gain * (ls.flat_ankle_roll - ls.yd_gain * ls.yd - q.ra_roll);     
      ls.ff_ra_roll = ls.ff_ra_roll_speed + ls.ankle_k * (ls.flat_ankle_roll - q.ra_roll) - ls.ankle_b * qd.ra_roll;
      
      /* Use hip to servo pitch, roll, and yaw */
      if (ls.right_force > 5.0)
	{
	  ls.ff_rh_pitch = -ls.t_gain * (ls.t_d - q.pitch) + ls.t_damp * qd.pitch;

	  ls.ff_hip_roll = (HIP_OFFSET_Y * cos(q.roll) 
			    - BODY_CG_Z * sin(q.roll)) * ls.ff_z;
	  ls.ff_rh_roll = ls.roll_gain * q.roll + ls.roll_damp * qd.roll -ls.ff_hip_roll;
	  ls.ff_rh_yaw = ls.yaw_gain * (q.yaw - q_d.yaw) + ls.yaw_damp * qd.yaw;
	}
      else 
	{
	  ls.ff_rh_pitch = 0.0;
	  ls.ff_rh_roll = 0.0;
	  ls.ff_rh_yaw = 0.0;
	}
      
      /* Keep knee straight */
      ls.ff_rk = ls.knee_gain * (ls.knee_d - q.rk) - ls.knee_damp * qd.rk;
      
      /* Use ankle to servo speed, position */
      ls.act_ra =  0.0;
      
      /* Ankle limit to go on toes and maintain cop */
      ls.pas_ra = passive_ankle_torques(q.ra_pitch, qd.ra_pitch);
      
    }
  
  else if (ls.right_state == TOE_OFF)
    {
      /* Decay roll instead of setting straight to zero */
      ls.ff_ra_roll_speed = (1.0 - ls.decay_freq * ls.control_dt) * ls.ff_ra_roll_speed;
      ls.ff_ra_roll = ls.ff_ra_roll_speed + ls.ankle_k * (ls.flat_ankle_roll - q.ra_roll) - ls.ankle_b * qd.ra_roll;

      ls.ff_hip_roll = (HIP_OFFSET_Y * cos(q.roll) 
			- BODY_CG_Z * sin(q.roll)) * ls.ff_z;
      ls.ff_rh_roll = ls.roll_gain * q.roll + ls.roll_damp * qd.roll -ls.ff_hip_roll; 
      if ((gc_rtoe_in.fs > 0.9) && (gc_rtoe_out.fs > 0.9))
	ls.ff_rh_yaw = ls.yaw_gain * (q.yaw - q_d.yaw) + ls.yaw_damp * qd.yaw;
      else (ls.ff_rh_yaw = 0.0);

      /* Use hip to servo pitch */
      ls.ff_rh_pitch = -ls.t_gain * (ls.t_d - q.pitch) + ls.t_damp * qd.pitch;
      
      /* Keep knee straight */
      ls.ff_rk = ls.knee_gain * (ls.knee_d - q.rk) - ls.knee_damp * qd.rk;
      
      /* Ankle limit to go on toes and maintain cop */
      ls.pas_ra = passive_ankle_torques(q.ra_pitch, qd.ra_pitch);
      
      /* Ankle push off */
      ls.act_ra = ls.push_gain * (ls.push_set - q.ra_pitch) - ls.push_damp * qd.ra_pitch;
    }
  
  else if (ls.right_state == SWING)
    {
      ls.ramp_right = (ls.t - ls.right_switch_time)/ls.tran_time;
      if (ls.ramp_right > 1.0) ls.ramp_right = 1.0;

      q_d.rh_yaw = -q.yaw + ls.lateral_yaw_mul * ls.heading_direction; 
      ls.k_rh_yaw = ls.ramp_right * ls.hip_k/10.0;      
      ls.b_rh_yaw = ls.ramp_right * ls.hip_b/4.0;
      ls.ff_rh_yaw = ls.k_rh_yaw * (q_d.rh_yaw - q.rh_yaw) - ls.b_rh_yaw * qd.rh_yaw;

      if ((gc_rtoe_in.fs < 0.1) && (gc_rtoe_out.fs < 0.1))
	ls.k_rh_roll = ls.ramp_right * ls.hip_k;
      else ls.k_rh_roll = 0.0;
      ls.b_rh_roll = ls.ramp_right * ls.hip_b;
      ls.ff_rh_roll = ls.k_rh_roll * (-q.roll - q.rh_roll) - ls.b_rh_roll * qd.rh_roll;

      ls.k_ra_roll = ls.ramp_right * ls.ankle_k;
      ls.b_ra_roll = ls.ramp_right * ls.ankle_b;      
      ls.ff_ra_roll = ls.k_ra_roll * (0.0 - q.ra_roll) - ls.b_ra_roll * qd.ra_roll;

      /* Servo hip up */
      ls.right_hip_set = -ls.hip_d - q.pitch;
      ls.ff_rh_pitch = ls.hip_gain * (ls.right_hip_set - q.rh_pitch) - ls.hip_damp * qd.rh_pitch;
      if (ls.ff_rh_pitch > ls.max_hip_torque) ls.ff_rh_pitch = ls.max_hip_torque;
      if (ls.ff_rh_pitch < -ls.max_hip_torque) ls.ff_rh_pitch = -ls.max_hip_torque;
      
      /* Damp the knee */
      ls.ff_rk = -ls.swing_damp_knee1 * qd.rk;
      
      /* Continue toe off until toe leaves ground */
      if (gc_rtoe_in.fs > 0.1)
	{
	  ls.pas_ra = passive_ankle_torques(q.ra_pitch, qd.ra_pitch);
	  ls.act_ra = ls.push_gain * (ls.push_set - q.ra_pitch) - ls.push_damp * qd.ra_pitch;
	}
      else 
	{
	  /* Servo ankle level to the ground */
	  ls.right_heel = -q.pitch - q.rh_pitch - q.rk - q.ra_pitch;
	  ls.act_ra = -ls.ankle_gain * (-ls.ankle_d - ls.right_heel) 
	    - ls.ankle_damp * qd.ra_pitch;
	  ls.pas_ra = 0.0;
	}
    }
  
  
  else if (ls.right_state == STRAIGHTEN)
    {
      q_d.rh_yaw = -q.yaw + ls.lateral_yaw_mul * ls.heading_direction; 
      ls.ff_rh_yaw = ls.hip_k/10.0 * (q_d.rh_yaw - q.rh_yaw) - ls.hip_b/4.0 * qd.rh_yaw;

      q_d.rh_roll = -q.roll - ls.swing_roll_off; 
      q_d.rh_roll = q_d.rh_roll + ls.yd_gain * ls.yd - ls.ceny_gain * ls.l_ceny;
      ls.ff_rh_roll = ls.hip_k * (q_d.rh_roll - q.rh_roll) - ls.hip_b * qd.rh_roll;

      ls.ff_ra_roll = ls.ankle_k * (0.0 - q.ra_roll) - ls.ankle_b * qd.ra_roll;

      /* Servo hip to a more shallow angle */
      ls.right_hip_set = -ls.hip_hold - q.pitch;
      ls.ff_rh_pitch = ls.hip_gain * (ls.right_hip_set - q.rh_pitch) - ls.hip_damp * qd.rh_pitch;
      
      /* Keep knee straight */
      ls.ff_rk = ls.swing_gain_knee * (ls.knee_d - q.rk) - ls.swing_damp_knee * qd.rk;
      
      /* Couple knee torque to hip so that leg doesn't go swinging
	 forward when knee is stopped... */
      ls.ff_rh_pitch = ls.ff_rh_pitch + ls.ff_rk;
      
      /* Servo ankle level to the ground */
      ls.right_heel = -q.pitch - q.rh_pitch - q.rk - q.ra_pitch;
      ls.act_ra = -ls.ankle_gain * (-ls.ankle_d - ls.right_heel) 
	- ls.ankle_damp * qd.ra_pitch;
      ls.pas_ra = 0.0;
      
    }
  
  
  /* Communication between legs for now... */
  
  /* Subtract swing hip torque from the stance leg so less disturbance
     on the body */
  if (ls.left_state == SWING)
    ls.ff_rh_pitch = ls.ff_rh_pitch - ls.ff_lh_pitch;
  
  if (ls.right_state == SWING)
    ls.ff_lh_pitch = ls.ff_lh_pitch - ls.ff_rh_pitch;
  
  
  
  /* Transition Conditions */
  
  
  if (ls.left_state == SUPPORT)
    {
      /* Transition if unloaded and travelling away from the foot */
      if (((gc_lheel_in.f_z+gc_lheel_out.f_z) < ls.force_thresh) && 
	  ((qd.x > 0.0) && (gc_lheel_in.at_x < q.x)) &&
	  ((gc_lheel_in.at_x < gc_rheel_in.at_x)) &&
	  (ls.t > ls.left_switch_time + ls.min_support_time))
	{
	  ls.left_state = TOE_OFF;
	  ls.left_switch_time = ls.t;
	}
    }
  
  else if (ls.left_state == TOE_OFF)
    {
      /* Once load on foot falls below a threshhold */
      if ((ls.right_state == SUPPORT) && (ls.left_force < ls.force_thresh))
	{
	  ls.left_state = SWING;
	  ls.left_switch_time = ls.t;
	}
    }
  
  else if (ls.left_state == SWING)
    {
      if (ls.t > ls.left_switch_time + ls.swing_time)
	{
	  ls.left_state = STRAIGHTEN;
	  ls.left_switch_time = ls.t;
	}
    }
  
  else if (ls.left_state == STRAIGHTEN)
    {
      if ( 
	  ((gc_ltoe_in.fs == 1.0) || (gc_lheel_in.fs == 1.0)) &&
	  (gc_lheel_in.at_x > gc_rheel_in.at_x))
	{
	  ls.left_state = SUPPORT;
	  ls.left_switch_time = ls.t;
	}
    }
  
  
  
  if (ls.right_state == SUPPORT)
    {
      /* Transition if unloaded and travelling away from the foot */
      if (((gc_rheel_in.f_z + gc_rheel_out.f_z) < ls.force_thresh) && 
	  ((qd.x > 0.0) && (gc_rheel_in.at_x < q.x)) &&
	  ((gc_rheel_in.at_x < gc_lheel_in.at_x)) &&
	  (ls.t > ls.right_switch_time + ls.min_support_time))
	{
	  ls.right_state = TOE_OFF;
	  ls.right_switch_time = ls.t;
	}
    }
  
  else if (ls.right_state == TOE_OFF)
    {
      /* Once load on foot falls below a threshhold */
      if ((ls.left_state == SUPPORT) && (ls.right_force < ls.force_thresh))
	{
	  ls.right_state = SWING;
	  ls.right_switch_time = ls.t;
	}
    }
  
  else if (ls.right_state == SWING)
    {
      if (ls.t > ls.right_switch_time + ls.swing_time)
	{
	  ls.right_state = STRAIGHTEN;
	  ls.right_switch_time = ls.t;
	}
    }
  
  else if (ls.right_state == STRAIGHTEN)
    {
      if (
	  ((gc_rtoe_in.fs == 1.0) || (gc_rheel_in.fs == 1.0)) &&
	  (gc_rheel_in.at_x > gc_lheel_in.at_x))
	{
	  ls.right_state = SUPPORT;
	  ls.right_switch_time = ls.t;
	}
    }
  

  /* Torques at the joints: */

  ls.ff_la_pitch = ls.act_la + ls.pas_la;
  ls.ff_ra_pitch = ls.act_ra + ls.pas_ra;

  
}