import numpy as np

blockName = 'stair'
blockSize = np.array([1.0, 0.28, 0.22]) # meters
blockTiltAngle = 0 # degrees


# F=sloping up forward (+x), B=sloping up backward (-x),
# R=sloping up rightward (-y), L=sloping up leftward (+y)
# last row is closest to robot (robot is on bottom looking up)
# column order is left-to-right on robot (+y to -y)
blockTypes = [
    [ 'B' ],
    [ 'A' ],
    [ 'A' ],
    [ 'A' ]
]
blockTypes.reverse()

# 0=ground level, 1=one cinderblock offset, etc
blockLevels = [
    [ 3 ],
    [ 2 ],
    [ 1 ],
    [ 0 ]
]
blockLevels.reverse()

# map between block types and yaw angles (degrees)
blockAngleMap = { 'A': 90, 'B': 90 }

# TODO: this is just an example
# which foot, block (row,col), offset (x,y), support
# (row,col) refer to which block
# (x,y) are offsets wrt the block center, in meters
# support is an enum indicating foot support type
#   0=heel-toe, 1=midfoot-toe, 2=heel-midfoot
footstepData = [
    [ 'left',  (0,0), (-0.10,  0.10),  2 ],
    [ 'right', (0,0), (-0.10, -0.10),  2 ],
    [ 'left',  (1,0), (-0.10,  0.10),  2 ],
    [ 'right', (1,0), (-0.10, -0.10),  2 ],
    [ 'left',  (2,0), (-0.10,  0.10),  2 ],
    [ 'right', (2,0), (-0.10, -0.10),  2 ],
    [ 'left',  (3,0), (-0.10,  0.10),  2 ],
    [ 'right', (3,0), (-0.10, -0.10),  2 ]
]

# where to stand, relative to front of first block
startingPosition = np.array([-0.5, 0, 0])
startingYaw = 0   # degrees

blockColor = [0.4, 0.6, 0.4]
blockColorMatched = [0.5, 0.8, 0.5]

@@ -959,33 +959,33 @@
  (0,0), (-0.
-1
 0
+9
 ,  0.1
-0
+3
 ),  2 %5D,%0A   
@@ -1003,33 +1003,33 @@
  (0,0), (-0.
-1
 0
+9
 , -0.1
-0
+3
 ),  2 %5D,%0A   
@@ -1047,33 +1047,33 @@
  (1,0), (-0.
-1
 0
+9
 ,  0.1
-0
+3
 ),  2 %5D,%0A   
@@ -1091,33 +1091,33 @@
  (1,0), (-0.
-1
 0
+9
 , -0.1
-0
+3
 ),  2 %5D,%0A   
@@ -1135,33 +1135,33 @@
  (2,0), (-0.
-1
 0
+9
 ,  0.1
-0
+3
 ),  2 %5D,%0A   
@@ -1179,33 +1179,33 @@
  (2,0), (-0.
-1
 0
+9
 , -0.1
-0
+3
 ),  2 %5D,%0A   
@@ -1231,17 +1231,17 @@
 (-0.
-1
 0
+9
 ,  0.1
-0
+3
 ),  
@@ -1275,17 +1275,17 @@
 (-0.
-1
 0
+9
 , -0.1
-0
+3
 ),  
@@ -1374,17 +1374,18 @@
 ray(%5B-0.
-5
+34
 , 0, 0%5D)

update steps on stairs