func (p *CartPole) outsideBounds() bool {
	const failureAngle = thirtySixDegrees
	return p.state[0] < -2.4 ||
		p.state[0] > 2.4 ||
		p.state[2] < -failureAngle ||
		p.state[2] > failureAngle ||
		p.state[4] < -failureAngle ||
		p.state[4] > failureAngle
}
Check if simulation goes outside of bounds
package pole
import (
	"fmt"
	"github.com/yaricom/goNEAT/v2/experiment"
	"github.com/yaricom/goNEAT/v2/neat"
	"github.com/yaricom/goNEAT/v2/neat/genetics"
	"github.com/yaricom/goNEAT/v2/neat/network"
	"math"
	"os"
	"sort"
)
const thirtySixDegrees = 36 * math.Pi / 180.0
const markovMaxSteps = 100000
const nonMarkovLongMaxSteps = 100000
const nonMarkovGeneralizationMaxSteps = 1000
type cartDoublePoleGenerationEvaluator struct {
	OutputPath string
	Markov bool
	ActionType ActionType
}
func NewCartDoublePoleGenerationEvaluator(outDir string, markov bool, actionType ActionType) experiment.GenerationEvaluator {
	return &cartDoublePoleGenerationEvaluator{
		OutputPath: outDir,
		Markov:     markov,
		ActionType: actionType,
	}
}
type CartPole struct {
	isMarkov bool
	nonMarkovLong bool
	generalizationTest bool
	state [6]float64
	balancedTimeSteps int
	jiggleStep [1000]float64
	cartPosSum      float64
	cartVelocitySum float64
	polePosSum      float64
	poleVelocitySum float64
}
func (e *cartDoublePoleGenerationEvaluator) GenerationEvaluate(pop *genetics.Population, epoch *experiment.Generation, context *neat.Options) (err error) {
	cartPole := newCartPole(e.Markov)
	cartPole.nonMarkovLong = false
	cartPole.generalizationTest = false
	for _, org := range pop.Organisms {
		winner, err := e.orgEvaluate(org, cartPole)
		if err != nil {
			return err
		}
		if winner && (epoch.Best == nil || org.Fitness > epoch.Best.Fitness) {
			epoch.Solved = true
			epoch.WinnerNodes = len(org.Genotype.Nodes)
			epoch.WinnerGenes = org.Genotype.Extrons()
			epoch.WinnerEvals = context.PopSize*epoch.Id + org.Genotype.Id
			epoch.Best = org
			org.IsWinner = true
		}
	}
	if !e.Markov {
		sortedSpecies := make([]*genetics.Species, len(pop.Species))
		copy(sortedSpecies, pop.Species)
		sort.Sort(sort.Reverse(genetics.ByOrganismFitness(sortedSpecies)))
		var currSpecies *genetics.Species
		for _, currSpecies = range sortedSpecies {
			max, _ := currSpecies.ComputeMaxAndAvgFitness()
			if max > currSpecies.MaxFitnessEver {
				currSpecies.IsChecked = false
			}
		}
		currSpecies = nil
		for _, currSpecies = range sortedSpecies {
			if !currSpecies.IsChecked {
				break
			}
		}
		if currSpecies == nil {
			currSpecies = sortedSpecies[0]
		}
		currSpecies.IsChecked = true
		champion := currSpecies.FindChampion()
		championFitness := champion.Fitness
		cartPole.nonMarkovLong = true
		cartPole.generalizationTest = false
		longRunPassed, err := e.orgEvaluate(champion, cartPole)
		if err != nil {
			return err
		}
		if longRunPassed {
			cartPole.nonMarkovLong = false
			cartPole.generalizationTest = true
			stateVals := [5]float64{0.05, 0.25, 0.5, 0.75, 0.95}
			generalizationScore := 0
			for s0c := 0; s0c < 5; s0c++ {
				for s1c := 0; s1c < 5; s1c++ {
					for s2c := 0; s2c < 5; s2c++ {
						for s3c := 0; s3c < 5; s3c++ {
							cartPole.state[0] = stateVals[s0c]*4.32 - 2.16
							cartPole.state[1] = stateVals[s1c]*2.70 - 1.35
							cartPole.state[2] = stateVals[s2c]*0.12566304 - 0.06283152  
							cartPole.state[3] = stateVals[s3c]*0.30019504 - 0.15009752  
							cartPole.state[4] = 0.0
							cartPole.state[5] = 0.0
							if _, err = champion.Phenotype.Flush(); err != nil {
								return err
							}
							if generalized, err := e.orgEvaluate(champion, cartPole); generalized {
								generalizationScore++
								if neat.LogLevel == neat.LogLevelDebug {
									neat.DebugLog(
										fmt.Sprintf("x: %f, xv: %f, t1: %f, t2: %f, angle: %f\n",
											cartPole.state[0], cartPole.state[1],
											cartPole.state[2], cartPole.state[4], thirtySixDegrees))
								}
							} else if err != nil {
								return err
							}
						}
					}
				}
			}
			if generalizationScore >= 200 {
				neat.InfoLog(
					fmt.Sprintf("The non-Markov champion found! (Generalization Score = %d)",
						generalizationScore))
				champion.Fitness = float64(generalizationScore)
				champion.IsWinner = true
				epoch.Solved = true
				epoch.WinnerNodes = len(champion.Genotype.Nodes)
				epoch.WinnerGenes = champion.Genotype.Extrons()
				epoch.WinnerEvals = context.PopSize*epoch.Id + champion.Genotype.Id
				epoch.Best = champion
			} else {
				neat.InfoLog("The non-Markov champion unable to generalize")
				champion.Fitness = championFitness  
				champion.IsWinner = false
			}
		} else {
			neat.InfoLog("The non-Markov champion missed the 100'000 run test")
			champion.Fitness = championFitness  
			champion.IsWinner = false
		}
	}
	epoch.FillPopulationStatistics(pop)
	if epoch.Solved || epoch.Id%context.PrintEvery == 0 {
		popPath := fmt.Sprintf("%s/gen_%d", experiment.OutDirForTrial(e.OutputPath, epoch.TrialId), epoch.Id)
		if file, err := os.Create(popPath); err != nil {
			return err
		} else if err = pop.WriteBySpecies(file); err != nil {
			neat.ErrorLog(fmt.Sprintf("Failed to dump population, reason: %s\n", err))
			return err
		}
	}
	if epoch.Solved {
		for _, org := range pop.Organisms {
			if org.IsWinner {
				orgPath := fmt.Sprintf("%s/%s_%.1f_%d-%d", experiment.OutDirForTrial(e.OutputPath, epoch.TrialId),
					"pole2_winner", org.Fitness, org.Phenotype.NodeCount(), org.Phenotype.LinkCount())
				if file, err := os.Create(orgPath); err != nil {
					return err
				} else if err = org.Genotype.Write(file); err != nil {
					neat.ErrorLog(fmt.Sprintf("Failed to dump winner organism genome, reason: %s\n", err))
					return err
				} else {
					neat.InfoLog(fmt.Sprintf("Generation #%d winner %d dumped to: %s\n", epoch.Id, org.Genotype.Id, orgPath))
				}
				break
			}
		}
	}
	return err
}
func (e *cartDoublePoleGenerationEvaluator) orgEvaluate(organism *genetics.Organism, cartPole *CartPole) (winner bool, err error) {
	organism.Fitness, err = cartPole.evalNet(organism.Phenotype, e.ActionType)
	if err != nil {
		return false, err
	}
	if neat.LogLevel == neat.LogLevelDebug {
		neat.DebugLog(fmt.Sprintf("Organism #%3d\tfitness: %f", organism.Genotype.Id, organism.Fitness))
	}
	if !(cartPole.nonMarkovLong && cartPole.generalizationTest) && organism.CheckChampionChildDamaged() {
		neat.WarnLog(fmt.Sprintf("ORGANISM DAMAGED:\n%s", organism.Genotype))
	}
	if cartPole.isMarkov {
		if organism.Fitness >= markovMaxSteps {
			winner = true
			organism.Fitness = 1.0
			organism.Error = 0.0
		} else {
			organism.Error = (markovMaxSteps - organism.Fitness) / markovMaxSteps
			organism.Fitness = 1.0 - organism.Error
		}
	} else if cartPole.nonMarkovLong {
		if organism.Fitness >= nonMarkovLongMaxSteps {
			winner = true
		}
	} else if cartPole.generalizationTest {
		if organism.Fitness >= nonMarkovGeneralizationMaxSteps {
			winner = true
		}
	} else {
		winner = false
	}
	return winner, err
}
func newCartPole(markov bool) *CartPole {
	return &CartPole{
		isMarkov: markov,
	}
}
func (p *CartPole) evalNet(net *network.Network, actionType ActionType) (steps float64, err error) {
	nonMarkovMax := nonMarkovGeneralizationMaxSteps
	if p.nonMarkovLong {
		nonMarkovMax = nonMarkovLongMaxSteps
	}
	p.resetState()
	if p.isMarkov {
		input := make([]float64, 7)
		for steps = 0; steps < markovMaxSteps; steps++ {
			input[0] = (p.state[0] + 2.4) / 4.8
			input[1] = (p.state[1] + 1.0) / 2.0
			input[2] = (p.state[2] + thirtySixDegrees) / (thirtySixDegrees * 2.0)  
			input[3] = (p.state[3] + 1.0) / 2.0
			input[4] = (p.state[4] + thirtySixDegrees) / (thirtySixDegrees * 2.0)  
			input[5] = (p.state[5] + 1.0) / 2.0
			input[6] = 0.5
			if err := net.LoadSensors(input); err != nil {
				return 0, err
			}
			if res, err := net.Activate(); err != nil {
				return 0, err
			} else if !res {
				neat.DebugLog(fmt.Sprintf("Failed to activate Network, reason: %s", err))
				return 1.0, nil
			}
			action := net.Outputs[0].Activation
			if actionType == DiscreteAction {
				if action < 0.5 {
					action = 0
				} else {
					action = 1
				}
			}
			p.performAction(action, steps)
			if p.outsideBounds() {
				break
			}
		}
		return steps, nil
	} else {
		input := make([]float64, 4)
		for steps = 0; steps < float64(nonMarkovMax); steps++ {
			input[0] = p.state[0] / 4.8
			input[1] = p.state[2] / 0.52
			input[2] = p.state[4] / 0.52
			input[3] = 1.0
			err = net.LoadSensors(input)
			if err != nil {
				return 0, err
			}
			if res, err := net.Activate(); err != nil {
				return 0, err
			} else if !res {
				neat.WarnLog(fmt.Sprintf("Failed to activate Network, reason: %s", err))
				return 0.0001, err
			}
			action := net.Outputs[0].Activation
			if actionType == DiscreteAction {
				if action < 0.5 {
					action = 0
				} else {
					action = 1
				}
			}
			p.performAction(action, steps)
			if p.outsideBounds() {
				break
			}
		}
		if p.generalizationTest {
			return float64(p.balancedTimeSteps), nil
		}
		jiggleTotal := 0.0
		if steps >= 100.0 && !p.nonMarkovLong {
			for count := int(steps - 100.0); count < int(steps); count++ {
				jiggleTotal += p.jiggleStep[count]
			}
		}
		if !p.nonMarkovLong {
			var nonMarkovFitness float64
			if p.balancedTimeSteps >= 100 {
				nonMarkovFitness = 0.1*float64(p.balancedTimeSteps)/1000.0 + 0.9*0.75/jiggleTotal
			} else {
				nonMarkovFitness = 0.1 * float64(p.balancedTimeSteps) / 1000.0
			}
			if neat.LogLevel == neat.LogLevelDebug {
				neat.DebugLog(fmt.Sprintf("Balanced time steps: %d, jiggle: %f ***\n",
					p.balancedTimeSteps, jiggleTotal))
			}
			return nonMarkovFitness, nil
		} else {
			return steps, nil
		}
	}
}
func (p *CartPole) performAction(action, stepNum float64) {
	const TAU = 0.01  
	var dydx [6]float64
	for i := 0; i < 2; i++ {
		dydx[0] = p.state[1]
		dydx[2] = p.state[3]
		dydx[4] = p.state[5]
		p.step(action, p.state, &dydx)
		p.rk4(action, p.state, dydx, &p.state, TAU)
	}
	p.cartPosSum += math.Abs(p.state[0])
	p.cartVelocitySum += math.Abs(p.state[1])
	p.polePosSum += math.Abs(p.state[2])
	p.poleVelocitySum += math.Abs(p.state[3])
	if stepNum < 1000 {
		p.jiggleStep[int(stepNum)] = math.Abs(p.state[0]) + math.Abs(p.state[1]) + math.Abs(p.state[2]) + math.Abs(p.state[3])
	}
	if !p.outsideBounds() {
		p.balancedTimeSteps++
	}
}
func (p *CartPole) step(action float64, st [6]float64, derivs *[6]float64) {
	const Mup = 0.000002
	const Gravity = -9.8
	const ForceMag = 10.0  
	const MassCart = 1.0   
	const MassPole1 = 1.0  
	const Length1 = 0.5    
	const Length2 = 0.05   
	const MassPole2 = 0.1  
	force := (action - 0.5) * ForceMag * 2.0
	cosTheta1 := math.Cos(st[2])
	sinTheta1 := math.Sin(st[2])
	gSinTheta1 := Gravity * sinTheta1
	cosTheta2 := math.Cos(st[4])
	sinTheta2 := math.Sin(st[4])
	gSinTheta2 := Gravity * sinTheta2
	ml1 := Length1 * MassPole1
	ml2 := Length2 * MassPole2
	temp1 := Mup * st[3] / ml1
	temp2 := Mup * st[5] / ml2
	fi1 := (ml1 * st[3] * st[3] * sinTheta1) + (0.75 * MassPole1 * cosTheta1 * (temp1 + gSinTheta1))
	fi2 := (ml2 * st[5] * st[5] * sinTheta2) + (0.75 * MassPole2 * cosTheta2 * (temp2 + gSinTheta2))
	mi1 := MassPole1 * (1 - (0.75 * cosTheta1 * cosTheta1))
	mi2 := MassPole2 * (1 - (0.75 * cosTheta2 * cosTheta2))
	derivs[1] = (force + fi1 + fi2) / (mi1 + mi2 + MassCart)
	derivs[3] = -0.75 * (derivs[1]*cosTheta1 + gSinTheta1 + temp1) / Length1
	derivs[5] = -0.75 * (derivs[1]*cosTheta2 + gSinTheta2 + temp2) / Length2
}
func (p *CartPole) rk4(f float64, y, dydx [6]float64, yout *[6]float64, tau float64) {
	var yt, dym, dyt [6]float64
	hh := tau * 0.5
	h6 := tau / 6.0
	for i := 0; i <= 5; i++ {
		yt[i] = y[i] + hh*dydx[i]
	}
	p.step(f, yt, &dyt)
	dyt[0] = yt[1]
	dyt[2] = yt[3]
	dyt[4] = yt[5]
	for i := 0; i <= 5; i++ {
		yt[i] = y[i] + hh*dyt[i]
	}
	p.step(f, yt, &dym)
	dym[0] = yt[1]
	dym[2] = yt[3]
	dym[4] = yt[5]
	for i := 0; i <= 5; i++ {
		yt[i] = y[i] + tau*dym[i]
		dym[i] += dyt[i]
	}
	p.step(f, yt, &dyt)
	dyt[0] = yt[1]
	dyt[2] = yt[3]
	dyt[4] = yt[5]
	for i := 0; i <= 5; i++ {
		yout[i] = y[i] + h6*(dydx[i]+dyt[i]+2.0*dym[i])
	}
}