-------------------------------------------------------------------------------
-- Copyright (c) 2015 Sierra Wireless. All rights reserved.
--
-- Contributors:
--     Randall Pfeifer - initial implementation
--
--	Telemetry Handler
--	Creates object to drive the over-all telemetry process: detects presence of
--	B&B Streamer, verifies correct model and firmware version, updates firmware
--	as necessary, collects set of PIDs supported by vehicle, queries to collect
--	data, suppresses redundant messages, and sends data.
-------------------------------------------------------------------------------

-- ----------------------------------------------------------------------------
-- REQUIRES
-- ----------------------------------------------------------------------------

local devtree = require 'devicetree'
local uptime = require 'sched.timer.core'

-- ----------------------------------------------------------------------------
-- CONSTANTS
-- ----------------------------------------------------------------------------

local MSCIID_STS_TELEM_J1939_FAULTS_SUPPORT	= 'system.vehicle.j1939.fault_support'
local MSCIID_STS_TELEM_J1939_FAULTS			= 'system.vehicle.j1939.fault_codes'

-- ----------------------------------------------------------------------------
-- HELPER FUNCTIONS
-- ----------------------------------------------------------------------------

------------------------------------------------------------------
-- filter_data - Filter out unneeded telemetry data 
-- 
-- Inputs:
-- 	data			- The response data table to filter
-- 	options			- config data table containing filter parameters
--
-- Outputs:
-- 	data			- The response data table to filter
--
-- Returns:
--
-- Notes
--	Filters telemetry data collected from B&B to reduce data send
--	bandwidth. Removes app parameters that are not enabled,
--	limits send rate, periodically allows a send, and triggers on threshold.
------------------------------------------------------------------
local function filter_data(self, data)
	local options = self.options
	local hist = self.history

	local now = uptime.time()
	for p, v in pairs(data) do
		-- not enabled - filter out
		if not options[p] or not options[p].en or options[p].en == 0 then
			data[p] = nil
		else
			if not hist[p] then
				-- If item never sent before (not in history), send it, save is history
				hist[p] = {now = now, value = v}
			else
				local trigger, priority = self.streamer.PIDInfoByName[p].cmp(options[p].th, v, hist[p].value, self.streamer.PIDInfoByName[p].soz)
				if priority then
					-- Send because it's important
					hist[p] = {now = now, value = v}
				-- It's there. Time to send (and periodic send enabled) ?
			    elseif  options[p].mx > 0 and now < hist[p].now + options[p].mx then
					-- Don't exceed max send rate
					data[p] = nil
				elseif options[p].mn > 0 and now >= hist[p].now + options[p].mn then
					-- time to send based on minimum send rate
					hist[p] = {now = now, value = v}
				elseif trigger then
					-- Threshold comparison (exact methon in table) says we need to send
					hist[p] = {now = now, value = v}
				else
					-- No reason to send - remove from list
					data[p] = nil
				end
			end
		end
	end
end

------------------------------------------------------------------
-- send_telemetry - Collect and send telemetry data 
-- 
-- Inputs:
--
-- Outputs:
--
-- Returns:
--	false		- on error (no response (disconnected) or error
--	true		- ok
--
-- Notes
--	Queries the B&B Streamer for telemetry data, passes the data
--	through the filter (to reduce bandwidth requirements), and
--	then send it to the management system.
------------------------------------------------------------------
function send_telemetry(self)
	local response
	local data = {}
	local active = false
	local commands = self.streamer.commands
	local status = self.streamer.status
	local now = uptime.time()

	-- Process any un-solicited data in the serial pipe
	repeat
		response = self.streamer:get_response(true)
		if response then
			if log.musttrace(self.app_name, "DEBUG") then
				log(self.app_name, "DEBUG", 'Unsolicited: command="%s"  data="%s"', showdata(response.command), showdata(response.data))
			end
			self.streamer:process_response(response, data)
		end
	until not response

	response = self.streamer:query(string.char(commands.GET_PARAMETER), self.active_PIDs)

	if response and string.byte(response.command, 1) == status.IGNITION_OFF then
		active = true
		data.ignition_status = 0
		self.engine_start_time = now	-- make sure engine run time resets
	else

		if response and string.byte(response.command, 1) == commands.GET_PARAMETER + 0x80 then
			active = true
			self.streamer:process_response(response, data)
		else
			log(self.app_name, "DETAIL", 'query GET_PARAMETER failed response: "' .. showdata(response) .. '"')
		end
		
		-- get J1979 DTCs via streamer interface
		response = self.streamer:query(string.char(commands.GET_VEHICLE_INFO), string.char(2))
		if response and string.byte(response.command, 1) == commands.GET_VEHICLE_INFO + 0x80 then
			active = true
			self.streamer:process_response(response, data)
		else
			log(self.app_name, "DETAIL", 'query GET_VEHICLE_INFO failed response: "' .. showdata(response) .. '"')
		end

		-- get J1939 DTCs (MP70 only)
		if devtree.get(MSCIID_STS_TELEM_J1939_FAULTS_SUPPORT) == "1" then
			local j1939dtcs = devtree.get(MSCIID_STS_TELEM_J1939_FAULTS)
			if j1939dtcs ~= nil and j1939dtcs ~= "" then
				log(self.app_name, "DETAIL", "Got j1939 DTCs: " .. j1939dtcs)
				if not data.dtcs then
					data.dtcs = j1939dtcs
				else
					data.dtcs = data.dtcs .. "," .. j1939dtcs
				end
			end
		end

		-- BnB Streamer does not provide time since engine start, so synthesize 
		-- it from other telemetry data. RPM > 0 allows timer to run.
		if not (self.history.engine_speed and self.history.engine_speed.value > 0 and
				data.engine_speed and data.engine_speed > 0) then
			self.engine_start_time = now
		end
		data.engine_run_time = math.floor(now - self.engine_start_time)
	end

	-- Filter and send telemetry data
	filter_data(self, data)
	if next(data) then
		self.send(data)
	end

	return active
end

------------------------------------------------------------------
-- get_active_PIDs - Query the streamer for set of PIDs to poll
--
-- Inputs:
--
-- Outputs:
--	self.supported_PIDs		-- all PIDs supported by styreamer/vehicle
--	self.active_PIDs		-- enabled PIDs (subset)
--
-- Returns:
--	nil, status	- on error (query status code, or nil if no response)
--	"ok"		- ok
--
-- Notes
--	Queries the B&B Streamer to get the set of supported PIDs,
--	then filters out the ones that are not enabled in config.
--	PIDs saved as a string, with each char holding the value
--	of the PID.
------------------------------------------------------------------
function get_active_PIDs(self)
	local commands = self.streamer.commands
	local status = nil

	-- Get Supported Parameters
	self.supported_PIDs = ""
	local response = self.streamer:query(string.char(commands.GET_SUPPORTED_PARAMETERS))
	if response and string.byte(response.command) ==  commands.GET_SUPPORTED_PARAMETERS + 0x80 then
		-- Raw response data
		self.supported_PIDs = response.data
		-- log(self.app_name, "INFO", "self.streamer Supported Parameters Response: %s:%s", showdata(response.command), showdata(response.data))
		for i = 1, string.len(response.data) do
			if not self.streamer.PIDInfoByNum[string.byte(response.data, i)] then
				log(self.app_name, "DETAIL", "Unrecognized PID 0x%02x supported by Streamer", string.byte(response.data, i))
			else
				log(self.app_name, "DETAIL", "Supported PID %d (%s)", i, self.streamer.PIDInfoByNum[string.byte(response.data, i)].name)
			end
		end
	else
--		log(self.app_name, "ERROR", "B&B error: ", errcd)
		if response then
			status = string.byte(response.command)
		end
		return nil, status
	end

	-- Filter disabled and unsupported PIDs from the set of active PIDs
	local PIDList = {}
	for i = 1, string.len(self.supported_PIDs) do
		local pid = string.byte(self.supported_PIDs, i)
		if self.streamer.PIDInfoByNum[pid] and self.options[self.streamer.PIDInfoByNum[pid].name].en > 0 then
			table.insert(PIDList, pid)
			log(self.app_name, "DETAIL", "PID %d (%s): ",pid, self.streamer.PIDInfoByNum[pid].name)
		end
	end

	self.active_PIDs = list2string(PIDList)

	return "ok"
end

-- Telemetry object to handle over-all telemetry operation

-- ----------------------------------------------------------------------------
-- METHODS
-- ----------------------------------------------------------------------------

------------------------------------------------------------------
-- reconfigure - Member func to re-configure the poll list 
-- 
-- Inputs:
--
-- Outputs:
--
-- Returns:
--
-- Notes
--	Update the PIDs to poll to reflect changes in configuration.
--	Call after confgiguration updated.
------------------------------------------------------------------
local function t_reconfigure (self) 
	log(self.app_name, "INFO", "Telemetry State: %s   %s", self.state, "Message: reconfigure")
	if self.state == "active" then
		self.state = "reconfigure"
	end
end

-- Poll the OBDII interface for telemetry data
------------------------------------------------------------------
-- poll - Poll the streamer 
-- 
-- Inputs:
--
-- Outputs:
--
-- Returns:
--
-- Notes
--	Poll and sequence the streamer. Call periodically (every second)
--	to poll and control the streamer. This is the meat of the telemetry
--	process.
------------------------------------------------------------------
local function t_poll (self)
	local response, errcd
	local commands = self.streamer.commands
	local status = self.streamer.status

	if self.state ~= self.laststate then
		log(self.app_name, "DETAIL", "Telemetry State: %s   %s", self.state, "Message: poll")
		self.laststate = self.state
	end

----------------------------------------------
	if self.state == "init" or self.state == "redetect" then
		if self.state == "init" then
			self.send({error="OBDII disconnected"})
		end

		while self.streamer:get_response(true) do end -- flush junk from port
		response, errcd = self.streamer:query(string.char(commands.READ_MODEL_NUMBER))
		if response then
			if string.byte(response.command) ~= 0xff then
				-- Initiate re-detect streamer and then wait
				self.streamer:query(string.char(commands.REDETECT_VEHICLE))
				self.streamer.serial_port:settimeout(90)
				log(self.app_name, "DEBUG", "waiting for Streamer re-detect ...")			
				local rsp = self.streamer.serial_port:read(5)
				log(self.app_name, "DEBUG", "...ok. Got %s", showdata(rsp))
			end			
			self.state = "disconnected"
		else
			self.state = "redetect"
		end
	
----------------------------------------------
	elseif self.state == "disconnected" then
		-- Disconnected (initial) state:
		-- Query Streamer for model number to check for life. If we get a response
		-- and the correct model is connected then continue on to verify the firmware version.
		while self.streamer:get_response(true) do end -- flush junk from port
		response, errcd = self.streamer:query(string.char(commands.READ_MODEL_NUMBER))
		if response then
			if string.byte(response.command, 1) == commands.READ_MODEL_NUMBER + 0x80 then
				log(self.app_name, "INFO", "Detected Streamer model: %s", response.data)
				if response.data == "LDVDSV2CAN-SWIR" then
					self.state = "configure"
					log(self.app_name, "INFO", "MP-70 Telemetry Detected - no FW Update")
				elseif response.data == "LDVDSV2CAN-S" then
					self.state = "fw_check"
				else
					log(self.app_name, "ERROR", "Unsupported Streamer model: %s", response.data)
					self.send({error="Unsupported B&B Streamer model: "..showdata(response.data)})
					self.state = "unsupported"
				end
			elseif string.byte(response.command, 1) == 0xff then
				local c = string.byte(response.command, 2)
				if c == 0x0b or c == 0x0c or c == 0x0d then 
					self.state = "fw_check"
				end
			end
		end

----------------------------------------------
	elseif self.state == "unsupported" then
		-- Unsupported (firmware) state
		-- If the initial firmware check reveals an unsupported device is connected,
		-- and just poll until disconnected (hopefully someone
		-- will connect the correct model).
		while self.streamer:get_response(true) do end -- flush junk from port
		response, errcd = self.streamer:query(string.char(commands.READ_MODEL_NUMBER))
		if not response then
			self.state = "disconnected"
		end

----------------------------------------------
	elseif self.state == "fw_check" then
	-- Firmware Check/Update state
	-- Check the firmware component versions against that expected (in fw_table in BnB).
	-- If all versions match, then continue on to configure AVTA, else update the
	-- non-matching components.

		-- convert a list if numbers to a string (fast way)
		local function l2s(t) return string.char(unpack(t)) end

		while self.streamer:get_response(true) do end -- flush junk from port

		response, errcd = self.streamer:query(string.char(commands.GET_COMPONENT_VERSIONS))
		if response then

			-- function to update a component
			local function update(i)
				self.send({info="Firmware update "..self.streamer.fw_table[i].name})
				log(self.app_name, "WARNING", "FW Update component %d: %s", i, self.streamer.fw_table[i].name)
				-- Update the component
				-- Will be disconnected - re-detect and try again
				local rtn, code = self.streamer:update_component(i)
				if rtn then
					self.send({info="FW update completed"})
					log(self.app_name, "WARNING", "FW Update component %s: completed", self.streamer.fw_table[i].name)
				else
					self.send({error=string.format("FW update (%s) failed - %s", self.streamer.fw_table[i].name, code)})
					log(self.app_name, "ERROR", "FW Update component %s: Failed", self.streamer.fw_table[i].name)
				end
			end

			if string.byte(response.command, 1) == commands.GET_COMPONENT_VERSIONS + 0x80 then
				if string.len(response.data) == 15 then
					-- Go thru the query response
					local updated = false
					for i,v in ipairs(self.streamer.fw_table) do
						-- Update all components that don't match expected versions.
						-- Update these in the prescibed order
						if string.sub(response.data, v.offset, v.offset + 2) ~= l2s(v.ver) then
							update(i)
							updated = true
						end
					end
					if updated then
						-- cold-start the provess to re-detect the vehicle
						self.state = "redetect"
					else
						-- All was good - setup configuration
						self.state = "configure"
						log(self.app_name, "DETAIL", "  FW OK")
					end
				else
					log(self.app_name, "WARNING", "Invalid response to version check: %s", showdata(response.data))
				end

			elseif string.byte(response.command, 1) == 0xff then
				-- If a partial updfate has been done in the past we might get error responses.
				-- Deal with those by updating the offending component.
				local code = {} ; code[0x0b]=3 ; code[0x0c]=1 code[0x0d]=2 
				local i = code[string.byte(response.command, 2)]
				if i then 
					update(i)
					self.state = "disconnected"
				else
					self.send({error="B&B Streamer error: "..showdata(response.command)})
					self.state = "unsupported"
				end
			end
				
		else
			log(self.app_name, "WARNING", "No response to version check")
			self.state = "disconnected"
		end

----------------------------------------------
	elseif self.state == "configure" or self.state == "reconfigure" then
		-- Determine the PIDs to query
		while self.streamer:get_response(true) do end -- flush junk from port
		
		local rc, stat = get_active_PIDs(self)
		
		if stat == status.VEHICLE_NOT_DETECTED then
			-- Streamer interface detected but no vehicle - just sit here until either
			-- a vehicle is connected or there's no response
			log(self.app_name, "DEBUG", "No vehicle detected")
		elseif not rc then
			log(self.app_name, "WARNING", "Failed PID query: " .. tostring(status))
			self.state = "disconnected"
		else
			-- Send a list of supported PIDs at connect
			local pidlist = ""
			for i = 1, string.len(self.supported_PIDs) do
				if pidlist ~= "" then
					pidlist = pidlist .. ","
				end

				if self.streamer.PIDInfoByNum[string.byte(self.supported_PIDs, i)] then
					-- can't report unrecognized pids
					pidlist = pidlist .. self.streamer.PIDInfoByNum[string.byte(self.supported_PIDs, i)].name
				end

			end

			local data = {supported_pids=pidlist}

			-- get VIN
			local response = self.streamer:query(string.char(commands.GET_VEHICLE_INFO), string.char(0))
			if response and string.byte(response.command, 1) == commands.GET_VEHICLE_INFO + 0x80 then
				self.streamer:process_response(response, data)
			end
	
			filter_data(self, data)	-- suppress redundant messages
			if self.state ~= "reconfigure" then
				data.info = "OBDII connected"
				self.send(data)
			end
	
			self.state = "active"
		end

----------------------------------------------
	elseif self.state == "active" then
		if not send_telemetry(self) then
			-- The streamer has been disconnected. Go to cold start so that
			-- the vehicle will be re-detected upon re-connect to ensure that
			-- the (possibly) new streamer knows this vehicle
			log(self.app_name, "WARNING", "Streamer disconnected")
			self.send({error="OBDII disconnected"})
			self.history = {}	-- reset filter so next connect sends everything
			self.state = "redetect"
		end

----------------------------------------------
	else
		log(self.app_name, "WARNING", "Undefined telemetry state: %s", self.state)
		self.state = "disconnected"
	end
end

------------------------------------------------------------------
-- isPresent - Indicate if streamer is connected
-- 
-- Inputs:
--
-- Outputs:
--
-- Returns:
--	True if a streamer device is detected
--
-- Notes
--	Indicates if a usable streamer device is detected
------------------------------------------------------------------
local function t_isPresent(self)
	return not (self.state == "redetect" or self.state == "unsupported")
end

------------------------------------------------------------------
-- 
--
------------------------------------------------------------------
local function t_trip_reset(self)
	if not self:isPresent() then
		return
	end
	self.streamer:trip_reset()
end

------------------------------------------------------------------
-- run - Start telemetry process
-- 
-- Inputs:
--
-- Outputs:
--
-- Returns:
--
-- Notes
--	Starts telemetry polling
------------------------------------------------------------------
local function t_start(self)
	if not self.poll_task then
		self.history = {}	-- reset filter so next connect sends everything
		self.state = "init"
		engine_start_time = uptime.time(),	-- variable to calculate engine run time

		log(self.app_name, "INFO", "Telemetry process started")

		self.poll_task = sched.run (
			function()
				repeat
					t_poll(self)
					sched.wait(1)
				until not self.poll_task
			end
		)
	end
end

------------------------------------------------------------------
-- stop - Start telemetry process
-- 
-- Inputs:
--
-- Outputs:
--
-- Returns:
--
-- Notes
--	Stops telemetry polling
------------------------------------------------------------------
local function t_stop(self)
	if self.poll_task then
		sched.kill(self.poll_task)
		self.poll_task = nil
		log(self.app_name, "INFO", "Telemetry process stopped")
	end
end

------------------------------------------------------------------
-- create - create the telemetry object 
-- 
-- Inputs:
--	app_name		-- Name of app (for log messages)
--	BnB				-- BnB Streamer handler object
--	send			-- function to format and send telemetry data
--
-- Outputs:
--
-- Returns:
--	telemetry object
--
-- Notes
------------------------------------------------------------------
local function create (app_name, BnB, send)
	devtree.init()

	local telemetry = {
		app_name = app_name,
		streamer = BnB,
		send = send,
		state = "init",					-- telemetry state machine state
		history = {}, 					-- Holds previous telemetry data history (for filter)
		options = {},					-- telemetry options

		filter = filter_data,
		reconfigure = t_reconfigure,
		start = t_start,
		stop = t_stop,
		isPresent = t_isPresent,
		trip_reset = t_trip_reset
	}
	return telemetry
end

return create
