#!/usr/bin/env python

import rospy
import rospkg
import rosparam
import yaml
import dynamic_reconfigure.client
import os
import argparse

import time
from std_msgs.msg import Float32MultiArray, Int32MultiArray, Bool, Int16, String, Int8, UInt16, UInt8
from jmoab_autopilot_ros.msg import GoalWaypoints
from sensor_msgs.msg import NavSatFix
from geometry_msgs.msg import TwistWithCovarianceStamped
import numpy as np
from simple_pid import PID

class GPS_NAV(object):

	def __init__(self, param_file, mission_file, NS):

		################################ Init/Pub/Sub #####################################
		rospy.init_node("gps_waypoints_autopilot_node", anonymous=True)

		################################ ROS Parameters #####################################
		sv_node = "gps_waypoints_server_node"

		# https://answers.ros.org/question/169866/load-yaml-with-code/
		# load yaml file to rosparam server without running server on python
		f = open(param_file, 'r')
		yamlfile = yaml.load(f)
		rosparam.upload_params("/", yamlfile)

		## get parameter from rosparam server that we just loaded above
		self.max_start_str = rosparam.get_param(sv_node+"/max_start_str")
		self.min_start_str = rosparam.get_param(sv_node+"/min_start_str")
		self.str_adj = rosparam.get_param(sv_node+"/str_adj")
		self.skid_adj = rosparam.get_param(sv_node+"/skid_adj")
		self.str_mid = rosparam.get_param(sv_node+"/str_mid")
		self.max_start_thr = rosparam.get_param(sv_node+"/max_start_thr")
		self.thr_adj = rosparam.get_param(sv_node+"/thr_adj")
		self.thr_slowest = rosparam.get_param(sv_node+"/thr_slowest")
		self.thr_mid = rosparam.get_param(sv_node+"/thr_mid")
		self.goal_dist_thresh = rosparam.get_param(sv_node+"/goal_dist_thresh")
		self.goal_ang_thresh = rosparam.get_param(sv_node+"/goal_ang_thresh")
		self.x_track_error_start = rosparam.get_param(sv_node+"/x_track_error_start")
		self.x_track_repose_dist = rosparam.get_param(sv_node+"/x_track_repose_dist")
		self.pid_hdg_out_thresh = rosparam.get_param(sv_node+"/pid_hdg_out_thresh")
		self.pid_x_out_thresh = rosparam.get_param(sv_node+"/pid_x_out_thresh")
		self.hdg_p = rosparam.get_param(sv_node+"/hdg_p")
		self.hdg_i = rosparam.get_param(sv_node+"/hdg_i")
		self.hdg_d = rosparam.get_param(sv_node+"/hdg_d")
		self.cross_p = rosparam.get_param(sv_node+"/cross_p")
		self.cross_i = rosparam.get_param(sv_node+"/cross_i")
		self.cross_d = rosparam.get_param(sv_node+"/cross_d")
		self.vel_p = rosparam.get_param(sv_node+"/vel_p")
		self.vel_i = rosparam.get_param(sv_node+"/vel_i")
		self.vel_d = rosparam.get_param(sv_node+"/vel_d")
		self.use_heartbeat = rosparam.get_param(sv_node+"/use_heartbeat")

		########################### GPS Parameters ##########################
		self.lat = 0.0
		self.lon = 0.0
		self.hdg = 0.0

		self.lat1 = 0.0
		self.lon1 = 0.0

		self.lat2 = 0.0
		self.lon2 = 0.0

		self.gps_abs_vel = 0.0

		self.use_gps1 = False
		self.use_gps2 = False

		mission_name = "mission.txt"
		# self.mission_path = os.path.join(jmoab_autopilot_ros_path, "waypoints", mission_name)
		self.mission_path = mission_file

		self.lat_target_list = []
		self.lon_target_list = []
		self.speed_target_list = []
		self.delay_target_list = []
		self.sbus_throttle_target_list = []
		self.relay_target_list = []
		self.total_points = 0
		self.target_wp = 0

		self.got_new_wps = False

		self.hb_timestamp = time.time()
		self.hb_timeout = 10.0
		self.hb_period = 0.0

		########################### PID Parameters ##########################
		## PID heading steering control
		self.setpoint_diff_hdg = 0.0

		self.max_err = 100.0	#10.0	#2.0

		self.pid_hdg = PID(self.hdg_p, self.hdg_i, self.hdg_d, setpoint=self.setpoint_diff_hdg)
		self.pid_hdg.tunings = (self.hdg_p, self.hdg_i, self.hdg_d)
		self.pid_hdg.sample_time = 0.001
		self.pid_hdg.output_limits = (-self.max_err, self.max_err)

		## PID cross track error steering control
		self.setpoint_x = 0.0

		self.max_x_err = 100.0	#2.0

		self.pid_x = PID(self.cross_p, self.cross_i, self.cross_d, setpoint=self.setpoint_x)
		self.pid_x.tunings = (self.cross_p, self.cross_i, self.cross_d)
		self.pid_x.sample_time = 0.001
		self.pid_x.output_limits = (-self.max_x_err, self.max_x_err)

		self.pid_vel = PID(self.vel_p, self.vel_i, self.vel_d, setpoint=1.0)
		self.pid_vel.tunings = (self.vel_p, self.vel_i, self.vel_d)
		self.pid_vel.sample_time = 0.001
		self.max_vel = 3.0
		self.pid_vel.output_limits = (0.0, self.max_vel)

		self.enable_update = True
		################################ Dynamic Reconfigure #####################################
		print("Wait for server node...")
		try:
			## This we will get live data from rqt_reconfigure
			self.client = dynamic_reconfigure.client.Client(sv_node, timeout=2, config_callback=self.param_callback)
			print("Open rqt_reconfigure to tune paramters realtime")
		except rospy.ROSException:
			print("Server node is not alive, load parameters from GpsWaypoints.yaml file")
			pass

		########################### Robot Parameters ##########################
		self.ROBOT_MODE = "MANUAL"
		#self.sbus_throttle_mid = 1024
		#self.sbus_steering_mid = 969	#1024
		self.cart_mode = 0
		self.completed_flag = False
		self.hdg_calib_flag = False

		self.sbus_max = 1680
		self.vel_min = 0.2
		self.vel_max = 2.2

		self.obst_flag = False

		self.update_local_param()

		########################### Pub/Sub Topics ##########################

		rospy.Subscriber(self.namespace_attaching(NS, "/ublox/fix"), NavSatFix, self.gps_callback)
		rospy.Subscriber(self.namespace_attaching(NS, "/ublox2/fix"), NavSatFix, self.gps2_callback)
		rospy.Subscriber(self.namespace_attaching(NS, "/jmoab_compass"), Float32MultiArray, self.compass_callback)
		rospy.Subscriber(self.namespace_attaching(NS, "/atcart_mode"), Int8, self.atcart_mode_callback)
		# rospy.Subscriber("/hdg_calib_flag", Bool, self.hdg_calib_flag_callback)
		rospy.Subscriber(self.namespace_attaching(NS, "/heartbeat"), Bool, self.heartbeat_callback)
		rospy.Subscriber(self.namespace_attaching(NS, "/goal_waypoints"), GoalWaypoints, self.goal_wp_callback)
		rospy.Subscriber(self.namespace_attaching(NS, "/request_store_waypoints"), Bool, self.request_wp_callback)
		rospy.Subscriber(self.namespace_attaching(NS, "/obstacle_detection"), UInt8, self.obst_detect_callback)
		rospy.Subscriber(self.namespace_attaching(NS, "/ublox/fix_velocity"), TwistWithCovarianceStamped, self.gps_vel_callback)

		self.sbus_cmd_pub = rospy.Publisher(self.namespace_attaching(NS, "/sbus_cmd"), Int32MultiArray, queue_size=10)
		self.sbus_cmd = Int32MultiArray()
		self.reply_wp_pub = rospy.Publisher(self.namespace_attaching(NS, "/reply_goal_waypoints"), GoalWaypoints, queue_size=10)
		self.reply_wp_msg = GoalWaypoints()
		self.arrived_wp_pub = rospy.Publisher(self.namespace_attaching(NS, "/arrived_waypoints"), UInt16, queue_size=10)
		self.arrived_wp_msg = UInt16()
		self.atcart_mode_cmd_pub = rospy.Publisher(self.namespace_attaching(NS, "/atcart_mode_cmd"), Int8, queue_size=10)
		self.atcart_mode_cmd_msg = Int8()
		self.jmoab_relay1_pub = rospy.Publisher(self.namespace_attaching(NS, "/jmoab_relay1"), Bool, queue_size=10)
		self.jmoab_relay1_msg = Bool()
		self.mission_completed_pub = rospy.Publisher(self.namespace_attaching(NS, "/mission_completed"), Bool, queue_size=10)
		self.mission_completed_msg = Bool()

		########################### Start ##########################
		self.get_target_points()

		time.sleep(1)
		self.loop()
		rospy.spin()

	def namespace_attaching(self, NS, topic_name):
		if NS is None:
			return topic_name
		else:
			if NS.startswith("/"):
				topic_name = NS + topic_name
			else:
				topic_name = "/" + NS + topic_name
			return topic_name

	def get_target_points(self):

		## Reset all previos data
		self.lat_target_list = []
		self.lon_target_list = []
		self.speed_target_list = []
		self.delay_target_list = []
		self.sbus_throttle_target_list = []
		self.relay_target_list = []

		if os.path.exists(self.mission_path):
			print("Extract lat/lon from mission.txt")
			file = open(self.mission_path, 'r')
			lines = file.read().splitlines()

			for line in lines:
				if len(line)<40:
					pass
				else:
					line_list = line.split('\t')
					self.speed_target_list.append(float(line_list[4]))
					self.delay_target_list.append(float(line_list[5]))
					self.relay_target_list.append(int(float(line_list[6])))
					self.lat_target_list.append(float(line_list[8]))
					self.lon_target_list.append(float(line_list[9]))

			file.close()

			self.total_points = len(self.lat_target_list)

			for speed in self.speed_target_list:
				self.sbus_throttle_target_list.append(self.vel2sbus(speed))

			print("lat_target_list", self.lat_target_list)
			print("lon_target_list", self.lon_target_list)
			print("speed_target_list", self.speed_target_list)
			print("delay_target_list", self.delay_target_list)
			print("sbus_throttle_target_list", self.sbus_throttle_target_list)
			print("relay_target_list", self.relay_target_list)

	def generate_mission_file(self, current_wp, coord_frame, command, param1, param2, param3, param4, lat_list, lon_list, alt_list, autocontinue):

		mission_file = open(self.mission_path, 'w+')
		mission_file.write("QGC WPL 110\n")

		for i in range(len(lat_list)):
			mission_file.write('{:d}'.format(i))
			mission_file.write("\t")
			mission_file.write("{:d}".format(current_wp[i]))
			mission_file.write("\t")
			mission_file.write("{:d}".format(coord_frame[i]))
			mission_file.write("\t")
			mission_file.write("{:d}".format(command[i]))
			mission_file.write("\t")
			mission_file.write("{:.6f}".format(param1[i]))
			mission_file.write("\t")
			mission_file.write("{:.6f}".format(param2[i]))
			mission_file.write("\t")
			mission_file.write("{:.6f}".format(param3[i]))
			mission_file.write("\t")
			mission_file.write("{:.6f}".format(param4[i]))
			mission_file.write("\t")
			mission_file.write('{:.16f}'.format(lat_list[i]))
			mission_file.write("\t")
			mission_file.write('{:.16f}'.format(lon_list[i]))
			mission_file.write("\t")
			mission_file.write("{:.2f}".format(alt_list[i]))
			mission_file.write("\t")
			mission_file.write("{:d}".format(autocontinue[i]))
			if i != (len(lat_list)-1):
				mission_file.write("\n")

		mission_file.close()

	def heartbeat_callback(self, msg):

		if msg.data == True:

			self.hb_timestamp = time.time()

	def param_callback(self, config):

		print("Got new parameters")
		# rospy.loginfo("Config set to {int_param}, {double_param}, {str_param}, {bool_param}".format(**config))
		# rospy.loginfo("Config set to {p}, {i}, {d}".format(**config))
		self.max_start_str = config["max_start_str"]
		self.min_start_str = config["min_start_str"]
		self.str_adj = config["str_adj"]
		self.skid_adj = config["skid_adj"]
		self.str_mid = config["str_mid"]
		self.max_start_thr = config["max_start_thr"]
		self.thr_adj = config["thr_adj"]
		self.thr_slowest = config["thr_slowest"]
		self.thr_mid = config["thr_mid"]
		self.goal_dist_thresh = config["goal_dist_thresh"]
		self.goal_ang_thresh = config["goal_ang_thresh"]
		self.x_track_error_start = config["x_track_error_start"]
		self.x_track_repose_dist = config["x_track_repose_dist"]
		self.pid_hdg_out_thresh = config["pid_hdg_out_thresh"]
		self.pid_x_out_thresh = config["pid_x_out_thresh"]
		self.hdg_p = config["hdg_p"]
		self.hdg_i = config["hdg_i"]
		self.hdg_d = config["hdg_d"]
		self.cross_p = config["cross_p"]
		self.cross_i = config["cross_i"]
		self.cross_d = config["cross_d"]
		self.vel_p = config["vel_p"]
		self.vel_i = config["vel_i"]
		self.vel_d = config["vel_d"]
		self.use_heartbeat = config["use_heartbeat"]
		self.pid_hdg.tunings = (self.hdg_p, self.hdg_i, self.hdg_d)
		self.pid_x.tunings = (self.cross_p, self.cross_i, self.cross_d)
		self.pid_vel.tunings = (self.vel_p, self.vel_i, self.vel_d)

		self.update_local_param()

	def update_local_param(self):
		self.sbus_throttle_mid = self.thr_mid #1024
		self.sbus_steering_mid = self.str_mid #969
		# Throttle
		self.sbus_throttle_const = self.max_start_thr + self.thr_adj

		# PIVOT Turning
		self.sbus_skidding_right = self.max_start_str + self.skid_adj
		self.sbus_skidding_left = self.min_start_str - self.skid_adj

		# Cross track steering
		self.sbus_steering_max = self.max_start_str + self.str_adj  # steer to right
		self.sbus_steering_min = self.min_start_str - self.str_adj  # steer to left

	def goal_wp_callback(self, msg):
		print("New goal_waypoints come")
		self.total_wps = len(msg.lat.data)
		current_wp_list = [0]*self.total_wps
		coord_frame_list = [3]*self.total_wps
		command_list = [16]*self.total_wps
		param1_list = msg.speed.data	# speed list
		param2_list = msg.delay.data	# delay list
		param3_list = msg.relay.data # relay list
		param4_list = [0]*self.total_wps
		lat_list = msg.lat.data
		lon_list = msg.lon.data
		alt_list = [10]*self.total_wps
		autocon_list = [1]*self.total_wps

		self.got_new_wps = True

		self.generate_mission_file(current_wp_list, coord_frame_list, command_list, \
			param1_list, param2_list, param3_list, param4_list, \
			lat_list, lon_list, alt_list, autocon_list)

		self.reply_wp_pub.publish(msg)

	def request_wp_callback(self,msg):

		print("Request waypoints")
		if msg.data == True:
			self.reply_wp_msg.lat.data = self.lat_target_list
			self.reply_wp_msg.lon.data = self.lon_target_list
			self.reply_wp_msg.speed.data = self.speed_target_list
			self.reply_wp_msg.delay.data = self.delay_target_list
			self.reply_wp_msg.relay.data = self.relay_target_list

			self.reply_wp_pub.publish(self.reply_wp_msg)

	def gps_callback(self, msg):

		self.lat1 = msg.latitude
		self.lon1 = msg.longitude

		self.use_gps1 = True

	def gps2_callback(self, msg):

		self.lat2 = msg.latitude
		self.lon2 = msg.longitude

		self.use_gps2 = True

	def compass_callback(self, msg):

		self.hdg = msg.data[2]

	def atcart_mode_callback(self, msg):
		self.cart_mode = msg.data

		# if self.cart_mode != 2:
		# 	self.completed_flag = False
	# def hdg_calib_flag_callback(self, msg):
	# 	self.hdg_calib_flag = msg.data

	# 	if msg.data == True:
	# 		print("=============== got calib flag =================")

	def gps_vel_callback(self, msg):
		vel_x = msg.twist.twist.linear.x
		vel_y = msg.twist.twist.linear.y
		self.gps_abs_vel = np.sqrt(vel_x**2 + vel_y**2)

	def obst_detect_callback(self, msg):
		if msg.data == 2:
			self.obst_flag = True
			# print("WARNING: Obstacle on front or back!!")
		else:
			self.obst_flag = False

	def vel2sbus(self, vel):

		if vel < 0.05:
			sbus = 1024
		elif 0.05 <= vel < self.vel_min:
			sbus = self.thr_slowest
		elif (self.vel_min <= vel <= self.vel_max):
			sbus = self.map_with_limit(vel, self.vel_min, self.vel_max, self.thr_slowest, self.sbus_max)
		elif vel > self.vel_max:
			sbus = self.sbus_max

		return int(sbus)

	def get_distance(self, lat1, lon1, lat2, lon2):

		R = 6371.0*1000.0
		lat_start = np.radians(lat1)
		lon_start = np.radians(lon1)
		lat_end = np.radians(lat2)
		lon_end = np.radians(lon2)
		dLat = lat_end - lat_start
		dLon = lon_end - lon_start

		a = np.sin(dLat/2.0)*np.sin(dLat/2.0) + np.cos(lat_start)*np.cos(lat_end)*np.sin(dLon/2.0)*np.sin(dLon/2.0)
		c = 2.0*np.arctan2(np.sqrt(a),np.sqrt(1-a))

		d = c*R

		return d

	def get_bearing(self, lat1, lon1, lat2, lon2):

		lat_start = np.radians(lat1)
		lon_start = np.radians(lon1)
		lat_end = np.radians(lat2)
		lon_end = np.radians(lon2)
		dLat = lat_end - lat_start
		dLon = lon_end - lon_start

		y = np.sin(dLon)*np.cos(lat_end)
		x = np.cos(lat_start)*np.sin(lat_end) - np.sin(lat_start)*np.cos(lat_end)*np.cos(dLon)
		bearing = np.degrees(np.arctan2(y,x))

		return bearing

	def ConvertTo180Range(self, deg):

		deg = self.ConvertTo360Range(deg)
		if deg > 180.0:
			deg = -(180.0 - (deg%180.0))

		return deg

	def ConvertTo360Range(self, deg):

		# if deg < 0.0:
		deg = deg%360.0

		return deg

	def find_smallest_diff_ang(self, goal, cur):

		## goal is in 180ranges, we need to convert to 360ranges first

		diff_ang1 = abs(self.ConvertTo360Range(goal) - cur)

		if diff_ang1 > 180.0:

			diff_ang = 180.0 - (diff_ang1%180.0)
		else:
			diff_ang = diff_ang1

		## check closet direction
		compare1 = self.ConvertTo360Range(self.ConvertTo360Range(goal) - self.ConvertTo360Range(cur + diff_ang))
		compare2 = self.ConvertTo180Range(goal - self.ConvertTo180Range(cur + diff_ang))
		# print(compare1, compare2)
		if (abs(compare1) < 0.5) or (compare1 == 360.0) or (abs(compare2) < 0.5) or (compare2 == 360.0):
			sign = 1.0 # clockwise count from current hdg to target
		else:
			sign = -1.0 # counter-clockwise count from current hdg to target

		return diff_ang, sign

	def map_with_limit(self, val, in_min, in_max, out_min, out_max):

		# out = ((val - in_min) * ((out_max - out_min) / (in_max - in_min))) + out_min
		## in_min must be the minimum input
		## in_max must be the maximum input

		## out_min is supposed to map with in_min value
		## out_max is sipposed to map with in_max value
		## out_min can be less/more than out_max, doesn't matter

		m = (out_max - out_min)/(in_max - in_min)
		out = m*(val - in_min) + out_min

		if out_min > out_max:
			if out > out_min:
				out = out_min
			elif out < out_max:
				out = out_max
			else:
				pass
		elif out_max > out_min:
			if out > out_max:
				out = out_max
			elif out < out_min:
				out = out_min
			else:
				pass
		else:
			pass

		# print(m, val, in_min, in_max, out_min, out_max)

		return out

	def control_relay(self, flag):

		for i in range(10):
			self.jmoab_relay1_msg.data = flag
			self.jmoab_relay1_pub.publish(self.jmoab_relay1_msg)

	def mission_is_completed(self, completed_flag):
		self.mission_completed_msg.data = completed_flag
		self.mission_completed_pub.publish(self.mission_completed_msg)

	def loop(self):

		rate = rospy.Rate(30)
		print("Start GPS Waypoints Navigation")

		## it must enter get_goal_ang_once first
		get_goal_ang_once = True
		turning_flag = False
		get_goal_dist_once = False
		throttle_flag = False
		end_flag = False
		cur_dist = 0.0
		goal_dist = 0.0

		## for skid to DIR heading
		get_goal_turn_ang_once = True
		skidding_flag = False
		reset_flag = False
		hdg_diff = 0.0

		from_hdg_diff = True
		from_x_track = True

		## for checking cart mode
		cart_mode_check_lock = False
		from_step = 0
		pause_flag = False
		from_obst_flag_detect = False
		## for heartbeat
		hb_trig_once = True

		while not rospy.is_shutdown():

			## average front/back gps to get somewhere in middle of the cart
			if self.use_gps1 and self.use_gps2:
				self.lat = (self.lat1 + self.lat2)/2.0
				self.lon = (self.lon1 + self.lon2)/2.0
			else:
				self.lat = self.lat1
				self.lon = self.lon1

			# if (not self.hdg_calib_flag):
			###########################################
			### Robot will start once got auto_mode ###
			###########################################
			if (self.cart_mode == 2) and (not self.completed_flag) and (not self.obst_flag):

				######################################################
				### In case of pause, these condition handle flags ###
				######################################################
				if (from_step == 1) and cart_mode_check_lock:
					get_goal_ang_once = True
					cart_mode_check_lock = False
					print("Resume step1")
				elif (from_step == 2) and cart_mode_check_lock:
					turning_flag = True
					cart_mode_check_lock = False
					print("Resume step2")
				elif (from_step == 3) and cart_mode_check_lock:
					get_goal_dist_once = True
					cart_mode_check_lock = False
					print("Resume step3")
				elif (from_step == 4) and cart_mode_check_lock:
					throttle_flag = True
					self.pid_hdg.auto_mode = True
					self.pid_x.auto_mode = True
					cart_mode_check_lock = False
					self.pid_vel.auto_mode = True
					print("Resume step4")
				elif (from_step == 5) and cart_mode_check_lock:
					end_flag = True
					cart_mode_check_lock = False
					print("Resume step5")
				elif from_obst_flag_detect:
					self.control_relay(self.relay_target_list[self.target_wp])
					self.pid_vel.auto_mode = True
					from_obst_flag_detect = False

				#######################################
				##### Step 1 Get how much to turn #####
				##### only one time               #####
				#######################################
				if get_goal_ang_once:
					goal_ang = self.get_bearing(self.lat, self.lon, self.lat_target_list[self.target_wp], self.lon_target_list[self.target_wp])
					## goal_ang is in 180ranges, hdg is in 360ranges
					diff_ang, sign = self.find_smallest_diff_ang(goal_ang, self.hdg)
					goal_hdg = self.hdg + (diff_ang*sign)

					get_goal_ang_once = False
					turning_flag = True
					end_flag = False
					print("step: 1 | goal_ang: {:.2f} | diff_ang: {:.2f} | sign: {:.1f} | hdg: {:.2f}".format(goal_ang, diff_ang, sign, self.hdg))
					step = 1

					self.pid_vel.auto_mode = False
					self.pid_vel.setpoint = self.speed_target_list[self.target_wp]

					self.control_relay(self.relay_target_list[self.target_wp])

				##########################################################
				##### Step 2 turning until less than angle threshold #####
				##### continuously                                   #####
				##########################################################
				if goal_hdg < 0.0:
					self.hdg = self.ConvertTo180Range(self.hdg)
				if goal_hdg > 360.0:
					goal_hdg = self.ConvertTo360Range(goal_hdg)

				if turning_flag and (abs(goal_hdg - self.hdg) > self.goal_ang_thresh) and (get_goal_ang_once==False):
					if sign == 1.0:
						sbus_steering = self.sbus_skidding_right # turn right
						sbus_throttle = self.sbus_throttle_mid
					elif sign == -1.0:
						sbus_steering = self.sbus_skidding_left # turn left
						sbus_throttle = self.sbus_throttle_mid
					step = 2

				elif (throttle_flag == False) and (turning_flag == True):
					print("finish turning")
					turning_flag = False
					get_goal_dist_once = True
					sbus_steering = self.sbus_steering_mid
					sbus_throttle = self.sbus_throttle_mid

				##############################################
				##### Step 3 Get how much distance to go #####
				##### only one time                      #####
				##############################################
				if get_goal_dist_once:
					goal_dist = self.get_distance(self.lat, self.lon, self.lat_target_list[self.target_wp], self.lon_target_list[self.target_wp])
					start_lat = self.lat
					start_lon = self.lon
					cur_dist = 0.0
					get_goal_dist_once = False
					throttle_flag = True
					end_flag = False

					step = 3
					print("step: 3 | hdg: {:.2f}".format(self.hdg))
					reset_tic = time.time()

					self.pid_vel.auto_mode = True

				#############################################################
				##### Step 4 go straight until less than dist threshold #####
				##### continuously                                      #####
				#############################################################
				if throttle_flag and (abs(goal_dist - cur_dist) > self.goal_dist_thresh) :

					## A is point of previous point
					## in case of just start there is no -1 element in target_list so we use previous lat/lon
					if (self.target_wp == 0):
						A_lat = start_lat
						A_lon = start_lon
					else:
						A_lat = self.lat_target_list[self.target_wp-1]
						A_lon = self.lon_target_list[self.target_wp-1]

					## reset_flag tells if the cart went out of the route, so it will re-target heading with current pose
					## if no reset_flag, it will just calculate angle to goal/bot with self.target_wp-1 and self.target_wp
					if reset_flag == True:
						ang_A_to_goal = self.get_bearing(start_lat, start_lon, self.lat_target_list[self.target_wp], self.lon_target_list[self.target_wp])
						ang_A_to_bot = self.get_bearing(start_lat, start_lon, self.lat, self.lon)
					else:
						ang_A_to_goal = self.get_bearing(A_lat, A_lon, self.lat_target_list[self.target_wp], self.lon_target_list[self.target_wp])
						ang_A_to_bot = self.get_bearing(A_lat, A_lon, self.lat, self.lon)

					if (ang_A_to_goal > 90.0) or (ang_A_to_goal < -90.0):
						ang_A_to_bot = self.ConvertTo360Range(ang_A_to_bot)
						ang_A_to_goal = self.ConvertTo360Range(ang_A_to_goal)

					#########################################################################################
					############################### cross-track-error calculation ###########################
					#########################################################################################

					##############################################################################################################################
					## "a" is a distance from target_wp-1 to bot, but if there is reset_flag, we use current pose of bot instead of target_wp-1 ##
					## "c" is a distance from target_wp-1 to target_wp                                                                          ##
					## "b" is a distance from bot to target_wp                                                                                ##
					## "k" is a distance projection of bot's pose back to route plane                                                           ##
					## "x" is a distance from bot to route (cross-track-distance)                                                               ##
					##############################################################################################################################
					if reset_flag == True:
						# previous point and current pose
						a = self.get_distance(start_lat, start_lon, self.lat, self.lon)
						# previous point and goal point
						c = self.get_distance(start_lat, start_lon, self.lat_target_list[self.target_wp], self.lon_target_list[self.target_wp])
					else:
						# previous point and current pose
						a = self.get_distance(A_lat, A_lon, self.lat, self.lon)
						# previous point and goal point
						c = self.get_distance(A_lat, A_lon, self.lat_target_list[self.target_wp], self.lon_target_list[self.target_wp])

					# current pose and goal point
					b = self.get_distance(self.lat, self.lon, self.lat_target_list[self.target_wp], self.lon_target_list[self.target_wp])

					k = (a**2 - b**2 + c**2)/(2*c)
					x = np.sqrt(a**2 - k**2)
					if np.isnan(x):
						x = 0.0
					# print("a: {:.2f} | b: {:.2f} | c: {:.2f} | k: {:.2f} | x: {:.2f}".format(a,b,c,k,x))

					##########################################################
					## To find the robot is on which side of the route      ##
					## both ang_A_to_goal/bot are in 180ranges,             ##
					## so if the path c is pointing to 3rd or 4th quadrant, ##
					## then convert ang_A_to_goal/bot to 360ranges          ##
					##########################################################
					if ang_A_to_bot < ang_A_to_goal:
						x_side = -1.0	# on the left side of route
					else:
						x_side = 1.0	# on the right side of route

					x_track_error = x*x_side

					#############################################################################################
					############################### bearing hdg compare calculation #############################
					#############################################################################################

					#########################
					## use cross track PID ##
					#########################
					if (abs(x_track_error) > self.x_track_error_start):

						## Check one time that it's from hdg_diff PID, then enable x_tracK PID and disable hdg_diff PID
						if from_hdg_diff:
							self.pid_hdg.auto_mode = False
							self.pid_x.auto_mode = True
							from_x_track = True
							from_hdg_diff = False

						output_pid_x = self.pid_x(x_track_error)
						if x_track_error < -self.pid_x_out_thresh: #if output_pid_x > self.pid_x_out_thresh:
							sbus_steering = int(self.map_with_limit(output_pid_x, 0.0, self.max_x_err, self.max_start_str, self.sbus_steering_max))
						elif x_track_error > self.pid_x_out_thresh:	#elif output_pid_x < -self.pid_x_out_thresh:
							sbus_steering = int(self.map_with_limit(output_pid_x, -self.max_x_err, 0.0, self.sbus_steering_min, self.min_start_str))
						else:
							sbus_steering = self.sbus_steering_mid

						# sbus_throttle = self.sbus_throttle_const

						pid_mode = "x_error"
						OUTPUT_PID = output_pid_x

					#################
					## use hdg PID ##
					#################
					else:

						## Check one time that it's from x_tracK PID, then enable hdg_diff PID and disable x_tracK PID
						if from_x_track:
							self.pid_hdg.auto_mode = True
							self.pid_x.auto_mode = False
							from_x_track = False
							from_hdg_diff = True

						## hdg might got converted to 180ranges somewhere, but we need 360ranges to use with find_smallest_diff_ang function
						self.hdg = self.ConvertTo360Range(self.hdg)
						## using find_smallest_diff_ang function to compare between goal bearing (180ranges) and current heading (360ranges)
						hdg_diff, diff_sign = self.find_smallest_diff_ang(ang_A_to_goal, self.hdg)
						hdg_diff = self.ConvertTo180Range(hdg_diff*(-diff_sign))
						output_pid = self.pid_hdg(hdg_diff)

						if hdg_diff < -self.pid_hdg_out_thresh:	#if output_pid > self.pid_hdg_out_thresh:
							sbus_steering = int(self.map_with_limit(output_pid, 0.0, self.max_err, self.max_start_str, self.sbus_steering_max))
						elif hdg_diff > self.pid_hdg_out_thresh:	#elif output_pid < -self.pid_hdg_out_thresh:
							sbus_steering = int(self.map_with_limit(output_pid, -self.max_err, 0.0, self.sbus_steering_min, self.min_start_str))
						else:
							sbus_steering = self.sbus_steering_mid

						pid_mode = "hdg_diff"
						OUTPUT_PID = output_pid

					###########################################################################
					## if very close distance, use very low speed, ignore user speed command ##
					###########################################################################
					output_pid_vel = self.pid_vel(self.gps_abs_vel)
					if goal_dist < 2.0:
						sbus_throttle = self.thr_slowest+20
					else:
						if abs(goal_dist - cur_dist) < 2.0:
							sbus_throttle = self.thr_slowest+20
						else:
							if self.speed_target_list[self.target_wp] != 0.0:
								# sbus_throttle = self.sbus_throttle_target_list[self.target_wp] #self.sbus_throttle_const
								sbus_throttle = int(self.map_with_limit(output_pid_vel, 0, self.max_vel, 1024, 1680))
							else:
								sbus_throttle = self.sbus_throttle_const

							# print("output_pid_vel: {:.2f} | thr_pid: {:d}".format(output_pid_vel, sbus_throttle_pid))

					###############################################################
					### Resest target heading again if cross track error is big ###
					###############################################################
					reset_timeout = time.time() - reset_tic
					if (abs(x_track_error) > self.x_track_repose_dist) and (reset_timeout > 2.0):
						throttle_flag = False
						get_goal_ang_once = True
						reset_flag = True
						print("Reset target heading")
						sbus_steering = self.sbus_steering_mid
						sbus_throttle = self.sbus_throttle_mid

						self.pid_x.auto_mode = False
						self.from_hdg_diff = True

					####################################
					### Print out all necessary info ###
					####################################
					print("PID: {:} | tg_wp: {:d} | x: {:.2f} | rst_T/O: {:.2f} | rst_flg: {:} | AngAtoBot: {:.2f} | hdg: {:.2f} | AngAtoGoal: {:.2f} | hdgDff: {:.2f} | outPID: {:.2f} | goalDist: {:.2f} curDist: {:.2f} | str: {:d} | thr: {:d} | vel: {:.2f} | vel_sp: {:.2f} | outPIDV: {:.2f}".format(\
						pid_mode, self.target_wp, x_track_error, reset_timeout, reset_flag, ang_A_to_bot, self.hdg, ang_A_to_goal, hdg_diff, OUTPUT_PID, goal_dist, cur_dist, sbus_steering, sbus_throttle, self.gps_abs_vel, self.speed_target_list[self.target_wp], output_pid_vel))

					## update cur_dist with current lat/lon compare to start lat/lon
					cur_dist = self.get_distance(start_lat, start_lon, self.lat, self.lon)
					step = 4

				elif (throttle_flag == True) and (turning_flag == False):
					throttle_flag = False
					get_goal_ang_once = True
					end_flag = True
					reset_flag = False
					print("finish throttle")
					sbus_steering = self.sbus_steering_mid
					sbus_throttle = self.sbus_throttle_mid

				#####################################################################################
				##### Step 5 check if turning and throttle are completed, then increment the wp #####
				#####################################################################################

				if (throttle_flag == False) and (turning_flag == False) and (end_flag == True):
					sbus_steering = self.sbus_steering_mid
					sbus_throttle = self.sbus_throttle_mid
					self.sbus_cmd.data = [int(sbus_steering), int(sbus_throttle)]
					self.sbus_cmd_pub.publish(self.sbus_cmd)
					print("Delay as {:.2f} seconds".format(self.delay_target_list[self.target_wp]))
					time.sleep(self.delay_target_list[self.target_wp])
					self.target_wp += 1
					self.pid_vel.auto_mode = False

				######################################################################
				##### Step 6 check every time that target_wp is completed or not #####
				######################################################################
				if self.target_wp == len(self.lat_target_list):
					self.target_wp = 0
					# self.ROBOT_MODE = "MANUAL"
					self.completed_flag = True
					sbus_steering = self.sbus_steering_mid
					sbus_throttle = self.sbus_throttle_mid
					print("Mission completed")
					print("Switch from auto to manual, and back to auto again to restart mission.")

				self.sbus_cmd.data = [int(sbus_steering), int(sbus_throttle)]

				if (step != 4) and (not cart_mode_check_lock):
					print("step: {:d} | target_wp: {:d} | goal_hdg: {:.3f} | hdg: {:.3f} | goal_dist: {:.3f} | cur_dist: {:.3f}".format(\
						step, self.target_wp, goal_hdg, self.hdg, goal_dist, cur_dist))

				pause_flag = True

				self.mission_is_completed(False)

			#########################################################
			### This is pause on manual or hold mode by Tx switch ###
			#########################################################
			elif (self.cart_mode != 2) and (pause_flag):
				if get_goal_ang_once:
					get_goal_ang_once = False
					from_step = 1
					print("Stop at step1")
				elif turning_flag:
					turning_flag = False
					from_step = 2
					print("Stop at step2")
				elif get_goal_dist_once:
					get_goal_dist_once = False
					from_step = 3
					print("Stop at step3")
				elif throttle_flag:
					throttle_flag = False
					from_step = 4
					self.pid_hdg.auto_mode = False
					self.pid_x.auto_mode = False
					self.pid_vel.auto_mode = False
					print("Stop at step4")
				elif end_flag:
					end_flag = False
					from_step = 5
					print("Stop at step5")

				cart_mode_check_lock = True

				if (self.got_new_wps):
					print("Reset lat/lon mission during mission")
					self.get_target_points()
					self.target_wp = 0
					self.got_new_wps = False

				self.mission_is_completed(False)

			#########################################
			### This is already completed mission ###
			#########################################
			elif (self.cart_mode == 2) and self.completed_flag:
				pause_flag = False
				self.mission_is_completed(True)
				pass

			########################################################################
			### Set back to manual or hold once, disable completed_flag to False ###
			########################################################################
			elif (self.cart_mode != 2) and self.completed_flag:
				self.completed_flag = False
				pause_flag = False

				if (self.got_new_wps):
					print("Completed previous mission, load new mission")
					self.get_target_points()
					self.target_wp = 0
					self.got_new_wps = False

				self.mission_is_completed(True)

			#########################################################################
			### Firstly start, maybe in manual/hold mode, completed_flag is False ###
			#########################################################################
			else:

				if (self.got_new_wps):
					print("Reset lat/lon mission before start")

					self.get_target_points()
					self.target_wp = 0
					self.got_new_wps = False

				if (self.obst_flag):
					## Trig only one time
					if from_obst_flag_detect == False:
						self.control_relay(False)
						self.pid_vel.auto_mode = False
						print("Detected obstacle....")
					from_obst_flag_detect = True

				self.sbus_cmd.data = [self.sbus_steering_mid, self.sbus_throttle_mid]

				self.mission_is_completed(False)

			self.sbus_cmd_pub.publish(self.sbus_cmd)

			##########################
			### heartbeat checking ###
			##########################
			self.hb_period = (time.time() - self.hb_timestamp)
			if self.use_heartbeat and (self.hb_period > self.hb_timeout):
				if (int(self.hb_period)%self.hb_timeout == 0) or hb_trig_once:
					print("Hearbeat timeout.. switch cart mode to hold")
					self.atcart_mode_cmd_msg.data = 0
					self.atcart_mode_cmd_pub.publish(self.atcart_mode_cmd_msg)
					hb_trig_once = False

			#############################################################
			### continuously send arrived wp                          ###
			### target_wp starts from 0, so we can send this directly ###
			#############################################################
			self.arrived_wp_msg.data = self.target_wp
			self.arrived_wp_pub.publish(self.arrived_wp_msg)

			rate.sleep()

if __name__ == "__main__":

	parser = argparse.ArgumentParser(description='GPS Waypoints Server node')
	parser.add_argument('--param_file',
						help="A file path of GpsWaypoints.yaml, default is the one in jmoab_autopilot_ros/cfg/")
	parser.add_argument('--mission_file',
						help="A file path of mission.txt, default is the one in jmoab_autopilot_ros/waypoints/")
	parser.add_argument('--ns',
						help="a namespace in front of topics")

	args = parser.parse_args(rospy.myargv()[1:])
	print(args.param_file)
	param_file = args.param_file
	mission_file = args.mission_file
	ns = args.ns

	if param_file is None:
		print("Use jmoab_autopilot_ros/cfg/GpsWaypoints.yaml")
		rospack = rospkg.RosPack()
		jmoab_autopilot_ros_path = rospack.get_path("jmoab_autopilot_ros")
		yaml_name = "GpsWaypoints.yaml"
		yaml_path = os.path.join(jmoab_autopilot_ros_path, "cfg", yaml_name)
	else:
		yaml_path = param_file

	if mission_file is None:
		print("Use jmoab_autopilot_ros/waypoints/mission.txt")
		rospack = rospkg.RosPack()
		jmoab_autopilot_ros_path = rospack.get_path("jmoab_autopilot_ros")
		mission_name = "mission.txt"
		mission_path = os.path.join(jmoab_autopilot_ros_path, "waypoints", mission_name)
	else:
		mission_path = mission_file

	if ns is not None:
		print("Use namespace as {:}".format(ns))
	else:
		print("No namespace, using default")

	a = GPS_NAV(yaml_path, mission_path, ns)


"""
    Crie um programa que vai ler vários números e colocar em uma lista. Depois disso, mostre:
    A) Quantos números foram digitados.
    B) A lista de valores, ordenada de forma decrescente.
    C) Se o valor 5 foi digitado e está ou não na lista.

"""

numeros = list()
while True:
    numeros.append(int(input("Digite um numero: ")))
    escolha = str(input("Deseja continuar? [S/N] "))
    if escolha in "Nn":
        break

print(f"Foram digitados {len(numeros)} numeros")
numeros.sort(reverse=True)
print(f"Os valores em ordem decrescente são: {numeros}")
if 5 in numeros:
    print("O valor 5 faz parte da lista")
else:
    print("O valor 5 não foi encontrado...")


import asyncio
from asyncio import Transport, Future, AbstractEventLoop
from typing import Optional

class HTTPGetClientProtocol(asyncio.Protocol):

    def __init__(self, host: str, loop: AbstractEventLoop):
        self._host: str = host
        self._future: Future = loop.create_future()
        self._transport: Optional[Transport] = None
        self._response_buffer: bytes = b''

    async def get_response(self):  # A
        return await self._future

    def _get_request_bytes(self) -> bytes:  # B
        request = f"GET / HTTP/1.1\r\n" \
                  f"Connection: close\r\n" \
                  f"Host: {self._host}\r\n\r\n"
        return request.encode()

    def connection_made(self, transport: Transport):
        print(f'Connection made to {self._host}')
        self._transport = transport
        self._transport.write(self._get_request_bytes())  # C

    def data_received(self, data):
        print(f'Data received!')
        self._response_buffer = self._response_buffer + data  # D

    def eof_received(self) -> Optional[bool]:
        self._future.set_result(self._response_buffer.decode())  # E
        return False

    def connection_lost(self, exc: Optional[Exception]) -> None:  # F
        if exc is None:
            print('Connection closed without error.')
        else:
            self._future.set_exception(exc)


from app.tokenizer import Tokenizer
from app.parser.parser import Parser
from app.error_handling import CustomSyntaxError
from app.parser.interference_graph import InterferenceGraph
from app.parser.register_allocation import RegisterAllocation
from ir_viz_tool.ir_viz import IRViz

# tokenizer = Tokenizer('if_then.txt')
# token = -1
# while(token != 30):
#     token = tokenizer.get_next()
#     value = list(tokenizer._token_map.keys())[list(tokenizer._token_map.values()).index(token)]
#     if value == 'identifier':
#         value = str(tokenizer.id) + ' ' + list(tokenizer._token_map.keys())[list(tokenizer._token_map.values()).index(tokenizer.id)]
#     elif value == 'number':
#         value = tokenizer.number
#     print(token, value)
if __name__ == '__main__':
    import os

    include = [
            # "if_then.txt",
            # "copy_prop_test.txt",
            # "nested_if_then_else.txt",
            # "var_copy_prop.txt",
            # "subexpr_1.txt",
            # "subexpr_if.txt",
            # "subexpr_if_2.txt",
            # "error_test.txt",
            "ig_1.txt"
            ]

    dirpath = "./tests/code/"
    for file in os.listdir(dirpath):
        try:
            if (file in include or not include) and file.endswith(".txt"):
                print(file, "\n")
                parser_obj = Parser(dirpath+file)
                parser_obj.parse_computation()
                parser_obj.cfg._instr_graph.debug()
                parser_obj.debug()
                print(end="\n\n\n")

                register_allocator = RegisterAllocation(cfg_map=parser_obj.cfg_map, filename=file)
                register_allocator.allocate_registers()
                register_allocator.debug()

                parser_obj.update_branch_instr()

                ir_viz = IRViz(parser_obj.cfg_map, register_allocator.reg_cfg_map, register_allocator.dead_code_map,
                               filename=file)
                ir_viz.generate_graph()
        except CustomSyntaxError as e:
            print(repr(e))


import sys
from itertools import combinations
sys.stdin = open('input.txt')

T = int(input())

for tc in range(1, T+1):
    n, m = map(int, input().split())
    city = [list(map(int, input().split())) for _ in range(n)]
    home = []
    chicken = []
    # 집과 치킨집의 위치를 분리해서 기록해둔다(둘 좌표값 차이를 계산하기 위해)
    for y in range(n):
        for x in range(n):
            if city[y][x] == 1:
                home.append((y, x))
            elif city[y][x] == 2:
                chicken.append((y, x))
    # 현재 들어있는 좌표값
    # print(home)
    # print(chicken)
    # print()
    # 최종 값을 담을 ans 1000은 그냥 임의로 큰 값을 넣어서 min 값을 찾을 수 있게
    ans = 1000000
    # 백트래킹은 시간초과가 발생해서 combinations라는 내장 함수를 써야 한다.
    # combinations란?
    for i in combinations(chicken, m):
        # print(i)
        # print()
        # 임시로 값을 받아줄 변수
        tmp = 0
        for j in home:
            # print(j)
            min_v = 1000000
            for k in i:
                distance = abs(k[0] - j[0]) + abs(k[1] - j[1])
                min_v = min(min_v, distance)
            tmp += min_v
        # print()

        ans = min(ans, tmp)

    print(ans)


#!/usr/bin/env python3

import unittest
import ipaddress
from framework import VppTestCase, VppTestRunner

from scapy.layers.inet6 import IPv6, ICMPv6EchoRequest
from scapy.layers.l2 import Ether
from scapy.packet import Raw

class TestNPT66(VppTestCase):
    """NPTv6 Test Case"""

    def setUp(self):
        super(TestNPT66, self).setUp()

        # create 2 pg interfaces
        self.create_pg_interfaces(range(2))

        for i in self.pg_interfaces:
            i.admin_up()
            i.config_ip6()
            i.resolve_ndp()

    def tearDown(self):
        for i in self.pg_interfaces:
            i.unconfig_ip6()
            i.admin_down()
        super(TestNPT66, self).tearDown()

    def send_and_verify(self, in2out, internal, external):
        if in2out:
            sendif = self.pg0
            recvif = self.pg1
            local_mac = self.pg0.local_mac
            remote_mac = self.pg0.remote_mac
            src = ipaddress.ip_interface(internal).ip + 1
            dst = self.pg1.remote_ip6
        else:
            sendif = self.pg1
            recvif = self.pg0
            local_mac = self.pg1.local_mac
            remote_mac = self.pg1.remote_mac
            src = self.pg1.remote_ip6
            dst = ipaddress.ip_interface(external).ip + 1

        p = (
            Ether(dst=local_mac, src=remote_mac)
            / IPv6(src=src, dst=dst)
            / ICMPv6EchoRequest()
        )
        rxs = self.send_and_expect(sendif, p, recvif)
        for rx in rxs:
            rx.show2()
            original_cksum = rx[ICMPv6EchoRequest].cksum
            del rx[ICMPv6EchoRequest].cksum
            rx = rx.__class__(bytes(rx))
            self.assertEqual(original_cksum, rx[ICMPv6EchoRequest].cksum)

    def do_test(self, internal, external):
        self.vapi.npt66_binding_add_del(
            sw_if_index=self.pg1.sw_if_index,
            internal=internal,
            external=external,
            is_add=True,
        )
        self.vapi.cli(f"ip route add {internal} via {self.pg0.remote_ip6}")

        self.send_and_verify(True, internal, external)
        self.send_and_verify(False, internal, external)

        self.vapi.npt66_binding_add_del(
            sw_if_index=self.pg1.sw_if_index,
            internal=internal,
            external=external,
            is_add=False,
        )

    def test_npt66_simple(self):
        """Send and receive a packet through NPT66"""

        self.do_test("fc00:1::/48", "2001:db8:1::/48")
        self.do_test("fc00:1234::/32", "2001:db8:1::/32")
        self.do_test("fc00:1234::/63", "2001:db8:1::/56")

if __name__ == "__main__":
    unittest.main(testRunner=VppTestRunner)


from matplotlib import pyplot as plt
import pandas as pd
from src.cluster import KMeans

df = pd.read_csv("data/assignment-example.csv", index_col="id")
sse = []
for k in range(1, 9):
    model = KMeans(k=k, verbose=True)
    model.fit(df)
    res = [model.predict(i) for i in df.values]
    s = [model.distance_fn(i, model.centers[r]) for i, r in zip(df.values, res)]
    print(f"Sum of squared errors: {sum(s)}")
    sse.append(sum(s))
plt.plot(range(1, 9), sse)
plt.xlabel("k")
plt.ylabel("SSE")
plt.show()
print(f"Centers: {model.centers}")


"""
1. Usa programación dinámica para resolver el problema de encontrar el máximo conjunto independiente con mayor peso.
Usa el archivo MWIS.txt para obtener los datos, cada dato es el peso de un vértice en un grafo camino.
Contesta los siguiente:
¿Cuáles de los vértices 1, 2, 3, 4, 17, 117, 517, y 997 forman parte de la solución óptima?
"""

def mwis(w):
    # Obtiene el valor optimo
    n = len(w)
    a = [0, w[0]]
    for i in range(2, n+1):
        a.append(max(
            a[i-1],
            a[i-2] + w[i-1]
        ))

    # Reconstruir la solucion
    i = n
    s = set()
    while i >= 1:
        if a[i] == a[i-1]:
            i -= 1
        else:
            s.add(i-1)
            i -= 2
    print(s)

w = []

with open("MWIS.txt", "r") as f:
    for line in f:
        line = line.strip()
        w.append(int(line))

mwis(w)


"""
Class METAR
Author: Matthieu BOUCHET

5cfd588b1a5871105cad98a099c67d44562fb2524bf8b4d00a7e3f9baca33ee2

For documentation, you can visit : https://www.link.url
and read the ReadMe
"""

from http.client import NOT_EXTENDED, PRECONDITION_FAILED
import urllib.request as url
import ssl
import re
import copy

ssl._create_default_https_context = ssl._create_unverified_context

class Metar:
    """Class METAR represents a METeorogical Aerodrome Report.

    Each attribute represent each information contained in
    METAR (International METAR Code). Informations are recovered from
    text message. Units returned are units used in text.
    This class can be imported in programm.

    More informations about METAR here
    - https://en.wikipedia.org/wiki/METAR/
    - https://www.skybrary.aero/index.php/Meteorological_Terminal_Air_Report_(METAR)/

    Args
    -----
    - code (string): OACI code of airport searched
    - text (string,optional): METAR

    Attributes
    -----------
    - airport (string): OACI code of METAR airport
    - data_date (string): Date provided by NOAA server. None if text enter manually
    - metar (string): Complete METAR message
    - changements (string) : Changements
    - auto (boolean): Define if a METAR isfrom an automatic station or not
    - date_time (tuple): Tuple of date with day, hour & minutes
    - wind (dictionary): Dictionary with wind information
    - rvr (tuple): Tuple of dictionnaries with RVR information
    - weather (dictionnary): Dictionnary of tuple with significant weather information
    - cloud (tuple): Tuple of dictionnaries with cloud detected information
    - temperatures (dictionnary): Dictionnary of integers with temperature and dewpoint information
    - qnh (integer OR float): Information of QNH (integer if hPA, float if inHG)
    - properties(dictionary): Dictionnary of METAR's attribute
    - vmc(dictionnary)
    """

    def __init__(self, code, text=None):
        """Constructor of class

        Args
        -----
            code (string): OACI code of airport searched
        """
        self.airport = code

        if text is None:
            text = self.text_recover()
            self.data_date = text[0]
            self.metar = text[1]

        else:
            self.data_date = None
            self.metar = text

        self.metarWithoutChangements = self.metar
        self.changements = self.analyzeChangements()  # delcare self.metarAnalysis
        self.auto = self.analyzeAuto()
        self.date_time = self.analyzeDateTime()
        self.wind = self.analyzeWind()
        self.rvr = self.analyzeRVR()
        self.weather = self.analyzeWeather()
        self.cloud = self.analyzeCloud()
        self.temperatures = self.analyzeTemperatures()
        self.qnh = self.analyzeQNH()
        self.visibility = self.analyzeVisibility()
        self.vmc = self.verifyVMC()

        self.properties = {
            'dateTime': self.date_time,
            'metar': self.metar,
            'auto': self.auto,
            'wind': self.wind,
            'rvr': self.rvr,
            'weather': self.weather,
            'cloud': self.cloud,
            'temperatures':self.temperatures,
            'qnh':self.qnh,
            'visibility':self.visibility,

            'changements': self.changements

        }

    def __str__(self):
        """Overload __str__
        """
        return self.metar

    def text_recover(self):
        """`text_recover()` method recover text file from NOAA
        weather FTP server (https://tgftp.nws.noaa.gov/data/observations/metar/stations/).

        Recovers text file with `url.urlretrieve()` function.
        Read the file with `readlines()` method. This method return a list
        of lines ([0] = Datetime ; [1] = METAR).
        A `for` loop browses list to remove line break character.
        Return a list with datetime index 0 and METAR index 1.

        Require
        --------
        - `NOAAServError(Exception)` from metar_error \n
        - `ulrlib.request` from built-in Pyhon modules \n
        - `re` from built-in Python modules \n

        Returns
        --------
            datas (tuple): List recovered from text file ([0] = Datetime ; [1] = METAR).

        Exception raised
        -------
            - NOAAServError
            - ReadFileError
        """

        try:
            request = url.urlretrieve(
                "https://tgftp.nws.noaa.gov/data/observations/metar/stations/{}.TXT".format(self.airport))
        except url.HTTPError as err:
            if(err.code == 404):
                raise NOAAServError(self.airport, 404)
            else:
                raise NOAAServError(self.airport)

        except:
            raise NOAAServError(self.airport)

        try:
            file_txt = open(request[0], 'r')

        except:
            raise ReadFileError

        datas = file_txt.readlines()  # List : [0] = Datetime ; [1] = METAR

        # Remove '\n' from string
        for i in range(len(datas)):
            datas[i] = re.sub("\n", '', datas[i])

        file_txt.close()
        return datas[0], datas[1]

    def analyzeChangements(self):
        """Method analysis and erase changements portions (create metarWithoutChangements variable)

        Returns:
        --------
            (dict): Dictionnary of changements (TEMPO & BECOMING). Return None if NOSIG
        """

        def changementsRecuperation(marker):
            regex = marker+'.+'
            search = re.search(regex, self.metar)
            if search is not None:
                search = search.group()
                portion = re.sub(marker + ' ', '', search)
                self.metarWithoutChangements = re.sub(
                    regex, '', self.metarWithoutChangements)
            else:
                return None

            return portion

        ##NOSIG##
        regex_nosig = r'NOSIG'
        search_nosig = re.search(regex_nosig, self.metar)
        if search_nosig is not None:
            self.metarWithoutChangements = re.sub(regex_nosig, '', self.metar)
            return None

        ##TEMPO##
        tempo = changementsRecuperation('TEMPO')

        ##BECMG##
        becoming = changementsRecuperation('BECMG')

        ##GRADU##
        gradu = changementsRecuperation('GRADU')

        ##RAPID##
        rapid = changementsRecuperation('RAPID')

        ##INTER##
        inter = changementsRecuperation('INTER')

        ##TEND##
        tend = changementsRecuperation('TEND')

        changements = {
            'TEMPO': tempo,
            'BECMG': becoming,
            'GRADU': gradu,
            'RAPID': rapid,
            'INTER': inter,
            'TEND': tend
        }

        return changements

    def analyzeDateTime(self):
        """Method parse METAR and return datetime portion

        Returns:
        --------
            - date_time (tuple): Tuple of strings (Day,Hour,Minute) in UTC.
            Ex: ("29","19","00") => 29th day of month, 19:00 UTC
            This is METAR date time.

            - None (NoneType): Return None if no date time found
        """
        date_time = re.findall(r'\d{6}Z', self.metar)
        if(len(date_time) == 0 or len(date_time) > 1):  # No match
            return None

        date_time = date_time[0]
        date_time = re.sub(r'Z', '', date_time)
        date_time = (date_time[:2], date_time[2:4], date_time[4:])

        return date_time

    def analyzeAuto(self):
        """Method verify if a METAR comes form an automatic station.
        If it is a METAR AUTO, return True.

        Returns
        --------
            boolean: True if auto, False if not auto
        """

        search = re.search(r'AUTO', self.metar)
        if search is None:
            return False

        return True

    def analyzeWind(self):
        """Method parse and analyze wind datas from METAR message and
        returns a dictionnary with wind informations.
        Support Knots (KT) and Meter Per Second (MPS) units.
        Units are not informations returned by method.
        If `analyzeWind()` can't decode wind information (in case of unavaibility
        indicated by ///////KT), method return None.

        Returns
        --------
            wind_tot (dict): Dictionnary with wind informations.
            Keys:
                - direction (integer), direction of wind
                - direction (string), "VRB" for variable
                - speed (integer), speed of wind
                - gust_speed (integer or None), speed of gust, None if no gust
                - variation(tuple), variation of wind (tuple of integer), None if no variation

            - None (NoneType): None if method can't decode wind informations.
        """
        search = None

        regex_list_kt = [r'\d{5}KT', r'\d{5}G\d{2}KT', r'VRB\d{2}KT']
        # [0] Normal (33005KT) [1] Gust (33010G25KT) [2] Variable direction (VRB03KT)
        regex_list_mps = [r'\d{5}MPS', r'\d{5}G\d{2}MPS', r'VRB\d{2}MPS']
        # Meters per second

        i = 0
        end = len(regex_list_kt)

        while search is None and i < end:
            search = re.search(regex_list_kt[i], self.metarWithoutChangements)
            i += 1

        if search is None:  # Knot verification failed, MPS verification
            i = 0
            end = len(regex_list_mps)

            while search is None and i < end:
                search = re.search(
                    regex_list_mps[i], self.metarWithoutChangements)
                i += 1

            if search is None:
                return None

        wind_tot = search.group()
        direction = wind_tot[:3]

        if direction != 'VRB':
            direction = int(direction)

        speed = wind_tot[3:5]
        speed = int(speed)

        if 'G' in wind_tot:  # Gust
            gust_speed = int(wind_tot[6:8])
        else:
            gust_speed = None

        ##Variations##
        regex = r'\d{3}V\d{3}'
        search = re.search(regex, self.metarWithoutChangements)

        if search is not None:
            variation = search.group()
            variation = variation.split('V')
            variation = [int(value) for value in variation]
            variation = tuple(variation)
        else:
            variation = None

        wind_infos = {
            'direction': direction,
            'speed': speed,
            'gust': gust_speed,
            'variation': variation
        }

        return wind_infos

    def analyzeVisibility(self):
        """Method analyzes the Visibility (distance, direction).
        Return a tuple with meter & direction
        If CAVOK, return (9999)
        Else, return (distance)

        Returns:
            (integer): A integer as (distance). E.g -> (9999),(5000)
        """
        verify = self.verifyWindAttribute('variation')
        if not verify:
            regex = r'KT \d{4}|KT CAVOK|KT \d{4}[A-Z]+'
        else:
            regex = r'\d{3}V\d{3} \d{4}|\d{3}V\d{3} \d{4}|\d{3}V\d{3} \d{4}[A-Z]+'

        search = re.search(regex, self.metarWithoutChangements)
        if search is None:
            return None

        visibility = search.group()
        # print(visibility)

        if not verify:
            visibility = re.sub(r'KT ', '', visibility)
        else:
            visibility = re.sub(r'\d{3}V\d{3} ', '', visibility)

        if visibility == 'CAVOK':
            return 9999

        return (int(visibility[:4]))

    def analyzeRVR(self):
        """Method parses and recovers RVR

        Returns:
            (tuple): Tuple of dictionnries : {
                'runway':runway (string)
                'visibility':visibility (integer)
            }

            (NoneType): If no RVR mentionned
        """
        ##PORTION METAR RECOVERING##
        regex_runway = r'R\d{2}[LCR]*'
        regex_rvr = r'[MP]*\d{4}'
        regex = regex_runway + '/' + regex_rvr

        search = re.findall(regex, self.metarWithoutChangements)
        if search == []:
            return None

        ##DATAS RECOVERING##
        rvr = []
        for k in search:
            search_runway = re.search(regex_runway, k)
            runway = search_runway.group()
            runway = runway[1:]

            search_visibility = re.search(r'\d{4}', k)
            visibility = int(search_visibility.group())

            rvr.append(
                {
                    'runway': runway,
                    'visibility': visibility
                }
            )

        return tuple(rvr)

    def analyzeWeather(self):
        """Method parses METAR and analyze significant weather.
        Return a dictionnary of tuples and boolean.

        Keys :
        ------
        - `intensity` (boolean): False (if -), True (if +)
        - `prefix` (tuple): Prefix (e.G = ('in Vicinity', 'Freezing'))
        - `weather` (tuple): Weather code (e.G = ('Rain','Snow'))

        If element not found (prefix or intensity), value is None

        Returns:
        --------
            (tuple): Tuple of dictionnaries (see above)
        """
        regex_intensity = r'[-+]+'

        prefixes = ({
            'code': 'VC',
            'meaning': 'in Vicinity'
        },
            {
            'code': 'MI',
            'meaning': 'Thin'
        },
            {
            'code': 'PR',
            'meaning': 'Partial'
        },
            {
            'code': 'DR',
            'meaning': 'Low Drifting'
        },
            {
            'code': 'BL',
            'meaning': 'Blowing'
        },
            {
            'code': 'FZ',
            'meaning': 'Freezing'
        },
            {
            'code': 'RE',
            'meaning': 'Recent'
        },
            {
            'code': 'BC',
            'meaning': 'Bank'
        },
            {
            'code': 'SH',
            'meaning': 'Shower'
        },
            {
            'code': 'XX',
            'meaning': 'Violent'
        })

        weathers = ({
            'code': 'RA',
            'meaning': 'Rain'
        },
            {
            'code': 'SN',
            'meaning': 'Snow'
        },
            {
            'code': 'GR',
            'meaning': 'Hail'
        },
            {
            'code': 'DZ',
            'meaning': 'Drizzle'
        },
            {
            'code': 'PL',
            'meaning': 'Ice Pellets'
        },
            {
            'code': 'GS',
            'meaning': 'Gresil'
        },
            {
            'code': 'SG',
            'meaning': 'Snow Grains'
        },
            {
            'code': 'IC',
            'meaning': 'Ice Crystals'
        },
            {
            'code': 'UP',
            'meaning': 'Unknown'
        },
            {
            'code': 'BR',
            'meaning': 'Brume'
        },
            {
            'code': 'FG',
            'meaning': 'Fog'
        },
            {
            'code': 'HZ',
            'meaning': 'Haze'
        },
            {
            'code': 'FU',
            'meaning': 'Smoke'
        },
            {
            'code': 'SA',
            'meaning': 'Sand'
        },
            {
            'code': 'DU',
            'meaning': 'Dust'
        },
            {
            'code': 'VA',
            'meaning': 'Volcanic Ash'
        },
            {
            'code': 'PO',
            'meaning': 'Dust whirlpool'
        },
            {
            'code': 'SS',
            'meaning': 'Sand Storm'
        },
            {
            'code': 'DS',
            'meaning': 'Dust Storm'
        },
            {
            'code': 'SQ',
            'meaning': 'Squalls'
        },
            {
            'code': 'FC',
            'meaning': 'Funnel Cloud'
        },
            {
            'code': 'TS',
            'meaning': 'Thunderstorm'
        })

        #Intensity#
        intensity = []
        search_intensity = re.findall(
            regex_intensity, self.metarWithoutChangements)

        if search_intensity == []:
            _intensity = None
        else:
            for _intensity in search_intensity:

                if _intensity == '-':
                    _intensity = False
                elif _intensity == '+':
                    _intensity = True
                else:
                    _intensity = None

                intensity.append(_intensity)

        intensity = tuple(intensity)

        #Prefixes#
        prefix = []
        for pre in prefixes:
            regex = pre['code']+'+'

            search_prefixes = re.search(regex, self.metarWithoutChangements)

            if search_prefixes is not None:
                prefix.append(pre['meaning'])

        if prefix == []:
            prefix = None
        else:
            prefix = tuple(prefix)

        # Weather
        weather = []
        for wea in weathers:
            regex = wea['code']+'+'
            search_weather = re.search(regex, self.metarWithoutChangements)

            if search_weather is not None:
                weather.append(wea['meaning'])

        if weather == []:
            weather = None
        else:
            weather = tuple(weather)

        if (intensity is None and prefix is None) and weather is None:
            return None

        return {
            'intensity': intensity,
            'prefix': prefix,
            'weather': weather
        }

        return returns

    def analyzeCloud(self):
        """`analyzeCloud()` method is a method from `Metar` class.
        Return a tuple of dictionnary

        Keys of dictionnary
        --------------------
        - `code` (string): class of cloud (e.G => SCT, BKN, OVC)
        - `meaning` (string): signification of `code` (e.G => Scatterd, Broken, Overcast)
        - `oktaMin` (integer): Okta of sky clouded (minimum)
        - `oktaMax`(integer): Okta of sky clouded (maximum)
        - `altitude` (integer): Altitude of clouds (in feet)
        - `presenceCB`(boolean): Presence of CB (True if CB, else False)
        - `presenceTCU`(boolean): Presence of TCU (True if TCU, else False)

        Returns
        -------
            - (tuple): Tuple of dictionnaries (see keys above)
            - (None): None if no cloud parsed, if NCD in METAR, if NSC in METAR

        """
        # Detect NCD
        regexNCD = re.search(r'NCD', self.metarWithoutChangements)
        regexNSC = re.search(r'NSC', self.metarWithoutChangements)
        if regexNCD or regexNSC:
            return None

        if re.search(r'VV///',self.metarWithoutChangements):
            return ({
                'code':'VV',
                'meaning':'Invisible Sky',
                'oktaMin':None,
                'oktaMax':None
            })

        classificationClouds = (
            {
                'code': 'SKC',
                'meaning': 'Sky Clear',
                'oktaMin': 0,
                'oktaMax': 0
            },
            {
                'code': 'FEW',
                'meaning': 'Few',
                'oktaMin': 1,
                'oktaMax': 2
            },
            {
                'code': 'SCT',
                'meaning': 'Scattered',
                'oktaMin': 3,
                'oktaMax': 4
            },
            {
                'code': 'BKN',
                'meaning': 'Broken',
                'oktaMin': 5,
                'oktaMax': 7
            },
            {
                'code': 'OVC',
                'meaning': 'Overcast',
                'oktaMin': 8,
                'oktaMax': 8
            }
        )

        matches = []
        for cloudClassification in classificationClouds:
            regex = cloudClassification['code'] + r'\d{3}.{2}'

            search = re.findall(regex, self.metarWithoutChangements)

            if search != []:

                for cloud in search:
                    matches.append(cloud)

        returnList = []
        for match in matches:
            search = None
            i = -1
            end = len(classificationClouds)
            while search is None and i < len(classificationClouds):
                i+=1
                search = re.search(classificationClouds[i]['code'], match)

            if search is not None:
                searchAltitude = re.search(r'\d{3}', match)
                searchCB = re.search(r'CB', match)
                searchTCU = re.search(r'TCU', match)

                returnDict = copy.deepcopy(classificationClouds[i])
                returnDict['altitude'] = int(searchAltitude.group())*100
                returnDict['presenceCB'] = False if searchCB is None else True
                returnDict['presenceTCU'] = False if searchTCU is None else True

                returnList.append(returnDict)

        return None if returnList == [] else tuple(returnList)

    def analyzeTemperatures(self):
        """analyzeTemperature() is a method from `Metar` class.
        If no temperature or dewpoint detected, return None.
        Else, return dictionnary with temperature and dewpoint.

        Keys:
        -----
        - `temperature` (integer): temperature
        - `dewpoint` (integer): dewpoint

        Returns:
            (dict): See keys above
            (None): If no temperature expression parsed
        """
        regexTemperature = r' [M]*\d{2}[/][M]*\d{2} '
        search = re.search(regexTemperature,self.metarWithoutChangements)

        if search is None:
            return None

        metarPortion = search.group()
        separation = metarPortion.split('/') #[0]=Temperature ; [1] = Dewpoint

        #Temperature
        temperatureSearch = re.search(r'\d{2}',separation[0])
        if temperatureSearch is not None:
            temperature = int(temperatureSearch.group())
            if 'M' in separation[0]:
                temperature = temperature * -1
        else:
            temperature = None

        #Dewpoint
        dewpointSearch = re.search(r'\d{2}',separation[1])
        if dewpointSearch is not None:
            dewpoint = int(dewpointSearch.group())
            if 'M' in separation[1]:
                dewpoint = dewpoint * -1
        else:
            dewpoint = None

        return {
            'temperature':temperature,
            'dewpoint':dewpoint
        }

    def analyzeQNH(self):
        """Method from `Metar`class.
        Analyze QNH and return integer if in HPA,
        return float if in inHG. None if pression undetected

        Returns:
            (integer): If in hPA, (e.G => 1013)
            (float): If in inHG, (e.G => 29.92)
        """
        regexHPA = r'Q\d{4}'
        regexINCH = r'A\d{4}'

        searchHPA = re.search(regexHPA,self.metarWithoutChangements)

        if searchHPA is None:
            searchINCH = re.search(regexINCH,self.metarWithoutChangements)

            if searchINCH is None:
                return None
            else:
                pression = int(searchINCH.group()[1:])
                pression = pression/100
                return pression
        else:
            return int(searchHPA.group()[1:])

    def verifyWindAttribute(self, key):
        """Verify if a key exists (gust or variation)

        Returns:
            boolean: False => No key, True => Key attributed
        """
        wind = self.analyzeWind()
        if wind is None:
            return False

        try:
            if wind[key] is None:
                return False

            return True

        except KeyError:
            return False

    def verifyVMC(self):
        """Method parses conditions and return a dictionnary with 2keys.
        These keys represents `controlled` and `uncontrolled` airspaces.
        In order to access thes keys : `metar.verifyVMC()['controlled']`

        Informations given without any warrantly.
        Conditions based SERA reglemention, for an aircraft with speed below 140kt.

        Return None if analysis impossible

        Returns:
            (dictionnary): Dictionnary with 2 keys
        """
        visibility = self.visibility
        clouds = self.cloud
        if visibility is None:
            return None

        if clouds is None:
            clouds_alt_list = [1*10**6]
        else:
            clouds_alt_list = []

            for cloud in clouds:
                clouds_alt_list.append(cloud['altitude'])

        min_altitude = min(clouds_alt_list)

        #UNCONTROLLED
        if visibility >= 1500:
            uncontroled = True
        else:
            uncontroled = False

        #CONTROLLED
        if visibility >= 5000 and min_altitude >=1000:
            controlled = True
        else:
            controlled = False

        return {
            'uncontrolled':uncontroled,
            'controlled':controlled
        }

    def getAttribute(self, attribute, display=False):
        """Getter attribute

        Args:
            attribute (string): Attribute searched. If attribute does not exist, raise `AttributeError`
            display (bool, optional): If True, print attribute. Defaults to False.

        Returns:
            (mixed): Attribute searched
        """
        attr = self.__getattribute__(attribute)

        if display:
            print(attr)

        return attr

    def getAll(self, display=False):
        """Getter properties attribute

        Args:
            display (bool, optional): If true, print attribute. Defaults to False.

        Returns:
            self.properties (dict): Properties
        """
        if display:
            print(self.properties)

        return self.properties

## ERRORS ##

class NOAAServError(Exception):
    """`NOAAServError` is an exception based on Exception basic class
    This exception is raised in methods of `Metar` class if an error occured
    during connection with servor (HTTP error 404, or another)
    """

    def __init__(self, airport, code=None):
        """Constructor

        Args:
        -----
            airport (string): OACI Code of airport (airport attribute of `Metar` class)
            code (integer, optional): Error code, code possible :
            - 404

            Defaults to None.
        """
        if code == 404:
            self.message = "No METAR found from {0}".format(airport)
        else:
            self.message = ("Problem during connection with "
                            "NOAA Weather")

        super().__init__(self.message)

class ReadingMETARError(Exception):
    """Errror raised during reading of one data from METAR
    """

    def __init__(self, data):
        """Constructor

        Args:
            data (STRING): Parameter readen during raising
            E.g->'Visibility','Wind',...
        """
        self.message = 'Error during reading of {0} data from METAR'.format(
            data)
        super().__init__(self.message)

class ReadFileError(Exception):
    """`ReadFileError` is an exception based on Exception basic class
    This exception is raised in methods of `Metar` class if an error occurred
    during reading of file downloaded in temp file by `readlines()` method.
    """

    def __init__(self):
        """Constructor
        """
        self.message = ("Datas have been downloaded from NOAA Weather"
                        "but can't be read by system")

        super().__init__(self.message)

"""a = Metar('LFQN', 'METAR LFQN 201630Z 18005KT 4000 -SHRA SCT030 BKN050CB 18/12 Q1014 NOSIG=')
b = Metar('LFLY', 'METAR LFLY 292200Z AUTO VRB03KT CAVOK 06/M00 Q1000 NOSIG')
#c = Metar('LFPG')
d = Metar('LFLY', 'METAR LFLY 192100Z AUTO 17012KT RASN 06/M02 Q1017 BECMG 19020G35KT')
e = Metar('LFPG', 'METAR LFPG 292200Z AUTO VRB03KT CAVOK 06/M00 Q1000 NOSIG')
f = Metar('CYWG', 'METAR CYWG 172000Z 30015G25KT 3/4SM R36/4000FT/D -SN BLSN BKN008 OVC040 M05/M08 A2992 REFZRA WS RWY36 RMK SF5NS3 SLP134')
g = Metar('LFLY','LFLY 231830Z AUTO 19012KT BKN008 06/02 Q0997')
pass"""


import numpy as np
import pickle
import pybertini
import random
import sys
import time

def main(n):
    tols = 10 ** (np.linspace(0, -10, n))
    import pybertini.tracking as tr
    P = tr.Predictor
    # preds = [p for p in pybertini.tracking.Predictor]
    # took the Heun and P.RKNorsett34 predictors out
    # omit constant because it is slow
    preds = [P.Euler, P.HeunEuler, P.RK4, P.RKCashKarp45, P.RKDormandPrince56, P.RKF45, P.RKVerner67]
    data_set = create_data(tols, preds)
    pickle.dump(data_set, open('data_set_10.p','wb'))

def create_data(tracking_tolerances, predictors):

    array_of_data_sets = np.ndarray(len(tracking_tolerances) * len(predictors), dtype=np.object)
    tr, td, hom = make_system()
    i = 0
    for t in tracking_tolerances:
        for p in predictors:
            #print('{} {}'.format(t, p))
            runtime, successcode = compute_values(t, p, tr, td)
            #str(t) use this if tk load fails
            array_of_data_sets[i] = (t, p, runtime, successcode)
            i += 1

    return array_of_data_sets

def compute_values(tol, pred, tr, td):

    tr.tracking_tolerance(tol)
    tr.predictor(pred)

    start_time = pybertini.multiprec.Complex("1")
    eg_boundary = pybertini.multiprec.Complex("0.1")

    midpath_points = [None]*td.num_start_points()
    overall_code = pybertini.tracking.SuccessCode.Success
    speed_start = time.time()

    target_tol_multiplier = 10
    same_point_tol = target_tol_multiplier * tol

    for ii in range(td.num_start_points()):
        #print("Tracking path {}...".format(ii))
        midpath_points[ii] = pybertini.multiprec.Vector()
        code = tr.track_path(result=midpath_points[ii], start_time=start_time, end_time=eg_boundary, start_point=td.start_point_mp(ii))
        if code != pybertini.tracking.SuccessCode.Success:
            overall_code = pybertini.tracking.SuccessCode.Failure

    from pybertini.multiprec import abs
    for ii in range(td.num_start_points()):
        for jj in range(ii + 1, td.num_start_points()):
            if abs((midpath_points[ii] - midpath_points[jj]).norm()) < same_point_tol:
                overall_code = pybertini.tracking.SuccessCode.Failure
                #print("encountered a path crossing")

    return time.time() - speed_start, overall_code

def make_system():

    gw = pybertini.System()

    x = pybertini.Variable("x")
    y = pybertini.Variable("y")

    vg = pybertini.VariableGroup()
    vg.append(x)
    vg.append(y)
    gw.add_variable_group(vg)

    gw.add_function(pybertini.function_tree.symbol.Rational('29/16')*x**3 - 2*x*y)
    gw.add_function(y - x**2)

    t = pybertini.Variable('t')
    td = pybertini.system.start_system.TotalDegree(gw)
    gamma = pybertini.function_tree.symbol.Rational.rand()
    hom = (1-t)*gw + t*gamma*td
    hom.add_path_variable(t)

    tr = pybertini.tracking.AMPTracker(hom)

#print hom and send danielle the output

    print(hom)
    return tr, td, hom

if __name__ == '__main__':
    main(10)


import numpy as np

def get_noised_input(input_x, sigma):
    new_input = input_x + np.random.normal(0, sigma, input_x.shape)
    new_input = new_input.astype(np.float32)
    return new_input

# 修改此处，改为添加高斯噪声
def get_noised_cfs(input_x, cfs, sigmas, evaluator, generate_cf_func):

    cfs_list = []

    for idx, sigma in enumerate(sigmas):
        tmp_result = {}
        # mapsolver1 = MapSolver(n)
        new_input = get_noised_input(input_x, sigma)
        # cfsolver1 = CFSolver(n, model, new_input, to_replace, desired_pred=1)
        # _cfs = []
        # for text, cf, mask in FindCF(cfsolver1, mapsolver1):
        #     _cfs.extend(cf)
        #
        # _cfs = np.reshape(_cfs, (-1, input_x.shape[1]))

        _cfs = generate_cf_func(idx, new_input)
        diversity = evaluator.diversity(_cfs)

        tmp_result['sigma'] = np.round(sigma, 4)
        tmp_result['cf'] = cfs
        tmp_result['cf2'] = _cfs
        tmp_result['num_of_cf2'] = _cfs.shape[0]
        tmp_result['diversity'] = diversity
        cfs_list.append(tmp_result)

    return cfs_list


import ui.lang_buttons as lb
from utils.utils import log_err

def _get_slice(data, page, slice=8):
    start = page*slice
    end = start + slice
    try:
        return data[start:end]
    except IndexError:
        return data[start:]

def _get_buttons(data, callback, columns=3):
    keyboard = {'inline_keyboard': []}
    if data and callback:
        page = 0
        while True:
            slices = _get_slice(data, page, slice=columns)
            if slices:
                row = [{'text': x[0], 'callback_data': f'{callback}-{x[1]}'} for x in slices]
                keyboard['inline_keyboard'].append(row)
                page += 1
            else:
                break
    return keyboard

def build_page_keyboard(data,
                        page,
                        data_callback,
                        menu_callback,
                        back_callback=None,
                        none_button=False,
                        lang=None):
    if page is None:
        page = 0
    keyboard = _get_buttons(_get_slice(data, page),
                            data_callback)
    control_buttons = []
    if page > 0:
        back_button = {'text': lb.back[lang],
                       'callback_data': f'{menu_callback}/{page - 1}-'}
        control_buttons.append(back_button)
    next_data = _get_slice(data, page + 1)
    if next_data:
        next_button = {'text': lb.next_[lang],
                       'callback_data': f'{menu_callback}/{page + 1}-'}
        control_buttons.append(next_button)
    keyboard['inline_keyboard'].append(control_buttons)
    if none_button:
        keyboard['inline_keyboard'].append([{'text': lb.doesnt_matter[lang],
                                             'callback_data': f'{data_callback}-0'}])
    if back_callback:
        keyboard['inline_keyboard'].append([{'text': lb.back[lang],
                                             'callback_data': f'{back_callback}-'}])
    return keyboard

# [[text, callback_data], ...]
def build_buttons(data):
    return [{'text': x[0], 'callback_data': x[1]} for x in data]

def build_common_keyboard(data,
                          data_callback,
                          buttons=None,
                          in_row=True):
    keyboard = _get_buttons(data, data_callback)
    if buttons:
        buttons_ = build_buttons(buttons)
        if in_row:
            keyboard['inline_keyboard'].append(buttons_)
        else:
            for b in buttons_:
                keyboard['inline_keyboard'].append([b])
    return keyboard

def search_bd(q, data):
    if q:
        for i in data:
            if i[1] == q:
                return i[0]

def normalize_text(input_string):
    if isinstance(input_string, str):
        return input_string.replace('\\n', '\n').replace('`', '')

def get_parameter(text : str, parameter):
    if parameter:
        return text % parameter
    else:
        return ''

async def translate_item(translator, lang, title, description):
    if title and description:
        text = f'{title} // {description}'
        try:
            translated = await translator.translate(text, dest=lang)
            title, description = str(translated.text).split('//')
            return title, description
        except Exception as err:
            log_err(err)
            return title, description


import os
import glob
from tqdm import tqdm
import time
from PIL import Image, ImageOps

base_path = "E:/Python/ImageNet/imagenet_2"

def hoge():
    train_dir = os.path.join(base_path, "train")
    train_classes = []
    for subdir in sorted(os.listdir(train_dir)):
        if os.path.isdir(os.path.join(train_dir, subdir)):
            train_classes.append(subdir)

    val_dir = os.path.join(base_path, "val")
    val_classes = []
    for subdir in sorted(os.listdir(val_dir)):
        if os.path.isdir(os.path.join(val_dir, subdir)):
            val_classes.append(subdir)

    flags = [x == y for x, y in zip(train_classes, val_classes)]
    print(all(flags))
    print(len(train_classes))
    print(len(val_classes))

def hoge():
    for file in sorted(os.walk(base_path+"/train/n01440764")):
        print(file)

def load_time():
    root_dir_hdd = base_path+"/train/n01440764/*"
    base_path_ssd = "G:/n01440764/*"
    files = sorted(glob.glob(base_path_ssd))
    start_time = time.time()
    for i in tqdm(range(10)):
        for f in files:
            with Image.open(f) as img:
                #img = ImageOps.autocontrast(img, i*0.1)
                img = img.resize((256, 256), Image.LINEAR)
                #img = ImageOps.autocontrast(img.resize((256, 256), Image.LINEAR), i*0.1)

                # HDD
                # LANCZOS -> 17.33s, 9.83s
                # autocon 11.33s, 10.99s
                # LANCZOS+autocon -> 15.02s, 14.99s
                # LINEAR+autocon -> 13.70, 13.07s

                # SSD
                # LINEAR -> 7.42s 7.49s 12分
                # LANCZOS -> 9.31s 9.31s
                # autocon -> 10.82s 11.04s
                # LANCZOS+autocon -> 14.85s 15.07s
                # LINEAR+autocon -> 13.01s 13.01s 22分

    print(time.time()-start_time)

import tensorflow as tf
import numpy as np

def test():
    root_dir_hdd = base_path+"/train/n01440764/*"
    base_path_ssd = "G:/n01440764/*"
    files = sorted(glob.glob(base_path_ssd))
    start_time = time.time()
    for i in tqdm(range(10)):
        for f in files:
            with Image.open(f) as img:
                array = np.expand_dims(np.asarray(img, np.uint8), 0)
                convert = tf.image.resize_bicubic(array, (256, 256))

test()


import threading

from kinesis_stream.consumer import KinesisConsumer
from kinesis_stream.record_queue import RecordQueueConsumer
from kinesis_stream.redis_wrapper import get_redis_conn

redis_conn = get_redis_conn(host="localhost", port=6379, db="0")

stream_name = "test-kinesis-stream"
region = "eu-west-1"
redis_state_key = "default-127.0.0.1-6379-0"  # <key-host-port-db>

kinesis_consumer = KinesisConsumer(stream_name, region, redis_conn)
# kinesis_consumer.start()
record_queue_consumer = RecordQueueConsumer(stream_name, redis_conn)
# record_queue_consumer.start()
kinesis_consumer_thread = threading.Thread(name='kinesis_consumer', target=kinesis_consumer.start)
kinesis_consumer_thread.start()

record_queue_consumer_thread = threading.Thread(name='record_queue_consumer', target=record_queue_consumer.start)
record_queue_consumer_thread.start()


import six
import chainer
import numpy as np
import chainer.links as L
import chainer.functions as F
import nutszebra_chainer
import functools
from collections import defaultdict

class BN_ReLU_Conv(nutszebra_chainer.Model):

    def __init__(self, in_channel, out_channel, filter_size=(3, 3), stride=(1, 1), pad=(1, 1)):
        super(BN_ReLU_Conv, self).__init__(
            conv=L.Convolution2D(in_channel, out_channel, filter_size, stride, pad),
            bn=L.BatchNormalization(in_channel),
        )

    def weight_initialization(self):
        self.conv.W.data = self.weight_relu_initialization(self.conv)
        self.conv.b.data = self.bias_initialization(self.conv, constant=0)

    def __call__(self, x, train=False):
        return self.conv(F.relu(self.bn(x, test=not train)))

    def count_parameters(self):
        return functools.reduce(lambda a, b: a * b, self.conv.W.data.shape)

class Highway_Conv(nutszebra_chainer.Model):

    def __init__(self, in_channel, out_channel, filter_size=(3, 3), stride=(1, 1), pad=(1, 1)):
        stride = stride if type(stride) is not int else (stride, stride)
        super(Highway_Conv, self).__init__()
        modules = []
        modules += [('conv', BN_ReLU_Conv(in_channel, out_channel, filter_size, stride, pad))]
        modules += [('conv_highway_3x3', BN_ReLU_Conv(out_channel, out_channel, filter_size, stride, pad))]
        modules += [('conv_highway_1x1_1', BN_ReLU_Conv(out_channel, out_channel, 1, 1, 0))]
        modules += [('conv_highway_1x1_2', BN_ReLU_Conv(out_channel, out_channel, 1, 1, 0))]
        # register layers
        [self.add_link(*link) for link in modules]
        self.modules = modules
        self.in_channel = in_channel
        self.out_channel = out_channel
        self.filter_size = filter_size
        self.stride = stride
        self.pad = pad

    def weight_initialization(self):
        [link.weight_initialization() for _, link in self.modules]

    def count_parameters(self):
        return int(np.sum([link.count_parameters() for _, link in self.modules]))

    @staticmethod
    def concatenate_zero_pad(x, h_shape, volatile, h_type):
        _, x_channel, _, _ = x.data.shape
        batch, h_channel, h_y, h_x = h_shape
        if x_channel == h_channel:
            return x
        pad = chainer.Variable(np.zeros((batch, h_channel - x_channel, h_y, h_x), dtype=np.float32), volatile=volatile)
        if h_type is not np.ndarray:
            pad.to_gpu()
        return F.concat((x, pad))

    def __call__(self, x, train=False):
        h = self.conv(x, train)
        x = self.concatenate_zero_pad(x, h.data.shape, h.volatile, type(h.data))
        highway = self.conv_highway_3x3(x, train)
        highway = self.conv_highway_1x1_1(highway, train)
        highway = F.sigmoid(self.conv_highway_1x1_2(highway, train))
        self.highway = highway
        return h * highway + x * (1.0 - highway)

class Highway_Fitnet4(nutszebra_chainer.Model):

    def __init__(self, category_num):
        super(Highway_Fitnet4, self).__init__()
        # conv
        modules = []
        modules += [('conv1_1', Highway_Conv(3, 32, 3, 1, 1))]
        modules += [('conv1_2', Highway_Conv(32, 32, 3, 1, 1))]
        modules += [('conv1_3', Highway_Conv(32, 32, 3, 1, 1))]
        modules += [('conv1_4', Highway_Conv(32, 48, 3, 1, 1))]
        modules += [('conv1_5', Highway_Conv(48, 48, 3, 1, 1))]
        modules += [('conv2_1', Highway_Conv(48, 80, 3, 1, 1))]
        modules += [('conv2_2', Highway_Conv(80, 80, 3, 1, 1))]
        modules += [('conv2_3', Highway_Conv(80, 80, 3, 1, 1))]
        modules += [('conv2_4', Highway_Conv(80, 80, 3, 1, 1))]
        modules += [('conv2_5', Highway_Conv(80, 80, 3, 1, 1))]
        modules += [('conv2_6', Highway_Conv(80, 80, 3, 1, 1))]
        modules += [('conv3_1', Highway_Conv(80, 128, 3, 1, 1))]
        modules += [('conv3_2', Highway_Conv(128, 128, 3, 1, 1))]
        modules += [('conv3_3', Highway_Conv(128, 128, 3, 1, 1))]
        modules += [('conv3_4', Highway_Conv(128, 128, 3, 1, 1))]
        modules += [('conv3_5', Highway_Conv(128, 128, 3, 1, 1))]
        modules += [('conv3_6', Highway_Conv(128, 128, 3, 1, 1))]
        modules += [('linear', BN_ReLU_Conv(128, category_num, filter_size=(1, 1), stride=(1, 1), pad=(0, 0)))]
        # register layers
        [self.add_link(*link) for link in modules]
        self.modules = modules
        self.category_num = category_num
        self.block = [5, 6, 6]
        self.name = 'highway_fitnet4_{}'.format(category_num)

    def weight_initialization(self):
        [link.weight_initialization() for _, link in self.modules]

    def count_parameters(self):
        return int(np.sum([link.count_parameters() for _, link in self.modules]))

    def __call__(self, x, train=False):
        for i in six.moves.range(len(self.block)):
            for ii in six.moves.range(1, self.block[i] + 1):
                x = self['conv{}_{}'.format(i + 1, ii)](x, train)
            if i < len(self.block) - 1:
                x = F.max_pooling_2d(x, ksize=(2, 2), stride=(2, 2), pad=(0, 0))
        batch, channels, height, width = x.data.shape
        h = F.reshape(F.average_pooling_2d(x, (height, width)), (batch, channels, 1, 1))
        return F.reshape(self.linear(h, train), (batch, self.category_num))

    def calc_loss(self, y, t):
        loss = F.softmax_cross_entropy(y, t)
        return loss

    def accuracy(self, y, t, xp=np):
        y.to_cpu()
        t.to_cpu()
        indices = np.where((t.data == np.argmax(y.data, axis=1)) == True)[0]
        accuracy = defaultdict(int)
        for i in indices:
            accuracy[t.data[i]] += 1
        indices = np.where((t.data == np.argmax(y.data, axis=1)) == False)[0]
        false_accuracy = defaultdict(int)
        false_y = np.argmax(y.data, axis=1)
        for i in indices:
            false_accuracy[(t.data[i], false_y[i])] += 1
        return accuracy, false_accuracy


import nltk
from nltk.chat.util import Chat, reflections

pairs = [
    [
        r"my name is (.*)",
        ["Hello %1, How are you today?"]
    ],
    [
        r"hi|hello",
        ["Hello", "Hi there"]
    ],
    [
        r"what is your name?",
        ["My name is Chatbot. Nice to meet you!"]
    ],
    [
        r"how are you?",
        ["I'm doing well. Thanks for asking."]
    ],
    [
        r"bye",
        ["Bye!", "Goodbye!"]
    ],
]

def chatbot():
    print("Hello, I'm Chatbot! How can I help you today?")

    chat = Chat(pairs, reflections)
    chat.converse()

if __name__ == "__main__":
    nltk.download('punkt')
    chatbot()


from PIL import Image
from PIL import ImageDraw
from PIL import ImageChops

import quad_coll
import os

depths = [4, 6, 8, 12, 14, 16]
sizes = [2, 4, 6, 8, 10]
ratios = [.01, .05, .1, .25]
opacities = [.1, .25, .5, .75, .9]

for f in os.listdir('.'):
    try:
        img = Image.open(f)
    except IOError:
        pass
    else:
        idx = f.rfind('.')
        if (idx == -1):
            imgName = f
            imgExtension = ''
        else:
            imgName = f[:idx]
            imgExtension = f[idx:]

        dirName = imgName + '_test'
        try:
            os.mkdir(dirName)
        except OSError as e:
            if (e.errno != 17):
                raise e

        n = 0
        csv = file(dirName + '/' + imgName + '_trees.csv', 'w')
        csv.write('Input file name,Output file name,Max tree depth,Min node size,Min fill ratio,Min pixel opacity,# of nodes,# of leaf nodes,Image pixels,Tree pixels,False positives,False negatives,Total error\n')

        for d in depths:
            quad_coll.MAX_DEPTH = d
            for s in sizes:
                quad_coll.MIN_SIZE = s
                for r in ratios:
                    quad_coll.MIN_RATIO = r
                    for o in opacities:
                        quad_coll.MIN_OPACITY = o

                        tree = quad_coll.makeTree(img)

                        img2 = Image.new('RGBA', img.size)
                        quad_coll.drawTree(img2, tree, True)
                        draw = ImageDraw.Draw(img2)
                        draw.text((0, 0), 'max tree depth: %d' % d)
                        draw.text((0, 10), 'min node size: %d pixels' % s)
                        draw.text((0, 20), 'min node fill ratio: %f pixels' % r)
                        draw.text((0, 30), 'min filled pixel opactiy:%f alpha' % o)

                        outfilename = dirName + '/' + imgName + ('%04d' % n) + f[f.rfind('.'):]
                        ImageChops.composite(img2, img, img2).save(outfilename)
                        n = n + 1

                        error = quad_coll.calcError(img, tree)
                        csv.write('%s,%s,%d,%d,%f,%f,%d,%d,%d,%d,%d,%d,%d\n' % (\
                            f,\
                            outfilename,\
                            d,\
                            s,\
                            r,\
                            o,\
                            tree.getNumNodes(),\
                            tree.getNumLeafNodes(),\
                            error['imgPixels'],\
                            error['treePixels'],\
                            error['falsePositives'],\
                            error['falseNegatives'],\
                            error['falsePositives'] + error['falseNegatives']\
                        ))

        csv.close()


"""
* Assignment: DataFrame Groupby Phones
* Complexity: easy
* Lines of code: 5 lines
* Time: 8 min

English:
    1. Use data from "Given" section (see below)
    2. Read data from `DATA` as `df: pd.DataFrame`
    3. Give information about total number of all phone calls for each calendar month

Polish:
    1. Użyj danych z sekcji "Given" (patrz poniżej)
    2. Wczytaj dane z `DATA` jako `df: pd.DataFrame`
    3. Podaj informacje o łącznej liczbie wszystkich połączeń telefonicznych dla każdego miesiąca kalendarzowego

Tests:
    >>> type(result) is pd.Series
    True
    >>> pd.set_option('display.width', 500)
    >>> pd.set_option('display.max_columns', 10)
    >>> pd.set_option('display.max_rows', 10)
    >>> result  # doctest: +NORMALIZE_WHITESPACE
    year  month
    1999  10       16309.0
          11       16780.0
          12       14861.0
    2000  1        18705.0
          2        11019.0
          3        14647.0
    Name: duration, dtype: float64
"""

# Given
import pandas as pd

DATA = 'https://raw.githubusercontent.com/AstroMatt/book-python/master/_data/csv/phones-pl.csv'

result = ...

# Solution
df = pd.read_csv(DATA, parse_dates=['datetime'])
df['year'] = df['datetime'].dt.year
df['month'] = df['datetime'].dt.month
calls = df[df['item'] == 'call']
result = calls.groupby(['year', 'month'])['duration'].sum()