<commit_before>#include <ros/ros.h> //For ROS itself
#include <opencv2/highgui/highgui.hpp> //FIXME Not exactly sure why this one is here; maybe it's for displaying the images that come in?
#include <cv_bridge/cv_bridge.h> //The openCV bridge that image_transport needs
#include <ros/console.h> //This is for ROS_ERROR and stuff.
#include <boost/filesystem.hpp> //For creating directtories if needed
#include <obe_toolset/ImageAndPose.h> //Needed for the custom Image and Pose message type
#include <string>
#include <vector>
#include <GeographicLib/UTMUPS.hpp> //for conversion from Lat Lon to UTM.
FIXME What should the include path really be??

//#include "std_msgs/String.h"
//#define ONEATATIME //if this is defined, it will do one file at a time and pause in between files.

//Global variables and classes that need to be accessed in the callback =(
double utm_x, utm_x_prev;
double utm_y, utm_y_prev;
double utm_z, utm_z_prev;
ros::Time timestamp, timestamp_prev;
static const double SEARCH_AREA_ALTITUDE = 0; //This probably needs to be updated for camera agl altitude location calculations to be correct in the Processor nodes
int fix_wait_ctr = -2;

void locationCallback(const sensor_msgs::NavSatFix msg) //all this does is update the global variables listed above (for location).
{
	if (msg.status == -1)
	{
		return; //we don't have a fix and can't update anything =/
	}
	else
	{
		if (fix_wait_ctr < 0)
			fix_wait_ctr += 1;
		//move the current data to the old data slot
		utm_x_prev = utm_x;
		utm_y_prev = utm_y;
		utm_z_prev = utm_z;
		timestamp_prev = timestamp;

		int zone; //dummy var for the UTMUPS conversion. This might be important at some point though, so maybe we should send it along with our ImAndPose stuff...
		bool northp; //Hemisphere (true==northern)
		GeographicLib::UTMUPS::Forward(msg.latitude, msg.longitude, &zone, &northp, &utm_x, &utm_y);
		utm_z = msg.altitude;
		timestamp = msg.header.stamp
	}
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "image_publisher_node");
	ros::NodeHandle n;
	int numOfPipes;
	int currentPipeMinusOne = 0;
	if(!(n.hasParam("numPipes")))
		ROS_INFO("the parameter 'numPipes' for the dispatcher node hasn't been specified; assuming 1 to ensure that no images are lost. This may cause severe back-up issues in a long mission.");
	n.param<int>("numPipes", numOfPipes, 1); //gets the numPipes param (so that we know where to publish), defaulting to 1 if it can't be read.

	n.subscribe("/mavros/global_position/global", 0, locationCallack);
	std::vector<ros::Publisher> impose_pub_vector(numOfPipes); //vector of publishers
	for(int i = 1; i <= numOfPipes; ++i)
	{
		std::string topic("obe/imagePipe");
		topic += std::to_string(i);
		impose_pub_vector[i-1] = n.advertise<obe_toolset::ImageAndPose>(topic.c_str(), 512); //publishes to the obe/imagePipe<i> and buffers up to 512 images per pipe in case it can't send them all that quickly.
	}

//	image_transport::ImageTransport it(n);
//	ros::Publisher pub = n.advertise<std_msgs::String>("obe/test", 100);
//	image_transport::Publisher impose_pub = it.advertise("obe/image", 255); //Buffers up to 255 pictures in case connection is lost
//	^^That used to be the old publisher.
//	cv::Mat image = cv::imread("/home/kennon/images/BrentRambo.jpg", CV_LOAD_IMAGE_COLOR);
//	cv::waitKey(30); //No idea what this line does...
//	cv::imshow("view", image); //This can be used to see the image as it's published

	//This block makes sure that the needed directories are set up (I'm pretty sure they should be, since this node might end up running from one of them).
	namespace fs = boost::filesystem;

		//NOTE: Not sure how relative paths will be handled when a launchfile runs this remotely...
	fs::path new_path("../images/new/"); //Will be used to get images.
	fs::path processed_path("../images/processed/");
	fs::path roi_path("../images/rois/");
	fs::path error_path("../images/unsuccessful/");

	fs::path im_fetch_path(new_path);

	fs::create_directories(im_fetch_path);
	fs::create_directories(processed_path);
	fs::create_directories(roi_path);
	fs::create_directories(error_path);
	//NOTE:: IF THERE IS AN ISSUE WITH PERMISSIONS FOR SOME REASON, IT MIGHT BE THAT THE LINES OF CODE ABOVE ARE REMOVING EXECUTE PERMISSIONS. JUST SOMETHING TO CONSIDER

	if (!(fs::exists(im_fetch_path)))
	{
		//If it doesn't exist, something went horribly wrong =/
		ROS_ERROR("Oh no. '%s' isn't a directory =(", im_fetch_path.string().c_str());
		return(-1);
	}
	else if (fs::is_regular_file(im_fetch_path))
	{
		ROS_ERROR("Oh no. '%s' is a regular file =(", im_fetch_path.string().c_str());
		return(-1);
	}

	#ifdef ONEATATIME
	ros::Rate loop_rate(.25);
	#else
	ros::Rate loop_rate(25); //rate is the number of outer loop iterations per second
	//Also, we need the loop rate to be relatively quick becuase we don't want delay in dispatching images. Delay in dispatching images could lead to wasted execution time and potential incorrect location stamps.
	#endif //ONEATATIME

	while(n.ok())
	{
		//We need to first make sure that the GPS Fix is ready (should take 1 second if the GPS is already "warmed up"
		if(fix_wait_ctr)
		{
			ROS_INFO_THROTTLE(5,"Waiting for GPS Fix"); //Display at most once every 5 seconds
		}
		else
		{
			//We need to dispatch each of the images in the new directory
			fs::directory_iterator end_itr;
			for(fs::directory_iterator cur_path_itr(im_fetch_path); cur_path_itr != end_itr; cur_path_itr++)
			{
				ROS_INFO("I'm looking at this file: %s", cur_path_itr->path().string().c_str());
				if(fs::is_directory(cur_path_itr->path()))
				{
					ROS_INFO_STREAM_ONCE("There's a directory in the ~/images/new path; it is being moved to the error folder.");
					fs::rename(cur_path_itr->path(), error_path / cur_path_itr->path().filename());
					//FIXME Don't forget to move this file to the folder for files with errors.

				}
				else
				{
					cv::Mat newImage = cv::imread(cur_path_itr->path().string(), CV_LOAD_IMAGE_COLOR);

					if(newImage.data == NULL)//The image was read improperly if this fails...
					{
						ROS_ERROR("I was unable to read the image file.");
						fs::rename(cur_path_itr->path(), error_path / cur_path_itr->path().filename());
					}
					else
					{
						obe_toolset::ImageAndPose impose_msg; //This is the custom message type that has a sensor_msgs::Image msg and a std_msgs::Pose message.
						impose_msg.image = *(cv_bridge::CvImage(std_msgs::Header(), "bgr8", newImage).toImageMsg()); //The meat of this line is from the image_transport tutorial; I just de-reference their piece to get a sensor_msgs::Image.  Note: There's probably a better way to do this, but it will work for now.

						//This is the point where the fakeing things comes in.
						impose_msg.x = 0; //fakes an x UTM value
						impose_msg.y = 0; //etc...
						impose_msg.z = 60.96; //This is actually a decent guess (~200 ft)
						impose_msg.roll = 0.0;
						impose_msg.pitch = 0.0;
						impose_msg.yaw = 0.0; //this is the only one that's used as of 4.26.17
						//End the faking it stuff.

						//publish to the current pipe that's due for another message. NOTE: In the future, this could have a system that keeps track of busy nodes so that no particular node gets bogged down. I'm kind of assuming that we have enough nodes and a fast enough ROI algorithm and randomness is on our side so that this doesn't get out of hand.
						impose_pub_vector[currentPipeMinusOne].publish(impose_msg); //send the impose message.

						//set up the current pipe for the next time we publish something.
						currentPipeMinusOne++;
						currentPipeMinusOne %= numOfPipes; //we want to wrap around

						//Now that we've published it, we can move the file to the processed folder
						fs::rename(cur_path_itr->path(), processed_path / cur_path_itr->path().filename());
						#ifdef ONEATATIME
						break;
						#endif
					}
				}
			} //Ends the for loop
		}//ends the if statement that waits for a GPS Fix
		ros::spinOnce();
		loop_rate.sleep();
	}
	//It shouldn't get here until you hit ctrl-C, but we still need to specify a return value:
	return 0;
}
<commit_msg>It doesn't link yet, but the DispatcherNode has been updated to be compatible with GeographicLib<commit_after>#include <ros/ros.h> //For ROS itself
#include <opencv2/highgui/highgui.hpp> //FIXME Not exactly sure why this one is here; maybe it's for displaying the images that come in?
#include <cv_bridge/cv_bridge.h> //The openCV bridge that image_transport needs
#include <ros/console.h> //This is for ROS_ERROR and stuff.
#include <boost/filesystem.hpp> //For creating directtories if needed
#include <obe_toolset/ImageAndPose.h> //Needed for the custom Image and Pose message type
#include <string>
#include <vector>
#include <GeographicLib/UTMUPS.hpp> //for conversion from Lat Lon to UTM. In order to get this, download the source, unzip it, cd into it, mkdir build, cd build, cmake .., make, make install
#include <sensor_msgs/NavSatFix.h> //for the mavros NavSatFix message

//#include "std_msgs/String.h"
//#define ONEATATIME //if this is defined, it will do one file at a time and pause in between files.

//Global variables and classes that need to be accessed in the callback =(
double utm_x, utm_x_prev;
double utm_y, utm_y_prev;
double utm_z, utm_z_prev;
ros::Time timestamp, timestamp_prev;
static const double SEARCH_AREA_ALTITUDE = 0; //This probably needs to be updated for camera agl altitude location calculations to be correct in the Processor nodes
int fix_wait_ctr = -2;

void locationCallback(const sensor_msgs::NavSatFix msg) //all this does is update the global variables listed above (for location).
{
	if (0) //(msg.status == -1)
	{
		return; //we don't have a fix and can't update anything =/
	}
	else
	{
		if (fix_wait_ctr < 0)
			fix_wait_ctr += 1;
		//move the current data to the old data slot
		utm_x_prev = utm_x;
		utm_y_prev = utm_y;
		utm_z_prev = utm_z;
		timestamp_prev = timestamp;

		int zone; //dummy var for the UTMUPS conversion. This might be important at some point though, so maybe we should send it along with our ImAndPose stuff...
		bool dummyNorthp; //Hemisphere (true==northern)
		double dummyGamma; //no idea, dummy var so it will compile
		double dummyK; //no idea; dummy var so it will compile
		double msgLat = msg.latitude;
		double msgLon = msg.longitude;
		GeographicLib::UTMUPS::Forward(msgLat, msgLon, zone, dummyNorthp, utm_x, utm_y, dummyGamma, dummyK, -1, false);
		utm_z = msg.altitude;
		timestamp = msg.header.stamp;
	}
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "image_publisher_node");
	ros::NodeHandle n;
	int numOfPipes;
	int currentPipeMinusOne = 0;
	if(!(n.hasParam("numPipes")))
		ROS_INFO("the parameter 'numPipes' for the dispatcher node hasn't been specified; assuming 1 to ensure that no images are lost. This may cause severe back-up issues in a long mission.");
	n.param<int>("numPipes", numOfPipes, 1); //gets the numPipes param (so that we know where to publish), defaulting to 1 if it can't be read.

	n.subscribe("/mavros/global_position/global", 0, locationCallback);
	std::vector<ros::Publisher> impose_pub_vector(numOfPipes); //vector of publishers
	for(int i = 1; i <= numOfPipes; ++i)
	{
		std::string topic("obe/imagePipe");
		topic += std::to_string(i);
		impose_pub_vector[i-1] = n.advertise<obe_toolset::ImageAndPose>(topic.c_str(), 512); //publishes to the obe/imagePipe<i> and buffers up to 512 images per pipe in case it can't send them all that quickly.
	}

	//This block makes sure that the needed directories are set up (I'm pretty sure they should be, since this node might end up running from one of them).
	namespace fs = boost::filesystem;

		//NOTE: Not sure how relative paths will be handled when a launchfile runs this remotely...
	fs::path new_path("../images/new/"); //Will be used to get images.
	fs::path processed_path("../images/processed/");
	fs::path roi_path("../images/rois/");
	fs::path error_path("../images/unsuccessful/");

	fs::path im_fetch_path(new_path);

	fs::create_directories(im_fetch_path);
	fs::create_directories(processed_path);
	fs::create_directories(roi_path);
	fs::create_directories(error_path);
	//NOTE:: IF THERE IS AN ISSUE WITH PERMISSIONS FOR SOME REASON, IT MIGHT BE THAT THE LINES OF CODE ABOVE ARE REMOVING EXECUTE PERMISSIONS. JUST SOMETHING TO CONSIDER

	if (!(fs::exists(im_fetch_path)))
	{
		//If it doesn't exist, something went horribly wrong =/
		ROS_ERROR("Oh no. '%s' isn't a directory =(", im_fetch_path.string().c_str());
		return(-1);
	}
	else if (fs::is_regular_file(im_fetch_path))
	{
		ROS_ERROR("Oh no. '%s' is a regular file =(", im_fetch_path.string().c_str());
		return(-1);
	}

	#ifdef ONEATATIME
	ros::Rate loop_rate(.25);
	#else
	ros::Rate loop_rate(25); //rate is the number of outer loop iterations per second
	//Also, we need the loop rate to be relatively quick becuase we don't want delay in dispatching images. Delay in dispatching images could lead to wasted execution time and potential incorrect location stamps.
	#endif //ONEATATIME

	while(n.ok())
	{
		//We need to first make sure that the GPS Fix is ready (should take 1 second if the GPS is already "warmed up"
		if(fix_wait_ctr)
		{
			ROS_INFO_THROTTLE(5,"Waiting for GPS Fix"); //Display at most once every 5 seconds
		}
		else
		{
			//We need to dispatch each of the images in the new directory
			fs::directory_iterator end_itr;
			for(fs::directory_iterator cur_path_itr(im_fetch_path); cur_path_itr != end_itr; cur_path_itr++)
			{
				ROS_INFO("I'm looking at this file: %s", cur_path_itr->path().string().c_str());
				if(fs::is_directory(cur_path_itr->path()))
				{
					ROS_INFO_STREAM_ONCE("There's a directory in the ~/images/new path; it is being moved to the error folder.");
					fs::rename(cur_path_itr->path(), error_path / cur_path_itr->path().filename());
					//FIXME Don't forget to move this file to the folder for files with errors.

				}
				else
				{
					cv::Mat newImage = cv::imread(cur_path_itr->path().string(), CV_LOAD_IMAGE_COLOR);

					if(newImage.data == NULL)//The image was read improperly if this fails...
					{
						ROS_ERROR("I was unable to read the image file.");
						fs::rename(cur_path_itr->path(), error_path / cur_path_itr->path().filename());
					}
					else
					{
						obe_toolset::ImageAndPose impose_msg; //This is the custom message type that has a sensor_msgs::Image msg and a std_msgs::Pose message.
						impose_msg.image = *(cv_bridge::CvImage(std_msgs::Header(), "bgr8", newImage).toImageMsg()); //The meat of this line is from the image_transport tutorial; I just de-reference their piece to get a sensor_msgs::Image.  Note: There's probably a better way to do this, but it will work for now.

						//This is the point where the fakeing things comes in.
						impose_msg.x = 0; //fakes an x UTM value
						impose_msg.y = 0; //etc...
						impose_msg.z = 60.96; //This is actually a decent guess (~200 ft)
						impose_msg.roll = 0.0;
						impose_msg.pitch = 0.0;
						impose_msg.yaw = 0.0; //this is the only one that's used as of 4.26.17
						//End the faking it stuff.

						//publish to the current pipe that's due for another message. NOTE: In the future, this could have a system that keeps track of busy nodes so that no particular node gets bogged down. I'm kind of assuming that we have enough nodes and a fast enough ROI algorithm and randomness is on our side so that this doesn't get out of hand.
						impose_pub_vector[currentPipeMinusOne].publish(impose_msg); //send the impose message.

						//set up the current pipe for the next time we publish something.
						currentPipeMinusOne++;
						currentPipeMinusOne %= numOfPipes; //we want to wrap around

						//Now that we've published it, we can move the file to the processed folder
						fs::rename(cur_path_itr->path(), processed_path / cur_path_itr->path().filename());
						#ifdef ONEATATIME
						break;
						#endif
					}
				}
			} //Ends the for loop
		}//ends the if statement that waits for a GPS Fix
		ros::spinOnce();
		loop_rate.sleep();
	}
	//It shouldn't get here until you hit ctrl-C, but we still need to specify a return value:
	return 0;
}
<|endoftext|>


<commit_before>#include <Arduino.h>

#include "DollhousePanel.h"

#include <Adafruit_TLC59711.h>
#include <Adafruit_NeoPixel.h>
#include <LiquidCrystal.h>
#include <SPI.h>
#include <Fsm.h>
#include <EnableInterrupt.h>

const char NUM_TLC59711 = 1;
const char TLC_DATA = 12;
const char TLC_CLK = 13;

const char PIXEL_COUNT = 3;
const char PIXEL_PIN = 8;

enum events {
  CHANGE_LIGHT_MODE,
  NEXT_ROOM,
  PREVIOUS_ROOM,
  RESET_ROOMS
};

// Lighting modes finite state machine
State state_lighting_mode(on_lighting_mode_enter, &on_lighting_mode_exit);
State state_party_mode(on_party_mode_enter, &on_party_mode_exit);
State state_nitelite_mode(on_nitelite_mode_enter, &on_nitelite_mode_exit);
State state_off_mode(on_off_mode_enter, &on_off_mode_exit);
Fsm modes(&state_off_mode);

enum Modes {
  LIGHTING_MODE,
  PARTY_MODE,
  NITELITE_MODE,
  OFF_MODE
};

String modeNames[] = {"Lighting", "Party", "Nitelite", "Off"};

// Rooms finite state machine
State state_all_rooms(on_all_enter, &on_all_exit);
State state_hall(on_hall_enter, &on_hall_exit);
State state_living_room(on_living_room_enter, &on_living_room_exit);
State state_kitchen(on_kitchen_enter, &on_kitchen_exit);
State state_bedroom(on_bedroom_enter, &on_bedroom_exit);
State state_bathroom(on_bathroom_enter, &on_bathroom_exit);
State state_attic(on_attic_enter, &on_attic_exit);
Fsm rooms(&state_all_rooms);

// LastROOM is included to make it easier to figure out the size of the enum
// for things like sizing the brightness state array
enum Rooms {
  ALL_ROOMS,
  LIVING_ROOM,
  HALL,
  KITCHEN,
  BEDROOM,
  BATHROOM,
  ATTIC,
  LastROOM
};

String roomNames[] = {"All", "Living", "Hall", "Kitchen", "Bedroom", "Bathroom", "Attic"};

// NeoPixels (for the attic & !!!PARTY MODE!!!)
Adafruit_NeoPixel strip = Adafruit_NeoPixel(PIXEL_COUNT, PIXEL_PIN, NEO_GRB + NEO_KHZ800);

// PWM board (controls the room lights)
Adafruit_TLC59711 tlc = Adafruit_TLC59711(NUM_TLC59711, TLC_CLK, TLC_DATA);

// 16x2 LCD display
LiquidCrystal lcd(7, 6, 5, 4, 3, 2);

// Panel RGB LED pins
const char RED_PIN = 9;
const char GREEN_PIN = 10;
const char BLUE_PIN = 11;

int brightness = 90;
int deltaLevel = 30;
int minLevel = 0;
int maxLevel = 180;
int roomBrightness[LastROOM];
int currentRoom = ALL_ROOMS;
int currentMode = OFF_MODE;

void setup() {
  // Fire up the LCD display
  lcd.begin(16, 2);
  lcd.print("Doll house");
  lcd.setCursor(0,1);
  lcd.print("lighting!");

  pinMode(RED_PIN, OUTPUT);
  pinMode(GREEN_PIN, OUTPUT);
  pinMode(BLUE_PIN, OUTPUT);

  // initialize the NeoPixel strand
  strip.begin();
  strip.show();

  // Initialize the PWM board
  tlc.begin();
  tlc.write();

  // set defualt room brightness
  setDefaultLightLevel();

  // enable interrupts on buttons
  // The button interface is a Smartmaker 5A5 (annoying, but it works)
  enableInterrupt(A0, handleButtonOne, FALLING);
  enableInterrupt(A1, handleButtonTwo, FALLING);
  enableInterrupt(A2, handleButtonThree, FALLING);
  enableInterrupt(A3, handleButtonFour, FALLING);
  enableInterrupt(A4, handleButtonFive, FALLING);

  // mode FSM transitions
  modes.add_transition(&state_off_mode, &state_lighting_mode, CHANGE_LIGHT_MODE, NULL);
  modes.add_transition(&state_lighting_mode, &state_party_mode, CHANGE_LIGHT_MODE, NULL);
  modes.add_transition(&state_party_mode, &state_nitelite_mode, CHANGE_LIGHT_MODE, NULL);
  modes.add_transition(&state_nitelite_mode, &state_off_mode, CHANGE_LIGHT_MODE, NULL);

  // rooms FSM transitions
  // looping "forward" through the rooms
  rooms.add_transition(&state_all_rooms, &state_hall, NEXT_ROOM, NULL);
  rooms.add_transition(&state_hall, &state_living_room, NEXT_ROOM, NULL);
  rooms.add_transition(&state_living_room, &state_kitchen, NEXT_ROOM, NULL);
  rooms.add_transition(&state_kitchen, &state_bedroom, NEXT_ROOM, NULL);
  rooms.add_transition(&state_bedroom, &state_bathroom, NEXT_ROOM, NULL);
  rooms.add_transition(&state_bathroom, &state_attic, NEXT_ROOM, NULL);
  rooms.add_transition(&state_attic, &state_all_rooms, NEXT_ROOM, NULL);

  // looping "backward" through the rooms
  rooms.add_transition(&state_all_rooms, &state_attic, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_attic, &state_bathroom, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_bathroom, &state_bedroom, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_bedroom, &state_kitchen, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_kitchen, &state_living_room, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_living_room, &state_hall, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_hall, &state_all_rooms, PREVIOUS_ROOM, NULL);

  // reseting to the default room (all rooms)
  rooms.add_transition(&state_hall, &state_all_rooms, RESET_ROOMS, NULL);
  rooms.add_transition(&state_living_room, &state_all_rooms, RESET_ROOMS, NULL);
  rooms.add_transition(&state_kitchen, &state_all_rooms, RESET_ROOMS, NULL);
  rooms.add_transition(&state_bedroom, &state_all_rooms, RESET_ROOMS, NULL);
  rooms.add_transition(&state_bathroom, &state_all_rooms, RESET_ROOMS, NULL);
  rooms.add_transition(&state_attic, &state_all_rooms, RESET_ROOMS, NULL);
}

// ***** Button event handlers ***** //
void handleButtonOne() {
  lcd.clear();
  rooms.trigger(RESET_ROOMS);
  modes.trigger(CHANGE_LIGHT_MODE);
}

void handleButtonTwo() {
  setRoomBrightness(currentRoom, min(roomBrightness[currentRoom] + deltaLevel, maxLevel));
  printCurrentRoom();
  delay(50);
}

void handleButtonThree() {
  lcd.clear();
  rooms.trigger(PREVIOUS_ROOM);
}

void handleButtonFour() {
  setRoomBrightness(currentRoom, max(roomBrightness[currentRoom] - deltaLevel, minLevel));
  printCurrentRoom();
  delay(50);
}

void handleButtonFive() {
  lcd.clear();
  rooms.trigger(NEXT_ROOM);
}

// ***** helpers ***** //

void setRGBColor(int red, int green, int blue) {
  int myRed = constrain(red, 0, 255);
  int myGreen = constrain(green, 0, 255);
  int myBlue = constrain(blue, 0, 255);

  analogWrite(RED_PIN, myRed);
  analogWrite(GREEN_PIN, myGreen);
  analogWrite(BLUE_PIN, myBlue);
}

void setRoomBrightness(int room, int level) {
  setRGBColor(0,0,level);
  roomBrightness[room] = level;
  tlc.setPWM(room * 3, roomBrightness[room] * maxLevel);
  tlc.write();
}

void setDefaultLightLevel() {
  setRGBColor(0,0,brightness);
  for (int i = 0; i != LastROOM; i++) {
    roomBrightness[i] = brightness;
  }
}

void setCurrentMode(int mode) {
  currentMode = mode;
  printCurrentMode();
}

void printCurrentMode() {
  lcd.clear();
  lcd.print("Mode: ");
  lcd.print(modeNames[currentMode]);
}

void setCurrentRoom(int room) {
  currentRoom = room;
  setRGBColor(0,0,roomBrightness[room]);
  printCurrentRoom();
}

void printCurrentRoom() {
  lcd.clear();
  lcd.print("room: ");
  lcd.print(roomNames[currentRoom]);
  lcd.setCursor(0,1);
  lcd.print("brightness: ");
  lcd.print(roomBrightness[currentRoom]);
}

// ***** FSM event handlers ***** //

// ---- lighting mode states ---- //

void on_lighting_mode_enter(){
  setCurrentMode(LIGHTING_MODE);
}

void on_lighting_mode_exit(){

}

void on_party_mode_enter(){
  setCurrentMode(PARTY_MODE);
}

void on_party_mode_exit(){

}

void on_nitelite_mode_enter(){
  setCurrentMode(NITELITE_MODE);
}

void on_nitelite_mode_exit(){

}

void on_off_mode_enter(){
  setCurrentMode(OFF_MODE);
}

void on_off_mode_exit(){

}

// ---- room selection states ---- //
void on_all_enter() {
  setCurrentRoom(ALL_ROOMS);
}

void on_all_exit() {

}

void on_hall_enter() {
  setCurrentRoom(HALL);
}

void on_hall_exit() {

}

void on_living_room_enter() {
  setCurrentRoom(LIVING_ROOM);
}

void on_living_room_exit() {

}

void on_kitchen_enter() {
  setCurrentRoom(KITCHEN);
}

void on_kitchen_exit() {

}

void on_bathroom_enter() {
  setCurrentRoom(BATHROOM);
}

void on_bathroom_exit() {

}

void on_bedroom_enter() {
  setCurrentRoom(BEDROOM);
}

void on_bedroom_exit() {

}

void on_attic_enter() {
  setCurrentRoom(ATTIC);
}

void on_attic_exit() {

}

void loop() {
  // do nothing; everything is handled via FSM events
}
<commit_msg>Add some debounce for each of the buttons<commit_after>#include <Arduino.h>

#include "DollhousePanel.h"

#include <Adafruit_TLC59711.h>
#include <Adafruit_NeoPixel.h>
#include <LiquidCrystal.h>
#include <SPI.h>
#include <Fsm.h>
#include <EnableInterrupt.h>

const char NUM_TLC59711 = 1;
const char TLC_DATA = 12;
const char TLC_CLK = 13;

const char PIXEL_COUNT = 3;
const char PIXEL_PIN = 8;

enum events {
  CHANGE_LIGHT_MODE,
  NEXT_ROOM,
  PREVIOUS_ROOM,
  RESET_ROOMS
};

// Lighting modes finite state machine
State state_lighting_mode(on_lighting_mode_enter, &on_lighting_mode_exit);
State state_party_mode(on_party_mode_enter, &on_party_mode_exit);
State state_nitelite_mode(on_nitelite_mode_enter, &on_nitelite_mode_exit);
State state_off_mode(on_off_mode_enter, &on_off_mode_exit);
Fsm modes(&state_off_mode);

enum Modes {
  LIGHTING_MODE,
  PARTY_MODE,
  NITELITE_MODE,
  OFF_MODE
};

String modeNames[] = {"Lighting", "Party", "Nitelite", "Off"};

// Rooms finite state machine
State state_all_rooms(on_all_enter, &on_all_exit);
State state_hall(on_hall_enter, &on_hall_exit);
State state_living_room(on_living_room_enter, &on_living_room_exit);
State state_kitchen(on_kitchen_enter, &on_kitchen_exit);
State state_bedroom(on_bedroom_enter, &on_bedroom_exit);
State state_bathroom(on_bathroom_enter, &on_bathroom_exit);
State state_attic(on_attic_enter, &on_attic_exit);
Fsm rooms(&state_all_rooms);

// LastROOM is included to make it easier to figure out the size of the enum
// for things like sizing the brightness state array
enum Rooms {
  ALL_ROOMS,
  LIVING_ROOM,
  HALL,
  KITCHEN,
  BEDROOM,
  BATHROOM,
  ATTIC,
  LastROOM
};

String roomNames[] = {"All", "Living", "Hall", "Kitchen", "Bedroom", "Bathroom", "Attic"};

// NeoPixels (for the attic & !!!PARTY MODE!!!)
Adafruit_NeoPixel strip = Adafruit_NeoPixel(PIXEL_COUNT, PIXEL_PIN, NEO_GRB + NEO_KHZ800);

// PWM board (controls the room lights)
Adafruit_TLC59711 tlc = Adafruit_TLC59711(NUM_TLC59711, TLC_CLK, TLC_DATA);

// 16x2 LCD display
LiquidCrystal lcd(7, 6, 5, 4, 3, 2);

// Panel RGB LED pins
const char RED_PIN = 9;
const char GREEN_PIN = 10;
const char BLUE_PIN = 11;

int brightness = 90;
int deltaLevel = 30;
int minLevel = 0;
int maxLevel = 180;
int roomBrightness[LastROOM];
int currentRoom = ALL_ROOMS;
int currentMode = OFF_MODE;

int debounceDelay = 100;

void setup() {
  // Fire up the LCD display
  lcd.begin(16, 2);
  lcd.print("Doll house");
  lcd.setCursor(0,1);
  lcd.print("lighting!");

  pinMode(RED_PIN, OUTPUT);
  pinMode(GREEN_PIN, OUTPUT);
  pinMode(BLUE_PIN, OUTPUT);

  // initialize the NeoPixel strand
  strip.begin();
  strip.show();

  // Initialize the PWM board
  tlc.begin();
  tlc.write();

  // set defualt room brightness
  setDefaultLightLevel();

  // enable interrupts on buttons
  // The button interface is a Smartmaker 5A5 (annoying, but it works)
  enableInterrupt(A0, handleButtonOne, FALLING);
  enableInterrupt(A1, handleButtonTwo, FALLING);
  enableInterrupt(A2, handleButtonThree, FALLING);
  enableInterrupt(A3, handleButtonFour, FALLING);
  enableInterrupt(A4, handleButtonFive, FALLING);

  // mode FSM transitions
  modes.add_transition(&state_off_mode, &state_lighting_mode, CHANGE_LIGHT_MODE, NULL);
  modes.add_transition(&state_lighting_mode, &state_party_mode, CHANGE_LIGHT_MODE, NULL);
  modes.add_transition(&state_party_mode, &state_nitelite_mode, CHANGE_LIGHT_MODE, NULL);
  modes.add_transition(&state_nitelite_mode, &state_off_mode, CHANGE_LIGHT_MODE, NULL);

  // rooms FSM transitions
  // looping "forward" through the rooms
  rooms.add_transition(&state_all_rooms, &state_hall, NEXT_ROOM, NULL);
  rooms.add_transition(&state_hall, &state_living_room, NEXT_ROOM, NULL);
  rooms.add_transition(&state_living_room, &state_kitchen, NEXT_ROOM, NULL);
  rooms.add_transition(&state_kitchen, &state_bedroom, NEXT_ROOM, NULL);
  rooms.add_transition(&state_bedroom, &state_bathroom, NEXT_ROOM, NULL);
  rooms.add_transition(&state_bathroom, &state_attic, NEXT_ROOM, NULL);
  rooms.add_transition(&state_attic, &state_all_rooms, NEXT_ROOM, NULL);

  // looping "backward" through the rooms
  rooms.add_transition(&state_all_rooms, &state_attic, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_attic, &state_bathroom, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_bathroom, &state_bedroom, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_bedroom, &state_kitchen, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_kitchen, &state_living_room, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_living_room, &state_hall, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_hall, &state_all_rooms, PREVIOUS_ROOM, NULL);

  // reseting to the default room (all rooms)
  rooms.add_transition(&state_hall, &state_all_rooms, RESET_ROOMS, NULL);
  rooms.add_transition(&state_living_room, &state_all_rooms, RESET_ROOMS, NULL);
  rooms.add_transition(&state_kitchen, &state_all_rooms, RESET_ROOMS, NULL);
  rooms.add_transition(&state_bedroom, &state_all_rooms, RESET_ROOMS, NULL);
  rooms.add_transition(&state_bathroom, &state_all_rooms, RESET_ROOMS, NULL);
  rooms.add_transition(&state_attic, &state_all_rooms, RESET_ROOMS, NULL);
}

// ***** Button event handlers ***** //
void handleButtonOne() {
  lcd.clear();
  rooms.trigger(RESET_ROOMS);
  modes.trigger(CHANGE_LIGHT_MODE);
  delay(debounceDelay);
}

void handleButtonTwo() {
  setRoomBrightness(currentRoom, min(roomBrightness[currentRoom] + deltaLevel, maxLevel));
  printCurrentRoom();
  delay(debounceDelay);
}

void handleButtonThree() {
  lcd.clear();
  rooms.trigger(PREVIOUS_ROOM);
  delay(debounceDelay);
}

void handleButtonFour() {
  setRoomBrightness(currentRoom, max(roomBrightness[currentRoom] - deltaLevel, minLevel));
  printCurrentRoom();
  delay(debounceDelay);
}

void handleButtonFive() {
  lcd.clear();
  rooms.trigger(NEXT_ROOM);
  delay(debounceDelay);
}

// ***** helpers ***** //

void setRGBColor(int red, int green, int blue) {
  int myRed = constrain(red, 0, 255);
  int myGreen = constrain(green, 0, 255);
  int myBlue = constrain(blue, 0, 255);

  analogWrite(RED_PIN, myRed);
  analogWrite(GREEN_PIN, myGreen);
  analogWrite(BLUE_PIN, myBlue);
}

void setRoomBrightness(int room, int level) {
  setRGBColor(0,0,level);
  roomBrightness[room] = level;
  tlc.setPWM(room * 3, roomBrightness[room] * maxLevel);
  tlc.write();
}

void setDefaultLightLevel() {
  setRGBColor(0,0,brightness);
  for (int i = 0; i != LastROOM; i++) {
    roomBrightness[i] = brightness;
  }
}

void setCurrentMode(int mode) {
  currentMode = mode;
  printCurrentMode();
}

void printCurrentMode() {
  lcd.clear();
  lcd.print("Mode: ");
  lcd.print(modeNames[currentMode]);
}

void setCurrentRoom(int room) {
  currentRoom = room;
  setRGBColor(0,0,roomBrightness[room]);
  printCurrentRoom();
}

void printCurrentRoom() {
  lcd.clear();
  lcd.print("room: ");
  lcd.print(roomNames[currentRoom]);
  lcd.setCursor(0,1);
  lcd.print("brightness: ");
  lcd.print(roomBrightness[currentRoom]);
}

// ***** FSM event handlers ***** //

// ---- lighting mode states ---- //

void on_lighting_mode_enter(){
  setCurrentMode(LIGHTING_MODE);
}

void on_lighting_mode_exit(){

}

void on_party_mode_enter(){
  setCurrentMode(PARTY_MODE);
}

void on_party_mode_exit(){

}

void on_nitelite_mode_enter(){
  setCurrentMode(NITELITE_MODE);
}

void on_nitelite_mode_exit(){

}

void on_off_mode_enter(){
  setCurrentMode(OFF_MODE);
}

void on_off_mode_exit(){

}

// ---- room selection states ---- //
void on_all_enter() {
  setCurrentRoom(ALL_ROOMS);
}

void on_all_exit() {

}

void on_hall_enter() {
  setCurrentRoom(HALL);
}

void on_hall_exit() {

}

void on_living_room_enter() {
  setCurrentRoom(LIVING_ROOM);
}

void on_living_room_exit() {

}

void on_kitchen_enter() {
  setCurrentRoom(KITCHEN);
}

void on_kitchen_exit() {

}

void on_bathroom_enter() {
  setCurrentRoom(BATHROOM);
}

void on_bathroom_exit() {

}

void on_bedroom_enter() {
  setCurrentRoom(BEDROOM);
}

void on_bedroom_exit() {

}

void on_attic_enter() {
  setCurrentRoom(ATTIC);
}

void on_attic_exit() {

}

void loop() {
  // do nothing; everything is handled via FSM events
}
<|endoftext|>


<commit_before>#include "stdafx.h"
#include "EmbeddedPython.h"
#include "ModsLocation.h"
#include "ExceptionFetcher.h"
#include "ResourceLoader.h"
#include <iostream>
#include "resource.h"
#include "Logger.h"
#include "ResponseWriter.h"
#include "SQFReader.h"
#include "SQFWriter.h"
#include "Paths.h"
#include "Modules/pythiainternal.h"
#include "Modules/pythialogger.h"

#define THROW_PYEXCEPTION(_msg_) throw std::runtime_error(_msg_ + std::string(": ") + PyExceptionFetcher().getError());
//#define EXTENSION_DEVELOPMENT 1

EmbeddedPython *python = nullptr;
std::string pythonInitializationError = "";
unsigned long multipartCounter = 0;
std::unordered_map<unsigned long long int, multipart_t> multiparts;

namespace
{
    class PyObjectGuard final
    {
    public:
        PyObjectGuard(PyObject* source) : ptr(source)
        {
        }

        ~PyObjectGuard()
        {
            if (ptr != nullptr)
            {
                Py_DECREF(ptr);
            }
        }

        PyObject* get() const
        {
            return ptr;
        }

        explicit operator bool() const
        {
            return ptr != nullptr;
        }

        /// Release ownership
        PyObject* transfer()
        {
            PyObject* tmp = ptr;
            ptr = nullptr;
            return tmp;
        }

    private:
        PyObjectGuard(const PyObjectGuard&) = delete;
        void operator=(const PyObjectGuard&) = delete;

    private:
        PyObject *ptr;
    };
}

void EmbeddedPython::DoPythonMagic(std::wstring path)
{
    // Python pre-initialization magic

    // Clear the env variables, just in case
    _wputenv_s(L"PYTHONHOME", L"");
    _wputenv_s(L"PYTHONPATH", L"");
    _wputenv_s(L"PYTHONNOUSERSITE", L"1");  // Disable custom user site

    // Py_SetPythonHome(L"D:\\Steam\\steamapps\\common\\Arma 3\\@Pythia\\python-embed-amd64");
    pythonHomeString = std::vector<wchar_t>(path.begin(), path.end());
    pythonHomeString.push_back(0);
    Py_SetPythonHome(pythonHomeString.data());
    LOG_INFO(std::string("Python home: ") + Logger::w2s(Py_GetPythonHome()));

    // Py_SetProgramName(L"D:\\Steam\\steamapps\\common\\Arma 3\\@Pythia\\python-embed-amd64\\python.exe");
    std::wstring programName = path + L"\\python.exe"; // Not sure if that should be the value here
    programNameString = std::vector<wchar_t>(programName.begin(), programName.end());
    programNameString.push_back(0);
    Py_SetProgramName(programNameString.data());
    LOG_INFO(std::string("Program name: ") + Logger::w2s(Py_GetProgramName()));

    /*
    Py_SetPath(L"D:\\Steam\\SteamApps\\common\\Arma 3\\@Pythia\\python-embed-amd64\\python35.zip;"
        L"D:\\Steam\\SteamApps\\common\\Arma 3\\@Pythia\\python-embed-amd64\\DLLs;"
        L"D:\\Steam\\SteamApps\\common\\Arma 3\\@Pythia\\python-embed-amd64\\lib;"
        L"D:\\Steam\\SteamApps\\common\\Arma 3");
    */
    std::wstring allPaths =
        path + L"\\python35.zip" + L";" +
        path + L"\\DLLs" + L";" +
        path + L"\\lib" + L";" +
        L""; // Local directory for `python/` directory
    pathString = std::vector<wchar_t>(allPaths.begin(), allPaths.end());
    pathString.push_back(0);
    // Not setting PySetPath overwrites the Py_SetProgramName value (it seems to be ignored then),
    Py_SetPath(pathString.data());
    LOG_INFO(std::string("Python paths: ") + Logger::w2s(Py_GetPath()));
    LOG_INFO(std::string("Current directory: ") + GetCurrentWorkingDir());
}

EmbeddedPython::EmbeddedPython(HMODULE moduleHandle): dllModuleHandle(moduleHandle)
{
    DoPythonMagic(getPythonPath());
    PyImport_AppendInittab("pythiainternal", PyInit_pythiainternal);
    PyImport_AppendInittab("pythialogger", PyInit_pythialogger);
    Py_Initialize();
    PyEval_InitThreads(); // Initialize and acquire GIL
    initialize();
    leavePythonThread();
}

void EmbeddedPython::enterPythonThread()
{
    PyEval_RestoreThread(pThreadState);
}

void EmbeddedPython::leavePythonThread()
{
    pThreadState = PyEval_SaveThread();
}

void EmbeddedPython::initialize()
{
    #ifdef EXTENSION_DEVELOPMENT
    PyObjectGuard mainModuleName(PyUnicode_DecodeFSDefault("python.Adapter"));
    if (!mainModuleName)
    {
        THROW_PYEXCEPTION("Failed to create unicode module name");
    }

    PyObjectGuard moduleOriginal(PyImport_Import(mainModuleName.get()));
    if (!moduleOriginal)
    {
        THROW_PYEXCEPTION("Failed to import adapter module");
    }

    // Reload the module to force re-reading the file
    PyObjectGuard module(PyImport_ReloadModule(moduleOriginal.get()));
    if (!module)
    {
        THROW_PYEXCEPTION("Failed to reload adapter module");
    }

    #else

    std::string text_resource = ResourceLoader::loadTextResource(dllModuleHandle, PYTHON_ADAPTER, TEXT("PYTHON")).c_str();
    PyObject *compiledString = Py_CompileString(
        text_resource.c_str(),
        "python-adapter.py",
        Py_file_input);

    PyObjectGuard pCompiledContents(compiledString);
    if (!pCompiledContents)
    {
        THROW_PYEXCEPTION("Failed to compile embedded python module");
    }

    PyObjectGuard module(PyImport_ExecCodeModule("adapter", pCompiledContents.get()));
    if (!module)
    {
        THROW_PYEXCEPTION("Failed to add compiled module");
    }
    #endif

    PyObjectGuard function(PyObject_GetAttrString(module.get(), "python_adapter"));
    if (!function || !PyCallable_Check(function.get()))
    {
        THROW_PYEXCEPTION("Failed to reference python function 'python_adapter'");
    }

    pModule = module.transfer();
    pFunc = function.transfer();
}

void EmbeddedPython::initModules(modules_t mods)
{
    /**
    Initialize python sources for modules.
    The sources passed here will be used to import `pythia.modulename`.
    */

    if (!pModule)
    {
        THROW_PYEXCEPTION("Pythia adapter not loaded correctly. Not initializing python modules.")
    }

    PyObjectGuard pDict(PyDict_New());
    if (!pDict)
    {
        THROW_PYEXCEPTION("Could not create a new python dict.")
    }

    // Fill the dict with the items in the unordered_map
    for (const auto& entry: mods)
    {
        PyObjectGuard pString(PyUnicode_FromWideChar(entry.second.c_str(), -1));
        if (!pString)
        {
            continue;
        }

        int retval = PyDict_SetItemString(pDict.get(), entry.first.c_str(), pString.get());
        if (retval == -1)
        {
            THROW_PYEXCEPTION("Error while running PyDict_SetItemString.")
        }
    }

    // Perform the call adding the mods sources
    PyObjectGuard function(PyObject_GetAttrString(pModule, "init_modules"));
    if (!function || !PyCallable_Check(function.get()))
    {
        THROW_PYEXCEPTION("Failed to reference python function 'init_modules'");
    }

    PyObjectGuard pResult(PyObject_CallFunctionObjArgs(function.get(), pDict.get(), NULL));
    if (pResult)
    {
        return; // Yay!
    }
    else
    {
        THROW_PYEXCEPTION("Failed to execute python init_modules function");
    }
}

void EmbeddedPython::deinitialize()
{
    Py_CLEAR(pFunc);
    Py_CLEAR(pModule);
}

void EmbeddedPython::reload()
{
    deinitialize();
    try
    {
        initialize();
        LOG_INFO("Python extension successfully reloaded");
    }
    catch (const std::exception& ex)
    {
        LOG_ERROR(std::string("Caught error when reloading the extension: ") + ex.what());
        pythonInitializationError = ex.what();
    }
}

EmbeddedPython::~EmbeddedPython()
{
    enterPythonThread();
    deinitialize();
    Py_Finalize();
}

// Note: outputSize is the size CONTAINING the null terminator
void handleMultipart(char *output, int outputSize, multipart_t entry)
{
    if (entry.empty())
    {
        return;
    }

    multiparts[multipartCounter] = entry;

    //["m", MULTIPART_COUNTER, len(responses)]
    snprintf(output, outputSize - 1, "[\"m\",%lu,%lu]", multipartCounter++, (unsigned long)entry.size());
}

// Note: outputSize is the size CONTAINING the null terminator
void returnMultipart(unsigned long multipartID, char *output, int outputSize)
{
    try
    {
        auto &entry = multiparts.at(multipartID);
        auto &retval = entry.front();

        size_t minSize = min((size_t)outputSize, retval.size() + 1);
        strncpy_s(output, minSize, retval.data(), _TRUNCATE);

        entry.pop();
        if (entry.empty())
        {
            multiparts.erase(multipartID);
        }
    }
    catch (std::out_of_range)
    {
        output[0] = '\0';
    }
}

// Note: outputSize is the size CONTAINING the null terminator
// A value of 10240 means that you can have 10239 characters + '\0' there
void EmbeddedPython::execute(char *output, int outputSize, const char *input)
{
    #ifdef EXTENSION_DEVELOPMENT
        reload();
    #endif

    if (!pFunc)
    {
        throw std::runtime_error("No bootstrapping function. Additional error: " + pythonInitializationError);
    }

    PyObjectGuard pArgs(SQFReader::decode(input));

    /*PyObjectGuard pArgs(PyUnicode_FromString(input));
    if (!pArgs)
    {
        throw std::runtime_error("Failed to transform given input to unicode");
    }*/

    PyObjectGuard pTuple(PyTuple_Pack(1, pArgs.get()));
    if (!pTuple)
    {
        throw std::runtime_error("Failed to convert argument string to tuple");
    }

    PyObject* PyFunction = PyList_GetItem(pArgs.get(), 0); // Borrows reference
    if (PyFunction)
    {
        // Multipart
        // TODO: Do a Python string comparison
        if (PyUnicode_CompareWithASCIIString(PyFunction, "pythia.multipart") == 0)
        {
            PyObject* PyMultipartID = PyList_GetItem(pArgs.get(), 1); // Borrows reference
            if (PyMultipartID)
            {
                int overflow;
                long multipartID = PyLong_AsLongAndOverflow(PyMultipartID, &overflow);
                if (overflow == 0 && multipartID >= 0)
                {
                    returnMultipart(multipartID, output, outputSize);
                    return;
                }
                else
                {
                    throw std::runtime_error("Could not read the multipart ID");
                }
            }
            else
            {
                throw std::runtime_error("Could not get the multipart ID from the request");
            }
        }
    }
    else
    {
        throw std::runtime_error("Failed to get the function name from the request");
    }

    PyObjectGuard pResult(PyObject_CallObject(pFunc, pTuple.get()));
    if (pResult)
    {
        MultipartResponseWriter writer(output, outputSize);
        writer.initialize();
        SQFWriter::encode(pResult.get(), &writer);
        writer.finalize();

        auto multipartResponse = writer.getMultipart();
        handleMultipart(output, outputSize, multipartResponse);
        return;
    }
    else
    {
        THROW_PYEXCEPTION("Failed to execute python extension");
    }
}
<commit_msg>Use executable directory instead of cwd for python path. Resolves #34.<commit_after>#include "stdafx.h"
#include "EmbeddedPython.h"
#include "ModsLocation.h"
#include "ExceptionFetcher.h"
#include "ResourceLoader.h"
#include <iostream>
#include "resource.h"
#include "Logger.h"
#include "ResponseWriter.h"
#include "SQFReader.h"
#include "SQFWriter.h"
#include "Paths.h"
#include "Modules/pythiainternal.h"
#include "Modules/pythialogger.h"

#define THROW_PYEXCEPTION(_msg_) throw std::runtime_error(_msg_ + std::string(": ") + PyExceptionFetcher().getError());
//#define EXTENSION_DEVELOPMENT 1

EmbeddedPython *python = nullptr;
std::string pythonInitializationError = "";
unsigned long multipartCounter = 0;
std::unordered_map<unsigned long long int, multipart_t> multiparts;

namespace
{
    class PyObjectGuard final
    {
    public:
        PyObjectGuard(PyObject* source) : ptr(source)
        {
        }

        ~PyObjectGuard()
        {
            if (ptr != nullptr)
            {
                Py_DECREF(ptr);
            }
        }

        PyObject* get() const
        {
            return ptr;
        }

        explicit operator bool() const
        {
            return ptr != nullptr;
        }

        /// Release ownership
        PyObject* transfer()
        {
            PyObject* tmp = ptr;
            ptr = nullptr;
            return tmp;
        }

    private:
        PyObjectGuard(const PyObjectGuard&) = delete;
        void operator=(const PyObjectGuard&) = delete;

    private:
        PyObject *ptr;
    };
}

void EmbeddedPython::DoPythonMagic(std::wstring path)
{
    // Python pre-initialization magic

    // Clear the env variables, just in case
    _wputenv_s(L"PYTHONHOME", L"");
    _wputenv_s(L"PYTHONPATH", L"");
    _wputenv_s(L"PYTHONNOUSERSITE", L"1");  // Disable custom user site

    // Py_SetPythonHome(L"D:\\Steam\\steamapps\\common\\Arma 3\\@Pythia\\python-embed-amd64");
    pythonHomeString = std::vector<wchar_t>(path.begin(), path.end());
    pythonHomeString.push_back(0);
    Py_SetPythonHome(pythonHomeString.data());
    LOG_INFO(std::string("Python home: ") + Logger::w2s(Py_GetPythonHome()));

    // Py_SetProgramName(L"D:\\Steam\\steamapps\\common\\Arma 3\\@Pythia\\python-embed-amd64\\python.exe");
    std::wstring programName = path + L"\\python.exe"; // Not sure if that should be the value here
    programNameString = std::vector<wchar_t>(programName.begin(), programName.end());
    programNameString.push_back(0);
    Py_SetProgramName(programNameString.data());
    LOG_INFO(std::string("Program name: ") + Logger::w2s(Py_GetProgramName()));

    /*
    Py_SetPath(L"D:\\Steam\\SteamApps\\common\\Arma 3\\@Pythia\\python-embed-amd64\\python35.zip;"
        L"D:\\Steam\\SteamApps\\common\\Arma 3\\@Pythia\\python-embed-amd64\\DLLs;"
        L"D:\\Steam\\SteamApps\\common\\Arma 3\\@Pythia\\python-embed-amd64\\lib;"
        L"D:\\Steam\\SteamApps\\common\\Arma 3");
    */
    std::wstring allPaths =
        path + L"\\python35.zip" + L";" +
        path + L"\\DLLs" + L";" +
        path + L"\\lib" + L";" +
        getProgramDirectory(); // For `python/` directory access. TODO: Use import hooks for that
    pathString = std::vector<wchar_t>(allPaths.begin(), allPaths.end());
    pathString.push_back(0);
    // Not setting PySetPath overwrites the Py_SetProgramName value (it seems to be ignored then),
    Py_SetPath(pathString.data());
    LOG_INFO(std::string("Python paths: ") + Logger::w2s(Py_GetPath()));
    LOG_INFO(std::string("Current directory: ") + GetCurrentWorkingDir());
}

EmbeddedPython::EmbeddedPython(HMODULE moduleHandle): dllModuleHandle(moduleHandle)
{
    DoPythonMagic(getPythonPath());
    PyImport_AppendInittab("pythiainternal", PyInit_pythiainternal);
    PyImport_AppendInittab("pythialogger", PyInit_pythialogger);
    Py_Initialize();
    PyEval_InitThreads(); // Initialize and acquire GIL
    initialize();
    leavePythonThread();
}

void EmbeddedPython::enterPythonThread()
{
    PyEval_RestoreThread(pThreadState);
}

void EmbeddedPython::leavePythonThread()
{
    pThreadState = PyEval_SaveThread();
}

void EmbeddedPython::initialize()
{
    #ifdef EXTENSION_DEVELOPMENT
    PyObjectGuard mainModuleName(PyUnicode_DecodeFSDefault("python.Adapter"));
    if (!mainModuleName)
    {
        THROW_PYEXCEPTION("Failed to create unicode module name");
    }

    PyObjectGuard moduleOriginal(PyImport_Import(mainModuleName.get()));
    if (!moduleOriginal)
    {
        THROW_PYEXCEPTION("Failed to import adapter module");
    }

    // Reload the module to force re-reading the file
    PyObjectGuard module(PyImport_ReloadModule(moduleOriginal.get()));
    if (!module)
    {
        THROW_PYEXCEPTION("Failed to reload adapter module");
    }

    #else

    std::string text_resource = ResourceLoader::loadTextResource(dllModuleHandle, PYTHON_ADAPTER, TEXT("PYTHON")).c_str();
    PyObject *compiledString = Py_CompileString(
        text_resource.c_str(),
        "python-adapter.py",
        Py_file_input);

    PyObjectGuard pCompiledContents(compiledString);
    if (!pCompiledContents)
    {
        THROW_PYEXCEPTION("Failed to compile embedded python module");
    }

    PyObjectGuard module(PyImport_ExecCodeModule("adapter", pCompiledContents.get()));
    if (!module)
    {
        THROW_PYEXCEPTION("Failed to add compiled module");
    }
    #endif

    PyObjectGuard function(PyObject_GetAttrString(module.get(), "python_adapter"));
    if (!function || !PyCallable_Check(function.get()))
    {
        THROW_PYEXCEPTION("Failed to reference python function 'python_adapter'");
    }

    pModule = module.transfer();
    pFunc = function.transfer();
}

void EmbeddedPython::initModules(modules_t mods)
{
    /**
    Initialize python sources for modules.
    The sources passed here will be used to import `pythia.modulename`.
    */

    if (!pModule)
    {
        THROW_PYEXCEPTION("Pythia adapter not loaded correctly. Not initializing python modules.")
    }

    PyObjectGuard pDict(PyDict_New());
    if (!pDict)
    {
        THROW_PYEXCEPTION("Could not create a new python dict.")
    }

    // Fill the dict with the items in the unordered_map
    for (const auto& entry: mods)
    {
        PyObjectGuard pString(PyUnicode_FromWideChar(entry.second.c_str(), -1));
        if (!pString)
        {
            continue;
        }

        int retval = PyDict_SetItemString(pDict.get(), entry.first.c_str(), pString.get());
        if (retval == -1)
        {
            THROW_PYEXCEPTION("Error while running PyDict_SetItemString.")
        }
    }

    // Perform the call adding the mods sources
    PyObjectGuard function(PyObject_GetAttrString(pModule, "init_modules"));
    if (!function || !PyCallable_Check(function.get()))
    {
        THROW_PYEXCEPTION("Failed to reference python function 'init_modules'");
    }

    PyObjectGuard pResult(PyObject_CallFunctionObjArgs(function.get(), pDict.get(), NULL));
    if (pResult)
    {
        return; // Yay!
    }
    else
    {
        THROW_PYEXCEPTION("Failed to execute python init_modules function");
    }
}

void EmbeddedPython::deinitialize()
{
    Py_CLEAR(pFunc);
    Py_CLEAR(pModule);
}

void EmbeddedPython::reload()
{
    deinitialize();
    try
    {
        initialize();
        LOG_INFO("Python extension successfully reloaded");
    }
    catch (const std::exception& ex)
    {
        LOG_ERROR(std::string("Caught error when reloading the extension: ") + ex.what());
        pythonInitializationError = ex.what();
    }
}

EmbeddedPython::~EmbeddedPython()
{
    enterPythonThread();
    deinitialize();
    Py_Finalize();
}

// Note: outputSize is the size CONTAINING the null terminator
void handleMultipart(char *output, int outputSize, multipart_t entry)
{
    if (entry.empty())
    {
        return;
    }

    multiparts[multipartCounter] = entry;

    //["m", MULTIPART_COUNTER, len(responses)]
    snprintf(output, outputSize - 1, "[\"m\",%lu,%lu]", multipartCounter++, (unsigned long)entry.size());
}

// Note: outputSize is the size CONTAINING the null terminator
void returnMultipart(unsigned long multipartID, char *output, int outputSize)
{
    try
    {
        auto &entry = multiparts.at(multipartID);
        auto &retval = entry.front();

        size_t minSize = min((size_t)outputSize, retval.size() + 1);
        strncpy_s(output, minSize, retval.data(), _TRUNCATE);

        entry.pop();
        if (entry.empty())
        {
            multiparts.erase(multipartID);
        }
    }
    catch (std::out_of_range)
    {
        output[0] = '\0';
    }
}

// Note: outputSize is the size CONTAINING the null terminator
// A value of 10240 means that you can have 10239 characters + '\0' there
void EmbeddedPython::execute(char *output, int outputSize, const char *input)
{
    #ifdef EXTENSION_DEVELOPMENT
        reload();
    #endif

    if (!pFunc)
    {
        throw std::runtime_error("No bootstrapping function. Additional error: " + pythonInitializationError);
    }

    PyObjectGuard pArgs(SQFReader::decode(input));

    /*PyObjectGuard pArgs(PyUnicode_FromString(input));
    if (!pArgs)
    {
        throw std::runtime_error("Failed to transform given input to unicode");
    }*/

    PyObjectGuard pTuple(PyTuple_Pack(1, pArgs.get()));
    if (!pTuple)
    {
        throw std::runtime_error("Failed to convert argument string to tuple");
    }

    PyObject* PyFunction = PyList_GetItem(pArgs.get(), 0); // Borrows reference
    if (PyFunction)
    {
        // Multipart
        // TODO: Do a Python string comparison
        if (PyUnicode_CompareWithASCIIString(PyFunction, "pythia.multipart") == 0)
        {
            PyObject* PyMultipartID = PyList_GetItem(pArgs.get(), 1); // Borrows reference
            if (PyMultipartID)
            {
                int overflow;
                long multipartID = PyLong_AsLongAndOverflow(PyMultipartID, &overflow);
                if (overflow == 0 && multipartID >= 0)
                {
                    returnMultipart(multipartID, output, outputSize);
                    return;
                }
                else
                {
                    throw std::runtime_error("Could not read the multipart ID");
                }
            }
            else
            {
                throw std::runtime_error("Could not get the multipart ID from the request");
            }
        }
    }
    else
    {
        throw std::runtime_error("Failed to get the function name from the request");
    }

    PyObjectGuard pResult(PyObject_CallObject(pFunc, pTuple.get()));
    if (pResult)
    {
        MultipartResponseWriter writer(output, outputSize);
        writer.initialize();
        SQFWriter::encode(pResult.get(), &writer);
        writer.finalize();

        auto multipartResponse = writer.getMultipart();
        handleMultipart(output, outputSize, multipartResponse);
        return;
    }
    else
    {
        THROW_PYEXCEPTION("Failed to execute python extension");
    }
}
<|endoftext|>


<commit_before>// This file is part of the dune-gdt project:
//   https://github.com/dune-community/dune-gdt
// Copyright 2010-2018 dune-gdt developers and contributors. All rights reserved.
// License: Dual licensed as BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//      or  GPL-2.0+ (http://opensource.org/licenses/gpl-license)
//          with "runtime exception" (http://www.dune-project.org/license.html)
// Authors:
//   Felix Schindler (2014 - 2017)
//   Rene Milk       (2014, 2016 - 2017)
//   Tobias Leibner  (2014, 2016)

#ifndef DUNE_GDT_SPACES_L2_FINITE_VOLUME_HH
#define DUNE_GDT_SPACES_L2_FINITE_VOLUME_HH

#include <dune/xt/common/type_traits.hh>
#include <dune/xt/grid/type_traits.hh>

#include <dune/gdt/spaces/basis/finite-volume.hh>
#include <dune/gdt/spaces/mapper/finite-volume.hh>
#include <dune/gdt/spaces/interface.hh>

namespace Dune {
namespace GDT {

// forward, to allow for specialization
template <class GV, size_t r, size_t rC = 1, class R = double>
class FiniteVolumeSpace
{
  static_assert(Dune::AlwaysFalse<GV>::value, "Untested for these dimensions!");
};

template <class GV, class R>
class FiniteVolumeSpace<GV, 1, 1, R> : public SpaceInterface<GV, 1, 1, R>
{
  using ThisType = FiniteVolumeSpace<GV, 1, 1, R>;
  using BaseType = SpaceInterface<GV, 1, 1, R>;

public:
  using typename BaseType::D;
  using BaseType::d;
  using typename BaseType::GridViewType;
  using typename BaseType::GlobalBasisType;
  using typename BaseType::MapperType;
  using typename BaseType::FiniteElementType;

private:
  using MapperImplementation = FiniteVolumeMapper<GridViewType, 1, 1>;
  using GlobalBasisImplementation = FiniteVolumeGlobalBasis<GridViewType, R>;

public:
  FiniteVolumeSpace(GridViewType grd_vw)
    : grid_view_(grd_vw)
    , finite_elements_(new std::map<GeometryType, std::shared_ptr<FiniteElementType>>())
    , mapper_(new MapperImplementation(grid_view_))
    , basis_(new GlobalBasisImplementation(grid_view_))
  {
    // create finite elements
    for (auto&& geometry_type : grid_view_.indexSet().types(0))
      finite_elements_->insert(
          std::make_pair(geometry_type, make_lagrange_local_finite_element<D, d, R>(geometry_type, 0)));
    // create communicator
    this->create_communicator();
  }

  FiniteVolumeSpace(const ThisType&) = default;
  FiniteVolumeSpace(ThisType&&) = default;

  ThisType& operator=(const ThisType&) = delete;
  ThisType& operator=(ThisType&&) = delete;

  const GridViewType& grid_view() const override final
  {
    return grid_view_;
  }

  const MapperType& mapper() const override final
  {
    return *mapper_;
  }

  const GlobalBasisType& basis() const override final
  {
    return *basis_;
  }

  const FiniteElementType& finite_element(const GeometryType& geometry_type) const override final
  {
    const auto finite_element_search_result = finite_elements_->find(geometry_type);
    if (finite_element_search_result == finite_elements_->end())
      DUNE_THROW(XT::Common::Exceptions::internal_error,
                 "This must not happen, the grid layer did not report all geometry types!"
                     << "\n   entity.geometry().type() = "
                     << geometry_type);
    return *finite_element_search_result->second;
  }

  SpaceType type() const override final
  {
    return SpaceType::finite_volume;
  }

  int min_polorder() const override final
  {
    return 0;
  }

  int max_polorder() const override final
  {
    return 0;
  }

  bool continuous(const int /*diff_order*/) const override final
  {
    return false;
  }

  bool continuous_normal_components() const override final
  {
    return false;
  }

  bool is_lagrangian() const override final
  {
    return true;
  }

private:
  const GridViewType grid_view_;
  std::shared_ptr<std::map<GeometryType, std::shared_ptr<FiniteElementType>>> finite_elements_;
  const std::shared_ptr<MapperImplementation> mapper_;
  const std::shared_ptr<GlobalBasisImplementation> basis_;
}; // class FiniteVolumeSpace< ..., 1, 1 >

template <size_t r, size_t rC, class R, class GV>
FiniteVolumeSpace<GV, r, rC, R> make_finite_volume_space(const GV& grid_view)
{
  return FiniteVolumeSpace<GV, r, rC, R>(grid_view);
}

template <size_t r, size_t rC, class GV>
FiniteVolumeSpace<GV, r, rC, double> make_finite_volume_space(const GV& grid_view)
{
  return FiniteVolumeSpace<GV, r, rC, double>(grid_view);
}

template <size_t r, class GV>
FiniteVolumeSpace<GV, r, 1, double> make_finite_volume_space(const GV& grid_view)
{
  return FiniteVolumeSpace<GV, r, 1, double>(grid_view);
}

} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_SPACES_L2_FINITE_VOLUME_HH
<commit_msg>[spaces.l2.finite-volume] let range_dim default to 1<commit_after>// This file is part of the dune-gdt project:
//   https://github.com/dune-community/dune-gdt
// Copyright 2010-2018 dune-gdt developers and contributors. All rights reserved.
// License: Dual licensed as BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//      or  GPL-2.0+ (http://opensource.org/licenses/gpl-license)
//          with "runtime exception" (http://www.dune-project.org/license.html)
// Authors:
//   Felix Schindler (2014 - 2017)
//   Rene Milk       (2014, 2016 - 2017)
//   Tobias Leibner  (2014, 2016)

#ifndef DUNE_GDT_SPACES_L2_FINITE_VOLUME_HH
#define DUNE_GDT_SPACES_L2_FINITE_VOLUME_HH

#include <dune/xt/common/type_traits.hh>
#include <dune/xt/grid/type_traits.hh>

#include <dune/gdt/spaces/basis/finite-volume.hh>
#include <dune/gdt/spaces/mapper/finite-volume.hh>
#include <dune/gdt/spaces/interface.hh>

namespace Dune {
namespace GDT {

// forward, to allow for specialization
template <class GV, size_t r = 1, size_t rC = 1, class R = double>
class FiniteVolumeSpace
{
  static_assert(Dune::AlwaysFalse<GV>::value, "Untested for these dimensions!");
};

template <class GV, class R>
class FiniteVolumeSpace<GV, 1, 1, R> : public SpaceInterface<GV, 1, 1, R>
{
  using ThisType = FiniteVolumeSpace<GV, 1, 1, R>;
  using BaseType = SpaceInterface<GV, 1, 1, R>;

public:
  using typename BaseType::D;
  using BaseType::d;
  using typename BaseType::GridViewType;
  using typename BaseType::GlobalBasisType;
  using typename BaseType::MapperType;
  using typename BaseType::FiniteElementType;

private:
  using MapperImplementation = FiniteVolumeMapper<GridViewType, 1, 1>;
  using GlobalBasisImplementation = FiniteVolumeGlobalBasis<GridViewType, R>;

public:
  FiniteVolumeSpace(GridViewType grd_vw)
    : grid_view_(grd_vw)
    , finite_elements_(new std::map<GeometryType, std::shared_ptr<FiniteElementType>>())
    , mapper_(new MapperImplementation(grid_view_))
    , basis_(new GlobalBasisImplementation(grid_view_))
  {
    // create finite elements
    for (auto&& geometry_type : grid_view_.indexSet().types(0))
      finite_elements_->insert(
          std::make_pair(geometry_type, make_lagrange_local_finite_element<D, d, R>(geometry_type, 0)));
    // create communicator
    this->create_communicator();
  }

  FiniteVolumeSpace(const ThisType&) = default;
  FiniteVolumeSpace(ThisType&&) = default;

  ThisType& operator=(const ThisType&) = delete;
  ThisType& operator=(ThisType&&) = delete;

  const GridViewType& grid_view() const override final
  {
    return grid_view_;
  }

  const MapperType& mapper() const override final
  {
    return *mapper_;
  }

  const GlobalBasisType& basis() const override final
  {
    return *basis_;
  }

  const FiniteElementType& finite_element(const GeometryType& geometry_type) const override final
  {
    const auto finite_element_search_result = finite_elements_->find(geometry_type);
    if (finite_element_search_result == finite_elements_->end())
      DUNE_THROW(XT::Common::Exceptions::internal_error,
                 "This must not happen, the grid layer did not report all geometry types!"
                     << "\n   entity.geometry().type() = "
                     << geometry_type);
    return *finite_element_search_result->second;
  }

  SpaceType type() const override final
  {
    return SpaceType::finite_volume;
  }

  int min_polorder() const override final
  {
    return 0;
  }

  int max_polorder() const override final
  {
    return 0;
  }

  bool continuous(const int /*diff_order*/) const override final
  {
    return false;
  }

  bool continuous_normal_components() const override final
  {
    return false;
  }

  bool is_lagrangian() const override final
  {
    return true;
  }

private:
  const GridViewType grid_view_;
  std::shared_ptr<std::map<GeometryType, std::shared_ptr<FiniteElementType>>> finite_elements_;
  const std::shared_ptr<MapperImplementation> mapper_;
  const std::shared_ptr<GlobalBasisImplementation> basis_;
}; // class FiniteVolumeSpace< ..., 1, 1 >

template <size_t r, size_t rC, class R, class GV>
FiniteVolumeSpace<GV, r, rC, R> make_finite_volume_space(const GV& grid_view)
{
  return FiniteVolumeSpace<GV, r, rC, R>(grid_view);
}

template <size_t r, size_t rC, class GV>
FiniteVolumeSpace<GV, r, rC, double> make_finite_volume_space(const GV& grid_view)
{
  return FiniteVolumeSpace<GV, r, rC, double>(grid_view);
}

template <size_t r, class GV>
FiniteVolumeSpace<GV, r, 1, double> make_finite_volume_space(const GV& grid_view)
{
  return FiniteVolumeSpace<GV, r, 1, double>(grid_view);
}

} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_SPACES_L2_FINITE_VOLUME_HH
<|endoftext|>


<commit_before>#include "../finite_element_gaussian.h"

#include <vector>

#include <deal.II/grid/grid_generator.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/fe/fe_dgq.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <gtest/gtest.h>

#include "problem/parameter_types.h"

#include "test_helpers/gmock_wrapper.h"

namespace {

class FiniteElementGaussianTest : public ::testing::Test {
 protected:
  using DiscretizationType = bart::problem::DiscretizationType;
};

TEST_F(FiniteElementGaussianTest, ConstructorContinuous) {
  bart::domain::FiniteElementGaussian<2> test_fe{DiscretizationType::kContinuousFEM, 2};
  auto fe_q_ptr =
      dynamic_cast<dealii::FE_Q<2>*>(test_fe.finite_element());
  auto fe_value_ptr =
      dynamic_cast<dealii::FEValues<2>*>(test_fe.values());
  auto fe_face_value_ptr = dynamic_cast<dealii::FEFaceValues<2>*>(
      test_fe.face_values());
  auto fe_neighbor_face_value_ptr = dynamic_cast<dealii::FEFaceValues<2>*>(
      test_fe.neighbor_face_values());
  auto cell_quad_ptr =
      dynamic_cast<dealii::QGauss<2>*>(test_fe.cell_quadrature());
  auto face_quad_ptr =
      dynamic_cast<dealii::QGauss<1>*>(test_fe.face_quadrature());

  ASSERT_EQ(test_fe.polynomial_degree(), 2);
  ASSERT_FALSE(fe_q_ptr == nullptr);
  ASSERT_FALSE(fe_value_ptr == nullptr);
  ASSERT_FALSE(fe_face_value_ptr == nullptr);
  ASSERT_TRUE(fe_neighbor_face_value_ptr == nullptr);
  ASSERT_FALSE(cell_quad_ptr == nullptr);
  ASSERT_FALSE(face_quad_ptr == nullptr);
  ASSERT_EQ(test_fe.dofs_per_cell(), 9);
  ASSERT_EQ(test_fe.n_cell_quad_pts(), 9);
  ASSERT_EQ(test_fe.n_face_quad_pts(), 3);
}

TEST_F(FiniteElementGaussianTest, ConstructorDiscontinuous) {
  bart::domain::FiniteElementGaussian<2> test_fe{DiscretizationType::kDiscontinuousFEM, 2};
  dealii::FE_DGQ<2> *fe_q_ptr =
      dynamic_cast<dealii::FE_DGQ<2>*>(test_fe.finite_element());
  auto fe_neighbor_face_value_ptr = dynamic_cast<dealii::FEFaceValues<2>*>(
      test_fe.neighbor_face_values());
  ASSERT_FALSE(fe_q_ptr == nullptr);
  ASSERT_FALSE(fe_neighbor_face_value_ptr == nullptr);
}

TEST_F(FiniteElementGaussianTest, ConstructorNone) {
  ASSERT_ANY_THROW({
      bart::domain::FiniteElementGaussian<2> test_fe(DiscretizationType::kNone, 2);
    });
}

TEST_F(FiniteElementGaussianTest, SetCellTest) {
  bart::domain::FiniteElementGaussian<2> test_fe{DiscretizationType::kDiscontinuousFEM, 2};
  dealii::Triangulation<2> triangulation;

  dealii::GridGenerator::hyper_cube(triangulation, -1, 1);
  triangulation.refine_global(2);

  dealii::DoFHandler dof_handler(triangulation);
  dof_handler.distribute_dofs(*test_fe.finite_element());

  auto cell = dof_handler.begin_active();
  auto cell_id = cell->id();

  test_fe.values()->reinit(cell);

  EXPECT_FALSE(test_fe.SetCell(cell)); // Shouldn't change anything
  EXPECT_EQ(cell_id, test_fe.values()->get_cell()->id()); // Cell didn't change

  auto next_cell = cell;
  ++next_cell;
  auto next_cell_id = next_cell->id();

  EXPECT_TRUE(test_fe.SetCell(next_cell));
  // Check changed
  EXPECT_NE(cell_id, test_fe.values()->get_cell()->id());
  EXPECT_EQ(next_cell_id, test_fe.values()->get_cell()->id());
}

TEST_F(FiniteElementGaussianTest, SetCellAndFace) {
  bart::domain::FiniteElementGaussian<2> test_fe{DiscretizationType::kDiscontinuousFEM, 2};
  dealii::Triangulation<2> triangulation;

  dealii::GridGenerator::hyper_cube(triangulation, -1, 1);
  triangulation.refine_global(2);

  dealii::DoFHandler dof_handler(triangulation);
  dof_handler.distribute_dofs(*test_fe.finite_element());

  auto cell = dof_handler.begin_active();
  auto cell_id = cell->id();
  int face = 0;
  int face_index = cell->face_index(face);

  test_fe.face_values()->reinit(cell, face);

  EXPECT_FALSE(test_fe.SetFace(cell, face));
  EXPECT_EQ(cell_id, test_fe.face_values()->get_cell()->id());
  EXPECT_EQ(face_index, test_fe.face_values()->get_face_index());

  auto next_cell = cell;
  ++next_cell;
  auto next_cell_id = next_cell->id();
  int next_face = face + 1;
  int next_face_index = next_cell->face_index(next_face);

  EXPECT_TRUE(test_fe.SetFace(next_cell, next_face));
  EXPECT_EQ(next_cell_id, test_fe.face_values()->get_cell()->id());
  EXPECT_NE(face_index, test_fe.face_values()->get_face_index());
  EXPECT_EQ(next_face_index, test_fe.face_values()->get_face_index());
}

} // namespace<commit_msg>added test to ensure SetCell doesn't throw before reinit has been called (causes seg-fault which isn't caught by the test)<commit_after>#include "../finite_element_gaussian.h"

#include <vector>

#include <deal.II/grid/grid_generator.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/fe/fe_dgq.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <gtest/gtest.h>

#include "problem/parameter_types.h"

#include "test_helpers/gmock_wrapper.h"

namespace {

class FiniteElementGaussianTest : public ::testing::Test {
 protected:
  using DiscretizationType = bart::problem::DiscretizationType;
};

TEST_F(FiniteElementGaussianTest, ConstructorContinuous) {
  bart::domain::FiniteElementGaussian<2> test_fe{DiscretizationType::kContinuousFEM, 2};
  auto fe_q_ptr =
      dynamic_cast<dealii::FE_Q<2>*>(test_fe.finite_element());
  auto fe_value_ptr =
      dynamic_cast<dealii::FEValues<2>*>(test_fe.values());
  auto fe_face_value_ptr = dynamic_cast<dealii::FEFaceValues<2>*>(
      test_fe.face_values());
  auto fe_neighbor_face_value_ptr = dynamic_cast<dealii::FEFaceValues<2>*>(
      test_fe.neighbor_face_values());
  auto cell_quad_ptr =
      dynamic_cast<dealii::QGauss<2>*>(test_fe.cell_quadrature());
  auto face_quad_ptr =
      dynamic_cast<dealii::QGauss<1>*>(test_fe.face_quadrature());

  ASSERT_EQ(test_fe.polynomial_degree(), 2);
  ASSERT_FALSE(fe_q_ptr == nullptr);
  ASSERT_FALSE(fe_value_ptr == nullptr);
  ASSERT_FALSE(fe_face_value_ptr == nullptr);
  ASSERT_TRUE(fe_neighbor_face_value_ptr == nullptr);
  ASSERT_FALSE(cell_quad_ptr == nullptr);
  ASSERT_FALSE(face_quad_ptr == nullptr);
  ASSERT_EQ(test_fe.dofs_per_cell(), 9);
  ASSERT_EQ(test_fe.n_cell_quad_pts(), 9);
  ASSERT_EQ(test_fe.n_face_quad_pts(), 3);
}

TEST_F(FiniteElementGaussianTest, ConstructorDiscontinuous) {
  bart::domain::FiniteElementGaussian<2> test_fe{DiscretizationType::kDiscontinuousFEM, 2};
  dealii::FE_DGQ<2> *fe_q_ptr =
      dynamic_cast<dealii::FE_DGQ<2>*>(test_fe.finite_element());
  auto fe_neighbor_face_value_ptr = dynamic_cast<dealii::FEFaceValues<2>*>(
      test_fe.neighbor_face_values());
  ASSERT_FALSE(fe_q_ptr == nullptr);
  ASSERT_FALSE(fe_neighbor_face_value_ptr == nullptr);
}

TEST_F(FiniteElementGaussianTest, ConstructorNone) {
  ASSERT_ANY_THROW({
      bart::domain::FiniteElementGaussian<2> test_fe(DiscretizationType::kNone, 2);
    });
}

TEST_F(FiniteElementGaussianTest, SetCellTest) {
  bart::domain::FiniteElementGaussian<2> test_fe{DiscretizationType::kDiscontinuousFEM, 2};
  dealii::Triangulation<2> triangulation;

  dealii::GridGenerator::hyper_cube(triangulation, -1, 1);
  triangulation.refine_global(2);

  dealii::DoFHandler dof_handler(triangulation);
  dof_handler.distribute_dofs(*test_fe.finite_element());

  auto cell = dof_handler.begin_active();
  auto cell_id = cell->id();

  EXPECT_NO_THROW(test_fe.SetCell(cell));

  test_fe.values()->reinit(cell);

  EXPECT_FALSE(test_fe.SetCell(cell)); // Shouldn't change anything
  EXPECT_EQ(cell_id, test_fe.values()->get_cell()->id()); // Cell didn't change

  auto next_cell = cell;
  ++next_cell;
  auto next_cell_id = next_cell->id();

  EXPECT_TRUE(test_fe.SetCell(next_cell));
  // Check changed
  EXPECT_NE(cell_id, test_fe.values()->get_cell()->id());
  EXPECT_EQ(next_cell_id, test_fe.values()->get_cell()->id());
}

TEST_F(FiniteElementGaussianTest, SetCellAndFace) {
  bart::domain::FiniteElementGaussian<2> test_fe{DiscretizationType::kDiscontinuousFEM, 2};
  dealii::Triangulation<2> triangulation;

  dealii::GridGenerator::hyper_cube(triangulation, -1, 1);
  triangulation.refine_global(2);

  dealii::DoFHandler dof_handler(triangulation);
  dof_handler.distribute_dofs(*test_fe.finite_element());

  auto cell = dof_handler.begin_active();
  auto cell_id = cell->id();
  int face = 0;
  int face_index = cell->face_index(face);

  test_fe.face_values()->reinit(cell, face);

  EXPECT_FALSE(test_fe.SetFace(cell, face));
  EXPECT_EQ(cell_id, test_fe.face_values()->get_cell()->id());
  EXPECT_EQ(face_index, test_fe.face_values()->get_face_index());

  auto next_cell = cell;
  ++next_cell;
  auto next_cell_id = next_cell->id();
  int next_face = face + 1;
  int next_face_index = next_cell->face_index(next_face);

  EXPECT_TRUE(test_fe.SetFace(next_cell, next_face));
  EXPECT_EQ(next_cell_id, test_fe.face_values()->get_cell()->id());
  EXPECT_NE(face_index, test_fe.face_values()->get_face_index());
  EXPECT_EQ(next_face_index, test_fe.face_values()->get_face_index());
}

} // namespace<|endoftext|>


<commit_before>#include <iostream>
#include <string>
#include <iomanip>
#include <stack>
#include <limits>
#include <algorithm>
#include <cmath>
#include <cstdint>
#include <cassert>

#include "matrix.h"

#ifdef DEBUG
    #define LOG(x) std::cerr << x
#else
    #define LOG(x)
#endif

using namespace std;

// Largest neighborhood to explore during 2-opt.
const size_t TWOOPT_MAX_NEIGHBORS = 100;

// Output stream operator (convenient for printing tours).
ostream& operator<<(ostream& os, const vector<size_t>& v) {
    for (auto e : v)
        os << e << ' ';
    os << endl;
    return os;
}

/**
 * Create a distance matrix from an input stream and return it.
 *
 * @param in Input stream.
 * @return The read distance matrix.
 */
Matrix<uint32_t> createDistanceMatrix(istream& in) {
    // Read vertex coordinates.
    size_t N;
    in >> N;
    vector<double> x(N);
    vector<double> y(N);
    for (size_t i = 0; i < N; ++i) {
        in >> x[i] >> y[i];
    }

    // Calculate distance matrix.
    Matrix<uint32_t> d(N, N);
    for (size_t i = 0; i < N; ++i) {
        for (size_t j = i + 1; j < N; ++j) {
            d[i][j] = d[j][i] = round(sqrt(pow(x[i]-x[j], 2) + pow(y[i]-y[j], 2)));
        }
    }

    return d;
}

/**
 * Calculate minimum spanning tree using Prim's algorithm.
 *
 * @param d Distance matrix.
 * @return The minimum spanning tree.
 */
Matrix<uint32_t> primMST(const Matrix<uint32_t>& d) {
    size_t N = d.rows();

    vector<bool> inMST(N);
    vector<uint32_t> cost(N);
    vector<uint32_t> parent(N);
    fill(inMST.begin(), inMST.end(), false);
    fill(cost.begin(), cost.end(), numeric_limits<uint32_t>::max());
    cost[0] = 0;

    for (size_t added = 0; added < N - 1; ++added) {
        // Find lowest cost city not in MST and add it to MST.
        size_t u = 0;
        uint32_t c = numeric_limits<int>::max();
        for (size_t v = 0; v < N; ++v) {
            if (cost[v] < c && !inMST[v]) {
                u = v;
                c = cost[v];
            }
        }
        inMST[u] = true;

        // For each neighbor v of u not already in MST.
        for (size_t v = 0; v < N; ++v) {
            if (0 < d[u][v] && d[u][v] < cost[v] && !inMST[v]) {
                // d[u][v] is lower than current cost of v, so mark u
                // as parent of v and set cost of v to d[u][v].
                parent[v] = u;
                cost[v] = d[u][v];
            }
        }
    }

    // Build MST matrix.
    Matrix<uint32_t> MST(N, N);
    for (size_t v = 1; v < N; ++v) {
        MST[parent[v]][v] = d[parent[v]][v];
    }

    return MST;
}

/**
 * Calculates a 2-approximation TSP tour from a minimum spanning tree.
 *
 * @param MST Input minimum spanning tree.
 * @return 2-approximation TSP tour.
 */
vector<uint16_t> twoApprox(const Matrix<uint32_t>& MST) {
    size_t N = MST.rows();

    vector<uint16_t> tour;
    vector<bool> visited(N);
    stack<uint16_t> stack;
    stack.push(0);

    while (tour.size() < N) {
        size_t u = stack.top();
        stack.pop();
        if (!visited[u]) {
            tour.push_back(u);
            visited[u] = true;
            for (uint16_t v = 0; v < N; ++v) {
                if (MST[u][v] != 0) {
                    stack.push(v);
                }
            }
        }
    }

    return tour;
}

/**
 * Calculates a greedy TSP tour.
 *
 * This is the naive algorithm given in the Kattis problem description.
 *
 * @param d Distance matrix.
 * @return Greedy TSP tour.
 */
vector<uint16_t> greedy(const Matrix<uint32_t>& d) {
    size_t N = d.rows();
    vector<uint16_t> tour(N);
    vector<bool> used(N, false);
    tour[0] = 0;
    used[0] = true;
    for (size_t i = 1; i < N; ++i) {
        // Find k, the closest city to the (i - 1):th city in tour.
        int32_t k = -1;
        for (uint16_t j = 0; j < N; ++j) {
            if (!used[j] && (k == -1 || d[tour[i-1]][j] < d[tour[i-1]][k])) {
                k = j;
            }
        }
        tour[i] = k;
        used[k] = true;
    }
    return tour;
}

/**
 * Calculate K-nearest neighbors matrix from a distance matrix.
 *
 * @param d Distance matrix.
 * @return d.rows() x (d.cols - 1) matrix where element i,j is
 *         the j:th nearest neighbor of city i.
 */
Matrix<uint16_t> createNeighborsMatrix(const Matrix<uint32_t>& d, size_t K) {
    size_t N = d.rows();
    size_t M = d.cols() - 1;
    K = min(M, K);
    Matrix<uint16_t> neighbor(N, K);
    std::vector<uint16_t> row(M); // For sorting.

    for (size_t i = 0; i < N; ++i) {
        // Fill row with 0, 1, ..., i - 1, i + 1, ..., M - 1.
        uint16_t k = 0;
        for (size_t j = 0; j < M; ++j, ++k) {
            row[j] = (i == j) ? ++k : k;
        }
        // Sort K nearest row elements by distance to i.
        partial_sort(row.begin(), row.begin() + K, row.end(),
            [&](uint16_t j, uint16_t k) {
                return d[i][j] < d[i][k];
            }
        );
        // Copy first K elements (now sorted) to neighbor matrix.
        copy(row.begin(), row.begin() + K, neighbor[i]);
    }
    return neighbor;
}

/**
 * Reverse a segment of a tour.
 *
 * This functions reverses the segment [start, end] of the given
 * tour and updates the position vector accordingly.
 *
 * @param tour The input tour.
 * @param start Start index of segment to reverse.
 * @param end End index of segment to reverse.
 * @param position Vector containing positions of cities in the tour, will
 *                 be updated to reflect the reversal.
 */
void reverse(vector<uint16_t> &tour, size_t start, size_t end,
        vector<uint16_t>& position) {
    size_t N = tour.size();
    size_t numSwaps = (((start <= end ? end - start : (end + N) - start) + 1)/2);
    uint16_t i = start;
    uint16_t j = end;
    for (size_t n = 0; n < numSwaps; ++n) {
        swap(tour[i], tour[j]);
        position[tour[i]] = i;
        position[tour[j]] = j;
        i = (i + 1) % N;
        j = ((j + N) - 1) % N;
    }
}

/**
 * Returns the shortest distance d[i][j], i != j in the given distance matrix.
 *
 * @param d Distance matrix.
 * @return Minimum distance in d.
 */
uint32_t minDistance(const Matrix<uint32_t>& d) {
    size_t N = d.rows();
    uint32_t min = numeric_limits<size_t>::max();
    for (size_t i = 0; i < N; ++i) {
        for (size_t j = 0; j < N; ++j) {
            if (i != j)
                min = std::min(min, d[i][j]);
        }
    }
    return min;
}

/**
 * Returns the longest inter-city distance in the given tour.
 *
 * @param tour Input tour.
 * @param d Distance matrix.
 * @return Maximum inter-city distance in tour.
 */
uint32_t maxDistance(const vector<uint16_t>& tour, const Matrix<uint32_t>& d) {
    size_t N = tour.size();
    uint32_t max = 0;
    for (size_t i = 0, j = 1; i < N; ++i, ++j) {
        max = std::max(max, d[tour[i]][tour[j % N]]);
    }
    return max;
}

/**
 * Returns the total length of a tour.
 *
 * @param tour The input tour.
 * @param d Distance matrix.
 * @return The total length of the tour.
 */
uint64_t length(const vector<uint32_t>& tour, const Matrix<uint32_t>& d) {
    size_t N = tour.size();
    uint64_t length = 0;
    for (size_t i = 0, j = 1; i < N; ++i, ++j) {
        length += d[tour[i]][tour[j % N]];
    }
    return length;
}

/**
 * Perform a 2-opt pass on the given tour.
 *
 * This function uses the fast approach described on page 12 of "Large-Step
 * Markov Chains for the Traveling Salesman Problem" (Martin/Otto/Felten, 1991)
 *
 * @param tour The tour to optimize.
 * @param d Distance matrix.
 * @param neighbor Nearest neighbors matrix.
 * @param min Shortest possible inter-city distance.
 * @return true if the tour was improved, otherwise false.
 */
bool twoOpt(vector<uint16_t>& tour, const Matrix<uint32_t>& d,
        const Matrix<uint16_t>& neighbor, uint32_t min) {
    size_t N = d.rows();
    bool didImprove = false;

    // Initialize city tour position vector.
    vector<uint16_t> position(N);
    for (uint16_t i = 0; i < N; ++i) {
        position[tour[i]] = i; // tour[i] is the i:th city in tour.
    }

    uint16_t u, v, w, z;
    size_t u_i, v_i, w_i, z_i;
    uint32_t max = maxDistance(tour, d); // Longest link in current tour.

    for (u_i = 0, v_i = 1; u_i < N; ++u_i, ++v_i) {
        // For each edge (u, v).
        u = tour[u_i];
        v = tour[v_i % N];
        for (size_t n = 0; n < neighbor.cols(); ++n) {
            // Visit nearby edges (w, z).
            w_i = position[neighbor[u][n]];
            z_i = w_i + 1;
            w = tour[w_i]; // w is the n:th closest neighbor of u.
            z = tour[z_i % N];

            if (v == w || z == u) {
                continue; // Skip adjacent edges.
            }

            // d[u][w] + min is a lower bound on new length.
            // d[u][v] + max is an upper bound on old length.
            if (d[u][w] + min > d[u][v] + max) {
                break; // Go to next edge (u, v).
            }

            if (d[u][w] + d[v][z] < d[u][v] + d[w][z]) {
                //   --u w--        --u-w->
                //      X     ===>
                //   <-z v->        <-z-v--
                reverse(tour, v_i % N, w_i, position);

                // FIXME: Is this enough to update max?
                max = std::max(max, std::max(d[u][w], d[v][z]));

                didImprove = true;
                break;
            }
        }
    }

    return didImprove;
}

int main(int argc, char *argv[]) {
    // Create distance matrix from standard input.
    Matrix<uint32_t> d = createDistanceMatrix(cin);

    // Calculate 2-approximation tour from minimum spanning tree.
    //Matrix<uint32_t> MST = primMST(d);
    //vector<uint16_t> tour = twoApprox(MST);

    // Calculate a greedy tour.
    vector<uint16_t> tour = greedy(d);

    // Calculate nearest neighbors and smallest possible city distance.
    Matrix<uint16_t> neighbor = createNeighborsMatrix(d, TWOOPT_MAX_NEIGHBORS);
    uint32_t min = minDistance(d);

    // Optimize tour using 2-opt.
    bool didImprove;
    do {
        didImprove = twoOpt(tour, d, neighbor, min);
    } while (didImprove);

    // Print tour.
    for (auto city : tour) {
        cout << city << endl;
    }

    return 0;
}

<commit_msg>Use k as index name instead of n.<commit_after>#include <iostream>
#include <string>
#include <iomanip>
#include <stack>
#include <limits>
#include <algorithm>
#include <cmath>
#include <cstdint>
#include <cassert>

#include "matrix.h"

#ifdef DEBUG
    #define LOG(x) std::cerr << x
#else
    #define LOG(x)
#endif

using namespace std;

// Largest neighborhood to explore during 2-opt.
const size_t TWOOPT_MAX_NEIGHBORS = 100;

// Output stream operator (convenient for printing tours).
ostream& operator<<(ostream& os, const vector<size_t>& v) {
    for (auto e : v)
        os << e << ' ';
    os << endl;
    return os;
}

/**
 * Create a distance matrix from an input stream and return it.
 *
 * @param in Input stream.
 * @return The read distance matrix.
 */
Matrix<uint32_t> createDistanceMatrix(istream& in) {
    // Read vertex coordinates.
    size_t N;
    in >> N;
    vector<double> x(N);
    vector<double> y(N);
    for (size_t i = 0; i < N; ++i) {
        in >> x[i] >> y[i];
    }

    // Calculate distance matrix.
    Matrix<uint32_t> d(N, N);
    for (size_t i = 0; i < N; ++i) {
        for (size_t j = i + 1; j < N; ++j) {
            d[i][j] = d[j][i] = round(sqrt(pow(x[i]-x[j], 2) + pow(y[i]-y[j], 2)));
        }
    }

    return d;
}

/**
 * Calculate minimum spanning tree using Prim's algorithm.
 *
 * @param d Distance matrix.
 * @return The minimum spanning tree.
 */
Matrix<uint32_t> primMST(const Matrix<uint32_t>& d) {
    size_t N = d.rows();

    vector<bool> inMST(N);
    vector<uint32_t> cost(N);
    vector<uint32_t> parent(N);
    fill(inMST.begin(), inMST.end(), false);
    fill(cost.begin(), cost.end(), numeric_limits<uint32_t>::max());
    cost[0] = 0;

    for (size_t added = 0; added < N - 1; ++added) {
        // Find lowest cost city not in MST and add it to MST.
        size_t u = 0;
        uint32_t c = numeric_limits<int>::max();
        for (size_t v = 0; v < N; ++v) {
            if (cost[v] < c && !inMST[v]) {
                u = v;
                c = cost[v];
            }
        }
        inMST[u] = true;

        // For each neighbor v of u not already in MST.
        for (size_t v = 0; v < N; ++v) {
            if (0 < d[u][v] && d[u][v] < cost[v] && !inMST[v]) {
                // d[u][v] is lower than current cost of v, so mark u
                // as parent of v and set cost of v to d[u][v].
                parent[v] = u;
                cost[v] = d[u][v];
            }
        }
    }

    // Build MST matrix.
    Matrix<uint32_t> MST(N, N);
    for (size_t v = 1; v < N; ++v) {
        MST[parent[v]][v] = d[parent[v]][v];
    }

    return MST;
}

/**
 * Calculates a 2-approximation TSP tour from a minimum spanning tree.
 *
 * @param MST Input minimum spanning tree.
 * @return 2-approximation TSP tour.
 */
vector<uint16_t> twoApprox(const Matrix<uint32_t>& MST) {
    size_t N = MST.rows();

    vector<uint16_t> tour;
    vector<bool> visited(N);
    stack<uint16_t> stack;
    stack.push(0);

    while (tour.size() < N) {
        size_t u = stack.top();
        stack.pop();
        if (!visited[u]) {
            tour.push_back(u);
            visited[u] = true;
            for (uint16_t v = 0; v < N; ++v) {
                if (MST[u][v] != 0) {
                    stack.push(v);
                }
            }
        }
    }

    return tour;
}

/**
 * Calculates a greedy TSP tour.
 *
 * This is the naive algorithm given in the Kattis problem description.
 *
 * @param d Distance matrix.
 * @return Greedy TSP tour.
 */
vector<uint16_t> greedy(const Matrix<uint32_t>& d) {
    size_t N = d.rows();
    vector<uint16_t> tour(N);
    vector<bool> used(N, false);
    tour[0] = 0;
    used[0] = true;
    for (size_t i = 1; i < N; ++i) {
        // Find k, the closest city to the (i - 1):th city in tour.
        int32_t k = -1;
        for (uint16_t j = 0; j < N; ++j) {
            if (!used[j] && (k == -1 || d[tour[i-1]][j] < d[tour[i-1]][k])) {
                k = j;
            }
        }
        tour[i] = k;
        used[k] = true;
    }
    return tour;
}

/**
 * Calculate K-nearest neighbors matrix from a distance matrix.
 *
 * @param d Distance matrix.
 * @return d.rows() x (d.cols - 1) matrix where element i,j is
 *         the j:th nearest neighbor of city i.
 */
Matrix<uint16_t> createNeighborsMatrix(const Matrix<uint32_t>& d, size_t K) {
    size_t N = d.rows();
    size_t M = d.cols() - 1;
    K = min(M, K);
    Matrix<uint16_t> neighbor(N, K);
    std::vector<uint16_t> row(M); // For sorting.

    for (size_t i = 0; i < N; ++i) {
        // Fill row with 0, 1, ..., i - 1, i + 1, ..., M - 1.
        uint16_t k = 0;
        for (size_t j = 0; j < M; ++j, ++k) {
            row[j] = (i == j) ? ++k : k;
        }
        // Sort K nearest row elements by distance to i.
        partial_sort(row.begin(), row.begin() + K, row.end(),
            [&](uint16_t j, uint16_t k) {
                return d[i][j] < d[i][k];
            }
        );
        // Copy first K elements (now sorted) to neighbor matrix.
        copy(row.begin(), row.begin() + K, neighbor[i]);
    }
    return neighbor;
}

/**
 * Reverse a segment of a tour.
 *
 * This functions reverses the segment [start, end] of the given
 * tour and updates the position vector accordingly.
 *
 * @param tour The input tour.
 * @param start Start index of segment to reverse.
 * @param end End index of segment to reverse.
 * @param position Vector containing positions of cities in the tour, will
 *                 be updated to reflect the reversal.
 */
void reverse(vector<uint16_t> &tour, size_t start, size_t end,
        vector<uint16_t>& position) {
    size_t N = tour.size();
    size_t numSwaps = (((start <= end ? end - start : (end + N) - start) + 1)/2);
    uint16_t i = start;
    uint16_t j = end;
    for (size_t n = 0; n < numSwaps; ++n) {
        swap(tour[i], tour[j]);
        position[tour[i]] = i;
        position[tour[j]] = j;
        i = (i + 1) % N;
        j = ((j + N) - 1) % N;
    }
}

/**
 * Returns the shortest distance d[i][j], i != j in the given distance matrix.
 *
 * @param d Distance matrix.
 * @return Minimum distance in d.
 */
uint32_t minDistance(const Matrix<uint32_t>& d) {
    size_t N = d.rows();
    uint32_t min = numeric_limits<size_t>::max();
    for (size_t i = 0; i < N; ++i) {
        for (size_t j = 0; j < N; ++j) {
            if (i != j)
                min = std::min(min, d[i][j]);
        }
    }
    return min;
}

/**
 * Returns the longest inter-city distance in the given tour.
 *
 * @param tour Input tour.
 * @param d Distance matrix.
 * @return Maximum inter-city distance in tour.
 */
uint32_t maxDistance(const vector<uint16_t>& tour, const Matrix<uint32_t>& d) {
    size_t N = tour.size();
    uint32_t max = 0;
    for (size_t i = 0, j = 1; i < N; ++i, ++j) {
        max = std::max(max, d[tour[i]][tour[j % N]]);
    }
    return max;
}

/**
 * Returns the total length of a tour.
 *
 * @param tour The input tour.
 * @param d Distance matrix.
 * @return The total length of the tour.
 */
uint64_t length(const vector<uint32_t>& tour, const Matrix<uint32_t>& d) {
    size_t N = tour.size();
    uint64_t length = 0;
    for (size_t i = 0, j = 1; i < N; ++i, ++j) {
        length += d[tour[i]][tour[j % N]];
    }
    return length;
}

/**
 * Perform a 2-opt pass on the given tour.
 *
 * This function uses the fast approach described on page 12 of "Large-Step
 * Markov Chains for the Traveling Salesman Problem" (Martin/Otto/Felten, 1991)
 *
 * @param tour The tour to optimize.
 * @param d Distance matrix.
 * @param neighbor Nearest neighbors matrix.
 * @param min Shortest possible inter-city distance.
 * @return true if the tour was improved, otherwise false.
 */
bool twoOpt(vector<uint16_t>& tour, const Matrix<uint32_t>& d,
        const Matrix<uint16_t>& neighbor, uint32_t min) {
    size_t N = d.rows();
    bool didImprove = false;

    // Initialize city tour position vector.
    vector<uint16_t> position(N);
    for (uint16_t i = 0; i < N; ++i) {
        position[tour[i]] = i; // tour[i] is the i:th city in tour.
    }

    uint16_t u, v, w, z;
    size_t u_i, v_i, w_i, z_i;
    uint32_t max = maxDistance(tour, d); // Longest link in current tour.

    for (u_i = 0, v_i = 1; u_i < N; ++u_i, ++v_i) {
        // For each edge (u, v).
        u = tour[u_i];
        v = tour[v_i % N];
        for (size_t k = 0; k < neighbor.cols(); ++k) {
            // Visit nearby edges (w, z).
            w_i = position[neighbor[u][k]];
            z_i = w_i + 1;
            w = tour[w_i]; // w is the k:th closest neighbor of u.
            z = tour[z_i % N];

            if (v == w || z == u) {
                continue; // Skip adjacent edges.
            }

            // d[u][w] + min is a lower bound on new length.
            // d[u][v] + max is an upper bound on old length.
            if (d[u][w] + min > d[u][v] + max) {
                break; // Go to next edge (u, v).
            }

            if (d[u][w] + d[v][z] < d[u][v] + d[w][z]) {
                //   --u w--        --u-w->
                //      X     ===>
                //   <-z v->        <-z-v--
                reverse(tour, v_i % N, w_i, position);

                // FIXME: Is this enough to update max?
                max = std::max(max, std::max(d[u][w], d[v][z]));

                didImprove = true;
                break;
            }
        }
    }

    return didImprove;
}

int main(int argc, char *argv[]) {
    // Create distance matrix from standard input.
    Matrix<uint32_t> d = createDistanceMatrix(cin);

    // Calculate 2-approximation tour from minimum spanning tree.
    //Matrix<uint32_t> MST = primMST(d);
    //vector<uint16_t> tour = twoApprox(MST);

    // Calculate a greedy tour.
    vector<uint16_t> tour = greedy(d);

    // Calculate nearest neighbors and smallest possible city distance.
    Matrix<uint16_t> neighbor = createNeighborsMatrix(d, TWOOPT_MAX_NEIGHBORS);
    uint32_t min = minDistance(d);

    // Optimize tour using 2-opt.
    bool didImprove;
    do {
        didImprove = twoOpt(tour, d, neighbor, min);
    } while (didImprove);

    // Print tour.
    for (auto city : tour) {
        cout << city << endl;
    }

    return 0;
}

<|endoftext|>