<commit_before>/*
 * Diameter.cpp
 *
 *  Created on: 19.02.2014
 *      Author: Daniel Hoske, Christian Staudt
 */

#include <numeric>

#include "Diameter.h"
#include "Eccentricity.h"
#include "../graph/BFS.h"
#include "../graph/Dijkstra.h"
#include "../components/ConnectedComponents.h"
#include "../structures/Partition.h"
#include "../graph/BFS.h"
#include "../auxiliary/Parallel.h"
#include <omp.h>

namespace NetworKit {

edgeweight Diameter::exactDiameter(const Graph& G) {
	using namespace std;

	Aux::SignalHandler handler;

	edgeweight diameter = 0.0;

	if (! G.isWeighted()) {
		std::tie(diameter, std::ignore) = estimatedDiameterRange(G, 0);
	} else {
		 G.forNodes([&](node v) {
			handler.assureRunning();
		 	Dijkstra dijkstra(G, v);
		 	dijkstra.run();
		 	auto distances = dijkstra.getDistances();
		 	for (auto distance : distances) {
		 		if (diameter < distance) {
		 			diameter = distance;
		 		}
		 	}
//			DEBUG("ecc(", v, "): ", *std::max_element(distances.begin(), distances.end()), " of ", distances);
		 });
	}

	if (diameter == std::numeric_limits<edgeweight>::max()) {
		throw std::runtime_error("Graph not connected - diameter is infinite");
	}
	return diameter;
}

std::pair<edgeweight, edgeweight> Diameter::estimatedDiameterRange(const NetworKit::Graph &G, double error) {
	if (G.isDirected() || G.isWeighted()) {
		throw std::runtime_error("Error, the diameter of directed or weighted graphs cannot be computed yet.");
	}

	Aux::SignalHandler handler;
	/*
	 * This is an implementation of a slightly modified version of the exactSumSweep algorithm as presented in
	 * Fast diameter and radius BFS-based computation in (weakly connected) real-world graphs: With an application to the six degrees of separation games
	 * by Michele Borassi, Pierluigi Crescenzi, Michel Habib, Walter A. Kosters, Andrea Marino, Frank W. Takes
	 * http://www.sciencedirect.com/science/article/pii/S0304397515001644
	 */

	std::vector<std::atomic<count>> sum(G.upperNodeIdBound());
	std::vector<std::atomic<count>> eccLowerBound(G.upperNodeIdBound()), eccUpperBound(G.upperNodeIdBound());
	std::vector<std::atomic<bool>> finished(G.upperNodeIdBound());

	for (node u = 0; u < G.upperNodeIdBound(); ++u) {
		if (G.hasNode(u)) {
			eccUpperBound[u] = G.numberOfNodes();
		}
	}

	count k = 4;

	ConnectedComponents comp(G);
	comp.run();
	count numberOfComponents = comp.numberOfComponents();

	std::vector<std::vector<count>> distFirst(omp_get_max_threads(), std::vector<count>(numberOfComponents, 0));
	std::vector<std::vector<count>> ecc(omp_get_max_threads(), std::vector<count>(numberOfComponents, 0));
	std::vector<std::vector<count>> distances(omp_get_max_threads(), std::vector<count>(G.upperNodeIdBound(), 0));

	auto runBFS = [&](const std::vector<node> &startNodes) {
		auto tid = omp_get_thread_num();
		std::vector<bool> foundFirstDeg2Node(numberOfComponents, false);

		auto &localDistances = distances[tid];
		auto &localEcc = ecc[tid];
		auto &localDistFirst = distFirst[tid];

		G.BFSfrom(startNodes, [&](node v, count dist) {
			localDistances[v] = dist;

			index c = comp.componentOfNode(v);
			localEcc[c] = std::max(dist, localEcc[c]);

			if (!foundFirstDeg2Node[c] && G.degree(v) > 1) {
				foundFirstDeg2Node[c] = true;
				localDistFirst[c] = dist;
			}
		});

		G.forNodes([&](node u) {
			sum[u] += localDistances[u]; // atomic operation

			if (finished[u]) return;

			auto c = comp.componentOfNode(u);

			if (localDistances[u] <= localDistFirst[c]) {
				// this is the final value, no extra care needed because of atomics
				auto eccValue = std::max(localDistances[u], localEcc[c] - localDistances[u]);
				eccUpperBound[u] = eccValue;
				eccLowerBound[u] = eccValue;
				finished[u] = true;
			} else {
				// use atomic min/max here
				Aux::Parallel::atomic_min(eccUpperBound[u], localDistances[u] + localEcc[c] - 2 * localDistFirst[c]);
				Aux::Parallel::atomic_max(eccLowerBound[u], localDistances[u]);

				if (eccUpperBound[u] == eccLowerBound[u]) {
					finished[u] = true; // this is only set to true, no problem with atomics/multi-threading here
				}
			}
		});

		localEcc.clear();
		localEcc.resize(numberOfComponents, 0);
		localDistFirst.clear();
		localDistFirst.resize(numberOfComponents, 0);
	};

	auto diameterBounds = [&]() {
		count maxExact = *std::max_element(eccLowerBound.begin(), eccLowerBound.end());
		count maxPotential = *std::max_element(eccUpperBound.begin(), eccUpperBound.end());
		return std::make_pair(maxExact, maxPotential);
	};

	std::vector<node> startNodes(numberOfComponents, 0), maxDist(numberOfComponents, 0);
	std::vector<count> minDeg(numberOfComponents, G.numberOfNodes());

	// for each component, find the node with the minimum degreee and add it as start node
	G.forNodes([&](node v) {
		count d = G.degree(v);
		count c = comp.componentOfNode(v);
		if (d < minDeg[c]) {
			startNodes[c] = v;
			minDeg[c] = d;
		}
	});

	for (index i = 0; i < k; ++i) {
		handler.assureRunning();
		runBFS(startNodes);

		G.forNodes([&](node v) {
			index c = comp.componentOfNode(v);
			if (sum[v] >= maxDist[c]) {
				maxDist[c] = sum[v];
				startNodes[c] = v;
			}
		});
	}

	handler.assureRunning();

	std::vector<count> minDist(numberOfComponents, G.numberOfNodes());
	G.forNodes([&](node u) {
		auto c = comp.componentOfNode(u);
		if (sum[u] < minDist[c]) {
			minDist[c] = sum[u];
			startNodes[c] = u;
		}
	});

	handler.assureRunning();

	runBFS(startNodes);

	count lb, ub;
	std::tie(lb, ub) = diameterBounds();

	for (index i = 0; i < G.numberOfNodes() && ub > (lb + error*lb); ++i) {
		handler.assureRunning();
		startNodes.clear();
		startNodes.resize(numberOfComponents, none);

		if ((i % 2) == 0) {
			G.forNodes([&](node u) {
				auto c = comp.componentOfNode(u);
				if (startNodes[c] == none || std::tie(eccUpperBound[u], sum[u]) > std::tie(eccUpperBound[startNodes[c]], sum[startNodes[c]])) {
					startNodes[c] = u;
				}
			});
		} else {
			G.forNodes([&](node u) {
				auto c = comp.componentOfNode(u);
				if (startNodes[c] == none || std::tie(eccLowerBound[u], sum[u]) < std::tie(eccLowerBound[startNodes[c]], sum[startNodes[c]])) {
					startNodes[c] = u;
				}
			});
		}

		handler.assureRunning();

		runBFS(startNodes);

		std::tie(lb, ub) = diameterBounds();
	}

	return {lb, ub};
}

edgeweight Diameter::estimatedVertexDiameter(const Graph& G, count samples) {

	edgeweight infDist = std::numeric_limits<edgeweight>::max();

	// TODO: consider weights

	auto estimateFrom = [&](node v) -> count {
		BFS bfs(G, v);
		bfs.run();
		auto distances = bfs.getDistances();

		// get two largest path lengths
		edgeweight maxD = 0;
		edgeweight maxD2 = 0; // second largest distance
		for (auto d : distances) {
			if ((d != infDist) && (d >= maxD)) {
				maxD2 = maxD;
				maxD = d;
			}
		}

		edgeweight dMax = maxD + maxD2;
		count vd = (count) dMax + 1; 	// count the nodes, not the edges
		return vd;
	};

	edgeweight vdMax = 0;
	#pragma omp parallel for
	for (count i = 0; i < samples; ++i) {
		node u = G.randomNode();
		edgeweight vd = estimateFrom(u);
		DEBUG("sampled vertex diameter from node ", u, ": ", vd);
		#pragma omp critical
		{
			if (vd > vdMax) {
				vdMax = vd;
			}
		}
	}

	return vdMax;

}

edgeweight Diameter::estimatedVertexDiameterPedantic(const Graph& G) {
	count vd = 0;
	if (!G.isWeighted()) {
		std::vector<bool> visited(G.upperNodeIdBound(), false);
		// perform breadth-first searches
		G.forNodes([&](node u) {
			if (visited[u] == false) {
				count maxDist = 0, maxDist2 = 0;
				G.BFSfrom(u, [&](node v, count dist) {
					visited[v] = true;
					if (dist > maxDist) {
						maxDist2 = maxDist;
						maxDist = dist;
					}
					else if (dist > maxDist2) {
						maxDist2 = dist;
					}
				});
				if (maxDist + maxDist2 > vd) {
					vd = maxDist + maxDist2;
				}
				assert (visited[u] == true);
			}
		});
		vd ++; //we need the number of nodes, not the number of edges
	}
	else {
		ConnectedComponents cc(G);
		DEBUG("finding connected components");
		cc.run();
		INFO("Number of components ", cc.numberOfComponents());
		DEBUG("Estimating size of the largest component");
		std::map<count, count> sizes = cc.getComponentSizes();
		count largest_comp_size = 0;
		for(auto it = sizes.cbegin(); it != sizes.cend(); ++it) {
			DEBUG(it->second);
			if (it->second > largest_comp_size) {
				largest_comp_size = it->second;
			}
		}
		INFO("Largest component size: ", largest_comp_size);
		vd = largest_comp_size;
	}
	return vd;
}

} /* namespace NetworKit */
<commit_msg>Diameter: use max instead of min deg for start<commit_after>/*
 * Diameter.cpp
 *
 *  Created on: 19.02.2014
 *      Author: Daniel Hoske, Christian Staudt
 */

#include <numeric>

#include "Diameter.h"
#include "Eccentricity.h"
#include "../graph/BFS.h"
#include "../graph/Dijkstra.h"
#include "../components/ConnectedComponents.h"
#include "../structures/Partition.h"
#include "../graph/BFS.h"
#include "../auxiliary/Parallel.h"
#include <omp.h>

namespace NetworKit {

edgeweight Diameter::exactDiameter(const Graph& G) {
	using namespace std;

	Aux::SignalHandler handler;

	edgeweight diameter = 0.0;

	if (! G.isWeighted()) {
		std::tie(diameter, std::ignore) = estimatedDiameterRange(G, 0);
	} else {
		 G.forNodes([&](node v) {
			handler.assureRunning();
		 	Dijkstra dijkstra(G, v);
		 	dijkstra.run();
		 	auto distances = dijkstra.getDistances();
		 	for (auto distance : distances) {
		 		if (diameter < distance) {
		 			diameter = distance;
		 		}
		 	}
//			DEBUG("ecc(", v, "): ", *std::max_element(distances.begin(), distances.end()), " of ", distances);
		 });
	}

	if (diameter == std::numeric_limits<edgeweight>::max()) {
		throw std::runtime_error("Graph not connected - diameter is infinite");
	}
	return diameter;
}

std::pair<edgeweight, edgeweight> Diameter::estimatedDiameterRange(const NetworKit::Graph &G, double error) {
	if (G.isDirected() || G.isWeighted()) {
		throw std::runtime_error("Error, the diameter of directed or weighted graphs cannot be computed yet.");
	}

	Aux::SignalHandler handler;
	/*
	 * This is an implementation of a slightly modified version of the exactSumSweep algorithm as presented in
	 * Fast diameter and radius BFS-based computation in (weakly connected) real-world graphs: With an application to the six degrees of separation games
	 * by Michele Borassi, Pierluigi Crescenzi, Michel Habib, Walter A. Kosters, Andrea Marino, Frank W. Takes
	 * http://www.sciencedirect.com/science/article/pii/S0304397515001644
	 */

	std::vector<std::atomic<count>> sum(G.upperNodeIdBound());
	std::vector<std::atomic<count>> eccLowerBound(G.upperNodeIdBound()), eccUpperBound(G.upperNodeIdBound());
	std::vector<std::atomic<bool>> finished(G.upperNodeIdBound());

	for (node u = 0; u < G.upperNodeIdBound(); ++u) {
		if (G.hasNode(u)) {
			eccUpperBound[u] = G.numberOfNodes();
		}
	}

	count k = 4;

	ConnectedComponents comp(G);
	comp.run();
	count numberOfComponents = comp.numberOfComponents();

	std::vector<std::vector<count>> distFirst(omp_get_max_threads(), std::vector<count>(numberOfComponents, 0));
	std::vector<std::vector<count>> ecc(omp_get_max_threads(), std::vector<count>(numberOfComponents, 0));
	std::vector<std::vector<count>> distances(omp_get_max_threads(), std::vector<count>(G.upperNodeIdBound(), 0));

	auto runBFS = [&](const std::vector<node> &startNodes) {
		auto tid = omp_get_thread_num();
		std::vector<bool> foundFirstDeg2Node(numberOfComponents, false);

		auto &localDistances = distances[tid];
		auto &localEcc = ecc[tid];
		auto &localDistFirst = distFirst[tid];

		G.BFSfrom(startNodes, [&](node v, count dist) {
			localDistances[v] = dist;

			index c = comp.componentOfNode(v);
			localEcc[c] = std::max(dist, localEcc[c]);

			if (!foundFirstDeg2Node[c] && G.degree(v) > 1) {
				foundFirstDeg2Node[c] = true;
				localDistFirst[c] = dist;
			}
		});

		G.forNodes([&](node u) {
			sum[u] += localDistances[u]; // atomic operation

			if (finished[u]) return;

			auto c = comp.componentOfNode(u);

			if (localDistances[u] <= localDistFirst[c]) {
				// this is the final value, no extra care needed because of atomics
				auto eccValue = std::max(localDistances[u], localEcc[c] - localDistances[u]);
				eccUpperBound[u] = eccValue;
				eccLowerBound[u] = eccValue;
				finished[u] = true;
			} else {
				// use atomic min/max here
				Aux::Parallel::atomic_min(eccUpperBound[u], localDistances[u] + localEcc[c] - 2 * localDistFirst[c]);
				Aux::Parallel::atomic_max(eccLowerBound[u], localDistances[u]);

				if (eccUpperBound[u] == eccLowerBound[u]) {
					finished[u] = true; // this is only set to true, no problem with atomics/multi-threading here
				}
			}
		});

		localEcc.clear();
		localEcc.resize(numberOfComponents, 0);
		localDistFirst.clear();
		localDistFirst.resize(numberOfComponents, 0);
	};

	auto diameterBounds = [&]() {
		count maxExact = *std::max_element(eccLowerBound.begin(), eccLowerBound.end());
		count maxPotential = *std::max_element(eccUpperBound.begin(), eccUpperBound.end());
		return std::make_pair(maxExact, maxPotential);
	};

	std::vector<node> startNodes(numberOfComponents, 0), maxDist(numberOfComponents, 0);
	std::vector<count> maxDeg(numberOfComponents, 0);

	// for each component, find the node with the minimum degreee and add it as start node
	G.forNodes([&](node v) {
		count d = G.degree(v);
		count c = comp.componentOfNode(v);
		if (d >= maxDeg[c]) {
			startNodes[c] = v;
			maxDeg[c] = d;
		}
	});

	for (index i = 0; i < k; ++i) {
		handler.assureRunning();
		runBFS(startNodes);

		G.forNodes([&](node v) {
			index c = comp.componentOfNode(v);
			if (sum[v] >= maxDist[c]) {
				maxDist[c] = sum[v];
				startNodes[c] = v;
			}
		});
	}

	handler.assureRunning();

	std::vector<count> minDist(numberOfComponents, G.numberOfNodes());
	G.forNodes([&](node u) {
		auto c = comp.componentOfNode(u);
		if (sum[u] < minDist[c]) {
			minDist[c] = sum[u];
			startNodes[c] = u;
		}
	});

	handler.assureRunning();

	runBFS(startNodes);

	count lb, ub;
	std::tie(lb, ub) = diameterBounds();

	for (index i = 0; i < G.numberOfNodes() && ub > (lb + error*lb); ++i) {
		handler.assureRunning();
		startNodes.clear();
		startNodes.resize(numberOfComponents, none);

		if ((i % 2) == 0) {
			G.forNodes([&](node u) {
				auto c = comp.componentOfNode(u);
				if (startNodes[c] == none || std::tie(eccUpperBound[u], sum[u]) > std::tie(eccUpperBound[startNodes[c]], sum[startNodes[c]])) {
					startNodes[c] = u;
				}
			});
		} else {
			G.forNodes([&](node u) {
				auto c = comp.componentOfNode(u);
				if (startNodes[c] == none || std::tie(eccLowerBound[u], sum[u]) < std::tie(eccLowerBound[startNodes[c]], sum[startNodes[c]])) {
					startNodes[c] = u;
				}
			});
		}

		handler.assureRunning();

		runBFS(startNodes);

		std::tie(lb, ub) = diameterBounds();
	}

	return {lb, ub};
}

edgeweight Diameter::estimatedVertexDiameter(const Graph& G, count samples) {

	edgeweight infDist = std::numeric_limits<edgeweight>::max();

	// TODO: consider weights

	auto estimateFrom = [&](node v) -> count {
		BFS bfs(G, v);
		bfs.run();
		auto distances = bfs.getDistances();

		// get two largest path lengths
		edgeweight maxD = 0;
		edgeweight maxD2 = 0; // second largest distance
		for (auto d : distances) {
			if ((d != infDist) && (d >= maxD)) {
				maxD2 = maxD;
				maxD = d;
			}
		}

		edgeweight dMax = maxD + maxD2;
		count vd = (count) dMax + 1; 	// count the nodes, not the edges
		return vd;
	};

	edgeweight vdMax = 0;
	#pragma omp parallel for
	for (count i = 0; i < samples; ++i) {
		node u = G.randomNode();
		edgeweight vd = estimateFrom(u);
		DEBUG("sampled vertex diameter from node ", u, ": ", vd);
		#pragma omp critical
		{
			if (vd > vdMax) {
				vdMax = vd;
			}
		}
	}

	return vdMax;

}

edgeweight Diameter::estimatedVertexDiameterPedantic(const Graph& G) {
	count vd = 0;
	if (!G.isWeighted()) {
		std::vector<bool> visited(G.upperNodeIdBound(), false);
		// perform breadth-first searches
		G.forNodes([&](node u) {
			if (visited[u] == false) {
				count maxDist = 0, maxDist2 = 0;
				G.BFSfrom(u, [&](node v, count dist) {
					visited[v] = true;
					if (dist > maxDist) {
						maxDist2 = maxDist;
						maxDist = dist;
					}
					else if (dist > maxDist2) {
						maxDist2 = dist;
					}
				});
				if (maxDist + maxDist2 > vd) {
					vd = maxDist + maxDist2;
				}
				assert (visited[u] == true);
			}
		});
		vd ++; //we need the number of nodes, not the number of edges
	}
	else {
		ConnectedComponents cc(G);
		DEBUG("finding connected components");
		cc.run();
		INFO("Number of components ", cc.numberOfComponents());
		DEBUG("Estimating size of the largest component");
		std::map<count, count> sizes = cc.getComponentSizes();
		count largest_comp_size = 0;
		for(auto it = sizes.cbegin(); it != sizes.cend(); ++it) {
			DEBUG(it->second);
			if (it->second > largest_comp_size) {
				largest_comp_size = it->second;
			}
		}
		INFO("Largest component size: ", largest_comp_size);
		vd = largest_comp_size;
	}
	return vd;
}

} /* namespace NetworKit */
<|endoftext|>


<commit_before>/**
@file FailureScene.cpp
*/
#include "Scene.h"

namespace SunnySideUp {

  using namespace Mai;

  /** Show the failure event.
  */
  class FailureScene : public Scene {
  public:
	FailureScene();
	virtual ~FailureScene() {}
	virtual int Update(Engine&, float);
	virtual bool Load(Engine&);
	virtual bool Unload(Engine&);
	virtual void Draw(Engine&);
	virtual void ProcessWindowEvent(Engine&, const Event&);

  private:
	static const int eventTime = 30;
	static const int enableInputTime = eventTime - 5;

	int DoUpdate(Engine&, float);
	int DoFadeOut(Engine&, float);
	int(FailureScene::*updateFunc)(Engine&, float);

	std::vector<ObjectPtr> objList;
	bool loaded;
	bool hasFinishRequest;
	float timer;
	float cameraRotation;
  };

  FailureScene::FailureScene()
	: updateFunc(&FailureScene::DoUpdate)
	, objList()
	, loaded(false)
	, hasFinishRequest(false)
	, timer(static_cast<float>(eventTime))
	, cameraRotation(0)
  {}

  bool FailureScene::Load(Engine& engine) {
	if (loaded) {
	  status = STATUSCODE_RUNNABLE;
	  return true;
	}

	objList.reserve(8);
	Renderer& r = engine.GetRenderer();
	const Vector3F shadowDir = GetSunRayDirection(r.GetTimeOfScene());
	r.SetShadowLight(Position3F(0, 0, 0) - shadowDir * 200.0f, shadowDir, 100, 300, Vector2F(3, 3 * 4));
	r.DoesDrawSkybox(false);

	{
	  auto obj = r.CreateObject("landscape", Material(Color4B(255, 255, 255, 255), 0, 0), "solidmodel", ShadowCapability::Disable);
	  obj->SetScale(Vector3F(12, 1, 12));
	  obj->SetTranslation(Vector3F(0, -3, 0));
	  objList.push_back(obj);
	}
	{ // for shadow.
	  auto obj = r.CreateObject("ground", Material(Color4B(255, 255, 255, 255), 0, 0), "default", ShadowCapability::ShadowOnly);
	  obj->SetTranslation(Vector3F(0, -3, 0));
	  obj->SetScale(Vector3F(0.01f, 0.01f, 0.01f));
	  obj->SetRotation(degreeToRadian(-90.0f), 0, 0);
	  objList.push_back(obj);
	}

	{
	  auto obj0 = r.CreateObject("BrokenEgg", Material(Color4B(255, 255, 255, 255), 0, 0), "default");
	  Object& o0 = *obj0;
	  o0.SetScale(Vector3F(3, 3, 3));
	  o0.SetTranslation(Vector3F(0, 0.75f, 0));
	  objList.push_back(obj0);

	  auto obj2 = r.CreateObject("EggYolk", Material(Color4B(255, 255, 255, 255), -0.05f, 0), "default");
	  Object& o2 = *obj2;
	  o2.SetScale(Vector3F(3, 3, 3));
	  o2.SetTranslation(Vector3F(0, 0.75f, 0));
	  objList.push_back(obj2);

	  auto obj1 = r.CreateObject("EggWhite", Material(Color4B(255, 255, 255, 96), -0.25f, 0), "defaultWithAlpha");
	  Object& o1 = *obj1;
	  o1.SetScale(Vector3F(3, 3, 3));
	  o1.SetTranslation(Vector3F(0, 1, 0));
	  objList.push_back(obj1);
	}

	loaded = true;
	status = STATUSCODE_RUNNABLE;
	return true;
  }

  bool FailureScene::Unload(Engine&) {
	if (loaded) {
	  objList.clear();
	  loaded = false;
	}
	status = STATUSCODE_STOPPED;
	return true;
  }

  void FailureScene::ProcessWindowEvent(Engine&, const Event& e) {
	switch (e.Type) {
	case Event::EVENT_MOUSE_BUTTON_PRESSED:
	  if ((timer <= enableInputTime) && (e.MouseButton.Button == MOUSEBUTTON_LEFT)) {
		hasFinishRequest = true;
	  }
	  break;
	default:
	  /* DO NOTING */
	  break;
	}
  }

  int FailureScene::Update(Engine& engine, float tick) {
	for (auto e : objList) {
	  e->Update(tick);
	}

	const float theta = degreeToRadian<float>(cameraRotation);
	const float distance = 20;
	const float x = std::cos(theta) * distance;
	const float z = std::sin(theta) * distance;
	const Position3F eyePos(x, 20, z);
	const Vector3F dir = objList[2]->Position() - eyePos;
	engine.GetRenderer().Update(tick, eyePos, dir, Vector3F(0, 1, 0));

	cameraRotation += tick * 10.0f;
	while (cameraRotation >= 360.0f) {
	  cameraRotation -= 360.0f;
	}

	return (this->*updateFunc)(engine, tick);
  }

  /** Wait user input.

  Transition to DoFadeOut() when receive user input or time out.
  This is the update function called from Update().
  */
  int FailureScene::DoUpdate(Engine& engine, float tick) {
	timer -= tick;
	if ((timer <= 0.0f) || hasFinishRequest) {
	  engine.GetRenderer().FadeOut(Color4B(0, 0, 0, 0), 1.0f);
	  updateFunc = &FailureScene::DoFadeOut;
	}
	return SCENEID_CONTINUE;
  }

  /** Do fade out.

  Transition to the scene of the success/failure event when the fadeout finished.
  This is the update function called from Update().
  */
  int FailureScene::DoFadeOut(Engine& engine, float deltaTime) {
	Renderer& r = engine.GetRenderer();
	if (r.GetCurrentFilterMode() == Renderer::FILTERMODE_NONE) {
	  r.FadeIn(1.0f);
	  return SCENEID_MAINGAME;
	}
	return SCENEID_CONTINUE;
  }

  void FailureScene::Draw(Engine& engine) {
	Renderer& r = engine.GetRenderer();
	const char str[] = "IT'S EDIBLE...";
	float scale = 1.0f;
	const float w = r.GetStringWidth(str) * scale;
	r.AddString(0.51f - w * 0.5f, 0.26f, scale, Color4B(20, 10, 10, 128), str);
	r.AddString(0.5f - w * 0.5f, 0.25f, scale, Color4B(200, 200, 200, 255), str);
	r.Render(&objList[0], &objList[0] + objList.size());
  }

  ScenePtr CreateFailureScene(Engine&) {
	return ScenePtr(new FailureScene());
  }

} // namespace SunnySideUp<commit_msg>Use the material parameter of the mesh data.<commit_after>/**
@file FailureScene.cpp
*/
#include "Scene.h"

namespace SunnySideUp {

  using namespace Mai;

  /** Show the failure event.
  */
  class FailureScene : public Scene {
  public:
	FailureScene();
	virtual ~FailureScene() {}
	virtual int Update(Engine&, float);
	virtual bool Load(Engine&);
	virtual bool Unload(Engine&);
	virtual void Draw(Engine&);
	virtual void ProcessWindowEvent(Engine&, const Event&);

  private:
	static const int eventTime = 30;
	static const int enableInputTime = eventTime - 5;

	int DoUpdate(Engine&, float);
	int DoFadeOut(Engine&, float);
	int(FailureScene::*updateFunc)(Engine&, float);

	std::vector<ObjectPtr> objList;
	bool loaded;
	bool hasFinishRequest;
	float timer;
	float cameraRotation;
  };

  FailureScene::FailureScene()
	: updateFunc(&FailureScene::DoUpdate)
	, objList()
	, loaded(false)
	, hasFinishRequest(false)
	, timer(static_cast<float>(eventTime))
	, cameraRotation(0)
  {}

  bool FailureScene::Load(Engine& engine) {
	if (loaded) {
	  status = STATUSCODE_RUNNABLE;
	  return true;
	}

	objList.reserve(8);
	Renderer& r = engine.GetRenderer();
	const Vector3F shadowDir = GetSunRayDirection(r.GetTimeOfScene());
	r.SetShadowLight(Position3F(0, 0, 0) - shadowDir * 200.0f, shadowDir, 100, 300, Vector2F(3, 3 * 4));
	r.DoesDrawSkybox(false);

	{
	  auto obj = r.CreateObject("landscape", Material(Color4B(255, 255, 255, 255), 0, 0), "solidmodel", ShadowCapability::Disable);
	  obj->SetScale(Vector3F(12, 1, 12));
	  obj->SetTranslation(Vector3F(0, -3, 0));
	  objList.push_back(obj);
	}
	{ // for shadow.
	  auto obj = r.CreateObject("ground", Material(Color4B(255, 255, 255, 255), 0, 0), "default", ShadowCapability::ShadowOnly);
	  obj->SetTranslation(Vector3F(0, -3, 0));
	  obj->SetScale(Vector3F(0.01f, 0.01f, 0.01f));
	  obj->SetRotation(degreeToRadian(-90.0f), 0, 0);
	  objList.push_back(obj);
	}

	{
	  auto obj0 = r.CreateObject("BrokenEgg", Material(Color4B(255, 255, 255, 255), 0, 0), "default");
	  Object& o0 = *obj0;
	  o0.SetScale(Vector3F(3, 3, 3));
	  o0.SetTranslation(Vector3F(0, 0.75f, 0));
	  objList.push_back(obj0);

	  auto obj2 = r.CreateObject("EggYolk", Material(Color4B(255, 255, 255, 255), 0, 0), "default");
	  Object& o2 = *obj2;
	  o2.SetScale(Vector3F(3, 3, 3));
	  o2.SetTranslation(Vector3F(0, 0.75f, 0));
	  objList.push_back(obj2);

	  auto obj1 = r.CreateObject("EggWhite", Material(Color4B(255, 255, 255, 96), 0, 0), "defaultWithAlpha");
	  Object& o1 = *obj1;
	  o1.SetScale(Vector3F(3, 3, 3));
	  o1.SetTranslation(Vector3F(0, 1, 0));
	  objList.push_back(obj1);
	}

	loaded = true;
	status = STATUSCODE_RUNNABLE;
	return true;
  }

  bool FailureScene::Unload(Engine&) {
	if (loaded) {
	  objList.clear();
	  loaded = false;
	}
	status = STATUSCODE_STOPPED;
	return true;
  }

  void FailureScene::ProcessWindowEvent(Engine&, const Event& e) {
	switch (e.Type) {
	case Event::EVENT_MOUSE_BUTTON_PRESSED:
	  if ((timer <= enableInputTime) && (e.MouseButton.Button == MOUSEBUTTON_LEFT)) {
		hasFinishRequest = true;
	  }
	  break;
	default:
	  /* DO NOTING */
	  break;
	}
  }

  int FailureScene::Update(Engine& engine, float tick) {
	for (auto e : objList) {
	  e->Update(tick);
	}

	const float theta = degreeToRadian<float>(cameraRotation);
	const float distance = 20;
	const float x = std::cos(theta) * distance;
	const float z = std::sin(theta) * distance;
	const Position3F eyePos(x, 20, z);
	const Vector3F dir = objList[2]->Position() - eyePos;
	engine.GetRenderer().Update(tick, eyePos, dir, Vector3F(0, 1, 0));

	cameraRotation += tick * 10.0f;
	while (cameraRotation >= 360.0f) {
	  cameraRotation -= 360.0f;
	}

	return (this->*updateFunc)(engine, tick);
  }

  /** Wait user input.

  Transition to DoFadeOut() when receive user input or time out.
  This is the update function called from Update().
  */
  int FailureScene::DoUpdate(Engine& engine, float tick) {
	timer -= tick;
	if ((timer <= 0.0f) || hasFinishRequest) {
	  engine.GetRenderer().FadeOut(Color4B(0, 0, 0, 0), 1.0f);
	  updateFunc = &FailureScene::DoFadeOut;
	}
	return SCENEID_CONTINUE;
  }

  /** Do fade out.

  Transition to the scene of the success/failure event when the fadeout finished.
  This is the update function called from Update().
  */
  int FailureScene::DoFadeOut(Engine& engine, float deltaTime) {
	Renderer& r = engine.GetRenderer();
	if (r.GetCurrentFilterMode() == Renderer::FILTERMODE_NONE) {
	  r.FadeIn(1.0f);
	  return SCENEID_MAINGAME;
	}
	return SCENEID_CONTINUE;
  }

  void FailureScene::Draw(Engine& engine) {
	Renderer& r = engine.GetRenderer();
	const char str[] = "IT'S EDIBLE...";
	float scale = 1.0f;
	const float w = r.GetStringWidth(str) * scale;
	r.AddString(0.51f - w * 0.5f, 0.26f, scale, Color4B(20, 10, 10, 128), str);
	r.AddString(0.5f - w * 0.5f, 0.25f, scale, Color4B(200, 200, 200, 255), str);
	r.Render(&objList[0], &objList[0] + objList.size());
  }

  ScenePtr CreateFailureScene(Engine&) {
	return ScenePtr(new FailureScene());
  }

} // namespace SunnySideUp<|endoftext|>


<commit_before>#include "robot_interface/arm_ctrl.h"
#include <pthread.h>

using namespace std;
using namespace geometry_msgs;
using namespace baxter_core_msgs;

ArmCtrl::ArmCtrl(string _name, string _limb, bool _no_robot) :
                 RobotInterface(_name,_limb, _no_robot), Gripper(_limb, _no_robot),
                 object(-1), action(""), sub_state("")
{
    std::string topic = "/"+getName()+"/state_"+_limb;
    state_pub = _n.advertise<baxter_collaboration::ArmState>(topic,1);
    ROS_INFO("[%s] Created state publisher with name : %s", getLimb().c_str(), topic.c_str());

    std::string other_limb = getLimb() == "right" ? "left" : "right";

    topic = "/"+getName()+"/service_"+_limb;
    service = _n.advertiseService(topic, &ArmCtrl::serviceCb, this);
    ROS_INFO("[%s] Created service server with name  : %s", getLimb().c_str(), topic.c_str());

    topic = "/"+getName()+"/service_"+_limb+"_to_"+other_limb;
    service_other_limb = _n.advertiseService(topic, &ArmCtrl::serviceOtherLimbCb,this);
    ROS_INFO("[%s] Created service server with name  : %s", getLimb().c_str(), topic.c_str());

    insertAction(ACTION_HOME,    &ArmCtrl::goHome);
    insertAction(ACTION_RELEASE, &ArmCtrl::releaseObject);

    // insertAction("recover_"+string(ACTION_HOME),    &ArmCtrl::notImplemented);
    // insertAction("recover_"+string(ACTION_RELEASE), &ArmCtrl::notImplemented);

    _n.param<bool>("internal_recovery",  internal_recovery, true);
    ROS_INFO("[%s] Internal_recovery flag set to %s", getLimb().c_str(),
                                internal_recovery==true?"true":"false");
}

void ArmCtrl::InternalThreadEntry()
{
    pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS,NULL);
    _n.param<bool>("internal_recovery",  internal_recovery, true);

    std::string a =     getAction();
    int         s = int(getState());

    setState(WORKING);

    if (a == ACTION_HOME || a == ACTION_RELEASE)
    {
        if (callAction(a))   setState(DONE);
    }
    else if (s == START || s == ERROR ||
             s == DONE  || s == KILLED )
    {
        if (doAction(s, a))   setState(DONE);
        else                  setState(ERROR);
    }
    else
    {
        ROS_ERROR("[%s] Invalid Action %s in state %i", getLimb().c_str(), a.c_str(), s);
    }

    if (getState()==WORKING)
    {
        setState(ERROR);
    }

    if (getState()==ERROR)
    {
        ROS_ERROR("[%s] Action %s not successful! State %s %s", getLimb().c_str(), a.c_str(),
                                          string(getState()).c_str(), getSubState().c_str());
    }

    closeInternalThread();
    return;
}

bool ArmCtrl::serviceOtherLimbCb(baxter_collaboration::AskFeedback::Request  &req,
                                 baxter_collaboration::AskFeedback::Response &res)
{
    res.success = false;
    res.reply   = "not implemented";
    return true;
}

bool ArmCtrl::serviceCb(baxter_collaboration::DoAction::Request  &req,
                        baxter_collaboration::DoAction::Response &res)
{
    string action = req.action;
    int    obj    = req.object;

    ROS_INFO("[%s] Service request received. Action: %s object: %i", getLimb().c_str(),
                                                                   action.c_str(), obj);

    if (action == PROT_ACTION_LIST)
    {
        printDB();
        res.success  = true;
        res.response = DBToString();
        return true;
    }

    if (is_no_robot())
    {
        ros::Duration(2.0).sleep();
        res.success = true;
        return true;
    }

    res.success = false;

    setAction(action);
    setObject(obj);

    startInternalThread();
    ros::Duration(0.5).sleep();

    ros::Rate r(100);
    while( ros::ok() && ( int(getState()) != START   &&
                          int(getState()) != ERROR   &&
                          int(getState()) != DONE    &&
                          int(getState()) != PICK_UP   ))
    {
        if (ros::isShuttingDown())
        {
            setState(KILLED);
            return true;
        }

        if (getState()==KILLED)
        {
            recoverFromError();
        }

        r.sleep();
    }

    if ( int(getState()) == START   ||
         int(getState()) == DONE    ||
         int(getState()) == PICK_UP   )
    {
        res.success = true;
    }

    ROS_INFO("[%s] Service reply with success: %s\n", getLimb().c_str(),
                                            res.success?"true":"false");
    return true;
}

bool ArmCtrl::notImplemented()
{
    ROS_ERROR("[%s] Action not implemented!", getLimb().c_str());
    return false;
}

bool ArmCtrl::insertAction(const std::string &a, ArmCtrl::f_action f)
{
    if (a == PROT_ACTION_LIST)
    {
        ROS_ERROR("[%s][action_db] Attempted to insert protected action key: %s",
                 getLimb().c_str(), a.c_str());
        return false;
    }

    if (isActionInDB(a, true)) // The action is in the db
    {
        ROS_WARN("[%s][action_db] Overwriting existing action with key %s",
                 getLimb().c_str(), a.c_str());
    }

    action_db.insert( std::make_pair( a, f ));
    return true;
}

bool ArmCtrl::removeAction(const std::string &a)
{
    if (isActionInDB(a)) // The action is in the db
    {
        action_db.erase(a);
        return true;
    }

    return false;
}

bool ArmCtrl::callAction(const std::string &a)
{
    if (isActionInDB(a)) // The action is in the db
    {
        f_action act = action_db[a];
        return (this->*act)();
    }

    return false;
}

bool ArmCtrl::doAction(int s, std::string a)
{
    if (isActionInDB(a))
    {
        if (callAction(a))         return true;
        else                recoverFromError();
    }
    else
    {
        ROS_ERROR("[%s] Invalid Action %s in state %i", getLimb().c_str(), a.c_str(), s);
    }

    return false;
}

bool ArmCtrl::isActionInDB(const std::string &a, bool insertAction)
{
    if (action_db.find(a) != action_db.end()) return true;

    if (!insertAction)
    {
        ROS_ERROR("[%s][action_db] Action %s is not in the database!",
                  getLimb().c_str(), a.c_str());
    }
    return false;
}

void ArmCtrl::printDB()
{
    ROS_INFO("[%s] Available actions in the database : %s",
              getLimb().c_str(), DBToString().c_str());
}

string ArmCtrl::DBToString()
{
    string res = "";
    map<string, f_action>::iterator it;

    for ( it = action_db.begin(); it != action_db.end(); it++ )
    {
        res = res + it->first + ", ";
    }
    res = res.substr(0, res.size()-2); // Remove the last ", "
    return res;
}

bool ArmCtrl::moveArm(string dir, double dist, string mode, bool disable_coll_av)
{
    Point start = getPos();
    Point final = getPos();

    Quaternion ori = getOri();

    if      (dir == "backward") final.x -= dist;
    else if (dir == "forward")  final.x += dist;
    else if (dir == "right")    final.y -= dist;
    else if (dir == "left")     final.y += dist;
    else if (dir == "down")     final.z -= dist;
    else if (dir == "up")       final.z += dist;
    else                               return false;

    ros::Time t_start = ros::Time::now();

    bool finish = false;

    ros::Rate r(100);
    while(RobotInterface::ok())
    {
        if (disable_coll_av)    suppressCollisionAv();

        double t_elap = (ros::Time::now() - t_start).toSec();

        double px = start.x;
        double py = start.y;
        double pz = start.z;

        if (!finish)
        {
            if (dir == "backward" | dir == "forward")
            {
                int sgn = dir=="backward"?-1:+1;
                px = px + sgn * PICK_UP_SPEED * t_elap;

                if (dir == "backward")
                {
                    if (px < final.x) finish = true;
                }
                else if (dir == "forward")
                {
                    if (px > final.x) finish = true;
                }
            }
            if (dir == "right" | dir == "left")
            {
                int sgn = dir=="right"?-1:+1;
                py = py + sgn * PICK_UP_SPEED * t_elap;

                if (dir == "right")
                {
                    if (py < final.y) finish = true;
                }
                else if (dir == "left")
                {
                    if (py > final.y) finish = true;
                }
            }
            if (dir == "down" | dir == "up")
            {
                int sgn = dir=="down"?-1:+1;
                pz = pz + sgn * PICK_UP_SPEED * t_elap;

                if (dir == "down")
                {
                    if (pz < final.z) finish = true;
                }
                else if (dir == "up")
                {
                    if (pz > final.z) finish = true;
                }
            }
        }
        else
        {
            px = final.x;
            py = final.y;
            pz = final.z;
        }

        double ox = ori.x;
        double oy = ori.y;
        double oz = ori.z;
        double ow = ori.w;

        vector<double> joint_angles;
        if (!computeIK(px, py, pz, ox, oy, oz, ow, joint_angles)) return false;

        if (!goToPoseNoCheck(joint_angles))   return false;

        if (isPositionReached(final.x, final.y, final.z, mode)) return true;

        r.sleep();
    }

    return false;
}

bool ArmCtrl::hoverAboveTable(double height, string mode, bool disable_coll_av)
{
    if (getLimb() == "right")
    {
        return RobotInterface::goToPose(HOME_POS_R, height, VERTICAL_ORI_R,
                                                     mode, disable_coll_av);
    }
    else if (getLimb() == "left")
    {
        return RobotInterface::goToPose(HOME_POS_L, height, VERTICAL_ORI_L,
                                                     mode, disable_coll_av);
    }
    else return false;
}

bool ArmCtrl::homePoseStrict(bool disable_coll_av)
{
    ROS_INFO("[%s] Going to home position strict..", getLimb().c_str());

    ros::Rate r(100);
    while(ros::ok())
    {
        if (disable_coll_av)    suppressCollisionAv();

        JointCommand joint_cmd;
        joint_cmd.mode = JointCommand::POSITION_MODE;
        setJointNames(joint_cmd);

        joint_cmd.command = home_conf.command;

        publish_joint_cmd(joint_cmd);

        r.sleep();

        if(isConfigurationReached(joint_cmd))
        {
            return true;
        }
    }
    ROS_INFO("[%s] Done", getLimb().c_str());
}

void ArmCtrl::setHomeConf(double s0, double s1, double e0, double e1,
                                     double w0, double w1, double w2)
{
    setJointCommands( s0, s1, e0, e1, w0, w1, w2, home_conf);
}

bool ArmCtrl::goHome()
{
    bool res = homePoseStrict();
    releaseObject();
    return res;
}

void ArmCtrl::recoverFromError()
{
    if (internal_recovery == true)
    {
        goHome();
    }
}

void ArmCtrl::setState(int _state)
{
    if (_state == KILLED && getState() != WORKING)
    {
        ROS_WARN("[%s] Attempted to kill a non-working controller", getLimb().c_str());
        return;
    }

    RobotInterface::setState(_state);

    // if (_state == DONE)
    // {
    //     setAction("");
    //     setMarkerID(-1);
    // }
    if (_state == DONE)
    {
        setSubState(getAction());
    }
    publishState();
}

void ArmCtrl::setAction(string _action)
{
    action = _action;
    publishState();
}

void ArmCtrl::publishState()
{
    baxter_collaboration::ArmState msg;

    msg.state  = string(getState());
    msg.action = getAction();
    msg.object = getObjName();

    state_pub.publish(msg);
}

string ArmCtrl::getObjName()
{
    if      (object == 17) return "left leg";
    else if (object == 21) return "top";
    else if (object == 24) return "central frame";
    else if (object == 26) return "right leg";
    else return "";
}

ArmCtrl::~ArmCtrl()
{
    killInternalThread();
}
<commit_msg>Better information in the baxter display<commit_after>#include "robot_interface/arm_ctrl.h"
#include <pthread.h>

using namespace std;
using namespace geometry_msgs;
using namespace baxter_core_msgs;

ArmCtrl::ArmCtrl(string _name, string _limb, bool _no_robot) :
                 RobotInterface(_name,_limb, _no_robot), Gripper(_limb, _no_robot),
                 object(-1), action(""), sub_state("")
{
    std::string topic = "/"+getName()+"/state_"+_limb;
    state_pub = _n.advertise<baxter_collaboration::ArmState>(topic,1);
    ROS_INFO("[%s] Created state publisher with name : %s", getLimb().c_str(), topic.c_str());

    std::string other_limb = getLimb() == "right" ? "left" : "right";

    topic = "/"+getName()+"/service_"+_limb;
    service = _n.advertiseService(topic, &ArmCtrl::serviceCb, this);
    ROS_INFO("[%s] Created service server with name  : %s", getLimb().c_str(), topic.c_str());

    topic = "/"+getName()+"/service_"+_limb+"_to_"+other_limb;
    service_other_limb = _n.advertiseService(topic, &ArmCtrl::serviceOtherLimbCb,this);
    ROS_INFO("[%s] Created service server with name  : %s", getLimb().c_str(), topic.c_str());

    insertAction(ACTION_HOME,    &ArmCtrl::goHome);
    insertAction(ACTION_RELEASE, &ArmCtrl::releaseObject);

    // insertAction("recover_"+string(ACTION_HOME),    &ArmCtrl::notImplemented);
    // insertAction("recover_"+string(ACTION_RELEASE), &ArmCtrl::notImplemented);

    _n.param<bool>("internal_recovery",  internal_recovery, true);
    ROS_INFO("[%s] Internal_recovery flag set to %s", getLimb().c_str(),
                                internal_recovery==true?"true":"false");
}

void ArmCtrl::InternalThreadEntry()
{
    pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS,NULL);
    _n.param<bool>("internal_recovery",  internal_recovery, true);

    std::string a =     getAction();
    int         s = int(getState());

    setState(WORKING);

    if (a == ACTION_HOME || a == ACTION_RELEASE)
    {
        if (callAction(a))   setState(DONE);
    }
    else if (s == START || s == ERROR ||
             s == DONE  || s == KILLED )
    {
        if (doAction(s, a))   setState(DONE);
        else                  setState(ERROR);
    }
    else
    {
        ROS_ERROR("[%s] Invalid Action %s in state %i", getLimb().c_str(), a.c_str(), s);
    }

    if (getState()==WORKING)
    {
        setState(ERROR);
    }

    if (getState()==ERROR)
    {
        ROS_ERROR("[%s] Action %s not successful! State %s %s", getLimb().c_str(), a.c_str(),
                                          string(getState()).c_str(), getSubState().c_str());
    }

    closeInternalThread();
    return;
}

bool ArmCtrl::serviceOtherLimbCb(baxter_collaboration::AskFeedback::Request  &req,
                                 baxter_collaboration::AskFeedback::Response &res)
{
    res.success = false;
    res.reply   = "not implemented";
    return true;
}

bool ArmCtrl::serviceCb(baxter_collaboration::DoAction::Request  &req,
                        baxter_collaboration::DoAction::Response &res)
{
    string action = req.action;
    int    obj    = req.object;

    ROS_INFO("[%s] Service request received. Action: %s object: %i", getLimb().c_str(),
                                                                   action.c_str(), obj);

    if (action == PROT_ACTION_LIST)
    {
        printDB();
        res.success  = true;
        res.response = DBToString();
        return true;
    }

    if (is_no_robot())
    {
        ros::Duration(2.0).sleep();
        res.success = true;
        return true;
    }

    res.success = false;

    setAction(action);
    setObject(obj);

    startInternalThread();
    ros::Duration(0.5).sleep();

    ros::Rate r(100);
    while( ros::ok() && ( int(getState()) != START   &&
                          int(getState()) != ERROR   &&
                          int(getState()) != DONE    &&
                          int(getState()) != PICK_UP   ))
    {
        if (ros::isShuttingDown())
        {
            setState(KILLED);
            return true;
        }

        if (getState()==KILLED)
        {
            recoverFromError();
        }

        r.sleep();
    }

    if ( int(getState()) == START   ||
         int(getState()) == DONE    ||
         int(getState()) == PICK_UP   )
    {
        res.success = true;
    }

    ROS_INFO("[%s] Service reply with success: %s\n", getLimb().c_str(),
                                            res.success?"true":"false");
    return true;
}

bool ArmCtrl::notImplemented()
{
    ROS_ERROR("[%s] Action not implemented!", getLimb().c_str());
    return false;
}

bool ArmCtrl::insertAction(const std::string &a, ArmCtrl::f_action f)
{
    if (a == PROT_ACTION_LIST)
    {
        ROS_ERROR("[%s][action_db] Attempted to insert protected action key: %s",
                 getLimb().c_str(), a.c_str());
        return false;
    }

    if (isActionInDB(a, true)) // The action is in the db
    {
        ROS_WARN("[%s][action_db] Overwriting existing action with key %s",
                 getLimb().c_str(), a.c_str());
    }

    action_db.insert( std::make_pair( a, f ));
    return true;
}

bool ArmCtrl::removeAction(const std::string &a)
{
    if (isActionInDB(a)) // The action is in the db
    {
        action_db.erase(a);
        return true;
    }

    return false;
}

bool ArmCtrl::callAction(const std::string &a)
{
    if (isActionInDB(a)) // The action is in the db
    {
        f_action act = action_db[a];
        return (this->*act)();
    }

    return false;
}

bool ArmCtrl::doAction(int s, std::string a)
{
    if (isActionInDB(a))
    {
        if (callAction(a))         return true;
        else                recoverFromError();
    }
    else
    {
        ROS_ERROR("[%s] Invalid Action %s in state %i", getLimb().c_str(), a.c_str(), s);
    }

    return false;
}

bool ArmCtrl::isActionInDB(const std::string &a, bool insertAction)
{
    if (action_db.find(a) != action_db.end()) return true;

    if (!insertAction)
    {
        ROS_ERROR("[%s][action_db] Action %s is not in the database!",
                  getLimb().c_str(), a.c_str());
    }
    return false;
}

void ArmCtrl::printDB()
{
    ROS_INFO("[%s] Available actions in the database : %s",
              getLimb().c_str(), DBToString().c_str());
}

string ArmCtrl::DBToString()
{
    string res = "";
    map<string, f_action>::iterator it;

    for ( it = action_db.begin(); it != action_db.end(); it++ )
    {
        res = res + it->first + ", ";
    }
    res = res.substr(0, res.size()-2); // Remove the last ", "
    return res;
}

bool ArmCtrl::moveArm(string dir, double dist, string mode, bool disable_coll_av)
{
    Point start = getPos();
    Point final = getPos();

    Quaternion ori = getOri();

    if      (dir == "backward") final.x -= dist;
    else if (dir == "forward")  final.x += dist;
    else if (dir == "right")    final.y -= dist;
    else if (dir == "left")     final.y += dist;
    else if (dir == "down")     final.z -= dist;
    else if (dir == "up")       final.z += dist;
    else                               return false;

    ros::Time t_start = ros::Time::now();

    bool finish = false;

    ros::Rate r(100);
    while(RobotInterface::ok())
    {
        if (disable_coll_av)    suppressCollisionAv();

        double t_elap = (ros::Time::now() - t_start).toSec();

        double px = start.x;
        double py = start.y;
        double pz = start.z;

        if (!finish)
        {
            if (dir == "backward" | dir == "forward")
            {
                int sgn = dir=="backward"?-1:+1;
                px = px + sgn * PICK_UP_SPEED * t_elap;

                if (dir == "backward")
                {
                    if (px < final.x) finish = true;
                }
                else if (dir == "forward")
                {
                    if (px > final.x) finish = true;
                }
            }
            if (dir == "right" | dir == "left")
            {
                int sgn = dir=="right"?-1:+1;
                py = py + sgn * PICK_UP_SPEED * t_elap;

                if (dir == "right")
                {
                    if (py < final.y) finish = true;
                }
                else if (dir == "left")
                {
                    if (py > final.y) finish = true;
                }
            }
            if (dir == "down" | dir == "up")
            {
                int sgn = dir=="down"?-1:+1;
                pz = pz + sgn * PICK_UP_SPEED * t_elap;

                if (dir == "down")
                {
                    if (pz < final.z) finish = true;
                }
                else if (dir == "up")
                {
                    if (pz > final.z) finish = true;
                }
            }
        }
        else
        {
            px = final.x;
            py = final.y;
            pz = final.z;
        }

        double ox = ori.x;
        double oy = ori.y;
        double oz = ori.z;
        double ow = ori.w;

        vector<double> joint_angles;
        if (!computeIK(px, py, pz, ox, oy, oz, ow, joint_angles)) return false;

        if (!goToPoseNoCheck(joint_angles))   return false;

        if (isPositionReached(final.x, final.y, final.z, mode)) return true;

        r.sleep();
    }

    return false;
}

bool ArmCtrl::hoverAboveTable(double height, string mode, bool disable_coll_av)
{
    if (getLimb() == "right")
    {
        return RobotInterface::goToPose(HOME_POS_R, height, VERTICAL_ORI_R,
                                                     mode, disable_coll_av);
    }
    else if (getLimb() == "left")
    {
        return RobotInterface::goToPose(HOME_POS_L, height, VERTICAL_ORI_L,
                                                     mode, disable_coll_av);
    }
    else return false;
}

bool ArmCtrl::homePoseStrict(bool disable_coll_av)
{
    ROS_INFO("[%s] Going to home position strict..", getLimb().c_str());

    ros::Rate r(100);
    while(ros::ok())
    {
        if (disable_coll_av)    suppressCollisionAv();

        JointCommand joint_cmd;
        joint_cmd.mode = JointCommand::POSITION_MODE;
        setJointNames(joint_cmd);

        joint_cmd.command = home_conf.command;

        publish_joint_cmd(joint_cmd);

        r.sleep();

        if(isConfigurationReached(joint_cmd))
        {
            return true;
        }
    }
    ROS_INFO("[%s] Done", getLimb().c_str());
}

void ArmCtrl::setHomeConf(double s0, double s1, double e0, double e1,
                                     double w0, double w1, double w2)
{
    setJointCommands( s0, s1, e0, e1, w0, w1, w2, home_conf);
}

bool ArmCtrl::goHome()
{
    bool res = homePoseStrict();
    releaseObject();
    return res;
}

void ArmCtrl::recoverFromError()
{
    if (internal_recovery == true)
    {
        goHome();
    }
}

void ArmCtrl::setState(int _state)
{
    if (_state == KILLED && getState() != WORKING)
    {
        ROS_WARN("[%s] Attempted to kill a non-working controller", getLimb().c_str());
        return;
    }

    RobotInterface::setState(_state);

    // if (_state == DONE)
    // {
    //     setAction("");
    //     setMarkerID(-1);
    // }
    if (_state == DONE)
    {
        setSubState(getAction());
    }
    publishState();
}

void ArmCtrl::setAction(string _action)
{
    action = _action;
    publishState();
}

void ArmCtrl::publishState()
{
    baxter_collaboration::ArmState msg;

    msg.state  = string(getState());
    msg.action = getAction();
    msg.object = getObjName();

    state_pub.publish(msg);
}

string ArmCtrl::getObjName()
{
    if      (object == 17) return "left leg";
    else if (object == 21) return "top";
    else if (object == 24) return "central frame";
    else if (object == 26) return "right leg";
    else if (object == 1)  return "wood bottom";
    else if (object == 2)  return "wood middle";
    else if (object == 3)  return "wood top";
    else if (object == 4)  return "white bottom";
    else if (object == 5)  return "white middle";
    else if (object == 6)  return "white top";
    else if (object == 7)  return "blue bottom";
    else if (object == 8)  return "blue middle";
    else if (object == 9)  return "blue top";
    else return "";
}

ArmCtrl::~ArmCtrl()
{
    killInternalThread();
}
<|endoftext|>


<commit_before>/*
* Copyright 2012 Google Inc.
*
* Use of this source code is governed by a BSD-style license that can be
* found in the LICENSE file.
*/

#include "gm.h"
#include "SkBlurMask.h"
#include "SkBlurMaskFilter.h"
#include "SkCanvas.h"
#include "SkPath.h"

#define STROKE_WIDTH    SkIntToScalar(10)

typedef void (*Proc)(SkCanvas*, const SkRect&, const SkPaint&);

static void fill_rect(SkCanvas* canvas, const SkRect& r, const SkPaint& p) {
    canvas->drawRect(r, p);
}

static void draw_donut(SkCanvas* canvas, const SkRect& r, const SkPaint& p) {
    SkRect  rect;
    SkPath  path;

    rect = r;
    rect.outset(STROKE_WIDTH/2, STROKE_WIDTH/2);
    path.addRect(rect);
    rect = r;
    rect.inset(STROKE_WIDTH/2, STROKE_WIDTH/2);

    path.addRect(rect);
    path.setFillType(SkPath::kEvenOdd_FillType);

    canvas->drawPath(path, p);
}

static void draw_donut_skewed(SkCanvas* canvas, const SkRect& r, const SkPaint& p) {
    SkRect  rect;
    SkPath  path;

    rect = r;
    rect.outset(STROKE_WIDTH/2, STROKE_WIDTH/2);
    path.addRect(rect);
    rect = r;
    rect.inset(STROKE_WIDTH/2, STROKE_WIDTH/2);

    rect.offset(7, -7);

    path.addRect(rect);
    path.setFillType(SkPath::kEvenOdd_FillType);

    canvas->drawPath(path, p);
}

#include "SkGradientShader.h"

typedef void (*PaintProc)(SkPaint*, SkScalar width);

class BlurRectGM : public skiagm::GM {
      SkAutoTUnref<SkMaskFilter> fMaskFilters[kLastEnum_SkBlurStyle + 1];
      SkString  fName;
      SkAlpha   fAlpha;
public:
    BlurRectGM(const char name[], U8CPU alpha)
        : fName(name)
        , fAlpha(SkToU8(alpha)) {
    }

protected:
    virtual void onOnceBeforeDraw() SK_OVERRIDE {
        for (int i = 0; i <= kLastEnum_SkBlurStyle; ++i) {
            fMaskFilters[i].reset(SkBlurMaskFilter::Create((SkBlurStyle)i,
                                  SkBlurMask::ConvertRadiusToSigma(SkIntToScalar(STROKE_WIDTH/2)),
                                  SkBlurMaskFilter::kHighQuality_BlurFlag));
        }
    }

    virtual SkString onShortName() {
        return fName;
    }

    virtual SkISize onISize() {
        return SkISize::Make(440, 820);
    }

    virtual void onDraw(SkCanvas* canvas) {
        canvas->translate(STROKE_WIDTH*3/2, STROKE_WIDTH*3/2);

        SkRect  r = { 0, 0, 100, 50 };
        SkScalar scales[] = { SK_Scalar1, 0.6f };

        for (size_t s = 0; s < SK_ARRAY_COUNT(scales); ++s) {
            canvas->save();
            for (size_t f = 0; f < SK_ARRAY_COUNT(fMaskFilters); ++f) {
                SkPaint paint;
                paint.setMaskFilter(fMaskFilters[f]);
                paint.setAlpha(fAlpha);

                static const Proc procs[] = {
                    fill_rect, draw_donut, draw_donut_skewed
                };

                canvas->save();
                canvas->scale(scales[s], scales[s]);
                this->drawProcs(canvas, r, paint, false, procs, SK_ARRAY_COUNT(procs));
                canvas->translate(r.width() * 4/3, 0);
                this->drawProcs(canvas, r, paint, true, procs, SK_ARRAY_COUNT(procs));
                canvas->restore();

                canvas->translate(0, SK_ARRAY_COUNT(procs) * r.height() * 4/3 * scales[s]);
            }
            canvas->restore();
            canvas->translate(2 * r.width() * 4/3 * scales[s], 0);
        }
    }

    virtual uint32_t onGetFlags() const { return kSkipPipe_Flag | kSkipTiled_Flag; }

private:
    void drawProcs(SkCanvas* canvas, const SkRect& r, const SkPaint& paint,
                   bool doClip, const Proc procs[], size_t procsCount) {
        SkAutoCanvasRestore acr(canvas, true);
        for (size_t i = 0; i < procsCount; ++i) {
            if (doClip) {
                SkRect clipRect(r);
                clipRect.inset(STROKE_WIDTH/2, STROKE_WIDTH/2);
                canvas->save();
                canvas->clipRect(r);
            }
            procs[i](canvas, r, paint);
            if (doClip) {
                canvas->restore();
            }
            canvas->translate(0, r.height() * 4/3);
        }
    }
private:
    typedef GM INHERITED;
};

class BlurRectDirectGM : public skiagm::GM {
    SkString  fName;
    int fGMWidth, fGMHeight;
    int fPadding, fMargin;
public:
    BlurRectDirectGM(const char name[])
        : fName(name),
          fGMWidth(1200),
          fGMHeight(1024),
          fPadding(10),
          fMargin(100)
    {
    }

protected:
    virtual SkString onShortName() {
        return fName;
    }

    virtual SkISize onISize() {
        return SkISize::Make(fGMWidth, fGMHeight);
    }

    virtual void onDraw(SkCanvas* canvas) {
        const int widths[] = {25, 5, 5, 100, 150, 25};
        const int heights[] = {100, 100, 5, 25, 150, 25};
        const SkBlurStyle styles[] = {kNormal_SkBlurStyle, kInner_SkBlurStyle, kOuter_SkBlurStyle};
        const float radii[] = {20, 5, 10};

        canvas->translate(50,20);

        int cur_x = 0;
        int cur_y = 0;

        int max_height = 0;

        for (size_t i = 0 ; i < SK_ARRAY_COUNT(widths) ; i++) {
            int width = widths[i];
            int height = heights[i];
            SkRect r;
            r.setWH(SkIntToScalar(width), SkIntToScalar(height));
            SkAutoCanvasRestore autoRestore(canvas, true);

            for (size_t j = 0 ; j < SK_ARRAY_COUNT(radii) ; j++) {
                float radius = radii[j];
                for (size_t k = 0 ; k < SK_ARRAY_COUNT(styles) ; k++) {
                    SkBlurStyle style = styles[k];

                    SkMask mask;
                    SkBlurMask::BlurRect(SkBlurMask::ConvertRadiusToSigma(radius), &mask, r, style);

                    SkAutoMaskFreeImage amfi(mask.fImage);

                    SkBitmap bm;
                    bm.installMaskPixels(mask);

                    if (cur_x + bm.width() >= fGMWidth - fMargin) {
                        cur_x = 0;
                        cur_y += max_height + fPadding;
                        max_height = 0;
                    }

                    canvas->save();
                    canvas->translate(cur_x, cur_y);
                    canvas->translate(-(bm.width() - r.width())/2, -(bm.height()-r.height())/2);
                    canvas->drawBitmap(bm, 0.f, 0.f, NULL);
                    canvas->restore();

                    cur_x += bm.width() + fPadding;
                    if (bm.height() > max_height)
                        max_height = bm.height();
                }
            }
        }
    }

    virtual uint32_t onGetFlags() const { return kSkipPipe_Flag; }

private:
    typedef GM INHERITED;
};

class BlurRectCompareGM : public skiagm::GM {
    SkString  fName;
    unsigned int fRectWidth, fRectHeight;
    SkScalar fRadius;
    SkBlurStyle fStyle;
public:
    BlurRectCompareGM(const char name[], unsigned int rectWidth,
                      unsigned int rectHeight, float radius,
                      SkBlurStyle style)
        : fName(name)
        , fRectWidth(rectWidth)
        , fRectHeight(rectHeight)
        , fRadius(radius)
        , fStyle(style) {
    }
    int width() const {
        return fRectWidth;
    }
    int height() const {
        return fRectHeight;
    }
    SkScalar radius() const {
        return fRadius;
    }
    SkBlurStyle style() const {
        return fStyle;
    }

protected:
    virtual SkString onShortName() {
        return fName;
    }

    virtual SkISize onISize() {
        return SkISize::Make(640, 480);
    }

    virtual bool makeMask(SkMask *m, const SkRect&) = 0;

    virtual void onDraw(SkCanvas* canvas) {
        SkRect r;
        r.setWH(SkIntToScalar(fRectWidth), SkIntToScalar(fRectHeight));

        SkISize canvas_size = canvas->getDeviceSize();
        int center_x = (canvas_size.fWidth - (int)(r.width()))/2;
        int center_y = (canvas_size.fHeight - (int)(r.height()))/2;

        SkMask mask;

        if (!this->makeMask(&mask, r)) {
            SkPaint paint;
            r.offset( SkIntToScalar(center_x), SkIntToScalar(center_y) );
            canvas->drawRect(r,paint);
            return;
        }
        SkAutoMaskFreeImage amfi(mask.fImage);

        SkBitmap bm;
        bm.installMaskPixels(mask);

        center_x = (canvas_size.fWidth - mask.fBounds.width())/2;
        center_y = (canvas_size.fHeight - mask.fBounds.height())/2;

        canvas->drawBitmap(bm, SkIntToScalar(center_x), SkIntToScalar(center_y), NULL);
    }

    virtual uint32_t onGetFlags() const { return kSkipPipe_Flag; }

private:
    typedef GM INHERITED;
};

class BlurRectFastGM: public BlurRectCompareGM {
public:
    BlurRectFastGM(const char name[], unsigned int rectWidth,
                   unsigned int rectHeight, float blurRadius,
                   SkBlurStyle style) :
        INHERITED(name, rectWidth, rectHeight, blurRadius, style) {
        }

protected:
    virtual bool makeMask(SkMask *m, const SkRect& r) SK_OVERRIDE {
        return SkBlurMask::BlurRect(SkBlurMask::ConvertRadiusToSigma(this->radius()),
                                    m, r, this->style());
    }
private:
    typedef BlurRectCompareGM INHERITED;
};

class BlurRectSlowGM: public BlurRectCompareGM {
public:
    BlurRectSlowGM(const char name[], unsigned int rectWidth, unsigned int rectHeight,
                   float blurRadius, SkBlurStyle style)
        : INHERITED(name, rectWidth, rectHeight, blurRadius, style) {
        }

protected:
    virtual bool makeMask(SkMask *m, const SkRect& r) SK_OVERRIDE {
        SkMask src;
        r.roundOut(&src.fBounds);
        src.fBounds.offset(-src.fBounds.fLeft, -src.fBounds.fTop);  // move to origin
        src.fFormat = SkMask::kA8_Format;
        src.fRowBytes = src.fBounds.width();
        src.fImage = SkMask::AllocImage(src.computeTotalImageSize());
        SkAutoMaskFreeImage amfi(src.fImage);

        memset(src.fImage, 0xff, src.computeTotalImageSize());

        return SkBlurMask::BoxBlur(m, src,
                                   SkBlurMask::ConvertRadiusToSigma(this->radius()),
                                   this->style(), this->getQuality());
    }

    virtual SkBlurQuality getQuality() {
        return kHigh_SkBlurQuality;
    }
private:
    typedef BlurRectCompareGM INHERITED;
};

class BlurRectSlowLowGM: public BlurRectSlowGM {
public:
    BlurRectSlowLowGM(const char name[], unsigned int rectWidth, unsigned int rectHeight,
                      float blurRadius, SkBlurStyle style)
        : INHERITED(name, rectWidth, rectHeight, blurRadius, style) {
        }

protected:
    virtual SkBlurQuality getQuality() SK_OVERRIDE {
        return kLow_SkBlurQuality;
    }
private:
    typedef BlurRectSlowGM INHERITED;
};

class BlurRectGroundTruthGM: public BlurRectCompareGM {
public:
    BlurRectGroundTruthGM(const char name[], unsigned int rectWidth, unsigned int rectHeight,
                          float blurRadius, SkBlurStyle style)
        : INHERITED(name, rectWidth, rectHeight, blurRadius, style) {
        }

protected:
    virtual bool makeMask(SkMask *m, const SkRect& r) SK_OVERRIDE {
        SkMask src;
        r.roundOut(&src.fBounds);
        src.fBounds.offset(-src.fBounds.fLeft, -src.fBounds.fTop);  // move to origin
        src.fFormat = SkMask::kA8_Format;
        src.fRowBytes = src.fBounds.width();
        src.fImage = SkMask::AllocImage(src.computeTotalImageSize());
        SkAutoMaskFreeImage amfi(src.fImage);

        memset(src.fImage, 0xff, src.computeTotalImageSize());

        return SkBlurMask::BlurGroundTruth(SkBlurMask::ConvertRadiusToSigma(this->radius()),
                                           m, src, this->style());
    }

    virtual SkBlurQuality getQuality() {
        return kHigh_SkBlurQuality;
    }
private:
    typedef BlurRectCompareGM INHERITED;
};

//////////////////////////////////////////////////////////////////////////////

DEF_GM(return new BlurRectGM("blurrects", 0xFF);)
DEF_GM(return new BlurRectDirectGM("blurrect_gallery");)
<commit_msg>Shut up windows build errors on new GM<commit_after>/*
* Copyright 2012 Google Inc.
*
* Use of this source code is governed by a BSD-style license that can be
* found in the LICENSE file.
*/

#include "gm.h"
#include "SkBlurMask.h"
#include "SkBlurMaskFilter.h"
#include "SkCanvas.h"
#include "SkPath.h"

#define STROKE_WIDTH    SkIntToScalar(10)

typedef void (*Proc)(SkCanvas*, const SkRect&, const SkPaint&);

static void fill_rect(SkCanvas* canvas, const SkRect& r, const SkPaint& p) {
    canvas->drawRect(r, p);
}

static void draw_donut(SkCanvas* canvas, const SkRect& r, const SkPaint& p) {
    SkRect  rect;
    SkPath  path;

    rect = r;
    rect.outset(STROKE_WIDTH/2, STROKE_WIDTH/2);
    path.addRect(rect);
    rect = r;
    rect.inset(STROKE_WIDTH/2, STROKE_WIDTH/2);

    path.addRect(rect);
    path.setFillType(SkPath::kEvenOdd_FillType);

    canvas->drawPath(path, p);
}

static void draw_donut_skewed(SkCanvas* canvas, const SkRect& r, const SkPaint& p) {
    SkRect  rect;
    SkPath  path;

    rect = r;
    rect.outset(STROKE_WIDTH/2, STROKE_WIDTH/2);
    path.addRect(rect);
    rect = r;
    rect.inset(STROKE_WIDTH/2, STROKE_WIDTH/2);

    rect.offset(7, -7);

    path.addRect(rect);
    path.setFillType(SkPath::kEvenOdd_FillType);

    canvas->drawPath(path, p);
}

#include "SkGradientShader.h"

typedef void (*PaintProc)(SkPaint*, SkScalar width);

class BlurRectGM : public skiagm::GM {
      SkAutoTUnref<SkMaskFilter> fMaskFilters[kLastEnum_SkBlurStyle + 1];
      SkString  fName;
      SkAlpha   fAlpha;
public:
    BlurRectGM(const char name[], U8CPU alpha)
        : fName(name)
        , fAlpha(SkToU8(alpha)) {
    }

protected:
    virtual void onOnceBeforeDraw() SK_OVERRIDE {
        for (int i = 0; i <= kLastEnum_SkBlurStyle; ++i) {
            fMaskFilters[i].reset(SkBlurMaskFilter::Create((SkBlurStyle)i,
                                  SkBlurMask::ConvertRadiusToSigma(SkIntToScalar(STROKE_WIDTH/2)),
                                  SkBlurMaskFilter::kHighQuality_BlurFlag));
        }
    }

    virtual SkString onShortName() {
        return fName;
    }

    virtual SkISize onISize() {
        return SkISize::Make(440, 820);
    }

    virtual void onDraw(SkCanvas* canvas) {
        canvas->translate(STROKE_WIDTH*3/2, STROKE_WIDTH*3/2);

        SkRect  r = { 0, 0, 100, 50 };
        SkScalar scales[] = { SK_Scalar1, 0.6f };

        for (size_t s = 0; s < SK_ARRAY_COUNT(scales); ++s) {
            canvas->save();
            for (size_t f = 0; f < SK_ARRAY_COUNT(fMaskFilters); ++f) {
                SkPaint paint;
                paint.setMaskFilter(fMaskFilters[f]);
                paint.setAlpha(fAlpha);

                static const Proc procs[] = {
                    fill_rect, draw_donut, draw_donut_skewed
                };

                canvas->save();
                canvas->scale(scales[s], scales[s]);
                this->drawProcs(canvas, r, paint, false, procs, SK_ARRAY_COUNT(procs));
                canvas->translate(r.width() * 4/3, 0);
                this->drawProcs(canvas, r, paint, true, procs, SK_ARRAY_COUNT(procs));
                canvas->restore();

                canvas->translate(0, SK_ARRAY_COUNT(procs) * r.height() * 4/3 * scales[s]);
            }
            canvas->restore();
            canvas->translate(2 * r.width() * 4/3 * scales[s], 0);
        }
    }

    virtual uint32_t onGetFlags() const { return kSkipPipe_Flag | kSkipTiled_Flag; }

private:
    void drawProcs(SkCanvas* canvas, const SkRect& r, const SkPaint& paint,
                   bool doClip, const Proc procs[], size_t procsCount) {
        SkAutoCanvasRestore acr(canvas, true);
        for (size_t i = 0; i < procsCount; ++i) {
            if (doClip) {
                SkRect clipRect(r);
                clipRect.inset(STROKE_WIDTH/2, STROKE_WIDTH/2);
                canvas->save();
                canvas->clipRect(r);
            }
            procs[i](canvas, r, paint);
            if (doClip) {
                canvas->restore();
            }
            canvas->translate(0, r.height() * 4/3);
        }
    }
private:
    typedef GM INHERITED;
};

class BlurRectDirectGM : public skiagm::GM {
    SkString  fName;
    int fGMWidth, fGMHeight;
    int fPadding, fMargin;
public:
    BlurRectDirectGM(const char name[])
        : fName(name),
          fGMWidth(1200),
          fGMHeight(1024),
          fPadding(10),
          fMargin(100)
    {
    }

protected:
    virtual SkString onShortName() {
        return fName;
    }

    virtual SkISize onISize() {
        return SkISize::Make(fGMWidth, fGMHeight);
    }

    virtual void onDraw(SkCanvas* canvas) {
        const int widths[] = {25, 5, 5, 100, 150, 25};
        const int heights[] = {100, 100, 5, 25, 150, 25};
        const SkBlurStyle styles[] = {kNormal_SkBlurStyle, kInner_SkBlurStyle, kOuter_SkBlurStyle};
        const float radii[] = {20, 5, 10};

        canvas->translate(50,20);

        int cur_x = 0;
        int cur_y = 0;

        int max_height = 0;

        for (size_t i = 0 ; i < SK_ARRAY_COUNT(widths) ; i++) {
            int width = widths[i];
            int height = heights[i];
            SkRect r;
            r.setWH(SkIntToScalar(width), SkIntToScalar(height));
            SkAutoCanvasRestore autoRestore(canvas, true);

            for (size_t j = 0 ; j < SK_ARRAY_COUNT(radii) ; j++) {
                float radius = radii[j];
                for (size_t k = 0 ; k < SK_ARRAY_COUNT(styles) ; k++) {
                    SkBlurStyle style = styles[k];

                    SkMask mask;
                    SkBlurMask::BlurRect(SkBlurMask::ConvertRadiusToSigma(radius), &mask, r, style);

                    SkAutoMaskFreeImage amfi(mask.fImage);

                    SkBitmap bm;
                    bm.installMaskPixels(mask);

                    if (cur_x + bm.width() >= fGMWidth - fMargin) {
                        cur_x = 0;
                        cur_y += max_height + fPadding;
                        max_height = 0;
                    }

                    canvas->save();
                    canvas->translate((SkScalar)cur_x, (SkScalar)cur_y);
                    canvas->translate(-(bm.width() - r.width())/2, -(bm.height()-r.height())/2);
                    canvas->drawBitmap(bm, 0.f, 0.f, NULL);
                    canvas->restore();

                    cur_x += bm.width() + fPadding;
                    if (bm.height() > max_height)
                        max_height = bm.height();
                }
            }
        }
    }

    virtual uint32_t onGetFlags() const { return kSkipPipe_Flag; }

private:
    typedef GM INHERITED;
};

class BlurRectCompareGM : public skiagm::GM {
    SkString  fName;
    unsigned int fRectWidth, fRectHeight;
    SkScalar fRadius;
    SkBlurStyle fStyle;
public:
    BlurRectCompareGM(const char name[], unsigned int rectWidth,
                      unsigned int rectHeight, float radius,
                      SkBlurStyle style)
        : fName(name)
        , fRectWidth(rectWidth)
        , fRectHeight(rectHeight)
        , fRadius(radius)
        , fStyle(style) {
    }
    int width() const {
        return fRectWidth;
    }
    int height() const {
        return fRectHeight;
    }
    SkScalar radius() const {
        return fRadius;
    }
    SkBlurStyle style() const {
        return fStyle;
    }

protected:
    virtual SkString onShortName() {
        return fName;
    }

    virtual SkISize onISize() {
        return SkISize::Make(640, 480);
    }

    virtual bool makeMask(SkMask *m, const SkRect&) = 0;

    virtual void onDraw(SkCanvas* canvas) {
        SkRect r;
        r.setWH(SkIntToScalar(fRectWidth), SkIntToScalar(fRectHeight));

        SkISize canvas_size = canvas->getDeviceSize();
        int center_x = (canvas_size.fWidth - (int)(r.width()))/2;
        int center_y = (canvas_size.fHeight - (int)(r.height()))/2;

        SkMask mask;

        if (!this->makeMask(&mask, r)) {
            SkPaint paint;
            r.offset( SkIntToScalar(center_x), SkIntToScalar(center_y) );
            canvas->drawRect(r,paint);
            return;
        }
        SkAutoMaskFreeImage amfi(mask.fImage);

        SkBitmap bm;
        bm.installMaskPixels(mask);

        center_x = (canvas_size.fWidth - mask.fBounds.width())/2;
        center_y = (canvas_size.fHeight - mask.fBounds.height())/2;

        canvas->drawBitmap(bm, SkIntToScalar(center_x), SkIntToScalar(center_y), NULL);
    }

    virtual uint32_t onGetFlags() const { return kSkipPipe_Flag; }

private:
    typedef GM INHERITED;
};

class BlurRectFastGM: public BlurRectCompareGM {
public:
    BlurRectFastGM(const char name[], unsigned int rectWidth,
                   unsigned int rectHeight, float blurRadius,
                   SkBlurStyle style) :
        INHERITED(name, rectWidth, rectHeight, blurRadius, style) {
        }

protected:
    virtual bool makeMask(SkMask *m, const SkRect& r) SK_OVERRIDE {
        return SkBlurMask::BlurRect(SkBlurMask::ConvertRadiusToSigma(this->radius()),
                                    m, r, this->style());
    }
private:
    typedef BlurRectCompareGM INHERITED;
};

class BlurRectSlowGM: public BlurRectCompareGM {
public:
    BlurRectSlowGM(const char name[], unsigned int rectWidth, unsigned int rectHeight,
                   float blurRadius, SkBlurStyle style)
        : INHERITED(name, rectWidth, rectHeight, blurRadius, style) {
        }

protected:
    virtual bool makeMask(SkMask *m, const SkRect& r) SK_OVERRIDE {
        SkMask src;
        r.roundOut(&src.fBounds);
        src.fBounds.offset(-src.fBounds.fLeft, -src.fBounds.fTop);  // move to origin
        src.fFormat = SkMask::kA8_Format;
        src.fRowBytes = src.fBounds.width();
        src.fImage = SkMask::AllocImage(src.computeTotalImageSize());
        SkAutoMaskFreeImage amfi(src.fImage);

        memset(src.fImage, 0xff, src.computeTotalImageSize());

        return SkBlurMask::BoxBlur(m, src,
                                   SkBlurMask::ConvertRadiusToSigma(this->radius()),
                                   this->style(), this->getQuality());
    }

    virtual SkBlurQuality getQuality() {
        return kHigh_SkBlurQuality;
    }
private:
    typedef BlurRectCompareGM INHERITED;
};

class BlurRectSlowLowGM: public BlurRectSlowGM {
public:
    BlurRectSlowLowGM(const char name[], unsigned int rectWidth, unsigned int rectHeight,
                      float blurRadius, SkBlurStyle style)
        : INHERITED(name, rectWidth, rectHeight, blurRadius, style) {
        }

protected:
    virtual SkBlurQuality getQuality() SK_OVERRIDE {
        return kLow_SkBlurQuality;
    }
private:
    typedef BlurRectSlowGM INHERITED;
};

class BlurRectGroundTruthGM: public BlurRectCompareGM {
public:
    BlurRectGroundTruthGM(const char name[], unsigned int rectWidth, unsigned int rectHeight,
                          float blurRadius, SkBlurStyle style)
        : INHERITED(name, rectWidth, rectHeight, blurRadius, style) {
        }

protected:
    virtual bool makeMask(SkMask *m, const SkRect& r) SK_OVERRIDE {
        SkMask src;
        r.roundOut(&src.fBounds);
        src.fBounds.offset(-src.fBounds.fLeft, -src.fBounds.fTop);  // move to origin
        src.fFormat = SkMask::kA8_Format;
        src.fRowBytes = src.fBounds.width();
        src.fImage = SkMask::AllocImage(src.computeTotalImageSize());
        SkAutoMaskFreeImage amfi(src.fImage);

        memset(src.fImage, 0xff, src.computeTotalImageSize());

        return SkBlurMask::BlurGroundTruth(SkBlurMask::ConvertRadiusToSigma(this->radius()),
                                           m, src, this->style());
    }

    virtual SkBlurQuality getQuality() {
        return kHigh_SkBlurQuality;
    }
private:
    typedef BlurRectCompareGM INHERITED;
};

//////////////////////////////////////////////////////////////////////////////

DEF_GM(return new BlurRectGM("blurrects", 0xFF);)
DEF_GM(return new BlurRectDirectGM("blurrect_gallery");)
<|endoftext|>


<commit_before>#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <sys/time.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <iostream>
#include <fstream>
#include <boost/lexical_cast.hpp>
#include "packet.h"

#define USAGE "Usage:\r\nc \r\n[tux machine number] \r\n[probability of packet corruption in int form] \r\n[probability of packet loss in int form] \r\n[probability of packet delay] \r\n[length of delay in ms] \r\n"
#define PORT 10038
#define PAKSIZE 512
#define ACK 0
#define NAK 1
#define BUFSIZE 505
#define FILENAME "Testfile"
#define TIMEOUT 15 //in ms
#define WIN_SIZE 16

using namespace std;

bool isvpack(unsigned char * p);
bool init(int argc, char** argv);
bool loadFile();
bool getFile();
bool sendFile();
bool isAck();
void handleAck();
void handleNak(int& x);
bool* gremlin(Packet * pack, int corruptProb, int lossProb, int delayProb);
Packet createPacket(int index);
void loadWindow();
bool sendPacket();
bool getGet();

struct sockaddr_in a;
struct sockaddr_in ca;
socklen_t calen;
int rlen;
int s;
bool ack;
string fstr;
char * file;
int probCorrupt;
int probLoss;
int probDelay;
int delayT;
Packet p;
Packet window[16];
int length;
bool dropPck;
bool delayPck;
int toms;
unsigned char b[BUFSIZE];
int base;

int main(int argc, char** argv) {

  if(!init(argc, argv)) return -1;

  if(sendFile()) cout << "GET Testfile complete." << endl;

  return 0;
}

bool init(int argc, char** argv){

  if(argc != 6) {
    cout << USAGE << endl;
    return false;
  }

  char * probCorruptStr = argv[2];
  probCorrupt = boost::lexical_cast<int>(probCorruptStr);
  char * probLossStr = argv[3];
  probLoss = boost::lexical_cast<int>(probLossStr);
  char * probDelayStr = argv[4];
  probDelay = boost::lexical_cast<int>(probDelayStr);
  char* delayTStr = argv[5];
  delayT = boost::lexical_cast<int>(delayTStr);

  struct timeval timeout;
  timeout.tv_usec = TIMEOUT * 1000;
  toms = TIMEOUT;

  /* Create our socket. */
  if ((s = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
    cout << "Socket creation failed. (socket s)" << endl;
    return 0;
  }

  setsockopt(s, SOL_SOCKET, SO_RCVTIMEO, (char*)&timeout, sizeof(timeout));

  /*
   * Bind our socket to an IP (whatever the computer decides)
   * and a specified port.
   *
   */

  if (!loadFile()) {
    cout << "Loading file failed. (filename FILENAME)" << endl;
    return false;
  }

  memset((char *)&a, 0, sizeof(a));
  a.sin_family = AF_INET;
  a.sin_addr.s_addr = htonl(INADDR_ANY);
  a.sin_port = htons(PORT);

  if (bind(s, (struct sockaddr *)&a, sizeof(a)) < 0) {
    cout << "Socket binding failed. (socket s, address a)" << endl;
    return 0;
  }

  fstr = string(file);
  cout << "File: " << endl << fstr << endl;

  base = 0;
  dropPck = false;
  calen = sizeof(ca);

  getGet();

  return true;
}

bool getGet(){
	unsigned char packet[PAKSIZE + 1];
	rlen = recvfrom(s, packet, PAKSIZE, 0, (struct sockaddr *)&ca, &calen);
	return (rlen > 0) ? true : false;
}

bool isvpack(unsigned char * p) {
  cout << endl << "=== IS VALID PACKET TESTING" << endl;

  char * sns = new char[2];
  memcpy(sns, &p[0], 1);
  sns[1] = '\0';

  char * css = new char[7];
  memcpy(css, &p[1], 6);
  css[6] = '\0';

  char * db = new char[121 + 1];
  memcpy(db, &p[2], 121);
  db[121] = '\0';

  cout << "Seq. num: " << sns << endl;
  cout << "Checksum: " << css << endl;
  cout << "Message: " << db << endl;

  int sn = boost::lexical_cast<int>(sns);
  int cs = boost::lexical_cast<int>(css);

  Packet pk (0, db);
  pk.setSequenceNum(sn);

  // change to validate based on checksum and sequence number

  if(sn == 0) return false; //doesn't matter, only for the old version (netwark)
  if(cs != pk.generateCheckSum()) return false;
  return true;
}

bool getFile(){

  /* Loop forever, waiting for messages from a client. */
  cout << "Waiting on port " << PORT << "..." << endl;

  ofstream file("Dumpfile");

  for (;;) {
    unsigned char packet[PAKSIZE + 1];
    unsigned char dataPull[PAKSIZE - 7 + 1];
    rlen = recvfrom(s, packet, PAKSIZE, 0, (struct sockaddr *)&ca, &calen);

    for(int x = 0; x < PAKSIZE - 7; x++) {
      dataPull[x] = packet[x + 7];
    }
    dataPull[PAKSIZE - 7] = '\0';
    packet[PAKSIZE] = '\0';
    if (rlen > 0) {
      char * css = new char[6];
      memcpy(css, &packet[1], 5);
      css[5] = '\0';
      cout << endl << endl << "=== RECEIPT" << endl;
      cout << "Seq. num: " << packet[0] << endl;
      cout << "Checksum: " << css << endl;
      cout << "Received message: " << dataPull << endl;
      cout << "Sent response: ";
      if(isvpack(packet)) {
        ack = ACK;
        //seqNum = (seqNum) ? false : true;
        file << dataPull;
      } else {
        ack = NAK;
      }
      cout << ((ack == ACK) ? "ACK" : "NAK") << endl;
      Packet p (false, reinterpret_cast<const char *>(dataPull));
      p.setCheckSum(boost::lexical_cast<int>(css));
      p.setAckNack(ack);

      if(sendto(s, p.str(), PAKSIZE, 0, (struct sockaddr *)&ca, calen) < 0) {
        cout << "Acknowledgement failed. (socket s, acknowledgement message ack, client address ca, client address length calen)" << endl;
        return 0;
      }
      delete css;
    }
  }
  file.close();
  return true;
}

bool loadFile() {

  ifstream is (FILENAME, ifstream::binary);

  if(is) {
    is.seekg(0, is.end);
    length = is.tellg();
    is.seekg(0, is.beg);

    file = new char[length];

    cout << "Reading " << length << " characters..." << endl;
    is.read(file, length);

    if(!is) { cout << "File reading failed. (filename " << FILENAME << "). Only " << is.gcount() << " could be read."; return false; }
    is.close();
  }
  return true;
}

void loadWindow(){
	for(int i = base; i < base + WIN_SIZE; i++) {
		window[i-base] = createPacket(i);
		if(strlen(window[i-base].getDataBuffer()) < BUFSIZE && window[i-base].chksm()) {
			cout << "In loadWindow's secret base, index is: " << i-base << endl;
			for(++i; i < base + WIN_SIZE; i++){
				window[i-base].loadDataBuffer("\0");
			}

		}
		/*cout << "window[i-base] seq. num: " << window[i-base].getSequenceNum() << endl;*/
	}

		/*cout << "packet " << base + 1 << ": " << window[1].str() << endl;
		cout << "packet " << base + 2 << ": " << window[2].str() << endl;*/
}

bool sendFile() {
	/*Currently causes the program to only send the first 16 packets of file out
		requires additional code later to sendFile again with updated window*/
	fd_set stReadFDS;

	struct timeval stTimeOut;

	FD_ZERO(&stReadFDS);
	stTimeOut.tv_sec = 0;
	stTimeOut.tv_usec = 1000 * TIMEOUT;
	FD_SET(s, &stReadFDS);

	base = 0;
	int desc_ready;
	int finale = -1;
	int max_sd;
	bool hasRead = false;
	while(base * BUFSIZE < length) {
		loadWindow();

		if(p.str()[0] == '\0') finale = p.getSequenceNum();
		for(int x = 0; x < WIN_SIZE; x++) {
			p = window[x];
			if(!sendPacket()) continue;
		}
		for(int x = 0; x < WIN_SIZE; x++) {
			cout << endl << "beginning of loop " << x << endl;
			FD_ZERO(&stReadFDS);
			stTimeOut.tv_sec = 0;
			stTimeOut.tv_usec = 1000 * TIMEOUT;
			FD_SET(s, &stReadFDS);

			max_sd = s;
			cout << endl << "before select" << endl;
			int t = select(max_sd + 1, &stReadFDS, NULL, NULL, &stTimeOut);
			cout << "after select" << endl;
			if (t == -1){
				perror("select()");
			}
			if (t == 0) {
				cout << "=== ACK TIMEOUT (select)" << endl;

			}
			desc_ready = t;
			for(int u = 0; u <= max_sd  &&  desc_ready > 0; u++){
				if(recvfrom(s, b, BUFSIZE + 7, 0, (struct sockaddr *)&ca, &calen) < 0) {
					cout << "=== ACK TIMEOUT (recvfrom)" << endl;
				} else hasRead = true;
				if(!hasRead) continue;
				if(isAck()) {
					cout << "if isAck()" << endl;
					handleAck();
				} else {
					cout << "if !isAck()" << endl;
					handleAck();
					//handleNak(x);
				}
			}
			cout << "end of loop " << x << endl;
			if(finale > 0 && base == finale) {cout << "Finale: " << finale << endl; break;}
			memset(b, 0, BUFSIZE);
		}

	}

	if(sendto(s, "\0", BUFSIZE, 0, (struct sockaddr *)&ca, sizeof(ca)) < 0) {
		cout << "Final package sending failed. (socket s, server address sa, message m)" << endl;
		return false;
	}
	return true;
}

Packet createPacket(int index){
    string mstr = fstr.substr(index * BUFSIZE, BUFSIZE);

	if(mstr.length() < BUFSIZE) {
		cout << "Null terminated mstr." << endl;
		mstr[length - (index * BUFSIZE)] = '\0';
    }

	cout << "index: " << index << endl;
    return Packet (index, mstr.c_str());
}

bool sendPacket(){
	cout << endl;
    cout << "=== TRANSMISSION START" << endl;
	cout << "Sequence number before gremlin function: " << p.getSequenceNum() << endl;
    int pc = probCorrupt; int pl = probLoss; int pd = probDelay;
	bool* pckStatus = gremlin(&p, pc, pl, pd);

	dropPck = pckStatus[0];
	delayPck = pckStatus[1];

	if (dropPck == true) return false;
	if (delayPck == true) p.setAckNack(1);

    if(sendto(s, p.str(), BUFSIZE + 8, 0, (struct sockaddr *)&ca, sizeof(ca)) < 0) {
		cout << "Package sending failed. (socket s, server address sa, message m)" << endl;
		return false;
    }
    return true;
}
bool isAck() {
    cout << endl << "=== SERVER RESPONSE TEST" << endl;
    cout << "Data: " << b << endl;
    if(b[6] == '\0') return true;
    else return false;
}
void handleAck() {
	int ack = boost::lexical_cast<int>(b);
	if(base < ack) base = ack;
	cout << "Window base: " << base << endl;
}
void handleNak(int& x) {

      char * sns = new char[2];
      memcpy(sns, &b[0], 1);
      sns[1] = '\0';

      char * css = new char[5];
      memcpy(css, &b[1], 5);

      char * db = new char[BUFSIZE + 1];
      memcpy(db, &b[2], BUFSIZE);
      db[BUFSIZE] = '\0';

      cout << "Sequence number: " << sns << endl;
      cout << "Checksum: " << css << endl;

      Packet pk (0, db);
      pk.setSequenceNum(boost::lexical_cast<int>(sns));
      pk.setCheckSum(boost::lexical_cast<int>(css));

      if(!pk.chksm()) x--;
      else x = (x - 2 > 0) ? x - 2 : 0;
}
bool* gremlin(Packet * pack, int corruptProb, int lossProb, int delayProb){

  bool* packStatus = new bool[2];
  int r = rand() % 100;

  cout << "Corruption probability: " << corruptProb << endl;
  cout << "Random number: " << r << endl;

  packStatus[0] = false;
  packStatus[1] = false;

  if(r <= (lossProb)){
	packStatus[0] = true;
    cout << "Dropped!" << endl;
  }
  else if(r <= (delayProb)){
	  packStatus[1] = true;
	  cout << "Delayed!" << endl;
  }
  else if(r <= (corruptProb)){
    cout << "Corrupted!" << endl;
	cout << "Seq. num (pre-gremlin): " << pack->getSequenceNum() << endl;
    pack->loadDataBuffer((char*)"GREMLIN LOL");
  }
  cout << "Seq. num: " << pack->getSequenceNum() << endl;
  cout << "Checksum: " << pack->getCheckSum() << endl;
  //cout << "Message: "  << pack->getDataBuffer() << endl;

  return packStatus;
}<commit_msg>changed for to while loop<commit_after>#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <sys/time.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <iostream>
#include <fstream>
#include <boost/lexical_cast.hpp>
#include "packet.h"

#define USAGE "Usage:\r\nc \r\n[tux machine number] \r\n[probability of packet corruption in int form] \r\n[probability of packet loss in int form] \r\n[probability of packet delay] \r\n[length of delay in ms] \r\n"
#define PORT 10038
#define PAKSIZE 512
#define ACK 0
#define NAK 1
#define BUFSIZE 505
#define FILENAME "Testfile"
#define TIMEOUT 15 //in ms
#define WIN_SIZE 16

using namespace std;

bool isvpack(unsigned char * p);
bool init(int argc, char** argv);
bool loadFile();
bool getFile();
bool sendFile();
bool isAck();
void handleAck();
void handleNak(int& x);
bool* gremlin(Packet * pack, int corruptProb, int lossProb, int delayProb);
Packet createPacket(int index);
void loadWindow();
bool sendPacket();
bool getGet();

struct sockaddr_in a;
struct sockaddr_in ca;
socklen_t calen;
int rlen;
int s;
bool ack;
string fstr;
char * file;
int probCorrupt;
int probLoss;
int probDelay;
int delayT;
Packet p;
Packet window[16];
int length;
bool dropPck;
bool delayPck;
int toms;
unsigned char b[BUFSIZE];
int base;

int main(int argc, char** argv) {

  if(!init(argc, argv)) return -1;

  if(sendFile()) cout << "GET Testfile complete." << endl;

  return 0;
}

bool init(int argc, char** argv){

  if(argc != 6) {
    cout << USAGE << endl;
    return false;
  }

  char * probCorruptStr = argv[2];
  probCorrupt = boost::lexical_cast<int>(probCorruptStr);
  char * probLossStr = argv[3];
  probLoss = boost::lexical_cast<int>(probLossStr);
  char * probDelayStr = argv[4];
  probDelay = boost::lexical_cast<int>(probDelayStr);
  char* delayTStr = argv[5];
  delayT = boost::lexical_cast<int>(delayTStr);

  struct timeval timeout;
  timeout.tv_usec = TIMEOUT * 1000;
  toms = TIMEOUT;

  /* Create our socket. */
  if ((s = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
    cout << "Socket creation failed. (socket s)" << endl;
    return 0;
  }

  setsockopt(s, SOL_SOCKET, SO_RCVTIMEO, (char*)&timeout, sizeof(timeout));

  /*
   * Bind our socket to an IP (whatever the computer decides)
   * and a specified port.
   *
   */

  if (!loadFile()) {
    cout << "Loading file failed. (filename FILENAME)" << endl;
    return false;
  }

  memset((char *)&a, 0, sizeof(a));
  a.sin_family = AF_INET;
  a.sin_addr.s_addr = htonl(INADDR_ANY);
  a.sin_port = htons(PORT);

  if (bind(s, (struct sockaddr *)&a, sizeof(a)) < 0) {
    cout << "Socket binding failed. (socket s, address a)" << endl;
    return 0;
  }

  fstr = string(file);
  cout << "File: " << endl << fstr << endl;

  base = 0;
  dropPck = false;
  calen = sizeof(ca);

  getGet();

  return true;
}

bool getGet(){
	unsigned char packet[PAKSIZE + 1];
	rlen = recvfrom(s, packet, PAKSIZE, 0, (struct sockaddr *)&ca, &calen);
	return (rlen > 0) ? true : false;
}

bool isvpack(unsigned char * p) {
  cout << endl << "=== IS VALID PACKET TESTING" << endl;

  char * sns = new char[2];
  memcpy(sns, &p[0], 1);
  sns[1] = '\0';

  char * css = new char[7];
  memcpy(css, &p[1], 6);
  css[6] = '\0';

  char * db = new char[121 + 1];
  memcpy(db, &p[2], 121);
  db[121] = '\0';

  cout << "Seq. num: " << sns << endl;
  cout << "Checksum: " << css << endl;
  cout << "Message: " << db << endl;

  int sn = boost::lexical_cast<int>(sns);
  int cs = boost::lexical_cast<int>(css);

  Packet pk (0, db);
  pk.setSequenceNum(sn);

  // change to validate based on checksum and sequence number

  if(sn == 0) return false; //doesn't matter, only for the old version (netwark)
  if(cs != pk.generateCheckSum()) return false;
  return true;
}

bool getFile(){

  /* Loop forever, waiting for messages from a client. */
  cout << "Waiting on port " << PORT << "..." << endl;

  ofstream file("Dumpfile");

  for (;;) {
    unsigned char packet[PAKSIZE + 1];
    unsigned char dataPull[PAKSIZE - 7 + 1];
    rlen = recvfrom(s, packet, PAKSIZE, 0, (struct sockaddr *)&ca, &calen);

    for(int x = 0; x < PAKSIZE - 7; x++) {
      dataPull[x] = packet[x + 7];
    }
    dataPull[PAKSIZE - 7] = '\0';
    packet[PAKSIZE] = '\0';
    if (rlen > 0) {
      char * css = new char[6];
      memcpy(css, &packet[1], 5);
      css[5] = '\0';
      cout << endl << endl << "=== RECEIPT" << endl;
      cout << "Seq. num: " << packet[0] << endl;
      cout << "Checksum: " << css << endl;
      cout << "Received message: " << dataPull << endl;
      cout << "Sent response: ";
      if(isvpack(packet)) {
        ack = ACK;
        //seqNum = (seqNum) ? false : true;
        file << dataPull;
      } else {
        ack = NAK;
      }
      cout << ((ack == ACK) ? "ACK" : "NAK") << endl;
      Packet p (false, reinterpret_cast<const char *>(dataPull));
      p.setCheckSum(boost::lexical_cast<int>(css));
      p.setAckNack(ack);

      if(sendto(s, p.str(), PAKSIZE, 0, (struct sockaddr *)&ca, calen) < 0) {
        cout << "Acknowledgement failed. (socket s, acknowledgement message ack, client address ca, client address length calen)" << endl;
        return 0;
      }
      delete css;
    }
  }
  file.close();
  return true;
}

bool loadFile() {

  ifstream is (FILENAME, ifstream::binary);

  if(is) {
    is.seekg(0, is.end);
    length = is.tellg();
    is.seekg(0, is.beg);

    file = new char[length];

    cout << "Reading " << length << " characters..." << endl;
    is.read(file, length);

    if(!is) { cout << "File reading failed. (filename " << FILENAME << "). Only " << is.gcount() << " could be read."; return false; }
    is.close();
  }
  return true;
}

void loadWindow(){
	for(int i = base; i < base + WIN_SIZE; i++) {
		window[i-base] = createPacket(i);
		if(strlen(window[i-base].getDataBuffer()) < BUFSIZE && window[i-base].chksm()) {
			cout << "In loadWindow's secret base, index is: " << i-base << endl;
			for(++i; i < base + WIN_SIZE; i++){
				window[i-base].loadDataBuffer("\0");
			}

		}
		/*cout << "window[i-base] seq. num: " << window[i-base].getSequenceNum() << endl;*/
	}

		/*cout << "packet " << base + 1 << ": " << window[1].str() << endl;
		cout << "packet " << base + 2 << ": " << window[2].str() << endl;*/
}

bool sendFile() {
	/*Currently causes the program to only send the first 16 packets of file out
		requires additional code later to sendFile again with updated window*/
	fd_set stReadFDS;

	struct timeval stTimeOut;

	FD_ZERO(&stReadFDS);
	stTimeOut.tv_sec = 0;
	stTimeOut.tv_usec = 1000 * TIMEOUT;
	FD_SET(s, &stReadFDS);

	base = 0;
	int desc_ready;
	int finale = -1;
	int max_sd;
	bool hasRead = false;
	while(base * BUFSIZE < length) {
		loadWindow();

		if(p.str()[0] == '\0') finale = p.getSequenceNum();
		for(int x = 0; x < WIN_SIZE; x++) {
			p = window[x];
			if(!sendPacket()) continue;
		}
		while(finale < 15) {
			cout << endl << "beginning of loop " << x << endl;
			FD_ZERO(&stReadFDS);
			stTimeOut.tv_sec = 0;
			stTimeOut.tv_usec = 1000 * TIMEOUT;
			FD_SET(s, &stReadFDS);

			max_sd = s;
			cout << endl << "before select" << endl;
			int t = select(max_sd + 1, &stReadFDS, NULL, NULL, &stTimeOut);
			cout << "after select" << endl;
			if (t == -1){
				perror("select()");
			}
			if (t == 0) {
				cout << "=== ACK TIMEOUT (select)" << endl;

			}
			desc_ready = t;
			for(int u = 0; u <= max_sd  &&  desc_ready > 0; u++){
				if(recvfrom(s, b, BUFSIZE + 7, 0, (struct sockaddr *)&ca, &calen) < 0) {
					cout << "=== ACK TIMEOUT (recvfrom)" << endl;
				} else hasRead = true;
				if(!hasRead) continue;
				if(isAck()) {
					cout << "if isAck()" << endl;
					handleAck();
					finale++;
				} else {
					cout << "Not an ACK!" << endl;
				}
			}
			cout << "end of loop " << x << endl;
			/*if(finale > 0 && base == finale) {cout << "Finale: " << finale << endl; break;}
			memset(b, 0, BUFSIZE);
			}*/

		}

	if(sendto(s, "\0", BUFSIZE, 0, (struct sockaddr *)&ca, sizeof(ca)) < 0) {
		cout << "Final package sending failed. (socket s, server address sa, message m)" << endl;
		return false;
	}
	return true;
}

Packet createPacket(int index){
    string mstr = fstr.substr(index * BUFSIZE, BUFSIZE);

	if(mstr.length() < BUFSIZE) {
		cout << "Null terminated mstr." << endl;
		mstr[length - (index * BUFSIZE)] = '\0';
    }

	cout << "index: " << index << endl;
    return Packet (index, mstr.c_str());
}

bool sendPacket(){
	cout << endl;
    cout << "=== TRANSMISSION START" << endl;
	cout << "Sequence number before gremlin function: " << p.getSequenceNum() << endl;
    int pc = probCorrupt; int pl = probLoss; int pd = probDelay;
	bool* pckStatus = gremlin(&p, pc, pl, pd);

	dropPck = pckStatus[0];
	delayPck = pckStatus[1];

	if (dropPck == true) return false;
	if (delayPck == true) p.setAckNack(1);

    if(sendto(s, p.str(), BUFSIZE + 8, 0, (struct sockaddr *)&ca, sizeof(ca)) < 0) {
		cout << "Package sending failed. (socket s, server address sa, message m)" << endl;
		return false;
    }
    return true;
}
bool isAck() {
    cout << endl << "=== SERVER RESPONSE TEST" << endl;
    cout << "Data: " << b << endl;
    if(b[6] == '\0') return true;
    else return false;
}
void handleAck() {
	int ack = boost::lexical_cast<int>(b);
	if(base < ack) base = ack;
	cout << "Window base: " << base << endl;
}
void handleNak(int& x) {

      char * sns = new char[2];
      memcpy(sns, &b[0], 1);
      sns[1] = '\0';

      char * css = new char[5];
      memcpy(css, &b[1], 5);

      char * db = new char[BUFSIZE + 1];
      memcpy(db, &b[2], BUFSIZE);
      db[BUFSIZE] = '\0';

      cout << "Sequence number: " << sns << endl;
      cout << "Checksum: " << css << endl;

      Packet pk (0, db);
      pk.setSequenceNum(boost::lexical_cast<int>(sns));
      pk.setCheckSum(boost::lexical_cast<int>(css));

      if(!pk.chksm()) x--;
      else x = (x - 2 > 0) ? x - 2 : 0;
}
bool* gremlin(Packet * pack, int corruptProb, int lossProb, int delayProb){

  bool* packStatus = new bool[2];
  int r = rand() % 100;

  cout << "Corruption probability: " << corruptProb << endl;
  cout << "Random number: " << r << endl;

  packStatus[0] = false;
  packStatus[1] = false;

  if(r <= (lossProb)){
	packStatus[0] = true;
    cout << "Dropped!" << endl;
  }
  else if(r <= (delayProb)){
	  packStatus[1] = true;
	  cout << "Delayed!" << endl;
  }
  else if(r <= (corruptProb)){
    cout << "Corrupted!" << endl;
	cout << "Seq. num (pre-gremlin): " << pack->getSequenceNum() << endl;
    pack->loadDataBuffer((char*)"GREMLIN LOL");
  }
  cout << "Seq. num: " << pack->getSequenceNum() << endl;
  cout << "Checksum: " << pack->getCheckSum() << endl;
  //cout << "Message: "  << pack->getDataBuffer() << endl;

  return packStatus;
}<|endoftext|>