package core

import (
	"errors"
	"git.timovolkmann.de/gyrogpsc/ublox"
	"github.com/google/uuid"
	"github.com/sirupsen/logrus"
	"github.com/tidwall/gjson"
	"math"
	"time"
)

type Tracking struct {
	TrackingMetadata
	Data []SensorData
}

type TrackingMetadata struct {
	UUID        uuid.UUID
	TimeCreated time.Time
	Collectors  []CollectorType
	Size        int
}

func newTracking() Tracking {
	return Tracking{
		TrackingMetadata: TrackingMetadata{
			UUID: uuid.New(),
		},
		Data: []SensorData{},
	}
}

func (s *Tracking) isEmpty() bool {
	if len(s.Data) != s.Size {
		logrus.Errorln("data inconsistent...", len(s.Data), s.Size)
	}
	return len(s.Data) == 0
}

type SourceId string

const (
	SOURCE_TCP    SourceId = "SOURCE_TCP"
	SOURCE_SERIAL SourceId = "SOURCE_SERIAL"
)

var lastTimeOffsetIphone int64
var lastTimeOffsetUblox int64

type SensorData struct {
	//MsgClass  string
	//FixType   string
	itow        uint32
	source      SourceId
	latency     int
	Servertime  time.Time
	Timestamp   time.Time
	Position    [3]float64 //`json:",omitempty"`
	HAcc        float64    //`json:",omitempty"`//[H,V]
	VAcc        float64    //`json:",omitempty"`//[H,V]
	Orientation [3]float64 //`json:",omitempty"`
	Speed       float64    //`json:",omitempty"`
	HeadDevice  float64    //`json:",omitempty"` // Course / Heading of Motion
	HeadMotion  float64    //`json:",omitempty"` // Course / Heading of Motion
	HeadingAcc  float64    //`json:",omitempty"`
	Gyroscope   [3]float64 //`json:",omitempty"`
	LinearAcc   [3]float64 //`json:",omitempty"`

}

func (s *SensorData) Source() SourceId {
	return s.source
}

func (s *SensorData) SetSource(si SourceId) {
	s.source = si
}

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

// Consolidates two sensordata elements if they are in the same epoch
func (s SensorData) ConsolidateEpochsOnly(n SensorData) SensorData {
	s.checkSources(&n)
	if s.isSameEpoch(n) {
		null := SensorData{}

		if n.Timestamp == null.Timestamp {
			n.Timestamp = s.Timestamp
		}
		if n.Position == null.Position {
			n.Position = s.Position
		}
		if n.Orientation == null.Orientation {
			n.Orientation = s.Orientation
		}
	}
	return n
}

// Consolidates two sensordata elements but ignores timestamps
func (s SensorData) ConsolidateExTime(n SensorData) SensorData {
	s.checkSources(&n)
	null := SensorData{}

	if n.Position == null.Position {
		n.Position = s.Position
	}
	if n.Orientation == null.Orientation {
		n.Orientation = s.Orientation
	}
	return n
}

func (s *SensorData) checkSources(n *SensorData) {
	if (s.source != n.source && *s != SensorData{}) {
		logrus.Println(s)
		logrus.Println(n)
		logrus.Fatalln("Do not consolidate SensorData from different Sources")
	}
}

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

func ConvertUbxSensorData(msg interface{}) (*SensorData, error) {
	sd := &SensorData{
		//Servertime: time.Now().UTC(),
		source: SOURCE_SERIAL,
	}
	switch v := msg.(type) {
	case *ublox.NavPvt:
		//logrus.Println("NAV-PVT")
		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)
		sd.Position[0] = float64(v.Lat_dege7) / 1e+7
		sd.Position[1] = float64(v.Lon_dege7) / 1e+7
		sd.Position[2] = float64(v.HMSL_mm) / 1e+3 // mm in m
		sd.HAcc = float64(v.HAcc_mm) / 1000
		sd.VAcc = float64(v.VAcc_mm) / 1000
		sd.HeadMotion = float64(v.HeadMot_dege5) / 1e+5
		sd.HeadDevice = float64(v.HeadVeh_dege5) / 1e+5
		sd.HeadingAcc = float64(v.HeadAcc_dege5) / 1e+5
		sd.Speed = float64(v.GSpeed_mm_s) * 3.6e-3
	case *ublox.HnrPvt:
		//logrus.Println("HNR-PVT")
		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)
		sd.Position[0] = float64(v.Lat_dege7) / 1e+7
		sd.Position[1] = float64(v.Lon_dege7) / 1e+7
		sd.Position[2] = float64(v.HMSL_mm) / 1e+3 // mm in m
		sd.HAcc = float64(v.HAcc) / 1000
		sd.VAcc = float64(v.VAcc) / 1000
		sd.HeadMotion = float64(v.HeadMot_dege5) / 1e+5
		sd.HeadDevice = float64(v.HeadVeh_dege5) / 1e+5
		sd.HeadingAcc = float64(v.HeadAcc_dege5) / 1e+5
		sd.Speed = float64(v.GSpeed_mm_s) + 3.6e-3
	case *ublox.NavAtt:
		//logrus.Println("NAV-ATT")
		sd.itow = v.ITOW_ms
		sd.Orientation[0] = float64(v.Pitch_deg) * 1e-5
		sd.Orientation[1] = float64(v.Roll_deg) * 1e-5
		sd.Orientation[2] = float64(v.Heading_deg) * 1e-5
	case *ublox.RawMessage:
		//class := make([]byte, 2)
		//binary.LittleEndian.PutUint16(class, v.ClassID())
		//logrus.Printf("%#v, %#v", class[0],class[1])
		return nil, nil
	default:
		return nil, errNotImplemented
	}

	if !sd.Timestamp.IsZero() && sd.Timestamp.Nanosecond() != 0 {
		lastTimeOffsetUblox = time.Now().UnixNano() - sd.Timestamp.UnixNano()
	} else {
		sd.Timestamp = time.Now().UTC().Add(time.Duration(lastTimeOffsetUblox))
	}

	return sd, nil
}

func ConvertByteSensorData(jsonData []byte) (*SensorData, error) {
	if gjson.Get(string(jsonData), "os").String() == "hyperimu" {
		return convertAndroidHyperImu(jsonData)
	}
	return convertIPhoneSensorLog(jsonData)
}

func convertIPhoneSensorLog(jsonData []byte) (*SensorData, error) {
	timestamp := gjson.Get(string(jsonData), "locationTimestamp_since1970").Float()
	lat := gjson.Get(string(jsonData), "locationLatitude").Float()
	lon := gjson.Get(string(jsonData), "locationLongitude").Float()
	alt := gjson.Get(string(jsonData), "locationAltitude").Float()
	pitch := gjson.Get(string(jsonData), "motionPitch").Float() * 180 / math.Pi
	roll := gjson.Get(string(jsonData), "motionRoll").Float() * 180 / math.Pi
	yaw := gjson.Get(string(jsonData), "motionYaw").Float() * 180 / math.Pi
	hAcc := gjson.Get(string(jsonData), "locationHorizontalAccuracy").Float()
	vAcc := gjson.Get(string(jsonData), "locationVerticalAccuracy").Float()
	headingAcc := gjson.Get(string(jsonData), "locationHeadingAccuracy").Float()
	headMotion := gjson.Get(string(jsonData), "locationCourse").Float()
	headDevice := gjson.Get(string(jsonData), "locationTrueHeading").Float()
	speed := gjson.Get(string(jsonData), "locationSpeed").Float()

	var ts time.Time
	if timestamp != 0 {
		ts = time.Unix(0, int64(timestamp*float64(time.Second))).UTC()
		lastTimeOffsetIphone = time.Now().UnixNano() - ts.UnixNano()
	} else {
		ts = time.Now().UTC().Add(time.Duration(lastTimeOffsetIphone))
	}
	//if ts == time.Date()
	sd := &SensorData{
		//Servertime:  time.Now().UTC(),
		source:      SOURCE_TCP,
		Timestamp:   ts,
		Position:    [3]float64{lat, lon, alt},
		Orientation: [3]float64{pitch, roll, yaw},
		HAcc:        hAcc,
		VAcc:        vAcc,
		HeadingAcc:  headingAcc,
		HeadMotion:  headMotion,
		HeadDevice:  headDevice,
		Speed:       speed,
	}
	if (*sd == SensorData{}) {
		return nil, errors.New("iphone sensorlog: convert empty")
	}
	return sd, nil
}

func convertAndroidHyperImu(jsonData []byte) (*SensorData, error) {
	timestamp := gjson.Get(string(jsonData), "Timestamp").Int()
	lat := gjson.Get(string(jsonData), "GPS.0").Float()
	lon := gjson.Get(string(jsonData), "GPS.1").Float()
	alt := gjson.Get(string(jsonData), "GPS.2").Float()
	pitch := gjson.Get(string(jsonData), "orientation.0").Float()
	roll := gjson.Get(string(jsonData), "orientation.1").Float()
	yaw := gjson.Get(string(jsonData), "orientation.2").Float()
	//hAcc := gjson.Get(string(jsonData), "locationHorizontalAccuracy").Float()
	//vAcc := gjson.Get(string(jsonData), "locationVerticalAccuracy").Float()
	//headingAcc := gjson.Get(string(jsonData), "locationHeadingAccuracy").Float()
	//headMotion := gjson.Get(string(jsonData), "locationCourse").Float()
	//headDevice := gjson.Get(string(jsonData), "locationTrueHeading").Float()
	//speed      := gjson.Get(string(jsonData), "locationSpeed").Float()

	sd := &SensorData{
		//Servertime:  time.Now().UTC(),
		source:      SOURCE_TCP,
		Timestamp:   time.Unix(0, timestamp*int64(time.Millisecond)).UTC(),
		Position:    [3]float64{lat, lon, alt},
		Orientation: [3]float64{pitch, roll, yaw},
	}
	if (*sd == SensorData{}) {
		return nil, errors.New("android hyperimu: convert empty")
	}
	return sd, nil
}