settle_points.go

package main

import (
	"fmt"
	"math"
	"math/rand"
	"sort"
	"time"
	"os"
	"bufio"
	"strings"
	"strconv"
)

type Point struct {
	Name   string
	X, Y   float64
	dx, dy float64
	Links  map[string]*Link
}

type Link struct {
	From, To *Point
	Distance float64 // Distance as seen, not corrected for bad signal level
	Signal   float64 // (0-1]
}

func (l *Link) String() string {
	return fmt.Sprintf("%s<->%s: %0.1f", l.From.Name, l.To.Name, l.Corrected())
}

func (l *Link) Corrected() float64 {
	return l.Distance * l.Signal
}

var points = map[string]*Point{}

// true if new point added, else if already present
func AddPoint(name string) bool {
	if _, found := points[name]; found {
		return false
	}
	points[name] = &Point{
		Name:  name,
		Links: map[string]*Link{},
	}
	return true
}

// true if link updated, false if previous link is better than newer
// panic if one of the points is missing
func UpdateLink(from string, to string, distance float64) bool {
	a, found := points[from]
	if !found {
		panic(fmt.Errorf("point %s is missing", from))
	}
	b, found := points[to]
	if !found {
		panic(fmt.Errorf("point %s is missing", to))
	}

	a2b, found1 := a.Links[to]
	b2a, found2 := b.Links[from]
	if (!found1 && !found2) {
		a.Links[to] = &Link{
			From:     a,
			To:       b,
			Distance: distance,
			Signal:   1.0,
		}
		return true
	}
	if (found1 && found2) {
		panic(fmt.Errorf("duplicate links between points %s and %s", from, to))
	}
	var old *Link
	if (found1) {
		old = a2b
	} else {
		old = b2a
	}
	
	// TODO: take signal correction into account?
	if (old.Distance > distance) {
		corr := old.Corrected()
		old.Distance = distance
		old.Signal = corr / old.Distance
		if (old.Signal > 1.0) {
			old.Signal = 1.0
		}
		return true
	}

	return false
}

func GetLink(from, to string) *Link {
	a, found := points[from]
	if !found {
		panic(fmt.Errorf("point %s is missing", from))
	}
	b, found := points[to]
	if !found {
		panic(fmt.Errorf("point %s is missing", to))
	}

	a2b, found1 := a.Links[to]
	b2a, found2 := b.Links[from]
	if (!found1 && !found2) {
		return nil
	}
	if (found1 && found2) {
		panic(fmt.Errorf("duplicate links between points %s and %s", from, to))
	}
	if (found1) {
		return a2b
	}
	return b2a
}

func SortAllLinks() []*Link {
	all := []*Link{}

	for _, a := range points {
		for _, link := range a.Links {
			all = append(all, link)
		}
	}

	sort.Slice(all, func(i, j int) bool {
		return all[i].Distance < all[j].Distance
	})

	return all
}

func SetupRandomLocations() {
	// TODO: any way to do this better?
	for _, a := range points {
		a.X = rand.Float64()
		a.Y = rand.Float64()
	}
}

const max_move_per_step = 0.1

func DoUpdate_1() {
	for _, a := range points {
		a.dx = 0.0
		a.dy = 0.0
	}

	links := SortAllLinks()
	best := links[0].Distance
	for _, link := range links {
		importance := best / link.Distance // decrease link importance as we move away
		move := max_move_per_step * importance * (0.25 + 0.75 * rand.Float64())
		x, y := link.To.X - link.From.X, link.To.Y - link.From.Y
		r := math.Sqrt(x*x + y*y)
		if (r < link.Distance) {
			move = -move
		}
		dir := 0.0
		if r < 0.0001 {
			// Distance is very small, lets go into any random direction
			dir = rand.Float64() * 2 * math.Pi - math.Pi
		} else {
			dir = math.Atan2(y, x)
		}

		// fmt.Println(link, move, x, y, r, dir / math.Pi, math.Sin(dir) * move, math.Cos(dir) * move)
		link.From.dx += math.Cos(dir) * move / 2
		link.To.dx -= math.Cos(dir) * move / 2
		link.From.dy += math.Sin(dir) * move / 2
		link.To.dy -= math.Sin(dir) * move / 2
	}

	for _, a := range points {
		a.X += a.dx
		a.Y += a.dy
	}
}

func PrintPoints() {
	all := make([]*Point, 0, len(points))
	for _, v := range points {
		all = append(all, v)
	}
	sort.Slice(all, func(i, j int) bool {
		return all[i].Name < all[j].Name
	})
	for _, a := range all {
		fmt.Printf("%s=(%0.1f, %0.1f)\n", a.Name, a.X, a.Y)
	}
}

func SIG(rssi float64) float64 {
	ret := math.Pow(10, (-0x45-rssi)/10/3.77) // 3.77?
	// fmt.Println(rssi, ret)
	return ret
}

func SetTestPoints_1() {
	AddPoint("c2"); AddPoint("c9"); AddPoint("df"); AddPoint("e6"); AddPoint("e7"); AddPoint("f2"); AddPoint("f8"); AddPoint("fb")/*; AddPoint("fd")*/

	// 1m square
	// UpdateLink("c2", "c9", SIG(-0x49)); UpdateLink("c2", "df", SIG(-0x46)); UpdateLink("c2", "e6", SIG(-0x49)); UpdateLink("c2", "e7", SIG(-0x4c)); UpdateLink("c2", "f2", SIG(-0x4a)); UpdateLink("c2", "f8", SIG(-0x4d)); UpdateLink("c2", "fb", SIG(-0x4f)); UpdateLink("c2", "fd", SIG(-0x4c));
	// UpdateLink("c9", "c2", SIG(-0x49)); UpdateLink("c9", "df", SIG(-0x4d)); UpdateLink("c9", "e6", SIG(-0x48)); UpdateLink("c9", "e7", SIG(-0x51)); UpdateLink("c9", "f2", SIG(-0x4f)); UpdateLink("c9", "f8", SIG(-0x45)); UpdateLink("c9", "fb", SIG(-0x4a)); UpdateLink("c9", "fd", SIG(-0x45));
	// UpdateLink("df", "c2", SIG(-0x46)); UpdateLink("df", "c9", SIG(-0x4d)); UpdateLink("df", "e6", SIG(-0x4a)); UpdateLink("df", "e7", SIG(-0x4b)); UpdateLink("df", "f2", SIG(-0x4d)); UpdateLink("df", "f8", SIG(-0x43)); UpdateLink("df", "fb", SIG(-0x43)); UpdateLink("df", "fd", SIG(-0x4b));
	// UpdateLink("e6", "c2", SIG(-0x49)); UpdateLink("e6", "c9", SIG(-0x48)); UpdateLink("e6", "df", SIG(-0x4a)); UpdateLink("e6", "e7", SIG(-0x50)); UpdateLink("e6", "f2", SIG(-0x48)); UpdateLink("e6", "f8", SIG(-0x46)); UpdateLink("e6", "fb", SIG(-0x46)); UpdateLink("e6", "fd", SIG(-0x4d));
	// UpdateLink("e7", "c2", SIG(-0x4c)); UpdateLink("e7", "c9", SIG(-0x51)); UpdateLink("e7", "df", SIG(-0x4b)); UpdateLink("e7", "e6", SIG(-0x50)); UpdateLink("e7", "f2", SIG(-0x4b)); UpdateLink("e7", "f8", SIG(-0x4d)); UpdateLink("e7", "fb", SIG(-0x4d)); UpdateLink("e7", "fd", SIG(-0x56));
	// UpdateLink("f2", "c2", SIG(-0x4a)); UpdateLink("f2", "c9", SIG(-0x4f)); UpdateLink("f2", "df", SIG(-0x4d)); UpdateLink("f2", "e6", SIG(-0x48)); UpdateLink("f2", "e7", SIG(-0x4b)); UpdateLink("f2", "f8", SIG(-0x49)); UpdateLink("f2", "fb", SIG(-0x49)); UpdateLink("f2", "fd", SIG(-0x52));
	// UpdateLink("f8", "c2", SIG(-0x4d)); UpdateLink("f8", "c9", SIG(-0x45)); UpdateLink("f8", "df", SIG(-0x43)); UpdateLink("f8", "e6", SIG(-0x46)); UpdateLink("f8", "e7", SIG(-0x4d)); UpdateLink("f8", "f2", SIG(-0x49)); UpdateLink("f8", "fb", SIG(-0x46)); UpdateLink("f8", "fd", SIG(-0x51));
	// UpdateLink("fb", "c2", SIG(-0x4f)); UpdateLink("fb", "c9", SIG(-0x4a)); UpdateLink("fb", "df", SIG(-0x43)); UpdateLink("fb", "e6", SIG(-0x46)); UpdateLink("fb", "e7", SIG(-0x4d)); UpdateLink("fb", "f2", SIG(-0x49)); UpdateLink("fb", "f8", SIG(-0x46)); UpdateLink("fb", "fd", SIG(-0x4c));
	// UpdateLink("fd", "c2", SIG(-0x4c)); UpdateLink("fd", "c9", SIG(-0x45)); UpdateLink("fd", "df", SIG(-0x4b)); UpdateLink("fd", "e6", SIG(-0x4d)); UpdateLink("fd", "e7", SIG(-0x56)); UpdateLink("fd", "f2", SIG(-0x52)); UpdateLink("fd", "f8", SIG(-0x51)); UpdateLink("fd", "fb", SIG(-0x4c));

	// L
	// UpdateLink("c2", "fb", SIG(-0x49)); UpdateLink("c2", "fd", SIG(-0x3f)); UpdateLink("c2", "c9", SIG(-0x44)); UpdateLink("c2", "df", SIG(-0x38)); UpdateLink("c2", "e6", SIG(-0x2d)); UpdateLink("c2", "e7", SIG(-0x49)); UpdateLink("c2", "f8", SIG(-0x42)); UpdateLink("c2", "f2", SIG(-0x4f));
	// UpdateLink("c9", "fb", SIG(-0x38)); UpdateLink("c9", "fd", SIG(-0x43)); UpdateLink("c9", "c2", SIG(-0x45)); UpdateLink("c9", "e6", SIG(-0x46)); UpdateLink("c9", "e7", SIG(-0x3a)); UpdateLink("c9", "f2", SIG(-0x4f)); UpdateLink("c9", "f8", SIG(-0x36)); UpdateLink("c9", "df", SIG(-0x3e));
	// UpdateLink("df", "fb", SIG(-0x45)); UpdateLink("df", "fd", SIG(-0x42)); UpdateLink("df", "c2", SIG(-0x39)); UpdateLink("df", "e7", SIG(-0x43)); UpdateLink("df", "f2", SIG(-0x48)); UpdateLink("df", "e6", SIG(-0x35)); UpdateLink("df", "c9", SIG(-0x3e)); UpdateLink("df", "f8", SIG(-0x3f));
	// UpdateLink("e6", "fd", SIG(-0x39)); UpdateLink("e6", "c2", SIG(-0x2d)); UpdateLink("e6", "e7", SIG(-0x49)); UpdateLink("e6", "f2", SIG(-0x4c)); UpdateLink("e6", "f8", SIG(-0x3d)); UpdateLink("e6", "fb", SIG(-0x45)); UpdateLink("e6", "c9", SIG(-0x45)); UpdateLink("e6", "df", SIG(-0x35));
	// UpdateLink("e7", "fd", SIG(-0x47)); UpdateLink("e7", "c9", SIG(-0x38)); UpdateLink("e7", "df", SIG(-0x43)); UpdateLink("e7", "e6", SIG(-0x4a)); UpdateLink("e7", "f2", SIG(-0x3b)); UpdateLink("e7", "f8", SIG(-0x3d)); UpdateLink("e7", "fb", SIG(-0x3f)); UpdateLink("e7", "c2", SIG(-0x4b));
	// UpdateLink("f2", "e7", SIG(-0x3e)); UpdateLink("f2", "fb", SIG(-0x4d)); UpdateLink("f2", "fd", SIG(-0x50)); UpdateLink("f2", "c2", SIG(-0x50)); UpdateLink("f2", "c9", SIG(-0x4e)); UpdateLink("f2", "df", SIG(-0x48)); UpdateLink("f2", "e6", SIG(-0x4b)); UpdateLink("f2", "f8", SIG(-0x4c));
	// UpdateLink("f8", "e7", SIG(-0x3c)); UpdateLink("f8", "fb", SIG(-0x3c)); UpdateLink("f8", "fd", SIG(-0x44)); UpdateLink("f8", "c2", SIG(-0x43)); UpdateLink("f8", "df", SIG(-0x36)); UpdateLink("f8", "e6", SIG(-0x3f)); UpdateLink("f8", "f2", SIG(-0x48)); UpdateLink("f8", "c9", SIG(-0x35));
	// UpdateLink("fb", "f2", SIG(-0x4c)); UpdateLink("fb", "f8", SIG(-0x3b)); UpdateLink("fb", "c2", SIG(-0x48)); UpdateLink("fb", "c9", SIG(-0x36)); UpdateLink("fb", "df", SIG(-0x44)); UpdateLink("fb", "e6", SIG(-0x45)); UpdateLink("fb", "fd", SIG(-0x48)); UpdateLink("fb", "e7", SIG(-0x41));

	// c9-f8 <=> 0x35=53 = 0.376m
	// c9-fb <=> 0x38=56 = 0.452m
	// c9-e7 <=> 0x3a=58 = 0.511m
	// fb-e7 <=> 0x41=65 = 0.783m

	// L - without FD (turns out its battery was almost dead, reason unknown, other tags are fine!)
	UpdateLink("c2", "fb", SIG(-0x49)); UpdateLink("c2", "c9", SIG(-0x44)); UpdateLink("c2", "df", SIG(-0x38)); UpdateLink("c2", "e6", SIG(-0x2d)); UpdateLink("c2", "e7", SIG(-0x49)); UpdateLink("c2", "f8", SIG(-0x42)); UpdateLink("c2", "f2", SIG(-0x4f));
	UpdateLink("c9", "fb", SIG(-0x38)); UpdateLink("c9", "c2", SIG(-0x45)); UpdateLink("c9", "e6", SIG(-0x46)); UpdateLink("c9", "e7", SIG(-0x3a)); UpdateLink("c9", "f2", SIG(-0x4f)); UpdateLink("c9", "f8", SIG(-0x36)); UpdateLink("c9", "df", SIG(-0x3e));
	UpdateLink("df", "fb", SIG(-0x45)); UpdateLink("df", "c2", SIG(-0x39)); UpdateLink("df", "e7", SIG(-0x43)); UpdateLink("df", "f2", SIG(-0x48)); UpdateLink("df", "e6", SIG(-0x35)); UpdateLink("df", "c9", SIG(-0x3e)); UpdateLink("df", "f8", SIG(-0x3f));
	UpdateLink("e6", "c2", SIG(-0x2d)); UpdateLink("e6", "e7", SIG(-0x49)); UpdateLink("e6", "f2", SIG(-0x4c)); UpdateLink("e6", "f8", SIG(-0x3d)); UpdateLink("e6", "fb", SIG(-0x45)); UpdateLink("e6", "c9", SIG(-0x45)); UpdateLink("e6", "df", SIG(-0x35));
	UpdateLink("e7", "c9", SIG(-0x38)); UpdateLink("e7", "df", SIG(-0x43)); UpdateLink("e7", "e6", SIG(-0x4a)); UpdateLink("e7", "f2", SIG(-0x3b)); UpdateLink("e7", "f8", SIG(-0x3d)); UpdateLink("e7", "fb", SIG(-0x3f)); UpdateLink("e7", "c2", SIG(-0x4b));
	UpdateLink("f2", "e7", SIG(-0x3e)); UpdateLink("f2", "fb", SIG(-0x4d)); UpdateLink("f2", "c2", SIG(-0x50)); UpdateLink("f2", "c9", SIG(-0x4e)); UpdateLink("f2", "df", SIG(-0x48)); UpdateLink("f2", "e6", SIG(-0x4b)); UpdateLink("f2", "f8", SIG(-0x4c));
	UpdateLink("f8", "e7", SIG(-0x3c)); UpdateLink("f8", "fb", SIG(-0x3c)); UpdateLink("f8", "c2", SIG(-0x43)); UpdateLink("f8", "df", SIG(-0x36)); UpdateLink("f8", "e6", SIG(-0x3f)); UpdateLink("f8", "f2", SIG(-0x48)); UpdateLink("f8", "c9", SIG(-0x35));
	UpdateLink("fb", "f2", SIG(-0x4c)); UpdateLink("fb", "f8", SIG(-0x3b)); UpdateLink("fb", "c2", SIG(-0x48)); UpdateLink("fb", "c9", SIG(-0x36)); UpdateLink("fb", "df", SIG(-0x44)); UpdateLink("fb", "e6", SIG(-0x45)); UpdateLink("fb", "e7", SIG(-0x41));

	// 2m square
	// UpdateLink("c2", "c9", SIG(-0x4d)); UpdateLink("c2", "f8", SIG(-0x53)); UpdateLink("c2", "e6", SIG(-0x4a)); UpdateLink("c2", "f2", SIG(-0x4c)); UpdateLink("c2", "fb", SIG(-0x4a)); UpdateLink("c2", "fd", SIG(-0x53)); UpdateLink("c2", "df", SIG(-0x4d)); UpdateLink("c2", "e7", SIG(-0x4e));
	// UpdateLink("c9", "c2", SIG(-0x4a)); UpdateLink("c9", "e7", SIG(-0x51)); UpdateLink("c9", "fb", SIG(-0x4b)); UpdateLink("c9", "f2", SIG(-0x4e)); UpdateLink("c9", "fd", SIG(-0x51)); UpdateLink("c9", "df", SIG(-0x4b)); UpdateLink("c9", "e6", SIG(-0x4e)); UpdateLink("c9", "f8", SIG(-0x46));
	// UpdateLink("df", "fb", SIG(-0x49)); UpdateLink("df", "fd", SIG(-0x4f)); UpdateLink("df", "c2", SIG(-0x4b)); UpdateLink("df", "e6", SIG(-0x4a)); UpdateLink("df", "e7", SIG(-0x46)); UpdateLink("df", "f2", SIG(-0x47)); UpdateLink("df", "f8", SIG(-0x49)); UpdateLink("df", "c9", SIG(-0x4b));
	// UpdateLink("e6", "fb", SIG(-0x4e)); UpdateLink("e6", "fd", SIG(-0x52)); UpdateLink("e6", "c2", SIG(-0x4b)); UpdateLink("e6", "e7", SIG(-0x47)); UpdateLink("e6", "df", SIG(-0x4a)); UpdateLink("e6", "f2", SIG(-0x4a)); UpdateLink("e6", "f8", SIG(-0x4b)); UpdateLink("e6", "c9", SIG(-0x4c));
	// UpdateLink("e7", "f2", SIG(-0x47)); UpdateLink("e7", "f8", SIG(-0x48)); UpdateLink("e7", "c9", SIG(-0x50)); UpdateLink("e7", "df", SIG(-0x46)); UpdateLink("e7", "e6", SIG(-0x48)); UpdateLink("e7", "fb", SIG(-0x52)); UpdateLink("e7", "fd", SIG(-0x4f)); UpdateLink("e7", "c2", SIG(-0x4d));
	// UpdateLink("f2", "f8", SIG(-0x4d)); UpdateLink("f2", "c9", SIG(-0x4f)); UpdateLink("f2", "df", SIG(-0x47)); UpdateLink("f2", "e6", SIG(-0x48)); UpdateLink("f2", "fb", SIG(-0x4d)); UpdateLink("f2", "fd", SIG(-0x4e)); UpdateLink("f2", "c2", SIG(-0x48)); UpdateLink("f2", "e7", SIG(-0x47));
	// UpdateLink("f8", "fd", SIG(-0x4f)); UpdateLink("f8", "c2", SIG(-0x53)); UpdateLink("f8", "e6", SIG(-0x53)); UpdateLink("f8", "e7", SIG(-0x48)); UpdateLink("f8", "f2", SIG(-0x4c)); UpdateLink("f8", "fb", SIG(-0x53)); UpdateLink("f8", "c9", SIG(-0x48)); UpdateLink("f8", "df", SIG(-0x50));
	// UpdateLink("fb", "fd", SIG(-0x56)); UpdateLink("fb", "c9", SIG(-0x4d)); UpdateLink("fb", "df", SIG(-0x49)); UpdateLink("fb", "e6", SIG(-0x4e)); UpdateLink("fb", "f2", SIG(-0x53)); UpdateLink("fb", "f8", SIG(-0x55)); UpdateLink("fb", "c2", SIG(-0x4e));
}

func SetTestPointsFromFile(file string) {
	f, err := os.Open(file)
	check(err)

	scanner := bufio.NewScanner(f)
	for scanner.Scan() {
		line := scanner.Text()
		if line == "" {
			continue
		}
		tokens := strings.Split(line, " ")
		if len(tokens) < 3 {
			continue
		}
		receiver := tokens[0]
		tokens = tokens[2:] // skip receiver and "=>"

		AddPoint(receiver)

		for _, token := range tokens {
			items := strings.Split(token, "/")
			if len(items) != 3 {
				panic(token)
			}
			rssi, err := strconv.ParseInt(items[2], 16, 64)
			check(err)
			rssi = -rssi // add missing sign

			AddPoint(items[0])
			UpdateLink(receiver, items[0], SIG(float64(rssi)))
		}
	}

	check(scanner.Err())
}

func init() {
	rand.Seed(time.Now().UnixNano())
}

func main() {
	if len(os.Args) < 2 {
		fmt.Println("No datafile provided. Testing with default points")
		SetTestPoints_1()
	} else {
		SetTestPointsFromFile(os.Args[1])
	}

	SetupRandomLocations()

	PrintPoints()

	for i := 0; i < 1000 * 1000; i++ {
		DoUpdate_1()
	}

	fmt.Println("")

	PrintPoints()
	fmt.Println("")
}

func check(err error) {
	if err != nil {
		panic(err)
	}
}