#include <vector>

#include "random.h"
#include "matrix.h"

#include "bendarea.h"


namespace {
	enum BendMode {
		BendNone,
		BendRound,
		BendCorner
	};
	
	class ContourChunk {
	public:
		Vector p1, t0, t1;
		explicit ContourChunk(const Vector &p1 = Vector(), const Vector &t0 = Vector(), const Vector &t1 = Vector()):
			p1(p1), t0(t0), t1(t1) { }
	};

	class BendPoint {
	public:
		Vector p;
		Vector t0, t1;
		bool e0, e1;
		Real l;
		Real length;
		BendMode mode;
		BendPoint(): e0(), e1(), l(), length(), mode(BendNone) { }
	};
	
	class Intersection {
	public:
		Real l;
		BendPoint point;
		explicit Intersection(const Real &l = Real(), const BendPoint &point = BendPoint()):
			l(l), point(point) { }
		bool operator<(const Intersection &other) const { return l < other.l; }
	};
	
	typedef std::vector<Intersection> IntersectionList;
	
	class Contour: public std::vector<ContourChunk> {
	public:
		void hermite_to(const Vector &p1, const Vector &t0, const Vector &t1)
			{ push_back( ContourChunk(p1, t0, t1) ); }
		void line_to(const Vector &p1) {
			if (empty()) {
				hermite_to(p1, Vector(), Vector());
			} else
			if (p1 != back().p1) {
				Vector t = p1 - back().p1;
				hermite_to(p1, t, t);
			}
		}

		void line(const Vector &p0, const Vector &p1)
			{ line_to(p0); line_to(p1); }
		void hermite(const Vector &p0, const Vector &p1, const Vector &t0, const Vector &t1)
			{ line_to(p0); hermite_to(p1, t0, t1); }
		void hermite(const Hermite &h)
			{ hermite(h.p0, h.p1, h.t0, h.t1); }
		
		void put(const Context &context, bool jump = false) const {
			if (empty()) return;
			const_iterator i = begin();
			Hermite h(i->p1, i->p1);
			for(++i; i != end(); ++i) {
				h.p0 = h.p1;
				h.p1 = i->p1;
				h.t0 = i->t0;
				h.t1 = i->t1;
				context.hermite(h, jump);
				jump = false;
			}
		}
		
		void smooth() {
			if (size() < 2) return;
			iterator i0, i2 = begin(), i1 = i2++;
			i2->t0 = i2->p1 - i1->p1;
			for(i0 = i1, i1 = i2++; i2 != end(); i0 = i1, i1 = i2++) {
				if (equal(0, dot(i1->t1, i2->t0.perp())) && less(0, dot(i1->t1, i2->t0))) {
					i1->t1 = i2->t0 = (i1->p1 - i0->p1)*0.5;
				} else {
					i1->t1 = i1->p1 - i0->p1;
					i2->t0 = i2->p1 - i1->p1;
				}
			}
			i1->t1 = i1->p1 - i0->p1;
		}
	};
	
	class BendContour: public std::vector<BendPoint> {
	public:
		const_iterator find(const Real &length) const {
			if (empty()) return end();
			const_iterator a = begin(), b = end() - 1, c;
			if (!less(a->length, length)) return a;
			if (!less(length, b->length)) return b;
			while( (c = a + (b - a)/2) != a ) {
				if (equal(length, c->length)) return c;
				(length < c->length ? b : a) = c;
			}
			return c;
		}
		
		const BendPoint* find_exact(const Real &length) const {
			const_iterator i = find(length);
			return i == end() || !equal(length, i->length) ? nullptr : &*i;
		}

		BendPoint interpolate(const Real &length) const {
			const_iterator i = find(length);
			if (i == end()) return BendPoint();
			
			if (equal(length, i->length)) return *i;
			
			if (length < i->length) {
				// extarpolation before
				Real dlen = length - i->length;
				BendPoint s;
				s.p = i->p + i->t0.norm()*dlen;
				s.t0 = s.t1 = i->t0.norm();
				s.l = i->l + dlen;
				s.length = length;
				s.e0 = s.e1 = i->e0;
				return s;
			}
			
			const_iterator j = i + 1;
			if (j == end()) {
				// extarpolation after
				Real dlen = length - i->length;
				BendPoint s;
				s.p = i->p + i->t1.norm()*dlen;
				s.t0 = s.t1 = i->t1.norm();
				s.l = i->l + dlen;
				s.length = length;
				s.e0 = s.e1 = i->e1;
				return s;
			}
			
			// interpolation
			Real l = (length - i->length)/(j->length - i->length);
			Real dl = j->l - i->l;
			Hermite h(i->p, j->p, i->t1*dl, j->t0*dl);
			
			BendPoint s;
			s.p = h.p(l);
			s.t0 = s.t1 = h.t(l);
			s.l = i->l + l*(j->l - i->l);
			s.length = length;
			s.e0 = s.e1 = (i->e1 && j->e0);
			
			return s;
		}
		
		void half_corner(Contour &dst, const BendPoint &point, const Real &radius, bool flip, bool out) const {
			if (point.mode == BendNone || equal(radius, 0))
				return;

			const Vector &t0 = flip ? point.t1 : point.t0;
			const Vector &t1 = flip ? point.t0 : point.t1;
			const Vector p0 = t0.perp().norm() * radius;
			const Vector p1 = t1.perp().norm() * radius;
			const Vector &p = out ? p1 : p0;
			const Vector &center = point.p;
			const Real rmod = fabs(radius);
			
			Real d = dot( t0, t1.perp() );
			if (!equal(0, d) && (d*radius < 0) == flip) {
				if (point.mode == BendRound) {
					const Vector pp = (p0 + p1).norm()*rmod;
					const Vector ppp = (p + pp).norm()*rmod;
					Real k = (ppp - p).len()/rmod;
					if (dot(p, ppp.perp()) < 0) k = -k;
					if (out) {
						dst.line_to(center + pp);
						dst.hermite_to(center + ppp, pp.perp()*k, ppp.perp()*k);
						dst.hermite_to(center + p, ppp.perp()*k, p.perp()*k);
					} else {
						k = -k;
						dst.line_to(center + p);
						dst.hermite_to(center + ppp, p.perp()*k, ppp.perp()*k);
						dst.hermite_to(center + pp, ppp.perp()*k, pp.perp()*k);
					}
				} else
				if (point.mode == BendCorner) {
					Vector pp = p0 + t0*(dot(p1 - p0, t1.perp())/d);
					if (out) dst.line(center + pp, center + p);
						else dst.line(center + p, center + pp);
				}
			} else {
				Vector pp = (p0 + p1)*0.5;
				if (out) dst.line(center + pp, center + p);
				    else dst.line(center + p, center + pp);
			}
		}

		void bend(Contour &dst, const Contour &src, const Matrix &matrix, int segments) const {
			if (empty() || src.empty())
				dst.insert(dst.end(), src.begin(), src.end());

			const int last_segment = segments - 1;
			const Real step = Real(1)/segments;
			Contour::const_iterator i = src.begin();
			Vector p0 = matrix.transform(i->p1);
			size_t dst_size = dst.size();
			
			IntersectionList intersections;
			while(++i != src.end()) {
				Hermite h(p0, matrix.transform(i->p1), matrix.transform(i->t0, false), matrix.transform(i->t1, false));
				p0 = h.p1;
				
				Real bends[3];
				int bends_count = h.bends_x(bends);
				Range r(h.p0.x);
				r.expand(h.p1.x);
				for(Real *i = bends, *end = i + bends_count; i != end; ++i)
					r.expand( Hermite::p(*i, h.p0.x, h.p1.x, h.t0.x, h.t1.x) );
				bends_count += h.inflection_x(bends + bends_count);
				
				for(const_iterator bi = find(r.a0); bi != end() && !less(r.a1, bi->length); ++bi) {
					Real roots[3];
					int count = h.intersections_x(roots, bi->length);
					for(Real *i = roots, *end = i + count; i != end; ++i)
						intersections.push_back( Intersection(*i, *bi) );
				}
				for(Real *i = bends, *end = i + bends_count; i != end; ++i)
					intersections.push_back( Intersection(*i, interpolate( h.p(*i).x )) );
				Real bsl = step;
				for(int i = 1; i < last_segment; ++i, bsl += step)
					intersections.push_back( Intersection(bsl, interpolate( h.p(bsl).x )) );
				sort(intersections.begin(), intersections.end());
				
				Intersection prev(0, interpolate(h.p0.x));
				intersections.push_back( Intersection(1, interpolate(h.p1.x)) );
				
				for(IntersectionList::const_iterator bi = intersections.begin(); bi != intersections.end(); ++bi) {
					const Intersection &next = *bi;
					if (equal(prev.l, next.l)) continue;

					bool flip = next.point.l < prev.point.l;
					bool e0 = flip ? prev.point.e0 : prev.point.e1;
					bool e1 = flip ? next.point.e1 : next.point.e0;
					if (e0 && e1) {
						Real dl = next.point.l - prev.point.l;
						Vector t0 = flip ? prev.point.t0 : prev.point.t1;
						Vector t1 = flip ? next.point.t1 : next.point.t0;
						
						Hermite bend_h( prev.point.p, next.point.p, t0*dl, t1*dl );
						Bezier src_b = h.sub(prev.l, next.l).get_bezier();
						
						Real a = std::min(src_b.p0.x, src_b.p1.x) - 0.1;
						Real b = std::max(src_b.p0.x, src_b.p1.x) + 0.1;
						assert( !less(src_b.pp0.x, a) && !less(src_b.pp1.x, a)
						     && !less(b, src_b.pp0.x) && !less(b, src_b.pp1.x) );
						
						Real k = next.point.length - prev.point.length;
						k = equal(k, 0) ? 0 : 1/k;
						Real ll0 = (src_b.pp0.x - prev.point.length)*k;
						Real ll1 = (src_b.pp1.x - next.point.length)*k + 1;
						Real kk = flip ? -1 : 1;
						Bezier dst_b(
							bend_h.p0     + bend_h.d(  0).perp()*src_b.p0.y*kk,
							bend_h.p1     + bend_h.d(  1).perp()*src_b.p1.y*kk,
							bend_h.p(ll0) + bend_h.d(ll0).perp()*src_b.pp0.y*kk,
							bend_h.p(ll1) + bend_h.d(ll1).perp()*src_b.pp1.y*kk );

						half_corner(dst, prev.point, src_b.p0.y, flip, true);
						dst.hermite( dst_b.get_hermite() );
						half_corner(dst, next.point, src_b.p1.y, flip, false);
					}
					
					prev = next;
				}
				
				intersections.clear();
			}
			
			if (dst_size < dst.size())
				dst.line_to(dst[dst_size].p1);
		}
		
		void draw(const Context &context) const {
			if (empty()) return;
			
			context->save();
			
			context.set_line_width_px(10);
			context.set_source_rgba(Color::blue().mulalpha(0.1));

			for(const_iterator i = begin(); i != end(); ++i) {
				context.move_to(i->p + i->t0);
				context.line_to(i->p);
				context.line_to(i->p + i->t1);
			}
			context->stroke();

			context.set_source_rgba(Color::blue().mulalpha(0.25));
			for(const_iterator j = begin(), i = j++; j != end(); i = j++) {
				Real dl = j->l - i->l;
				context.hermite(Hermite(i->p, j->p, i->t1*dl, j->t0*dl), true);
			}
			context->stroke();

			context->restore();
		}
	};
	
	void build_contour(Contour &dst, const ActiveCurve &curve) {
		size_t dst_size = dst.size();
		for(ActiveCurve::SegIter i(curve); i; i.next())
			dst.hermite( i.hermite() );
		if (dst_size < dst.size())
			dst.line_to( dst[dst_size].p1 );
	}

	void build_bend(BendContour &dst, const ActiveCurve &curve, bool round, int segments) {
		const Real step = Real(1)/segments;
		size_t dst_size = dst.size();
		BendPoint bp;
		BendMode mode = round ? BendRound : BendCorner;
		
		bp.p = curve.front().get_p();
		bp.e0 = bp.e1 = true;
		bp.l = 0;
		bp.length = 0;
		bp.mode = BendNone;
		dst.push_back( bp );
		
		int index = 0;
		for(ActiveCurve::SegIter i(curve); i; i.next()) {
			Hermite h = i.hermite();
			
			dst.back().p = h.p0;
			dst.back().t1 = h.t0;
			
			Real l0 = index;
			Real l = step;
			bp.mode = BendNone;
			for(int k = 2; k < segments; ++k, l += step) {
				Vector p = h.p(l);
				bp.l = l0 + l;
				bp.length += (p - bp.p).len();
				bp.p = p;
				bp.t0 = bp.t1 = h.t(l);
				dst.push_back( bp );
			}
			
			bp.l = ++index;
			bp.length += (h.p1 - bp.p).len();
			bp.p = h.p1;
			bp.t0 = h.t1;
			bp.mode = mode;
			dst.push_back( bp );
		}
		
		BendPoint &first = dst[dst_size];
		BendPoint &last = dst.back();
		if (curve.loop) {
			first.mode = mode;
			first.t0 = last.t0;
			last.t1 = first.t1;
			first.e0 = last.e1 = false;
		} else {
			first.t0 = first.t1.norm();
			last.t1 = last.t0.norm();
			last.mode = BendNone;
		}
	}
}




BendArea::BendArea():
	p0(new ActivePoint(Vector(100, 100), Color::blue())),
	p1(new ActivePoint(Vector(900, 100), Color::blue()))
{
	set_size_request(1000, 900);

	add_point(p0);
	add_point(p1);
	
	// generate src curve
	Vector b0 = p0->position - Vector(20, 80);
	Vector b1 = p1->position + Vector(20, 0);
	Vector pp(b0.x, b1.y);
	while(true) {
		Vector pt0 = pp + Vector(-50, 0); //rnd::vector(pp.x - 20, b0.y, pp.x + 20, b1.y);
		Vector pt1 = pp + Vector( 50, 0); //rnd::vector(pp.x - 20, b0.y, pp.x + 20, b1.y);

		src_curve.push_back(ActiveCurvePoint(pp, pt0 - pp, pt1 - pp));
		src_curve.back().add_to(*this);

		if (!less(pp.x, b1.x)) break;
		pp = Vector(pp.x + 60, 0.5*(b0.y + b1.y)); //rnd::vector(pp.x + 20, b0.y, pp.x + 100, b1.y);
		if (!less(pp.x, b1.x)) pp = b1;
	}
	src_curve.set_tail_tangents_visible();

	// generate dst curve
	b0.x = p0->position.x;
	b0.y = p0->position.y + 200;
	b1.x = p1->position.x;
	b1.y = 780;
	Vector bt(100, 100);
	//dst_curve.loop = true;
	for(int i = 0; i < 5; ++i) {
		dst_curve.push_back(ActiveCurvePoint(rnd::vector(b0, b1), rnd::vector(-bt, bt), rnd::vector(-bt, bt)));
		dst_curve.back().add_to(*this);
	}
	dst_curve.set_tail_tangents_visible();
}

void BendArea::on_point_move(const ActivePoint::Handle &point, const Vector&oldposition, const Vector &newposition) {
	src_curve.on_point_move(point, oldposition, newposition);
	dst_curve.on_point_move(point, oldposition, newposition);
	if (point == p0 || point == p1)
		p0->position.y = p1->position.y = newposition.y;
}

void BendArea::on_draw_content(const Context &context) {
	context->save();
	
	context.set_line_width_px(1);
	context.set_source_rgba(Color::blue());
	context.move_to(p0->position);
	context.line_to(p1->position);
	context->stroke();
	
	if (!src_curve.empty()) {
		context.set_source_rgba(Color::gray().mulalpha(0.5));
		src_curve.put(context);
		context->close_path();
		context->fill();

		if (!dst_curve.empty()) {
			Contour src_contour;
			BendContour bend;
			Contour dst_contour;
			
			build_contour(src_contour, src_curve);
			build_bend(bend, dst_curve, false, 16);
			
			bend.draw(context);
			
			Real dst_length = bend.back().length;
			Real src_length = p1->position.x - p0->position.x;
			if (!equal(src_length, 0) && !equal(dst_length, 0)) {
				Real kx = dst_length/src_length;
				Real ky = 1;
				Matrix m( kx, 0, 0,
						  0, ky, 0,
						  -kx*p0->position.x, -ky*p0->position.y, 16 );
				bend.bend(dst_contour, src_contour, m, 1);
				//dst_contour.smooth();
				
				dst_contour.put(context, true);
				context->close_path();
				context->fill_preserve();
				context.set_source_rgba(Color::blue());
				context->stroke();
			}
		}
	}
	
	context->restore();
	
	src_curve.draw(context);
	dst_curve.draw(context);
}