onsite_parking/planner/user_run.py

import json
import os
import sys

from matplotlib import pyplot as plt
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                "/../../onsite_parking/")
import input.make_map as mp
from input import make_car
from utils import replay
from utils import map_display as mdp
from planner.hybridastar import planner as planner
import numpy as np
import time


def linear_interpolation(x1,x2,y1,y2,num):
    x = np.linspace(x1,x2,num)
    y = np.linspace(y1,y2,num)
    return x,y

def changeOxyB01(C_,park,ori_ox,ori_oy):
  ox = ori_ox.copy()
  oy = ori_oy.copy()
  C = C_
  isSearchFailed = False

  if 1<=int(park)<=8:
    x = np.linspace(10300, 10300, 500)
    y = np.linspace(7000, 1000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

    x = np.linspace(9500, 9500, 500)
    y = np.linspace(7000, 1000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

    x = np.linspace(8800, 8800, 500)
    y = np.linspace(5500, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

    x = np.linspace(4500, 8800, 500)
    y = np.linspace(6000, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

    x = np.linspace(0, 9500, 500)
    y = np.linspace(6700, 6700, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

    x = np.linspace(8800, 8800, 500)
    y = np.linspace(2800, 5500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(7200, 8800, 500)
    y = np.linspace(1300, 2800, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(9200, 10300, 500)
    y = np.linspace(870, 1800, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    
    C.XY_RESO = C.XY_RESO - 600

  if 9<=int(park)<=10:
    x = np.linspace(8200, 8200, 500)
    y = np.linspace(7000, 0, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

    x = np.linspace(1500, 5800, 500)
    y = np.linspace(2000, 2000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

    x = np.linspace(2000, 2000, 500)
    y = np.linspace(4200, 2800, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

    x = np.linspace(2000, 5400, 500)
    y = np.linspace(2800, 2800, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    C.XY_RESO = C.XY_RESO - 500

  if 11<=int(park)<=12:
    x = np.linspace(8200, 8200, 500)
    y = np.linspace(7000, 0, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

    x = np.linspace(1500, 5800, 500)
    y = np.linspace(2000, 2000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

    x = np.linspace(1500, 1500, 500)
    y = np.linspace(2000, 4200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

    x = np.linspace(2800, 2800, 500)
    y = np.linspace(2800, 4200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

    x = np.linspace(2800, 5800, 500)
    y = np.linspace(2800, 2800, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

    C.XY_RESO = C.XY_RESO - 400

  if 13<=int(park)<=15:
    x = np.linspace(8200, 8200, 500)
    y = np.linspace(7000, 0, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

    x = np.linspace(1500, 5800, 500)
    y = np.linspace(2000, 2000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

    x = np.linspace(2400, 2400, 500)
    y = np.linspace(2000, 4200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

    x = np.linspace(4200, 4200, 500)
    y = np.linspace(2800, 4200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

    x = np.linspace(4200, 5800, 500)
    y = np.linspace(2800, 2800, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

    C.XY_RESO = C.XY_RESO - 400

  if int(park)==16:
    x = np.linspace(8200, 8200, 500)
    y = np.linspace(7000, 0, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

    x = np.linspace(1500, 5800, 500)
    y = np.linspace(1800, 1800, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

    x = np.linspace(1500, 5800, 500)
    y = np.linspace(3200, 3200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

    C.XY_RESO = C.XY_RESO - 300

  if park == '17':  # TODO
    isSearchFailed = True

  if park == '18':  # TODO
    isSearchFailed = True

  if park == '19' or park == '20' or park == '21'or park == '22'or park == '23'or park == '24'or park == '25':
    x = np.linspace(8300, 8300, 500)
    y = np.linspace(2800, 500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1500, 5100, 500)
    y = np.linspace(2900, 2900, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1500, 3000, 500)
    y = np.linspace(2200, 2200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5300, 3000, 500)
    y = np.linspace(2800, 2200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    C.XY_RESO = C.XY_RESO - 200
  
  if park == '26' or park == '27' or park == '28' or park == '29': # TODO
    isSearchFailed = True

  if park == '30' or park == '31' or park == '32' or park == '33' or park == '34' or park == '35':
    x = np.linspace(10000, 7000, 500)
    y = np.linspace(2000, 2000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  if park == '36' or park == '37' or park == '38' or park == '39' or park == '40':
    x = np.linspace(10000, 7000, 500)
    y = np.linspace(2000, 2000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(12000, 14000, 500)
    y = np.linspace(860, 860, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

  if park == '41' or park == '42' or park == '43':  # TODO
    isSearchFailed = True

  if park == '44' or park == '45' or park == '46'or park == '47'or park == '48'or park == '49':
    x = np.linspace(9000, 9000, 500)
    y = np.linspace(2800, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(7200, 8800, 500)
    y = np.linspace(2000, 2800, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(8500, 10200, 500)
    y = np.linspace(940, 1800, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(9000, 10500, 500)
    y = np.linspace(6300, 6300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    C.XY_RESO =  C.XY_RESO - 200
  if park == '50':
    x = np.linspace(9000, 9000, 500)
    y = np.linspace(2800, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(7200, 8800, 500)
    y = np.linspace(2000, 2800, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(8500, 10200, 500)
    y = np.linspace(940, 1800, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(9000, 10500, 500)
    y = np.linspace(6300, 6300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    C.XY_RESO =  C.XY_RESO - 500
    x = np.linspace(10100, 10100, 500)
    y = np.linspace(2500, 4500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(8200, 8200, 500)
    y = np.linspace(510, 1000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

  return ox,oy,isSearchFailed

def changeOxyB02(C_,park,ori_ox,ori_oy):
  ox = ori_ox.copy()
  oy = ori_oy.copy()
  isSearchFailed = False
  C = C_

  if park == '1':
    C.XY_RESO =  C.XY_RESO - 200
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(6000, 6000, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(4000, 4000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 4000, 500)
    y = np.linspace(5000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '2':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(6000, 6000, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(4500, 4500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 4000, 500)
    y = np.linspace(5000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '3':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(6000, 6000, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(4000, 4000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 4000, 500)
    y = np.linspace(5000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
  elif park == '4':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(6000, 6000, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(3500, 3500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 4000, 500)
    y = np.linspace(5000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
  elif park == '5':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(6000, 6000, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(3000, 3000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 4000, 500)
    y = np.linspace(5000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
  elif park == '6':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(6000, 6000, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(1500, 1500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 4000, 500)
    y = np.linspace(1500, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
  elif park == '7':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(6000, 6000, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(1000, 1000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 4000, 500)
    y = np.linspace(1000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
  elif park == '8':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(6000, 6000, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    # x = np.linspace(4000, 6000, 500)
    # y = np.linspace(1000, 1000, 500)
    # for i in range(len(x)):
    #   ox.append(x[i])
    #   oy.append(y[i])
    x = np.linspace(4000, 4000, 500)
    y = np.linspace(1000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
  elif park == '9':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5000, 6000, 500)
    y = np.linspace(6000, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4500, 4500, 500)
    y = np.linspace(0, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(5000, 5000, 500)
    y = np.linspace(1000, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])  
  elif park == '10':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5000, 6000, 500)
    y = np.linspace(6000, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '11':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5000, 6000, 500)
    y = np.linspace(6000, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4500, 4500, 500)
    y = np.linspace(1500, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5000, 5000, 500)
    y = np.linspace(0, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3000, 4500, 500)
    y = np.linspace(1500, 1500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '12':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5000, 6000, 500)
    y = np.linspace(6000, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4500, 4500, 500)
    y = np.linspace(1000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5000, 5000, 500)
    y = np.linspace(0, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3000, 4500, 500)
    y = np.linspace(1000, 1000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '13':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5000, 6000, 500)
    y = np.linspace(6000, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4500, 4500, 500)
    y = np.linspace(1000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5000, 5000, 500)
    y = np.linspace(0, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(2000, 4500, 500)
    y = np.linspace(1000, 1000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3000, 5000, 500)
    y = np.linspace(300, 300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '14':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5000, 6000, 500)
    y = np.linspace(6000, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4500, 4500, 500)
    y = np.linspace(1000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5000, 5000, 500)
    y = np.linspace(0, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(0, 4500, 500)
    y = np.linspace(1000, 1000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(2000, 5000, 500)
    y = np.linspace(300, 300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '15' or park == '16' or park == '17' or park == '18' or park == '19' or park == '20' or park == '21' or park == '22':
    C.XY_RESO =  C.XY_RESO - 200
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1800, 1800, 500)
    y = np.linspace(1000, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])     
    x = np.linspace(500, 5000, 500)
    y = np.linspace(7300, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '23' or park == '24': # TOD: 23,24
    C.XY_RESO =  C.XY_RESO - 300
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1800, 1800, 500)
    y = np.linspace(1000, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])     
    x = np.linspace(500, 5000, 500)
    y = np.linspace(7300, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])     
  elif park =='25':
    C.XY_RESO =  C.XY_RESO - 300
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1800, 1800, 500)
    y = np.linspace(1000, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])     
    x = np.linspace(500, 5000, 500)
    y = np.linspace(7300, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(500, 2500, 500)
    y = np.linspace(5500, 5500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])       
  elif park == '26':
    C.XY_RESO =  C.XY_RESO - 200
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1800, 1800, 500)
    y = np.linspace(1000, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])     
    x = np.linspace(500, 5000, 500)
    y = np.linspace(7300, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '27':
    C.XY_RESO =  C.XY_RESO - 300
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(1500, 5000, 500)
    y = np.linspace(7300, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '28':
    C.XY_RESO =  C.XY_RESO - 300
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(3500, 5000, 500)
    y = np.linspace(7300, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '29': # TOD: 29
    C.XY_RESO =  C.XY_RESO - 300
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(4000, 5000, 500)
    y = np.linspace(7300, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '30':
    C.XY_RESO =  C.XY_RESO - 200
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(500, 5000, 500)
    y = np.linspace(7300, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '31':
    C.XY_RESO =  C.XY_RESO - 200
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(500, 5000, 500)
    y = np.linspace(7300, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1000, 1000, 500)
    y = np.linspace(200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i]) 
  elif park == '32':
    C.XY_RESO =  C.XY_RESO - 700
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(500, 5500, 500)
    y = np.linspace(7200, 7200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1200, 1200, 500)
    y = np.linspace(200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(1800, 1800, 500)
    y = np.linspace(5500, 6500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])  
    x = np.linspace(500, 5500, 500)
    y = np.linspace(4200, 4200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i]) 
  elif park == '33' or park == '34' or park == '36':
    C.XY_RESO =  C.XY_RESO - 200
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(500, 5000, 500)
    y = np.linspace(7300, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1000, 1000, 500)
    y = np.linspace(200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '35':
    C.XY_RESO =  C.XY_RESO - 700
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(500, 5500, 500)
    y = np.linspace(7200, 7200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1200, 1200, 500)
    y = np.linspace(200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(1800, 1800, 500)
    y = np.linspace(4500, 6500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])  
    x = np.linspace(500, 5500, 500)
    y = np.linspace(1500, 1500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i]) 
  elif park == '37':
    C.XY_RESO =  C.XY_RESO - 700
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(500, 5500, 500)
    y = np.linspace(7200, 7200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1200, 1200, 500)
    y = np.linspace(200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(1800, 1800, 500)
    y = np.linspace(2500, 6500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])  
    x = np.linspace(500, 5500, 500)
    y = np.linspace(500, 500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '38':
    C.XY_RESO =  C.XY_RESO - 700
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(500, 5500, 500)
    y = np.linspace(7200, 7200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1200, 1200, 500)
    y = np.linspace(200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(1800, 1800, 500)
    y = np.linspace(2500, 6500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])  
    x = np.linspace(500, 5500, 500)
    y = np.linspace(500, 500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
  elif park == '39' or park == '42' or park == '43' or park == '44':
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(500, 5500, 500)
    y = np.linspace(7200, 7200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])    
    x = np.linspace(4500, 4500, 500)
    y = np.linspace(500, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])  
    x = np.linspace(3500, 3500, 500)
    y = np.linspace(6200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])    
  elif park == '40' or park == '41' or park == '45':
    x = np.linspace(3500, 3500, 500)
    y = np.linspace(6200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])  
    x = np.linspace(3500, 5000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])  
    x = np.linspace(5100, 5100, 500)
    y = np.linspace(3700, 6500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
  elif park == '46' or park == '47':
    x = np.linspace(3500, 3500, 500)
    y = np.linspace(6200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])  
    x = np.linspace(3500, 5000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])  
    x = np.linspace(5100, 5100, 500)
    y = np.linspace(3700, 6500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(3500, 5000, 500)
    y = np.linspace(4500, 4500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])       
  return ox,oy,isSearchFailed

def changeOxyB03(C_,park,ori_ox,ori_oy):
  ox = ori_ox.copy()
  oy = ori_oy.copy()
  isSearchFailed = False
  C = C_
  if park == '1' or park == '2' or park == '3' or park == '4' or park == '5' or park == '6' or park == '7' or park == '8' or park == '9' or park == '10' or park == '11' or park == '12' or park == '13' or park == '14' or park == '15' or park == '16': 
    C.XY_RESO =  C.XY_RESO - 200
    x = np.linspace(4500, 4500, 500)
    y = np.linspace(6000, 5000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 3500, 500)
    y = np.linspace(3100, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(0, 2100, 500)
    y = np.linspace(5600, 5600, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])

    x = np.linspace(2100, 2100, 500)
    y = np.linspace(4500, 5700, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1000, 4000, 500)
    y = np.linspace(6600, 6600, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1000, 1000, 100)
    y = np.linspace(7700, 6600, 100)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1000, 3500, 100)
    y = np.linspace(3100, 3100, 100)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '17' or park == '18' or park == '19' or park == '20':
    x = np.linspace(4500, 4500, 500)
    y = np.linspace(6000, 5000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  if park == '24' or park == '25' or park == '26' or park == '27' or park == '30' or park == '31' or park == '32':
     isSearchFailed = True
  if park == '28' or park == '29':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(1000, 1000, 500)
    y = np.linspace(7700, 6600, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1000, 5000, 500)
    y = np.linspace(6600, 6600, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])  
    x = np.linspace(1000, 5000, 500)
    y = np.linspace(4500, 4500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])  
    x = np.linspace(4500, 4500, 500)
    y = np.linspace(5000, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  if park == '62'or park == '61':
      for i in range(0, 3000, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 1):
          ox.append(i)
          oy.append(6500)
      for i in range(6500, 7700, 1):
          ox.append(1000)
          oy.append(i)
      x = np.linspace(3000, 4500, 500)
      y = np.linspace(5600, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 7800, 500)
      y = np.linspace(5000, 5300, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(8200, 10500, 500)
      y = np.linspace(5720, 7700, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 9200, 500)
      y = np.linspace(4050, 4400, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
  if park == '60' or park == '59':
      x = np.linspace(0, 3000, 500)
      y = np.linspace(5600, 5600, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      for i in range(1000, 4500, 1):
          ox.append(i)
          oy.append(6500)
      for i in range(6500, 7700, 1):
          ox.append(1000)
          oy.append(i)
      x = np.linspace(3000, 4500, 500)
      y = np.linspace(5600, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 9200, 500)
      y = np.linspace(5000, 4400, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 7000, 500)
      y = np.linspace(3100, 3100, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(8500, 7500, 500)
      y = np.linspace(3100, 4800, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
  if park == '58' or park == '57':
      for i in range(0, 3000, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 1):
          ox.append(i)
          oy.append(6500)
      for i in range(6500, 7700, 1):
          ox.append(1000)
          oy.append(i)
      x = np.linspace(3000, 4500, 500)
      y = np.linspace(5600, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 9200, 500)
      y = np.linspace(5000, 4400, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 7000, 500)
      y = np.linspace(4000, 3100, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
  if park == '56':
      x = np.linspace(0, 3000, 500)
      y = np.linspace(5600, 5600, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      for i in range(1000, 4500, 1):
          ox.append(i)
          oy.append(6500)
      for i in range(6500, 7700, 1):
          ox.append(1000)
          oy.append(i)
      x = np.linspace(3000, 4500, 500)
      y = np.linspace(5600, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 9200, 500)
      y = np.linspace(5000, 4100, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 8500, 500)
      y = np.linspace(4100, 3100, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      C.XY_RESO = C.XY_RESO - 200
  if park == '55':
      for i in range(0, 3000, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 1):
          ox.append(i)
          oy.append(6500)
      for i in range(6500, 7700, 1):
          ox.append(1000)
          oy.append(i)
      x = np.linspace(3000, 4500, 500)
      y = np.linspace(5600, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 9200, 500)
      y = np.linspace(5000, 4100, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 8500, 500)
      y = np.linspace(4100, 3100, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      C.XY_RESO = C.XY_RESO - 300
  if park == '54':
      x = np.linspace(0, 3000, 500)
      y = np.linspace(5750, 5750, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      for i in range(1000, 4500, 1):
          ox.append(i)
          oy.append(6500)
      for i in range(6500, 7700, 1):
          ox.append(1000)
          oy.append(i)
      x = np.linspace(3000, 4500, 500)
      y = np.linspace(5750, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 9600, 500)
      y = np.linspace(5000, 4050, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 9200, 500)
      y = np.linspace(4000, 3100, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(4500, 4200, 500)
      y = np.linspace(6000, 6500, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      C.XY_RESO = C.XY_RESO - 400
  if park == '53' or park == '52':
      for i in range(0, 3000, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 1):
          ox.append(i)
          oy.append(6500)
      for i in range(6500, 7700, 1):
          ox.append(1000)
          oy.append(i)
      x = np.linspace(3000, 4500, 500)
      y = np.linspace(5600, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 9900, 500)
      y = np.linspace(5000, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(7500, 7500, 500)
      y = np.linspace(3100, 6000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      # x = np.linspace(6000, 7500, 500)
      # y = np.linspace(730, 730, 500)
      # for i in range(len(x)):
      #     ox.append(x[i])
      #     oy.append(y[i])
      x = np.linspace(7000, 7500, 500)
      y = np.linspace(1700, 1700, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      C.XY_RESO = C.XY_RESO-100
  if park == '51' or park == '50' or park == '49'or park == '48'or park == '47':
      for i in range(0, 3000, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 1):
          ox.append(i)
          oy.append(6500)
      for i in range(6500, 7700, 1):
          ox.append(1000)
          oy.append(i)
      x = np.linspace(3000, 4500, 500)
      y = np.linspace(5600, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 9100, 500)
      y = np.linspace(5000, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      # x = np.linspace(6000, 10500, 500)
      # y = np.linspace(700, 700, 500)
      # for i in range(len(x)):
      #     ox.append(x[i])
      #     oy.append(y[i])
      x = np.linspace(7500, 7500, 500)
      y = np.linspace(3100, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(7000, 8000, 500)
      y = np.linspace(3100, 3100, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      C.XY_RESO = C.XY_RESO - 100
  if park == '46' or park == '45':
      for i in range(0, 2200, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 1):
          ox.append(i)
          oy.append(6500)
      for i in range(6500, 7700, 1):
          ox.append(1000)
          oy.append(i)
      x = np.linspace(2100, 4500, 500)
      y = np.linspace(5600, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 9100, 500)
      y = np.linspace(5000, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
  if park == '44' or park == '43':
      for i in range(0, 2200, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 1):
          ox.append(i)
          oy.append(6500)
      for i in range(6500, 7700, 1):
          ox.append(1000)
          oy.append(i)
      x = np.linspace(2100, 4500, 500)
      y = np.linspace(5600, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(7000, 10500, 500)
      y = np.linspace(3100, 3100, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 10500, 500)
      y = np.linspace(5000, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(8500, 8500, 500)
      y = np.linspace(3100, 6000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
  if park == '42':
      for i in range(0, 2200, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 1):
          ox.append(i)
          oy.append(6500)
      for i in range(6500, 7700, 1):
          ox.append(1000)
          oy.append(i)
      x = np.linspace(2100, 4500, 500)
      y = np.linspace(5600, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(7000, 10500, 500)
      y = np.linspace(3100, 3100, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 8000, 500)
      y = np.linspace(5000, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6900, 6900, 500)
      y = np.linspace(3100, 6000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6950, 6950, 500)
      y = np.linspace(1700, 2700, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      C.XY_RESO = C.XY_RESO - 100
  if park == '41':
      for i in range(0, 2200, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 1):
          ox.append(i)
          oy.append(6500)
      for i in range(6500, 7700, 1):
          ox.append(1000)
          oy.append(i)
      x = np.linspace(2100, 4500, 500)
      y = np.linspace(5600, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(7000, 10500, 500)
      y = np.linspace(3100, 3100, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 8000, 500)
      y = np.linspace(5000, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6900, 6900, 500)
      y = np.linspace(3100, 6000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(7000, 8000, 500)
      y = np.linspace(750, 750, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      C.XY_RESO = C.XY_RESO - 100
  if park == '40':
      for i in range(0, 2200, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 1):
          ox.append(i)
          oy.append(6500)
      for i in range(6500, 7700, 1):
          ox.append(1000)
          oy.append(i)
      x = np.linspace(2100, 4500, 500)
      y = np.linspace(5600, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(7000, 10500, 500)
      y = np.linspace(3100, 3100, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 8000, 500)
      y = np.linspace(5000, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6900, 6900, 500)
      y = np.linspace(3100, 6000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      C.XY_RESO = C.XY_RESO - 100
  if park == '39':
      for i in range(0, 2200, 1):
          ox.append(i)
          oy.append(5700)
      for i in range(1000, 4500, 1):
          ox.append(i)
          oy.append(6500)
      for i in range(6500, 7700, 1):
          ox.append(1000)
          oy.append(i)
      x = np.linspace(2100, 4500, 500)
      y = np.linspace(5700, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(7000, 10500, 500)
      y = np.linspace(3100, 3100, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 8000, 500)
      y = np.linspace(5700, 5700, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(7000, 7000, 500)
      y = np.linspace(3100, 6000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(7000, 9000, 500)
      y = np.linspace(1700, 1700, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 8500, 500)
      y = np.linspace(5000, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6950, 6950, 500)
      y = np.linspace(1600, 3700, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 8500, 500)
      y = np.linspace(780, 780, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 6500, 500)
      y = np.linspace(1200, 750, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(4500, 4000, 500)
      y = np.linspace(6000, 6500, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      C.XY_RESO = C.XY_RESO - 300
  if park == '38':
      for i in range(0, 2200, 1):
          ox.append(i)
          oy.append(5700)
      for i in range(1000, 4500, 1):
          ox.append(i)
          oy.append(6500)
      for i in range(6500, 7700, 1):
          ox.append(1000)
          oy.append(i)
      x = np.linspace(2200, 4500, 500)
      y = np.linspace(5700, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(7000, 10500, 500)
      y = np.linspace(3100, 3100, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 8000, 500)
      y = np.linspace(5700, 5700, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(7000, 7000, 500)
      y = np.linspace(3100, 6000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(7000, 9500, 500)
      y = np.linspace(1700, 1700, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 8500, 500)
      y = np.linspace(5000, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6950, 6950, 500)
      y = np.linspace(1600, 3700, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 9500, 500)
      y = np.linspace(780, 780, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 6500, 500)
      y = np.linspace(1200, 750, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(4500, 4100, 500)
      y = np.linspace(6000, 6500, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      C.XY_RESO = C.XY_RESO - 400
  if park == '37'or park == '36' or park == '35' or park == '34':
      for i in range(0, 2200, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 1):
          ox.append(i)
          oy.append(6500)
      for i in range(6500, 7700, 1):
          ox.append(1000)
          oy.append(i)
      x = np.linspace(2100, 4500, 500)
      y = np.linspace(5600, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 9200, 500)
      y = np.linspace(4000, 4400, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
  if park == '33':
      for i in range(0, 2200, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 1):
          ox.append(i)
          oy.append(6500)
      for i in range(6500, 7700, 1):
          ox.append(1000)
          oy.append(i)
      x = np.linspace(2100, 4500, 500)
      y = np.linspace(5600, 5000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(6000, 9900, 500)
      y = np.linspace(4000, 5200, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      x = np.linspace(8800, 8800, 500)
      y = np.linspace(6700, 4000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
  if park == '23' or park == '22' or park == '21':
    x = np.linspace(4500, 4500, 500)
    y = np.linspace(6000, 5000, 500)
    for i in range(len(x)):
        ox.append(x[i])
        oy.append(y[i])
    x = np.linspace(0, 2100, 500)
    y = np.linspace(5700, 5700, 500)
    for i in range(len(x)):
        ox.append(x[i])
        oy.append(y[i])
    x = np.linspace(0, 1000, 500)
    y = np.linspace(3000, 3000, 500)
    for i in range(len(x)):
        ox.append(x[i])
        oy.append(y[i])
    x = np.linspace(3500, 3500, 500)
    y = np.linspace(3100, 7000, 500)
    for i in range(len(x)):
        ox.append(x[i])
        oy.append(y[i])
    x = np.linspace(0, 2100, 500)
    y = np.linspace(4000, 4000, 500)
    for i in range(len(x)):
        ox.append(x[i])
        oy.append(y[i])
    x = np.linspace(2100, 2100, 100)
    y = np.linspace(4600, 5100, 100)
    for i in range(len(x)):
        ox.append(x[i])
        oy.append(y[i])
    x = np.linspace(1000, 4000, 500)
    y = np.linspace(6600, 6600, 500)
    for i in range(len(x)):
        ox.append(x[i])
        oy.append(y[i])
    x = np.linspace(1000, 1000, 100)
    y = np.linspace(7700, 6600, 100)
    for i in range(len(x)):
        ox.append(x[i])
        oy.append(y[i])
    x = np.linspace(2500, 3500, 100)
    y = np.linspace(3100, 3100, 100)
    for i in range(len(x)):
        ox.append(x[i])
        oy.append(y[i])
    C.XY_RESO = C.XY_RESO - 100
  return ox,oy,isSearchFailed

def main():
  in_path="./input/"
  # 遍历input文件夹下所有文件
  start = time.time()
  for root,dirs,files in os.walk(in_path):
    for file in files:
      if "json" in file: # 找到.json文件
        t1 = time.time()
        map_path = in_path+file
        # mdp.map_display(map_path) #  仅绘制地图
        ori_ox, ori_oy,sp,gp = mp.make_map(in_path+file) #获取地图信息（障碍物，起点，车位）

        sx, sy, syaw0 = sp['x'], sp['y'], sp['yaw']# 获取起点的信息
        C = make_car.C # 获取车辆的属性信息
        isSearchFailed = False
        # 遍历所有车位
        for i in range(1,len(gp)+1):
          # print(f"parking:{str(i)}")
          # 获取该车位的信息
          gx, gy, gyaw0 = gp[str(i)]['x_end'], gp[str(i)]['y_end'], gp[str(i)]['yaw']
          if file == "B01.json":
            # continue
            add_ox,add_oy,isSearchFailed = changeOxyB01(C,str(i),ori_ox, ori_oy)
            # continue
          elif file == "B02.json":
            # continue
            add_ox,add_oy,isSearchFailed = changeOxyB02(C,str(i),ori_ox, ori_oy)
          elif file == "B03.json":
            add_ox,add_oy,isSearchFailed = changeOxyB03(C,str(i),ori_ox, ori_oy)
          # 请在此调用planer文件夹下的规控算法
          if isSearchFailed:
            print(f"Searching failed! parking:{str(i)}")
            with open(f"./output/result_{str(file).split('.json')[0]}_{str(i)}.json", "w") as file1:
              json.dump({}, file1)
            isSearchFailed = False
            continue
          try:
            path = planner.hybrid_astar_planning(sx, sy, syaw0, gx, gy, gyaw0, add_ox, add_oy, C.XY_RESO, C.YAW_RESO)
            C.XY_RESO = 1000
          except:
            continue
          # 算法测试结果保存
          # print(C.XY_RESO)
          if not path:
            print("Searching failed!")
            continue
          output_dit={
            "parking":str(i),
            "output_x": path.x,
            "output_y": path.y,
            "output_yaw": path.yaw,
            "output_dir": path.direction,
          }
          with open(f"./output/result_{str(file).split('.json')[0]}_{str(i)}.json", "w") as file1:
            json.dump(output_dit, file1)
          print(f"output/result_{str(file).split('.json')[0]}_{str(i)}.json saved!")

          # 仿真回放
          # result_path = f"./Onsite_Parking/output/result_{map_path.split('/')[-1].split('.json')[0]}_{str(i)}.json"
          # replay.replay(map_path, result_path)
        t2 = time.time()
        print(f"Time in map {file}: {t2-t1}")
  end = time.time()
  print("Time:", end-start)
if __name__ == '__main__':
    main()