package serial_ubx

import (
	"errors"
	"fmt"
	"git.timovolkmann.de/gyrogpsc/ublox"
	"go.bug.st/serial"
	"log"
	"time"
)

type Measurement struct {
	//Timestamp		int64		`json:"timestamp"`
	Timestamp   time.Time `json:"timestamp"`
	Position    [3]int32  `json:"position"`    // Latitude, Longitude, Height
	Orientation [3]int32  `json:"orientation"` // Pitch, Roll, Heading
}

type ubxReceiver struct {
	decoder     *ublox.Decoder
	currentMeas Measurement
}

func Setup(portname string) (*ubxReceiver, error) {
	mode := &serial.Mode{
		BaudRate: 115200,
	}
	port, err := serial.Open(portname, mode)
	if err != nil {
		return nil, err
	}
	return &ubxReceiver{
		decoder: ublox.NewDecoder(port),
	}, nil
}

func (u *ubxReceiver) Next() (ublox.Message, error) {
	return u.decoder.Decode()
}

var (
	errNotImplemented = errors.New("message not implemented")
)

// TODO: additional callback with adjustable timing
func (u *ubxReceiver) NextMeasurement() (*Measurement, error) {

	msg, err := u.decoder.Decode()
	if err != nil {
		return nil, err
	}
	//t := time.Time{}
	switch v := msg.(type) {
	case *ublox.NavPvt:
		t, err := time.Parse(time.RFC3339Nano, formatTime(v.Year_y, v.Month_month, v.Day_d, v.Hour_h, v.Min_min, v.Sec_s, v.Nano_ns))
		if err != nil {
			log.Println(err)
		}
		//u.currentMeas.Timestamp = t.UnixNano()
		u.currentMeas.Timestamp = t
		u.currentMeas.Position[0] = v.Lat_dege7
		u.currentMeas.Position[1] = v.Lon_dege7
		u.currentMeas.Position[2] = v.Height_mm
		//fmt.Printf("%T %v\n", *v, *v)
	case *ublox.HnrPvt:
		t, err := time.Parse(time.RFC3339Nano, formatTime(v.Year_y, v.Month_month, v.Day_d, v.Hour_h, v.Min_min, v.Sec_s, v.Nano_ns))
		if err != nil {
			log.Println(err)
		}
		u.currentMeas.Timestamp = t
		u.currentMeas.Position[0] = v.Lat_dege7
		u.currentMeas.Position[1] = v.Lon_dege7
		u.currentMeas.Position[2] = v.Height_mm
		//fmt.Printf("%T %v\n", *v, *v)
	case *ublox.NavAtt:
		u.currentMeas.Orientation[0] = v.Pitch_deg
		u.currentMeas.Orientation[1] = v.Roll_deg
		u.currentMeas.Orientation[2] = v.Heading_deg
		//fmt.Printf("%T %v\n", *v, *v)

	//case *ublox.RawMessage:
	//	//fmt.Printf("%T %v\n\n", *v, *v)
	default:
		return nil, errNotImplemented
	}
	m := u.currentMeas
	return &m, nil
}

func formatTime(Year_y uint16, Month_month byte, Day_d byte, Hour_h byte, Min_min byte, Sec_s byte, Nano_ns int32) string {
	//Nano_ns *= 1e+3
	if Nano_ns < 0 {
		Nano_ns += int32(time.Second)
		if Sec_s > 0 {
			Sec_s--
		} else if Min_min > 0 {
			Sec_s = 59
			Min_min--
		} else if Hour_h > 0 {
			Sec_s = 59
			Min_min = 59
			Hour_h--
		} else if Day_d > 1 {
			Sec_s = 59
			Min_min = 59
			Hour_h = 23
			Day_d--
		} // TODO: more cases for exact behavior! good for now...
	}
	//fmt.Printf("%04d-%02d-%02dT%02d:%02d:%02d.%09dZ00:00\n", Year_y, Month_month, Day_d, Hour_h,  Min_min, Sec_s, Nano_ns )
	return fmt.Sprintf("%04d-%02d-%02dT%02d:%02d:%02d.%09dZ", Year_y, Month_month, Day_d, Hour_h, Min_min, Sec_s, Nano_ns)
}

func (u *ubxReceiver) Printloop() {

	for {
		meas, err := u.NextMeasurement()
		if err != nil {
			continue
		}
		fmt.Println(meas)
	}

}