#!/usr/bin/ruby

# /* LICENSE:
#   =========================================================================
#     CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
#     Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
#     School of Computer Science, Carnegie Mellon University
#     All rights reserved.
#   ========================================================================= */

motion_name = "unknown"

$poses = []
$cur_pose = 0

$string_idx = 0
$head_idx   = 1
$mouth_idx  = 2
$tail_idx   = 3
$legs_idx   = 4
$interp_idx = 5
$frames_idx = 6

$empty_pose = 
  [false,
   [0.0,0.0,0.0],
   0.0,
   [0.0,0.0],
   [[0.0,0.0,0.0],
    [0.0,0.0,0.0],
    [0.0,0.0,0.0],
    [0.0,0.0,0.0]],
   false,
   0]

# this isn't the right safe head position
$head_defs = [0.5, 1.5, 0.0]

$dangerous_kicks_do_not_run = ["boundaryKickLeft","boundaryKickRight","boundaryKick","headKick"];

$motion_name = "unknown"

def output_pose(pose,time)
  if(!all_tildes(pose[$head_idx]))
    printf("  HeadAng (b,  %6g,%6g,%6g);\n",
           value(pose[$head_idx][0])/1000.0, value(pose[$head_idx][1])/1000.0, value(pose[$head_idx][2])/1000.0)
  else
    printf("  HeadAng (b,  %6g,%6g,%6g);\n",
           value($head_defs[0]), value($head_defs[1]), value($head_defs[2]))
  end
  if(!all_tildes([pose[$mouth_idx]]))
    printf("  MouthAng(b, %6g);\n",value(pose[$mouth_idx])/1000.0)
  end
  if(!all_tildes(pose[$tail_idx]))
    printf("  // ignoring tail commands for motion #{$motion_name}\n")
  end
  for i in (0..3).to_a()
    if(!all_tildes(pose[$legs_idx][i]))
      printf("  LegAng  (b,%d,%6g,%6g,%6g);\n",i,
             value(pose[$legs_idx][i][0])/1000.0,
             value(pose[$legs_idx][i][1])/1000.0,
             value(pose[$legs_idx][i][2])/1000.0)
    end
  end
  printf("  m[n].body = b;\n")
  printf("  m[n].time = %d;\n",time)
  printf("  n++;\n\n");
end

def simple_start()
  printf("\n")
  printf("  mzero(b);\n")
  printf("\n")
  printf("  n = 0;\n")
  printf("  m[n].body = b;\n")
  printf("  m[n].time = 100;\n")
  printf("  n++;\n\n")
end

def interp_start(time)
  printf("\n")
  printf("  mzero(b);\n")
  printf("\n")
  printf("  n = 0;\n")
  printf("  m[n].body = b;\n")
  printf("  m[n].time = %d;\n",time)
  printf("  n++;\n\n")
end

def output_kick()
  #p $poses

  outputted_start = false
  last_pose = nil
  cur_pose = nil

  num_poses = $cur_pose
  for pose_idx in 0..num_poses-1
    if($poses[pose_idx][$string_idx])
      printf("%s",$poses[pose_idx][$string_idx])
    else
      last_pose = cur_pose
      cur_pose = $poses[pose_idx]
      #p cur_pose
      if(cur_pose[$interp_idx])
        if(!outputted_start)
          interp_start(cur_pose[$frames_idx]*8)
          outputted_start = true
        end
        if(!last_pose.nil?)
          output_pose(last_pose,cur_pose[$frames_idx]*8)
        end
      else
        if(!outputted_start)
          simple_start()
          outputted_start = true
        end
        if(!last_pose.nil?)
          output_pose(last_pose,0)
        end
        output_pose(cur_pose,cur_pose[$frames_idx]*8)
      end
    end
  end

  if(!cur_pose.nil?)
    output_pose(cur_pose,0)
  end

  printf("  out = fopen(\"#{$motion_name}.mot\",\"wb\");\n");
  printf("  fwrite(&n,sizeof(n),1,out);\n");
  printf("  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);\n");
  printf("  fclose(out);\n\n\n");
  outputted_end = true
end

def deep_clone(arr)
  if(arr.is_a?(Array))
    return arr.collect {|el|
      deep_clone(el)
    }
  else
    return arr
  end
end

def add_string(string)
  $poses[$cur_pose] = deep_clone($empty_pose)
  $poses[$cur_pose][$string_idx] = string
  $cur_pose+=1
end

def convert_num_or_tilde(val)
  if(val =~ /~/)
    return "~"
  else
    return val.to_f()
  end
end

def has_tilde(arr)
  arr.each {|el|
    if(el.is_a?(String) && el =~ /~/)
      return true
    end
  }
  return false
end

def all_tildes(arr)
  arr.each {|el|
    if(!el.is_a?(String) || el !~ /~/)
      return false
    end
  }
  return true
end

def value(val)
  if(val.is_a?(String))
    return 0.0
  else
    return val
  end
end

printf("/* Kick motions copied from German Team */\n")
printf("/* The kicks in this file were developed by the GermanTeam (http://www.robocup.de/germanteam/). */\n")
printf("/* THIS FILE IS AUTOMATICALLY GENERATED! */\n")
printf("\n")
printf("#include \"genmot.h\"\n\n\n")
printf("void make_kicks_de()\n")
printf("{\n")
printf("  Motion::BodyStateMotion m[64];\n")
printf("  Motion::BodyState b;\n")
printf("  FILE *out;\n")
printf("  int n;\n\n")

outputted_end = true
output_on = true
while(filename = ARGV.shift())
  outputted_start = false
  log_part_hit = false
  file = File.open(filename,"r")
  while(line = file.gets())
    case line
    when /^\s*motion_id\s*=\s*(\S+)/
      output_on = $dangerous_kicks_do_not_run.index($1).nil?
      if(!output_on)
        printf($stderr,"supressing motion file '#{$1}'\n")
      #else
      #  printf($stderr,"passing    motion file '#{$1}'\n")
      end
    end

    if(output_on)
      case line
      when /^\s*motion_id\s*=\s*(\S+)/
        if(!outputted_end)
          output_kick()

          $cur_pose=0
          $poses = []
          outputted_end = true
        end
        
        $motion_name = $1
        outputted_end = false

        add_string("  //==== #{$motion_name} ====//\n")
      when /^\s*\"(.*)/
        add_string("  // #{$1}\n")
      when %r%^\s*//\s*\$Log(.*)\$%
          log_part_hit = true
      when %r%^\s*//\s*(.*)%
          if(!log_part_hit)
            add_string("  // #{$1}\n")
          end
      when /^\s*transition\s+(\S+)\s+(\S+)\s+(\S+)/
        target_mof_name=$1
        start_mof_name =$2
        label_name     =$3
        if(target_mof_name != "allMotions" ||
           start_mof_name  != "extern"     ||
           label_name      != "start")
          add_string(sprintf("  // unhandled transition in motion #{$motion_name}, 'transition #{target_mof_name} #{start_mof_name} #{label_name}'\n"))
        end
      when /^\s*([-0-9]+|~)\s+([-0-9]+|~)\s+([-0-9]+|~)\s+([-0-9]+|~)\s+([-0-9]+|~)\s+([-0-9]+|~)\s+([-0-9]+|~)\s+([-0-9]+|~)\s+([-0-9]+|~)\s+([-0-9]+|~)\s+([-0-9]+|~)\s+([-0-9]+|~)\s+([-0-9]+|~)\s+([-0-9]+|~)\s+([-0-9]+|~)\s+([-0-9]+|~)\s+([-0-9]+|~)\s+([-0-9]+|~)\s+([-0-9]+|~)\s+([-0-9]+|~)/
        # 0=tilt, 1=pan, 2=roll
        $poses[$cur_pose] = deep_clone($empty_pose)
        $poses[$cur_pose][$head_idx  ][0]   =convert_num_or_tilde($~[ 1])
        $poses[$cur_pose][$head_idx  ][1]   =convert_num_or_tilde($~[ 2])
        $poses[$cur_pose][$head_idx  ][2]   =convert_num_or_tilde($~[ 3])
        $poses[$cur_pose][$mouth_idx ]      =convert_num_or_tilde($~[ 4])
        # 0=pan, 1=tilt
        $poses[$cur_pose][$tail_idx  ][0]   =convert_num_or_tilde($~[ 5])
        $poses[$cur_pose][$tail_idx  ][1]   =convert_num_or_tilde($~[ 6])
        $poses[$cur_pose][$legs_idx  ][1][0]=convert_num_or_tilde($~[ 7])
        $poses[$cur_pose][$legs_idx  ][1][1]=convert_num_or_tilde($~[ 8])
        $poses[$cur_pose][$legs_idx  ][1][2]=convert_num_or_tilde($~[ 9])
        $poses[$cur_pose][$legs_idx  ][0][0]=convert_num_or_tilde($~[10])
        $poses[$cur_pose][$legs_idx  ][0][1]=convert_num_or_tilde($~[11])
        $poses[$cur_pose][$legs_idx  ][0][2]=convert_num_or_tilde($~[12])
        $poses[$cur_pose][$legs_idx  ][3][0]=convert_num_or_tilde($~[13])
        $poses[$cur_pose][$legs_idx  ][3][1]=convert_num_or_tilde($~[14])
        $poses[$cur_pose][$legs_idx  ][3][2]=convert_num_or_tilde($~[15])
        $poses[$cur_pose][$legs_idx  ][2][0]=convert_num_or_tilde($~[16])
        $poses[$cur_pose][$legs_idx  ][2][1]=convert_num_or_tilde($~[17])
        $poses[$cur_pose][$legs_idx  ][2][2]=convert_num_or_tilde($~[18])
        $poses[$cur_pose][$interp_idx]      =($~[19].to_i() == 1)
        $poses[$cur_pose][$frames_idx]      =$~[20].to_i()
        $cur_pose+=1;
        #print "interp=#{$interp} frames=#{$frames} frames_src='#{$~[19]}'\n"
      end
    end
  end
  file.close()
end

if(!outputted_end)
  output_kick()
end

printf("}\n")
