already_AddRefed<DOMRequest>
BluetoothAdapter::Connect(BluetoothDevice& aDevice,
                          const Optional<short unsigned int>& aServiceUuid,
                          ErrorResult& aRv)
{
  nsCOMPtr<nsPIDOMWindow> win = GetOwner();
  if (!win) {
    aRv.Throw(NS_ERROR_FAILURE);
    return nullptr;
  }

  nsRefPtr<DOMRequest> request = new DOMRequest(win);
  nsRefPtr<BluetoothVoidReplyRunnable> results =
    new BluetoothVoidReplyRunnable(request);

  nsAutoString address;
  aDevice.GetAddress(address);
  uint32_t deviceClass = aDevice.Class();
  uint16_t serviceUuid = 0;
  if (aServiceUuid.WasPassed()) {
    serviceUuid = aServiceUuid.Value();
  }

  BluetoothService* bs = BluetoothService::Get();
  if (!bs) {
    aRv.Throw(NS_ERROR_FAILURE);
    return nullptr;
  }
  bs->Connect(address, deviceClass, serviceUuid, results);

  return request.forget();
}
already_AddRefed<DOMRequest>
BluetoothAdapter::PairUnpair(bool aPair, BluetoothDevice& aDevice,
                             ErrorResult& aRv)
{
  nsCOMPtr<nsPIDOMWindow> win = GetOwner();
  if (!win) {
    aRv.Throw(NS_ERROR_FAILURE);
    return nullptr;
  }

  nsRefPtr<DOMRequest> request = new DOMRequest(win);
  nsRefPtr<BluetoothVoidReplyRunnable> results =
    new BluetoothVoidReplyRunnable(request);

  nsAutoString addr;
  aDevice.GetAddress(addr);

  BluetoothService* bs = BluetoothService::Get();
  if (!bs) {
    aRv.Throw(NS_ERROR_FAILURE);
    return nullptr;
  }
  nsresult rv;
  if (aPair) {
    rv = bs->CreatePairedDeviceInternal(addr,
                                        kCreatePairedDeviceTimeout,
                                        results);
  } else {
    rv = bs->RemoveDeviceInternal(addr, results);
  }
  if (NS_FAILED(rv)) {
    NS_WARNING("Pair/Unpair failed!");
    aRv.Throw(rv);
    return nullptr;
  }

  return request.forget();
}
void threadLoop(gpointer data)
{
	GAsyncQueue *privCommandQueue = g_async_queue_ref(((OBD2Source*)data)->commandQueue);
	GAsyncQueue *privResponseQueue = g_async_queue_ref(((OBD2Source*)data)->responseQueue);
	GAsyncQueue *privSingleShotQueue = g_async_queue_ref(((OBD2Source*)data)->singleShotQueue);
	GAsyncQueue *privSubscriptionAddQueue = g_async_queue_ref(((OBD2Source*)data)->subscriptionAddQueue);
	GAsyncQueue *privSubscriptionRemoveQueue = g_async_queue_ref(((OBD2Source*)data)->subscriptionRemoveQueue);
	GAsyncQueue *privStatusQueue = g_async_queue_ref(((OBD2Source*)data)->statusQueue);
	
	obdLib *obd = new obdLib();
	OBD2Source *source = (OBD2Source*)data;

	obd->setCommsCallback([](const char* mssg, void* data) { DebugOut(6)<<mssg<<endl; },NULL);
	obd->setDebugCallback([](const char* mssg, void* data, obdLib::DebugLevel debugLevel) { DebugOut(debugLevel)<<mssg<<endl; },NULL);
	
	std::list<ObdPid*> reqList;
	std::list<ObdPid*> repeatReqList;
	ObdPid::ByteArray replyVector;
	std::string reply;
	std::string port;
	std::string baud;
	bool connected=false;
	int emptycount = 0;
	int timeoutCount = 0;
	while (source->m_threadLive)
	{
		//gpointer query = g_async_queue_pop(privCommandQueue);
		
		
		gpointer query = g_async_queue_try_pop(privSingleShotQueue);
		if (query != nullptr)
		{
			//printf("Got request!\n");
			
			ObdPid *req = (ObdPid*)query;
			DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Got single shot request: " << req->pid.substr(0,req->pid.length()-1) << ":" << req->property <<endl;
			repeatReqList.push_back(req);
		}
		query = g_async_queue_try_pop(privSubscriptionAddQueue);
		if (query != nullptr)
		{

			ObdPid *req = (ObdPid*)query;
			//DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Got subscription request for "<<req->req<<endl;
			reqList.push_back(req);
		}
		query = g_async_queue_try_pop(privCommandQueue);
		if (query != nullptr)
		{
			//ObdPid *req = (ObdPid*)query;
			CommandRequest *req = (CommandRequest*)query;
			//commandMap[req->req] = req->arg;
			//printf("Command: %s\n",req->req.c_str());
			DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Command:" << req->req << endl;
			if (req->req == "connect" )
			{

				if (source->m_isBluetooth)
				{
					BluetoothDevice bt;
					std::string tempPort = bt.getDeviceForAddress(source->m_btDeviceAddress, source->m_btAdapterAddress);
					if(tempPort != "")
					{
						DebugOut(3)<<"Using bluetooth device \""<<source->m_btDeviceAddress<<"\" bound to: "<<tempPort<<endl;
						port = tempPort;
					}
				}
				else
				{
					port = req->arglist[0];
					baud = req->arglist[1];
				}
				connected = connect(obd,port,baud);

				if(connected)
				{
					StatusMessage *statusreq = new StatusMessage();
					statusreq->statusStr = "connected";
					g_async_queue_push(privStatusQueue,statusreq);
				}
				else
				{
					StatusMessage *statusreq = new StatusMessage();
					statusreq->statusStr = "disconnected";
					g_async_queue_push(privStatusQueue,statusreq);
				}
				
			}
			else if (req->req == "connectifnot")
			{
				if (!connected)
				{
					if (source->m_isBluetooth)
					{
						BluetoothDevice bt;
						std::string tempPort = bt.getDeviceForAddress(source->m_btDeviceAddress, source->m_btAdapterAddress);
						if(tempPort != "")
						{
							DebugOut(3)<<"Using bluetooth device \""<<source->m_btDeviceAddress<<"\" bound to: "<<tempPort<<endl;
							port = tempPort;
						}
						else
						{
							DebugOut(DebugOut::Error)<<"Error creating bluetooth device"<<endl;
							continue;
						}
					}

					connected = connect(obd,port,baud);

					if(connected)
					{
						StatusMessage *statusreq = new StatusMessage();
						statusreq->statusStr = "connected";
						g_async_queue_push(privStatusQueue,statusreq);
					}
					else
					{
						StatusMessage *statusreq = new StatusMessage();
						statusreq->statusStr = "disconnected";
						g_async_queue_push(privStatusQueue,statusreq);
					}
				}
			}
			else if (req->req == "setportandbaud")
			{
				port = req->arglist[0];
				baud = req->arglist[1];
			}
			else if (req->req == "disconnect")
			{
				DebugOut() << __SMALLFILE__ << ":" << __LINE__ << "Using queued disconnect" << (ulong)req << endl;
				obd->closePort();
				BluetoothDevice bt;
				bt.disconnect(source->m_btDeviceAddress, source->m_btAdapterAddress);
				connected = false;
				StatusMessage *statusreq = new StatusMessage();
				statusreq->statusStr = "disconnected";
				g_async_queue_push(privStatusQueue,statusreq);
			}
			delete req;
		}
		query = g_async_queue_try_pop(privSubscriptionRemoveQueue);
		if (query != nullptr)
		{
			DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Got unsubscription request"<<endl;
			ObdPid *req = (ObdPid*)query;
			for (std::list<ObdPid*>::iterator i=reqList.begin();i!= reqList.end();i++)
			{
				if ((*i)->property == req->property)
				{
					reqList.erase(i);
					delete (*i);
					i--;
					if (reqList.size() == 0)
					{
						break;
					}
				}
			}
			//reqList.push_back(req->req);
			delete req;
		}
		if (reqList.size() > 0 && !connected)
		{
			/*CommandRequest *req = new CommandRequest();
			req->req = "connect";
			req->arglist.push_back(port);
			req->arglist.push_back(baud);
			g_async_queue_push(privCommandQueue,req);
			continue;*/
		}
		else if (reqList.size() == 0 && connected)
		{
			emptycount++;
			if (emptycount < 1000)
			{
				usleep(10000);
				continue;
			}
			emptycount = 0;
			CommandRequest *req = new CommandRequest();
			req->req = "disconnect";
			g_async_queue_push(privCommandQueue,req);
			continue;
		}
		if (!connected)
		{
			usleep(10000);
			continue;
		}
		for (std::list<ObdPid*>::iterator i=reqList.begin();i!= reqList.end();i++)
		{
			repeatReqList.push_back(*i);
		}
		int badloop = 0;
		for (std::list<ObdPid*>::iterator i=repeatReqList.begin();i!= repeatReqList.end();i++)
		{
			DebugOut(10) << __SMALLFILE__ << ":" << __LINE__ << "Requesting pid: " << (*i)->pid.substr(0,(*i)->pid.length()-1) << (*i)->property << endl;
			if (source->m_blacklistPidCountMap.find((*i)->pid) != source->m_blacklistPidCountMap.end())
			{
				//Don't erase the pid, just skip over it.
				int count = (*source->m_blacklistPidCountMap.find((*i)->pid)).second;
				if (count > 10)
				{
					continue;
				}
			}
			badloop++;

			bool result = false;

			if(beginsWith((*i)->pid,"AT") || beginsWith((*i)->pid, "ST"))
			{
				result = obd->sendObdRequestString((*i)->pid.c_str(),(*i)->pid.length(),&replyVector);
			}
			else result = obd->sendObdRequestString((*i)->pid.c_str(),(*i)->pid.length(),&replyVector,5,3);

			if (!result)
			{
				//This only happens during a error with the com port. Close it and re-open it later.
				DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Unable to send request:" << (*i)->pid.substr(0,(*i)->pid.length()-1) << endl;
				if (obd->lastError() == obdLib::NODATA)
				{
					DebugOut() << __SMALLFILE__ << ":" << __LINE__ << "OBDLib::NODATA for pid" << (*i)->pid.substr(0,(*i)->pid.length()-1) << " expected property: " << (*i)->property << endl;
					if (source->m_blacklistPidCountMap.find((*i)->pid) != source->m_blacklistPidCountMap.end())
					{
						//pid value i not yet in the list.
						int count = (*source->m_blacklistPidCountMap.find((*i)->pid)).second;
						if (count > 10)
						{
							
						}
						source->m_blacklistPidCountMap.erase(source->m_blacklistPidCountMap.find((*i)->pid));
						source->m_blacklistPidCountMap.insert(pair<std::string,int>((*i)->pid,count));
					}
					else
					{
						source->m_blacklistPidCountMap.insert(pair<std::string,int>((*i)->pid,1));
					}
					StatusMessage *statusreq = new StatusMessage();
					statusreq->statusStr = "error:nodata";
					statusreq->property = (*i)->property;
					g_async_queue_push(privStatusQueue,statusreq);
					continue;
				}
				else if (obd->lastError() == obdLib::TIMEOUT)
				{
					timeoutCount++;
					if (timeoutCount < 2)
					{
						DebugOut() << __SMALLFILE__ << ":" << __LINE__ << "OBDLib::TIMEOUT for pid" << (*i)->pid << endl;
						StatusMessage *statusreq = new StatusMessage();
						statusreq->statusStr = "error:timeout";
						g_async_queue_push(privStatusQueue,statusreq);
						continue;
					}
				}
				else
				{
					DebugOut() << __SMALLFILE__ << ":" << __LINE__ << "OBD Other error:" << obd->lastError() << endl;
				}
				
				CommandRequest *req = new CommandRequest();
				DebugOut() << __SMALLFILE__ << ":" << __LINE__ << "Queuing up a disconnect" << (ulong)req << endl;
				req->req = "disconnect";
				g_async_queue_push(privCommandQueue,req);
				i = repeatReqList.end();
				i--;
				continue;
			}
			if (source->m_blacklistPidCountMap.find((*i)->pid) != source->m_blacklistPidCountMap.end())
			{
				//If we get the pid response, then we want to clear out the blacklist list.
				source->m_blacklistPidCountMap.erase(source->m_blacklistPidCountMap.find((*i)->pid));
			}
			timeoutCount = 0;
			//ObdPid *pid = ObdPid::pidFromReply(replyVector);
			ObdPid *pid = obd2AmbInstance->createPidFromReply(replyVector);
			if (!pid)
			{
				//Invalid reply
				DebugOut() << "Invalid reply"<<endl;
				continue;
			}
			else
			{
				DebugOut(11) << __SMALLFILE__ <<":"<< __LINE__ << "Reply recieved and queued for:" << (*i)->pid.substr(0,(*i)->pid.length()-1) << endl;
				std::string repstr;
				for (int i=0;i<replyVector.size();i++)
				{
				  if (replyVector[i] != 13)
				  {
				  repstr += (char)replyVector[i];
				  }
					//DebugOut(11) << replyVector[i];
				}
				DebugOut(11) << "Reply:" << repstr << endl;
			}
			g_async_queue_push(privResponseQueue,pid);
		}
		if (badloop == 0)
		{
			//We had zero non-blacklisted events. Pause for a moment here to keep from burning CPU.
			//usleep(10000);
		}
		repeatReqList.clear();
		
	}
	if (connected)
	{
		obd->closePort();
	}
}
GpsNmeaSource::GpsNmeaSource(AbstractRoutingEngine *re, map<string, string> config)
	:AbstractSource(re,config), mUuid("33d86462-1708-4f78-a001-99ea8d55422b")
{
	location =new Location(re, mUuid);

	VehicleProperty::registerProperty(GPSTIME,[](){ return new BasicPropertyType<double>(GPSTIME,0); });

	addPropertySupport(VehicleProperty::Latitude, Zone::None);
	addPropertySupport(VehicleProperty::Longitude, Zone::None);
	addPropertySupport(VehicleProperty::Altitude, Zone::None);
	addPropertySupport(VehicleProperty::VehicleSpeed, Zone::None);
	addPropertySupport(VehicleProperty::Direction, Zone::None);
	addPropertySupport(GPSTIME, Zone::None);


	///test:

	if(config.find("test") != config.end())
	{
		Location location(routingEngine, mUuid);
		location.parse("GPRMC,061211,A,2351.9605,S,15112.5239,E,000.0,053.4,170303,009.9,E*6E");

		DebugOut()<<"lat: "<<location.latitude().toString()<<endl;

		g_assert(location.latitude().toString() == "-23.86600833");
		g_assert(location.gpsTime().toString() == "1050585131");

		location.parse("GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47");

		DebugOut()<<"alt: "<<location.altitude().toString()<<endl;
		DebugOut()<<"lat: "<<location.latitude().toString()<<endl;
		g_assert(location.altitude().toString() == "545.4");
		g_assert(location.latitude().toString() == "48.1173");
	}

	std::string btaddapter = config["bluetoothAdapter"];

	if(config.find("device")!= config.end())
	{
		std::string dev = config["device"];
		if(dev.find(":") != string::npos)
		{
			BluetoothDevice bt;
			dev = bt.getDeviceForAddress(dev, btaddapter);
		}

		device = new SerialPort(dev);

		if(!device->open())
		{
			DebugOut(DebugOut::Error)<<"Failed to open gps tty: "<<config["device"]<<endl;
			perror("Error");
			return;
		}

		DebugOut()<<"read from device: "<<device->read()<<endl;

		GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
		g_io_add_watch(chan, GIOCondition(G_IO_IN | G_IO_HUP | G_IO_ERR),(GIOFunc)readCallback, this);
		g_io_channel_set_close_on_unref(chan, true);
		g_io_channel_unref(chan); //Pass ownership of the GIOChannel to the watch.
	}

	re->setSupported(supported(), this);
}
OpenXCPlugin::OpenXCPlugin(AbstractRoutingEngine* re, map<string, string> config)
:AbstractSource(re, config)
{
	re->setSupported(supported(), this);	

	/// populate the openxc to amb map:

	openXC2AmbMap["steering_wheel_angle"] = VehicleProperty::SteeringWheelAngle;
	//openXC2AmbMap["torque_at_transmission"] = VehicleProperty::Engine;
	openXC2AmbMap["engine_speed"] = VehicleProperty::EngineSpeed;
	openXC2AmbMap["vehicle_speed"] = VehicleProperty::VehicleSpeed;
	openXC2AmbMap["accelerator_pedal_position"] = VehicleProperty::ThrottlePosition;
	openXC2AmbMap["parking_brake_status"] = VehicleProperty::ParkingBrakeStatus;
	openXC2AmbMap["brake_pedal_status"] = VehicleProperty::LightBrake;
	openXC2AmbMap["transmission_gear_position"] = VehicleProperty::TransmissionGearPosition;
	openXC2AmbMap["odometer"] = VehicleProperty::Odometer;
	openXC2AmbMap["ignition_status"] = VehicleProperty::VehiclePowerMode;
	openXC2AmbMap["fuel_level"] = VehicleProperty::FuelLevel;
	openXC2AmbMap["fuel_consumed_since_restart"] = VehicleProperty::FuelConsumption;
	openXC2AmbMap["headlamp_status"] = VehicleProperty::LightHead;
	openXC2AmbMap["high_beam_status"] = VehicleProperty::LightHighBeam;
	openXC2AmbMap["windshield_wiper_status"] = VehicleProperty::WindshieldWiper;
	openXC2AmbMap["latitude"] = VehicleProperty::Latitude;
	openXC2AmbMap["longitude"] = VehicleProperty::Longitude;
	openXC2AmbMap["button_event"] = VehicleProperty::ButtonEvent;

	bool test = false;
	if(config.find("test") != config.end())
	{
		test = config["test"] == "true";
	}

	if(test)
	{
		testParseEngine();
	}

	std::string bluetoothAddy = config["device"];
	std::string bluetoothAdapter = config["bluetoothAdapter"];
	std::string serialDevice;

	if(bluetoothAddy != "")
	{
		BluetoothDevice btDevice;

		serialDevice = btDevice.getDeviceForAddress(bluetoothAddy, bluetoothAdapter);
	}

	device = new SerialPort(serialDevice);
	if(!device->open())
	{
		throw std::runtime_error("unable to open serial device " + serialDevice);
	}

	GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
	g_io_add_watch(chan, G_IO_IN,(GIOFunc)gioPollingFunc, this);
	g_io_add_watch(chan,G_IO_HUP,(GIOFunc)gioPollingFunc,this);
	g_io_add_watch(chan,G_IO_ERR,(GIOFunc)gioPollingFunc,this);
	g_io_channel_unref(chan); //Pass ownership of the GIOChannel to the watch.


}