package main

import "fmt"
import "github.com/tarm/serial"
import "bufio"
import "strings"
import "strconv"
import "time"
import "os"

const loopLength = 2.05 // In meters

func main() {
	config := &serial.Config{
			Name: "/dev/ttyUSB0",
			Baud: 9600,
	}

    stream, err := serial.OpenPort(config)
    if err != nil {
            panic(err)
    }

	reader := bufio.NewReader(stream)

	lastCoil := 0;
	lastMillis := 0;
	rAvg := 0.0;
	rAvgCount := 0;
	
	datafile, err := os.OpenFile("data.csv", os.O_APPEND|os.O_WRONLY|os.O_CREATE, 0600)
	if err != nil {
		panic(err)
	}

	defer datafile.Close()


	for {
		lineb, _, _ := reader.ReadLine()
		line := string(lineb)
		coilNum,_ := strconv.Atoi(strings.Split(line, " ")[0])
		coilNum = coilNum % 3
		millis,_ := strconv.Atoi(strings.Split(line, " ")[1])
		d := float64(coilDistance(lastCoil, coilNum)) * (loopLength/3)
		lastCoil = coilNum
		t := float64(millis - lastMillis)/1000
		lastMillis = millis
		s := d/t
		if (s < 0.1 || s > 4) {
			continue
		}
		a := (rAvg*float64(rAvgCount) + s)/float64(rAvgCount+1)
		rAvg = a
		rAvgCount++
		fmt.Printf("u: %d, d: %fm, t: %fs, s: %fm/s, a: %fm/s\n", time.Now().UnixNano()/1000000,d,t,s,a)
		fmt.Fprintf(datafile, "%d, %f\n", time.Now().UnixNano()/1000000, s)
	}


}



func coilDistance(previous int, now int) int {
	d := (previous-now+3)%3
	if d == 0 {
		d = 3
	}
	return d
}