package core

import (
	"errors"
	"git.timovolkmann.de/gyrogpsc/ublox"
	"log"
	"time"

	"github.com/m7shapan/njson"
)

/*{
"smartphone": {
// hier daten von hyperimu
},
"serial": {
// hier Daten von M8U:
"timestamp": 37539672354
"position": [0, 0, 0],
"orientation": [0, 0, 0]
}
}
*/

type sourceId string

const (
	SOURCE_TCP    sourceId = "SOURCE_TCP"
	SOURCE_SERIAL sourceId = "SOURCE_SERIAL"
)

type Sensordata struct {
	itow        uint32
	SourceId    sourceId
	Timestamp   int64
	Position    [3]float64
	Orientation [3]float64
}

func (s *Sensordata) isSameEpoch(n *Sensordata) bool {
	if n.itow == 0 {
		return false
	}
	return s.itow == n.itow
}

func (s *Sensordata) Consolidate(n *Sensordata) *Sensordata {
	if s.SourceId != n.SourceId {
		log.Fatalln("Do not consolidate Sensordata from different Sources")
	}
	if s.isSameEpoch(n) {
		// TODO: implement epoch consolidation logic
	}
	return n
}

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

func ConvertUbxToSensorData(msg interface{}) (*Sensordata, error) {
	sd := &Sensordata{}
	switch v := msg.(type) {
	case *ublox.NavPvt:
		sd.itow = v.ITOW_ms
		sd.Timestamp = time.Date(int(v.Year_y), time.Month(v.Month_month), int(v.Day_d), int(v.Hour_h), int(v.Min_min), int(v.Sec_s), int(v.Nano_ns), time.UTC).UnixNano()
		sd.Position[0] = float64(v.Lat_dege7)
		sd.Position[1] = float64(v.Lon_dege7)
		sd.Position[2] = float64(v.Height_mm)
	case *ublox.HnrPvt:
		sd.itow = v.ITOW_ms
		sd.Timestamp = time.Date(int(v.Year_y), time.Month(v.Month_month), int(v.Day_d), int(v.Hour_h), int(v.Min_min), int(v.Sec_s), int(v.Nano_ns), time.UTC).UnixNano()
		sd.Position[0] = float64(v.Lat_dege7)
		sd.Position[1] = float64(v.Lon_dege7)
		sd.Position[2] = float64(v.Height_mm)
	case *ublox.NavAtt:
		sd.itow = v.ITOW_ms
		sd.Orientation[0] = float64(v.Pitch_deg)
		sd.Orientation[1] = float64(v.Roll_deg)
		sd.Orientation[2] = float64(v.Heading_deg)
	default:
		return nil, errNotImplemented
	}
	return sd, nil
}

func ConvertSensorDataPhone(jsonData []byte) (*Sensordata, error) {
	return convertAndroidHyperImu(jsonData)
}

func convertAndroidHyperImu(jsonData []byte) (*Sensordata, error) {
	prep := struct {
		Timestamp   int64      `njson:"Timestamp"`
		Position    [3]float64 `njson:"GPS"`
		Orientation [3]float64 `njson:"orientation"`
	}{}
	err := njson.Unmarshal(jsonData, &prep)
	if err != nil {
		return nil, err
	}
	sd := &Sensordata{
		Timestamp: prep.Timestamp * int64(time.Millisecond),
		//Timestamp:   time.Unix(0, prep.Timestamp * int64(time.Millisecond)),
		Position:    prep.Position,
		Orientation: prep.Orientation,
	}
	return sd, nil
}