package producer

import (
	log "github.com/sirupsen/logrus"
	"gonum.org/v1/gonum/mat"
	"gonum.org/v1/gonum/optimize"
)

type NormElements struct {
	LineOff, SampOff, LatOff, LonOff, HeightOff           float64
	LineScale, SampScale, LatScale, LonScale, HeightScale float64
}

func objectiveFunc(numerator, denominator, f, latVec, lonVec, heightVec *mat.VecDense) float64 {
	errorSquared := 0.0
	n := f.Len()
	for i := 0; i < n; i++ {
		predictedV := project(numerator, denominator, latVec.AtVec(i), lonVec.AtVec(i), heightVec.AtVec(i))
		errorV := predictedV - f.AtVec(i)
		errorSquared += errorV * errorV
	}
	// lambda := 1e-2
	// coeffsSquaredError := 0.0
	// for i := 0; i < 20; i++ {
	// 	coeffsSquaredError += lambda * (numerator.AtVec(i)*numerator.AtVec(i) + denominator.AtVec(i)*denominator.AtVec(i))
	// }

	return errorSquared
}

func solveNelderMead(num, den, f, latVec, lonVec, heightVec *mat.VecDense) (*mat.VecDense, *mat.VecDense, error) {
	var initCoeffs []float64 // a0-a19,b1-b19
	initCoeffs = append(initCoeffs, num.RawVector().Data...)
	for i := 1; i < 20; i++ {
		initCoeffs = append(initCoeffs, den.AtVec(i))
	}
	problem := optimize.Problem{
		Func: func(coeffs []float64) float64 {
			numerator := mat.NewVecDense(20, nil)
			denominator := mat.NewVecDense(20, nil)
			denominator.SetVec(0, 1.0)
			numerator.SetVec(0, coeffs[0])
			for i := 1; i < 20; i++ {
				numerator.SetVec(i, coeffs[i])
				denominator.SetVec(i, coeffs[i+19])
			}
			return objectiveFunc(numerator, denominator, f, latVec, lonVec, heightVec)
		},
	}

	result, err := optimize.Minimize(problem, initCoeffs, &optimize.Settings{}, &optimize.NelderMead{})
	if err != nil {
		log.Error("Error in RPC optimization: ", err)
		return nil, nil, err
	}

	numerator := mat.NewVecDense(20, nil)
	denominator := mat.NewVecDense(20, nil)
	denominator.SetVec(0, 1.0)
	numerator.SetVec(0, result.X[0])
	for i := 1; i < 20; i++ {
		numerator.SetVec(i, result.X[i])
		denominator.SetVec(i, result.X[i+19])
	}

	return numerator, denominator, nil
}
