package main

import (
	"fmt"
	"math"
	"math/rand/v2"
	"os"

	"github.com/gofiber/fiber/v2/log"
	"gonum.org/v1/gonum/optimize"
)

type Detector struct {
	Name           string
	End            float64    // samples
	Position       [3]float64 // m
	EventEpochs    []float64  // samples
	ClockSpeedMult float64
}

func main() {
	samplerate := 44100. // Hz
	start := 2106421.    // samples
	sos := 347.99        // m/s
	// Detector 1 is reference clock
	data := []Detector{{
		Name:     "Computer",
		End:      7134747,
		Position: [3]float64{-0.817, -0.701, 1.343},
		EventEpochs: []float64{
			5226608,
			5408665,
			5599824,
			5759440,
			5879540,
		},
	}, {
		Name:     "Xiaomi Phone",
		End:      7134723,
		Position: [3]float64{-0.964, 0.394, 0.031},
		EventEpochs: []float64{
			5226652,
			5408683,
			5599858,
			5759346,
			5879681,
		},
	}, {
		Name:     "Google Pixel Phone",
		End:      7134793,
		Position: [3]float64{1.113, -0.722, 1.001},
		EventEpochs: []float64{
			5226677,
			5408768,
			5599835,
			5759503,
			5879506,
		},
	}, {
		Name:     "Camera",
		End:      7134678,
		Position: [3]float64{1.157, 0.548, 0.037},
		EventEpochs: []float64{
			5226654,
			5408705,
			5599821,
			5759352,
			5879593,
		},
	},
	}

	// Compute clock speed multiplier
	for detector_index := range data {
		data[detector_index].ClockSpeedMult = (data[detector_index].End - start) / (data[0].End - start) // How much to divide the time since start by to get the same sample count as the reference clock
	}

	realEventPositions := [][3]float64{{-0.229, -0.2942, 1.1319}, {-0.249, 0.4158, 1.0519}, {0.178, -0.4332, 0.8139}, {0.087, 0.5478, 0.3449}, {0.601, -0.8742, 1.9019}}

	for event_index := range data[0].EventEpochs {
		problem := optimize.Problem{
			Func: func(x []float64) float64 {
				guessedX := x[0]
				guessedY := x[1]
				guessedZ := x[2]
				guessedEpoch := x[3] // samples
				errorSum := 0.
				for detector_index := range data {
					distance := math.Sqrt((guessedX-data[detector_index].Position[0])*(guessedX-data[detector_index].Position[0]) + (guessedY-data[detector_index].Position[1])*(guessedY-data[detector_index].Position[1]) + (guessedZ-data[detector_index].Position[2])*(guessedZ-data[detector_index].Position[2]))
					expectedEpoch := guessedEpoch + (distance/sos)*samplerate
					offset := ((data[detector_index].EventEpochs[event_index]-start)/data[detector_index].ClockSpeedMult + start - expectedEpoch)
					errorSum += offset * offset
				}
				return errorSum
			},
		}
		result, err := optimize.Minimize(problem, []float64{0, 0, 0, data[0].EventEpochs[event_index]}, nil, &optimize.NelderMead{})
		if err != nil {
			log.Error(err)
		}
		fmt.Printf("[Event %d]: (%0.4f m, %0.4f m, %0.4f m, %0.4f s) | Error: %0.4fm\n", event_index, result.X[0], result.X[1], result.X[2], result.X[3]/samplerate, math.Sqrt((realEventPositions[event_index][0]-result.X[0])*(realEventPositions[event_index][0]-result.X[0])+(realEventPositions[event_index][1]-result.X[1])*(realEventPositions[event_index][1]-result.X[1])+(realEventPositions[event_index][2]-result.X[2])*(realEventPositions[event_index][2]-result.X[2])))
	}

	expected_error_samples_sd_per_detector := 2. // to be multiplied by *sqrt(3) when doing the uniform distrib
	montecarlosims := 1000
	stepsPerDirection := 10
	max_error_exposure := 1. // m (for point cloud vis)
	boundsX := []float64{-1, 1}
	boundsY := []float64{-1, 1}
	boundsZ := []float64{0, 2}
	resStr := fmt.Sprintf(`ply
format ascii 1.0
element vertex %d          
property float x
property float y
property float z
property uchar red
property uchar green
property uchar blue
end_header
`, stepsPerDirection*stepsPerDirection*stepsPerDirection)

	for i := range stepsPerDirection {
		for j := range stepsPerDirection {
			fmt.Printf("%.1f %%\n", 100*float64(i)/float64(stepsPerDirection)+100*float64(j)/float64(stepsPerDirection)/float64(stepsPerDirection))
			for k := range stepsPerDirection {
				S := 0.
				realPos := []float64{boundsX[0] + float64(i)/float64(stepsPerDirection-1)*(boundsX[1]-boundsX[0]), boundsY[0] + float64(j)/float64(stepsPerDirection-1)*(boundsY[1]-boundsY[0]), boundsZ[0] + float64(k)/float64(stepsPerDirection-1)*(boundsZ[1]-boundsZ[0])}
				epochs := []float64{}

				for detector_index := range data {
					distance := math.Sqrt((realPos[0]-data[detector_index].Position[0])*(realPos[0]-data[detector_index].Position[0]) + (realPos[1]-data[detector_index].Position[1])*(realPos[1]-data[detector_index].Position[1]) + (realPos[2]-data[detector_index].Position[2])*(realPos[2]-data[detector_index].Position[2]))
					expectedEpoch := (distance/sos)*samplerate + 2*(rand.Float64()-0.5)*expected_error_samples_sd_per_detector*math.Sqrt(3)
					epochs = append(epochs, expectedEpoch)
				}

				for range montecarlosims {
					problem := optimize.Problem{
						Func: func(x []float64) float64 {
							guessedX := x[0]
							guessedY := x[1]
							guessedZ := x[2]
							guessedEpoch := x[3] // samples
							errorSum := 0.
							for detector_index := range data {
								distance := math.Sqrt((guessedX-data[detector_index].Position[0])*(guessedX-data[detector_index].Position[0]) + (guessedY-data[detector_index].Position[1])*(guessedY-data[detector_index].Position[1]) + (guessedZ-data[detector_index].Position[2])*(guessedZ-data[detector_index].Position[2]))
								expectedEpoch := guessedEpoch + (distance/sos)*samplerate
								offset := (epochs[detector_index] - expectedEpoch)
								errorSum += offset * offset
							}
							return errorSum
						},
					}
					result, err := optimize.Minimize(problem, []float64{realPos[0], realPos[1], realPos[2], 0}, nil, &optimize.NelderMead{})
					if err != nil {
						log.Error(err)
					}
					S += math.Sqrt((realPos[0]-result.X[0])*(realPos[0]-result.X[0]) + (realPos[1]-result.X[1])*(realPos[1]-result.X[1]) + (realPos[2]-result.X[2])*(realPos[2]-result.X[2]))
				}
				avg_error := S / float64(montecarlosims)
				color := int(min(avg_error/max_error_exposure*255., 255))
				resStr += fmt.Sprintf("%.6f %.6f %.6f %d %d %d\n", realPos[0], realPos[1], realPos[2], color, color, color)
			}
		}
	}
	file, err := os.Create("error.ply") // Usage: https://blender.stackexchange.com/questions/310858/how-to-visualize-point-cloud-colors-in-blender-4-0-after-ply-data-import
	if err != nil {
		log.Fatal(err)
	}
	defer file.Close()

	file.WriteString(resStr)
}
