implement TouchCal;

#
# calibrate a touch screen
#
# Copyright © 2001 Vita Nuova Holdings Limited.  All rights reserved.
#

include "sys.m";
	sys:	Sys;

include "draw.m";
	draw:	Draw;
	Display, Font, Image, Point, Pointer, Rect: import draw;

include "tk.m";
	tk: Tk;

include "translate.m";
	translate: Translate;
	Dict: import translate;

TouchCal: module
{
	init:	fn(ctxt: ref Draw->Context, argv: list of string);
};


Margin: con 20;

tkconfig(w, h: int): array of string
{
	return array[] of {
	"frame .c -borderwidth 0 -bg white -width "+string w+" -height "+string h,
	"bind .c <Button-1> {send cmd push %x %y}",
	"bind .c <Motion-Button-1> {send cmd move %x %y}",
	"bind .c <ButtonRelease-1> {send cmd release %x %y}",
	"pack .c -expand 1 -fill both",
	"pack . propagate 0",
	"update"
	};
}

prompt:= "Please tap the centre\nof the cross\nwith the stylus";

mousepid := 0;

init(ctxt: ref Draw->Context, args: list of string)
{
	r: Rect;
	disp: ref Image;
	top: ref Tk->Toplevel;

	if(args != nil)
		args = tl args;
	debug := args != nil && hd args == "-d";
	sys = load Sys Sys->PATH;
	draw = load Draw Draw->PATH;
	if(draw == nil)
		err(sys->sprint("no Draw module: %r"));
	translate = load Translate Translate->PATH;
	if(translate != nil){
		translate->init();
		(dict, nil) := translate->opendict(translate->mkdictname("", "touchcal"));
		if(dict != nil)
			prompt = dict.xlate(prompt);
		dict = nil;
		translate = nil;
	}
	ptr := chan of ref Pointer;
	pidc := chan of int;
	if(ctxt != nil && ctxt.screen != nil){
		# under Tk, allocate a screen-sized frame to catch mouse events
		tk = load Tk Tk->PATH;
		if(tk == nil)
			err(sys->sprint("no Tk module: %r"));
		disp = ctxt.screen.image;
		r = disp.r;
		top = tk->toplevel(ctxt.screen, sys->sprint("-width %d -height %d -x %d -y %d", r.dx(), r.dy(), r.min.x, r.min.y));
		tkf := tkconfig(r.dx(), r.dy());
		for(i := 0; i < len tkf; i++)
			tk->cmd(top, tkf[i]);
		cmd := chan of string;
		tk->namechan(top, cmd, "cmd");
		spawn tkmouse(cmd, ptr, pidc);
	}else{
		# standalone, catch them ourselves
		mfd := sys->open("/dev/pointer", Sys->OREAD);
		if(mfd == nil)
			err(sys->sprint("can't open /dev/pointer: %r"));
		spawn mouse(mfd, ptr, pidc);
	}
	mousepid = <-pidc;
	display := draw->Display.allocate(nil);
	disp = display.image;
	r = disp.r;
	white := display.color(Draw->White);
	black := display.color(Draw->Black);
	red := display.color(Draw->Red);
	disp.draw(r, white, display.ones, display.ones.r.min);
	samples := array[4] of Point;
	points := array[4] of Point;
	points[0] = (r.min.x+Margin, r.min.y+Margin);
	points[1] = (r.max.x-Margin, r.min.y+Margin);
	points[2] = (r.max.x-Margin, r.max.y-Margin);
	points[3] = (r.min.x+Margin, r.max.y-Margin);
	midpoint := Point((r.min.x+r.max.x)/2, (r.min.y+r.max.y)/2);
	refx := FX((points[1].x - points[0].x) + (points[2].x - points[3].x), 1);
	refy := FX((points[3].y - points[0].y) + (points[2].y - points[1].y), 1);
	ctl := sys->open("/dev/touchctl", Sys->ORDWR);
	if(ctl == nil)
		ctl = sys->open("/dev/null", Sys->ORDWR);
	if(ctl == nil)
		err(sys->sprint("can't open /dev/touchctl: %r"));
	#oldvalues := array[128] of byte;
	#nr := sys->read(ctl, oldvalues, len oldvalues);
	#if(nr < 0)
	#	err(sys->sprint("can't read old values from /dev/touchctl: %r"));
	#oldvalues = oldvalues[0:nr];
	sys->fprint(ctl, "X %d %d %d\nY %d %d %d\n", FX(1,1), 0, 0, 0, FX(1,1), 0);	# identity
	font := Font.open(display, sys->sprint("/fonts/lucida/unicode.%d.font", 6+(r.dx()/512)));
	if(font == nil)
		font = Font.open(display, "*default*");
	if(font != nil){
		drawtext(disp, midpoint, black, font, prompt);
		font = nil;
	}
	for(;;) {
		tm := array[] of {0 to 2 =>array[] of {0, 0, 0}};
		for(i := 0; i < 4; i++){
			cross(disp, points[i], red);
			samples[i] = getpoint(ptr);
			cross(disp, points[i], white);
		}
		# first, rotate if necessary
		rotate := 0;
		if(abs(samples[1].x-samples[2].x) > 80 && abs(samples[2].y-samples[3].y) > 80){
			rotate = 1;
			for(i = 0; i < len samples; i++)
				samples[i] = (samples[i].y, samples[i].x);
		}
		# calculate scaling and offset transformations
		actx := (samples[1].x-samples[0].x)+(samples[2].x-samples[3].x);
		acty := (samples[3].y-samples[0].y)+(samples[2].y-samples[1].y);
		if(actx == 0 || acty == 0)
			continue;		# either the user or device is not trying
		tm[0][rotate] = refx/actx;
		tm[0][2] = FX(points[0].x - XF(tm[0][rotate]*samples[0].x), 1);
		tm[1][1-rotate] = refy/acty;
		tm[1][2] = FX(points[0].y - XF(tm[1][1-rotate]*samples[0].y), 1);
		cross(disp, midpoint, red);
		m := getpoint(ptr);
		cross(disp, midpoint, white);
		p := Point(ptmap(tm[0], m.x, m.y), ptmap(tm[1], m.x, m.y));
		if(debug){
			for(k:=0; k<4; k++)
				sys->print("%d %d,%d %d,%d\n", k, points[k].x,points[k].y, samples[k].x, samples[k].y);
			if(rotate)
				sys->print("rotated\n");
			sys->print("rx=%d ax=%d ry=%d ay=%d tm[0][0]=%d\n", refx, actx, refy, acty, tm[0][0]);
			sys->print("%g %g %g\n%g %g %g\n",
				G(tm[0][0]), G(tm[0][1]), G(tm[0][2]),
				G(tm[1][0]), G(tm[1][1]), G(tm[1][2]));
			sys->print("%d %d -> %d %d (%d %d)\n", m.x, m.y, p.x, p.y, midpoint.x, midpoint.y);
		}
		if(abs(p.x-midpoint.x) > 5 || abs(p.y-midpoint.y) > 5)
			continue;
		printmat(sys->fildes(1), tm);
		if(debug || printmat(ctl, tm) >= 0){
			disp.draw(r, white, display.ones, display.ones.r.min);
			break;
		}
		sys->fprint(sys->fildes(2), "touchcal: can't set calibration: %r\n");
	}
	if(mousepid > 0)
		kill(mousepid);
}

printmat(fd: ref Sys->FD, tm: array of array of int): int
{
	return sys->fprint(fd, "X %d %d %d\nY %d %d %d\n",
		    tm[0][0], tm[0][1], tm[0][2],
		    tm[1][0], tm[1][1], tm[1][2]);
}

FX(a, b: int): int
{
	return (a << 16)/b;
}

XF(v: int): int
{
	return v>>16;
}

G(v: int): real
{
	return real v / 65536.0;
}

ptmap(m: array of int, x, y: int): int
{
	return XF(m[0]*x + m[1]*y + m[2]);
}

mouse(fd: ref Sys->FD, mc: chan of ref Pointer, pidc: chan of int)
{
	pidc <-= sys->pctl(0, nil);
	buf := array[64] of byte;
	for(;;){
		n := sys->read(fd, buf, len buf);
		if(n <= 0)
			err(sys->sprint("can't read /dev/pointer: %r"));

		if(int buf[0] != 'm' || n != 37)
			continue;

		x := int string buf[ 1:13];
		y := int string buf[12:25];
		b := int string buf[24:37];
		mc <-= ref Pointer(b, (x,y));
	}
}

tkmouse(cmd: chan of string, mc: chan of ref Pointer, pidc: chan of int)
{
	pidc <-= sys->pctl(0, nil);
	m := Pointer(0, (0,0));
	for(;;){
		s := <-cmd;
		(nf, flds) := sys->tokenize(s, " \t");
		if(nf < 3)
			continue;
		m.xy = (int hd tl flds, int hd tl tl flds);
		case hd flds {
		"push" =>
			m.buttons = 1;
		"release" =>
			m.buttons = 0;
		}
		mc <-= ref m;
	}
}

getpoint(mousec: chan of ref Pointer): Point
{
	p := Point(0,0);
	while((m := <-mousec).buttons == 0)
		p = m.xy;
	n := 0;
	do{
		if(abs(p.x-m.xy.x) > 10 || abs(p.y-m.xy.y) > 10){
			n = 0;
			p = m.xy;
		}else{
			p = p.mul(n).add(m.xy).div(n+1);
			n++;
		}
	}while((m = <-mousec).buttons & 7);
	return p;
}

cross(im: ref Image, p: Point, col: ref Image)
{
	im.line(p.sub((0,10)), p.add((0,10)), Draw->Endsquare, Draw->Endsquare, 0, col, col.r.min);
	im.line(p.sub((10,0)), p.add((10,0)), Draw->Endsquare, Draw->Endsquare, 0, col, col.r.min);
	im.flush(Draw->Flushnow);
}

drawtext(im: ref Image, p: Point, col: ref Image, font: ref Font, text: string)
{
	(n, lines) := sys->tokenize(text, "\n");
	p = p.sub((0, (n+1)*font.height));
	for(; lines != nil; lines = tl lines){
		s := hd lines;
		w := font.width(s);
		im.text(p.sub((w/2, 0)), col, col.r.min, font, s);
		p = p.add((0, font.height));
	}
}

abs(x: int): int
{
	if(x < 0)
		return -x;
	return x;
}

kill(pid: int)
{
	fd := sys->open("#p/"+string pid+"/ctl", Sys->OWRITE);
	if(fd != nil)
		sys->fprint(fd, "kill");
}

err(s: string)
{
	sys->fprint(sys->fildes(2), "touchcal: %s\n", s);
	if(mousepid > 0)
		kill(mousepid);
	sys->raise("fail:touch");
}