#!/usr/bin/env python
import sys
import time, json

from selfdrive.test.plant import plant
from selfdrive.config import Conversions as CV, CruiseButtons as CB
from maneuver import *

maneuvers = [
  Maneuver(
    'while cruising at 40 mph, change cruise speed to 50mph', 
    duration=30.,
    initial_speed = 40. * CV.MPH_TO_MS,
    cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3),
                             (CB.RES_ACCEL, 10.), (0, 10.1),
                             (CB.RES_ACCEL, 10.2), (0, 10.3)]
  ),
  Maneuver(
    'while cruising at 60 mph, change cruise speed to 50mph', 
    duration=30., 
    initial_speed=60. * CV.MPH_TO_MS,
    cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3),
                             (CB.DECEL_SET, 10.), (0, 10.1),
                             (CB.DECEL_SET, 10.2), (0, 10.3)]
  ),
  Maneuver(
    'while cruising at 20mph, grade change +10%', 
    duration=25.,
    initial_speed=20. * CV.MPH_TO_MS, 
    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
    grade_values = [0., 0., 1.0],
    grade_breakpoints = [0., 10., 11.]
  ),
  Maneuver(
    'while cruising at 20mph, grade change -10%', 
    duration=25.,
    initial_speed=20. * CV.MPH_TO_MS, 
    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
    grade_values = [0., 0., -1.0],
    grade_breakpoints = [0., 10., 11.]
  ),
  Maneuver(
    'approaching a 40mph car while cruising at 60mph from 100m away', 
    duration=30., 
    initial_speed = 60. * CV.MPH_TO_MS, 
    lead_relevancy=True, 
    initial_distance_lead=100., 
    speed_lead_values = [40.*CV.MPH_TO_MS, 40.*CV.MPH_TO_MS],
    speed_lead_breakpoints = [0., 100.],
    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
  ),
  Maneuver(
    'approaching a 0mph car while cruising at 40mph from 150m away', 
    duration=30., 
    initial_speed = 40. * CV.MPH_TO_MS, 
    lead_relevancy=True, 
    initial_distance_lead=150., 
    speed_lead_values = [0.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
    speed_lead_breakpoints = [0., 100.],
    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
  ),
  Maneuver(
    'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2', 
    duration=50., 
    initial_speed = 20., 
    lead_relevancy=True, 
    initial_distance_lead=35., 
    speed_lead_values = [20.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
    speed_lead_breakpoints = [0., 15., 35.0],
    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
  ),
  Maneuver(
    'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2', 
    duration=50., 
    initial_speed = 20., 
    lead_relevancy=True, 
    initial_distance_lead=35., 
    speed_lead_values = [20.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
    speed_lead_breakpoints = [0., 15., 25.0],
    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
  ),
  Maneuver(
    'starting at 0mph, approaching a stopped car 100m away', 
    duration=30., 
    initial_speed = 0., 
    lead_relevancy=True, 
    initial_distance_lead=100., 
    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
                             (CB.RES_ACCEL, 1.4), (0.0, 1.5), 
                             (CB.RES_ACCEL, 1.6), (0.0, 1.7), 
                             (CB.RES_ACCEL, 1.8), (0.0, 1.9)]
  ),
  Maneuver(
    "following a car at 60mph, lead accel and decel at 0.5m/s^2 every 2s",
    duration=25.,
    initial_speed=30.,
    lead_relevancy=True,
    initial_distance_lead=49.,
    speed_lead_values=[30.,30.,29.,31.,29.,31.,29.],
    speed_lead_breakpoints=[0., 6., 8., 12.,16.,20.,24.],
    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
                             (CB.RES_ACCEL, 1.4), (0.0, 1.5), 
                             (CB.RES_ACCEL, 1.6), (0.0, 1.7)]
  ),
  Maneuver(
    "following a car at 10mph, stop and go at 1m/s2 lead dece1 and accel",
    duration=70.,
    initial_speed=10.,
    lead_relevancy=True,
    initial_distance_lead=20.,
    speed_lead_values=[10., 0., 0., 10., 0.,10.],
    speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.],
    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
                             (CB.RES_ACCEL, 1.4), (0.0, 1.5), 
                             (CB.RES_ACCEL, 1.6), (0.0, 1.7)]
  ),
  Maneuver(
    "green light: stopped behind lead car, lead car accelerates at 1.5 m/s",
    duration=30.,
    initial_speed=0.,
    lead_relevancy=True,
    initial_distance_lead=4.,
    speed_lead_values=[0,  0 , 45],
    speed_lead_breakpoints=[0, 10., 40.],
    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
                             (CB.RES_ACCEL, 1.4), (0.0, 1.5), 
                             (CB.RES_ACCEL, 1.6), (0.0, 1.7), 
                             (CB.RES_ACCEL, 1.8), (0.0, 1.9), 
                             (CB.RES_ACCEL, 2.0), (0.0, 2.1), 
                             (CB.RES_ACCEL, 2.2), (0.0, 2.3)]
  ),
  Maneuver(
    "stop and go with 1m/s2 lead decel and accel, with full stops",
    duration=70.,
    initial_speed=0.,
    lead_relevancy=True,
    initial_distance_lead=20.,
    speed_lead_values=[10., 0., 0., 10., 0., 0.] ,
    speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.],
    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
                             (CB.RES_ACCEL, 1.4), (0.0, 1.5), 
                             (CB.RES_ACCEL, 1.6), (0.0, 1.7)] 
  ),
  Maneuver(
    "accelerate from 20 while lead vehicle decelerates from 40 to 20 at 1m/s2",
    duration=30.,
    initial_speed=10.,
    lead_relevancy=True,
    initial_distance_lead=10.,
    speed_lead_values=[20., 10.],
    speed_lead_breakpoints=[1., 11.],
    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
                             (CB.RES_ACCEL, 1.4), (0.0, 1.5), 
                             (CB.RES_ACCEL, 1.6), (0.0, 1.7), 
                             (CB.RES_ACCEL, 1.8), (0.0, 1.9), 
                             (CB.RES_ACCEL, 2.0), (0.0, 2.1), 
                             (CB.RES_ACCEL, 2.2), (0.0, 2.3)]
  ),
  Maneuver(
    "accelerate from 20 while lead vehicle decelerates from 40 to 0 at 2m/s2",
    duration=30.,
    initial_speed=10.,
    lead_relevancy=True,
    initial_distance_lead=10.,
    speed_lead_values=[20., 0.],
    speed_lead_breakpoints=[1., 11.],
    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
                             (CB.RES_ACCEL, 1.4), (0.0, 1.5), 
                             (CB.RES_ACCEL, 1.6), (0.0, 1.7), 
                             (CB.RES_ACCEL, 1.8), (0.0, 1.9), 
                             (CB.RES_ACCEL, 2.0), (0.0, 2.1), 
                             (CB.RES_ACCEL, 2.2), (0.0, 2.3)]
  )
]

css_style = """
.maneuver_title {
  font-size: 24px;
  text-align: center;
}
.maneuver_graph {
  width: 100%;
}
"""

def main(output_dir):
  view_html = "<html><head><style>%s</style></head><body><table>" % (css_style,)
  for i, man in enumerate(maneuvers):
    view_html += "<tr><td class='maneuver_title' colspan=5><div>%s</div></td></tr><tr>" % (man.title,)
    for c in ['distance.svg', 'speeds.svg', 'acceleration.svg', 'pedals.svg', 'pid.svg']:
      view_html += "<td><img class='maneuver_graph' src='%s'/></td>" % (os.path.join("maneuver" + str(i+1).zfill(2), c), )
    view_html += "</tr>"

  with open(os.path.join(output_dir, "index.html"), "w") as f:
    f.write(view_html)

  for i, man in enumerate(maneuvers):
    score, plot = man.evaluate()
    plot.write_plot(output_dir, "maneuver" + str(i+1).zfill(2))

if __name__ == "__main__":
  if len(sys.argv) <= 1:
    print "Usage:", sys.argv[0], "<output_dir>"
    exit(1)

  main(sys.argv[1])