package main

import (
	"fmt"
	"log"
	"math"
	"path/filepath"
	"strings"
	"time"

	"../goflying/ahrs"
	"../goflying/ahrsweb"
	"../sensors"
	"github.com/kidoman/embd"
	_ "github.com/kidoman/embd/host/all"
)

const (
	numRetries uint8 = 5
	calCLimit        = 0.15
	calDLimit        = 10.0

	// WHO_AM_I values to differentiate between the different IMUs.
	MPUREG_WHO_AM_I     = 0x75
	MPUREG_WHO_AM_I_VAL = 0x71 // Expected value.
	ICMREG_WHO_AM_I     = 0x00
	ICMREG_WHO_AM_I_VAL = 0xEA // Expected value.
)

var (
	i2cbus           embd.I2CBus
	myPressureReader sensors.PressureReader
	myIMUReader      sensors.IMUReader
	cal              chan (string)
	analysisLogger   *ahrs.AHRSLogger
	ahrsCalibrating  bool
	logMap           map[string]interface{}
)

func initI2CSensors() {
	i2cbus = embd.NewI2CBus(1)

	go pollSensors()
	go sensorAttitudeSender()
	go updateAHRSStatus()
}

func pollSensors() {
	timer := time.NewTicker(4 * time.Second)
	for {
		<-timer.C

		// If it's not currently connected, try connecting to pressure sensor
		if globalSettings.BMP_Sensor_Enabled && !globalStatus.BMPConnected {
			globalStatus.BMPConnected = initPressureSensor() // I2C temperature and pressure altitude.
			go tempAndPressureSender()
		}

		// If it's not currently connected, try connecting to IMU
		if globalSettings.IMU_Sensor_Enabled && !globalStatus.IMUConnected {
			globalStatus.IMUConnected = initIMU() // I2C accel/gyro/mag.
		}
	}
}

func initPressureSensor() (ok bool) {
	bmp, err := sensors.NewBMP280(&i2cbus, 100*time.Millisecond)
	if err == nil {
		myPressureReader = bmp
		return true
	}

	//TODO westphae: make bmp180.go to fit bmp interface

	return false
}

func tempAndPressureSender() {
	var (
		temp     float64
		press    float64
		altLast  = -9999.9
		altitude float64
		err      error
		dt       = 0.1
		failNum  uint8
	)

	// Initialize variables for rate of climb calc
	u := 5 / (5 + float32(dt)) // Use 5 sec decay time for rate of climb, slightly faster than typical VSI

	timer := time.NewTicker(time.Duration(1000*dt) * time.Millisecond)
	for globalSettings.BMP_Sensor_Enabled && globalStatus.BMPConnected {
		<-timer.C

		// Read temperature and pressure altitude.
		temp, err = myPressureReader.Temperature()
		if err != nil {
			addSingleSystemErrorf("pressure-sensor-temp-read", "AHRS Error: Couldn't read temperature from sensor: %s", err)
		}
		press, err = myPressureReader.Pressure()
		if err != nil {
			addSingleSystemErrorf("pressure-sensor-pressure-read", "AHRS Error: Couldn't read pressure from sensor: %s", err)
			failNum++
			if failNum > numRetries {
				//				log.Printf("AHRS Error: Couldn't read pressure from sensor %d times, closing BMP: %s", failNum, err)
				myPressureReader.Close()
				globalStatus.BMPConnected = false // Try reconnecting a little later
				break
			}
		}

		// Update the Situation data.
		mySituation.muBaro.Lock()
		mySituation.BaroLastMeasurementTime = stratuxClock.Time
		mySituation.BaroTemperature = float32(temp)
		altitude = CalcAltitude(press)
		mySituation.BaroPressureAltitude = float32(altitude)
		if altLast < -2000 {
			altLast = altitude // Initialize
		}
		// Assuming timer is reasonably accurate, use a regular ewma
		mySituation.BaroVerticalSpeed = u*mySituation.BaroVerticalSpeed + (1-u)*float32(altitude-altLast)/(float32(dt)/60)
		mySituation.muBaro.Unlock()
		altLast = altitude
	}
	mySituation.BaroPressureAltitude = 99999
	mySituation.BaroVerticalSpeed = 99999
}

func initIMU() (ok bool) {
	// Check if the chip is the ICM-20948 or MPU-9250.
	v, err := i2cbus.ReadByteFromReg(0x68, ICMREG_WHO_AM_I)
	if err != nil {
		log.Printf("Error identifying IMU: %s\n", err.Error())
		return false
	}
	v2, err := i2cbus.ReadByteFromReg(0x68, MPUREG_WHO_AM_I)
	if err != nil {
		log.Printf("Error identifying IMU: %s\n", err.Error())
		return false
	}

	if v == ICMREG_WHO_AM_I_VAL {
		log.Println("ICM-20948 detected.")
		imu, err := sensors.NewICM20948(&i2cbus)
		if err == nil {
			myIMUReader = imu
			return true
		}
	} else if v2 == MPUREG_WHO_AM_I_VAL {
		log.Println("MPU-9250 detected.")
		imu, err := sensors.NewMPU9250(&i2cbus)
		if err == nil {
			myIMUReader = imu
			return true
		}
	} else {
		log.Printf("Could not identify MPU. v=%02x, v2=%02x.\n", v, v2)
		return false
	}

	return false
}

//FIXME: Shoud be moved to managementinterface.go and standardized on management interface port.

func sensorAttitudeSender() {
	var (
		t                    time.Time
		roll, pitch, heading float64
		mpuError, magError   error
		failNum              uint8
	)

	s := ahrs.NewSimpleAHRS()
	m := ahrs.NewMeasurement()
	cal = make(chan (string), 1)

	// Set up loggers for analysis
	ahrswebListener, err := ahrsweb.NewKalmanListener()
	if err != nil {
		// addSingleSystemErrorf("ahrs-web-start", "AHRS Info: couldn't start ahrswebListener: %s\n", err.Error())
	} else {
		defer ahrswebListener.Close()
	}

	// Need a sampling freq faster than 10Hz
	timer := time.NewTicker(50 * time.Millisecond) // ~20Hz update.
	for {
		// Set sensor gyro calibrations
		if c, d := &globalSettings.C, &globalSettings.D; d[0]*d[0]+d[1]*d[1]+d[2]*d[2] > 0 {
			s.SetCalibrations(c, d)
			log.Printf("AHRS Info: IMU Calibrations read from settings: accel %6f %6f %6f; gyro %6f %6f %6f\n",
				c[0], c[1], c[2], d[0], d[1], d[2])
		} else {
			// Do an initial calibration
			select { // Don't block if cal isn't receiving: only need one calibration in the queue at a time.
			case cal <- "cal":
			default:
			}
		}

		// Set sensor quaternion
		if f := &globalSettings.SensorQuaternion; f[0]*f[0]+f[1]*f[1]+f[2]*f[2]+f[3]*f[3] > 0 {
			s.SetSensorQuaternion(f)
		} else {
			select { // Don't block if cal isn't receiving: only need one calibration in the queue at a time.
			case cal <- "level":
			default:
			}
		}

		failNum = 0
		<-timer.C
		time.Sleep(950 * time.Millisecond)
		for globalSettings.IMU_Sensor_Enabled && globalStatus.IMUConnected {
			<-timer.C

			// Process calibration and level requests
			select {
			case action := <-cal:
				log.Printf("AHRS Info: cal received action %s\n", action)
				ahrsCalibrating = true
				myIMUReader.Read() // Clear out the averages
				var (
					nTries uint8
					cc, dd float64
				)
				for (math.Abs(cc-1) > calCLimit || dd > calDLimit) && nTries < numRetries {
					time.Sleep(1 * time.Second)
					_, d1, d2, d3, c1, c2, c3, _, _, _, mpuError, _ := myIMUReader.Read()
					cc = math.Sqrt(c1*c1 + c2*c2 + c3*c3)
					dd = math.Sqrt(d1*d1 + d2*d2 + d3*d3)
					nTries++
					log.Printf("AHRS Info: IMU calibration attempt #%d\n", nTries)
					if mpuError != nil {
						log.Printf("AHRS Info: Error reading IMU while calibrating: %s\n", mpuError)
					} else {
						if strings.Contains(action, "cal") { // Calibrate gyros
							globalSettings.D = [3]float64{d1, d2, d3}
							s.SetCalibrations(nil, &globalSettings.D)
							log.Printf("AHRS Info: IMU gyro calibration: %3f %3f %3f\n", d1, d2, d3)
						}
						if strings.Contains(action, "level") { // Calibrate accel / level
							globalSettings.C = [3]float64{c1, c2, c3}
							s.SetCalibrations(&globalSettings.C, nil)
							globalSettings.SensorQuaternion = *makeOrientationQuaternion(globalSettings.C)
							s.SetSensorQuaternion(&globalSettings.SensorQuaternion)
							s.Reset()
							log.Printf("AHRS Info: IMU accel calibration: %3f %3f %3f\n", c1, c2, c3)
							log.Printf("AHRS Info: Caged to quaternion %v\n", globalSettings.SensorQuaternion)
						}
						saveSettings()
					}
				}
				ahrsCalibrating = false
				<-timer.C // Make sure we get data for the actual algorithm
			default:
			}

			// Make the IMU sensor measurements.
			t = stratuxClock.Time
			m.T = float64(t.UnixNano()/1000) / 1e6
			_, m.B1, m.B2, m.B3, m.A1, m.A2, m.A3, m.M1, m.M2, m.M3, mpuError, magError = myIMUReader.Read()
			m.SValid = mpuError == nil
			m.MValid = magError == nil
			if mpuError != nil {
				log.Printf("AHRS Gyro/Accel Error: %s\n", mpuError)
				failNum++
				if failNum > numRetries {
					log.Printf("AHRS Gyro/Accel Error: failed to read %d times, restarting: %s\n",
						failNum-1, mpuError)
					myIMUReader.Close()
					globalStatus.IMUConnected = false
				}
				continue
			}
			failNum = 0
			if magError != nil {
				if globalSettings.DEBUG {
					log.Printf("AHRS Magnetometer Error, not using for this run: %s\n", magError)
				}
				m.MValid = false
			}

			// Make the GPS measurements.
			m.TW = float64(mySituation.GPSLastGroundTrackTime.UnixNano()/1000) / 1e6
			m.WValid = isGPSGroundTrackValid()
			if m.WValid {
				m.W1 = mySituation.GPSGroundSpeed * math.Sin(float64(mySituation.GPSTrueCourse)*ahrs.Deg)
				m.W2 = mySituation.GPSGroundSpeed * math.Cos(float64(mySituation.GPSTrueCourse)*ahrs.Deg)
				if globalSettings.BMP_Sensor_Enabled && globalStatus.BMPConnected {
					m.W3 = float64(mySituation.BaroVerticalSpeed * 60 / 6076.12)
				} else {
					m.W3 = float64(mySituation.GPSVerticalSpeed) * 3600 / 6076.12
				}
			}

			// Run the AHRS calculations.
			s.Compute(m)

			// If we have valid AHRS info, then update mySituation.
			mySituation.muAttitude.Lock()
			if s.Valid() {
				roll, pitch, heading = s.RollPitchHeading()
				mySituation.AHRSRoll = roll / ahrs.Deg
				mySituation.AHRSPitch = pitch / ahrs.Deg
				mySituation.AHRSGyroHeading = heading
				if !isAHRSInvalidValue(heading) {
					mySituation.AHRSGyroHeading /= ahrs.Deg
				}

				//TODO westphae: until magnetometer calibration is performed, no mag heading
				mySituation.AHRSMagHeading = ahrs.Invalid
				mySituation.AHRSSlipSkid = s.SlipSkid()
				mySituation.AHRSTurnRate = s.RateOfTurn()
				mySituation.AHRSGLoad = s.GLoad()
				if mySituation.AHRSGLoad < mySituation.AHRSGLoadMin || mySituation.AHRSGLoadMin == 0 {
					mySituation.AHRSGLoadMin = mySituation.AHRSGLoad
				}
				if mySituation.AHRSGLoad > mySituation.AHRSGLoadMax {
					mySituation.AHRSGLoadMax = mySituation.AHRSGLoad
				}

				mySituation.AHRSLastAttitudeTime = t
			} else {
				mySituation.AHRSRoll = ahrs.Invalid
				mySituation.AHRSPitch = ahrs.Invalid
				mySituation.AHRSGyroHeading = ahrs.Invalid
				mySituation.AHRSMagHeading = ahrs.Invalid
				mySituation.AHRSSlipSkid = ahrs.Invalid
				mySituation.AHRSTurnRate = ahrs.Invalid
				mySituation.AHRSGLoad = ahrs.Invalid
				mySituation.AHRSGLoadMin = ahrs.Invalid
				mySituation.AHRSGLoadMax = 0
				mySituation.AHRSLastAttitudeTime = time.Time{}
				s.Reset()
			}
			mySituation.muAttitude.Unlock()

			makeAHRSGDL90Report() // Send whether or not valid - the function will invalidate the values as appropriate

			// Send to AHRS debugging server.
			if ahrswebListener != nil {
				if err = ahrswebListener.Send(s.GetState(), m); err != nil {
					log.Printf("AHRS Error: couldn't write to ahrsweb: %s\n", err)
					ahrswebListener = nil
				}
			}

			// Log it to csv for later analysis.
			if globalSettings.AHRSLog && usage.Usage() < 0.95 {
				if analysisLogger == nil {
					analysisFilename := fmt.Sprintf("sensors_%s.csv", time.Now().Format("20060102_150405"))
					logMap = s.GetLogMap()
					updateExtraLogging()
					analysisLogger = ahrs.NewAHRSLogger(filepath.Join(logDirf, analysisFilename), logMap)
				}

				if analysisLogger != nil {
					updateExtraLogging()
					analysisLogger.Log()
				}
			} else {
				analysisLogger = nil
			}
		}
	}
}

func updateExtraLogging() {
	logMap["GPSNACp"] = float64(mySituation.GPSNACp)
	logMap["GPSTrueCourse"] = mySituation.GPSTrueCourse
	logMap["GPSVerticalAccuracy"] = mySituation.GPSVerticalAccuracy
	logMap["GPSHorizontalAccuracy"] = mySituation.GPSHorizontalAccuracy
	logMap["GPSAltitudeMSL"] = mySituation.GPSAltitudeMSL
	logMap["GPSFixQuality"] = float64(mySituation.GPSFixQuality)
	logMap["BaroPressureAltitude"] = float64(mySituation.BaroPressureAltitude)
	logMap["BaroVerticalSpeed"] = float64(mySituation.BaroVerticalSpeed)
}

func makeOrientationQuaternion(g [3]float64) (f *[4]float64) {
	if globalSettings.IMUMapping[0] == 0 { // if unset, default to some standard orientation
		globalSettings.IMUMapping[0] = -1 // +2 for RY836AI
	}

	// This is the "forward direction" chosen during the orientation process.
	var x *[3]float64 = new([3]float64)
	if globalSettings.IMUMapping[0] < 0 {
		x[-globalSettings.IMUMapping[0]-1] = -1
	} else {
		x[+globalSettings.IMUMapping[0]-1] = +1
	}

	// Normalize the gravity vector to be 1 G.
	z, _ := ahrs.MakeUnitVector(g)

	rotmat, _ := ahrs.MakeHardSoftRotationMatrix(*z, *x, [3]float64{0, 0, 1}, [3]float64{1, 0, 0})
	f = new([4]float64)
	f[0], f[1], f[2], f[3] = ahrs.RotationMatrixToQuaternion(*rotmat)
	return
}

// This is used in the orientation process where the user specifies the forward and up directions.
func getMinAccelDirection() (i int, err error) {
	_, _, _, _, a1, a2, a3, _, _, _, err, _ := myIMUReader.ReadOne()
	if err != nil {
		return
	}
	log.Printf("AHRS Info: sensor orientation accels %1.3f %1.3f %1.3f\n", a1, a2, a3)
	switch {
	case math.Abs(a1) > math.Abs(a2) && math.Abs(a1) > math.Abs(a3):
		if a1 > 0 {
			i = 1
		} else {
			i = -1
		}
	case math.Abs(a2) > math.Abs(a3) && math.Abs(a2) > math.Abs(a1):
		if a2 > 0 {
			i = 2
		} else {
			i = -2
		}
	case math.Abs(a3) > math.Abs(a1) && math.Abs(a3) > math.Abs(a2):
		if a3 > 0 {
			i = 3
		} else {
			i = -3
		}
	default:
		err = fmt.Errorf("couldn't determine biggest accel from %1.3f %1.3f %1.3f", a1, a2, a3)
	}

	return
}

// CageAHRS sends a signal to the AHRSProvider that it should recalibrate and reset its level orientation.
func CageAHRS() {
	cal <- "level"
}

// CageAHRS sends a signal to the AHRSProvider that it should recalibrate and reset its level orientation.
func CalibrateAHRS() {
	cal <- "cal"
}

// ResetAHRSGLoad resets the min and max to the current G load value.
func ResetAHRSGLoad() {
	mySituation.AHRSGLoadMax = mySituation.AHRSGLoad
	mySituation.AHRSGLoadMin = mySituation.AHRSGLoad
}

func updateAHRSStatus() {
	var (
		msg    uint8
		imu    bool
		ticker *time.Ticker
	)

	ticker = time.NewTicker(250 * time.Millisecond)

	for {
		<-ticker.C
		msg = 0

		// GPS ground track valid?
		if isGPSGroundTrackValid() {
			msg++
		}
		// IMU is being used
		imu = globalSettings.IMU_Sensor_Enabled && globalStatus.IMUConnected
		if imu {
			msg += 1 << 1
		}
		// BMP is being used
		if globalSettings.BMP_Sensor_Enabled && globalStatus.BMPConnected {
			msg += 1 << 2
		}
		// IMU is doing a calibration
		if ahrsCalibrating {
			msg += 1 << 3
		}
		// Logging to csv
		if imu && analysisLogger != nil {
			msg += 1 << 4
		}
		mySituation.AHRSStatus = msg
	}
}

func isAHRSInvalidValue(val float64) bool {
	return math.Abs(val-ahrs.Invalid) < 0.01
}