1 files changed, 48 insertions, 0 deletions
diff --git a/climb/cotacol.go b/climb/cotacol.go
new file mode 100644
index 0000000..85b096b
--- /dev/null
+++ b/climb/cotacol.go
@@ -0,0 +1,48 @@
+package climb
+
+import "math"
+
+const (
+	CotacolAlgorithmVersion = "v1"
+	cotacolSegmentLengthM   = 100
+)
+
+func Cotacol(distances, elevations []float64) float64 {
+	startIndex, endIndex := 0, len(distances)-1
+	if endIndex-startIndex < 1 {
+		return 0
+	}
+	startDistance := distances[startIndex]
+	lastDistance := distances[endIndex]
+	if lastDistance <= startDistance {
+		return 0
+	}
+	cotacol := 0.0
+	i := startIndex
+	for start := startDistance; start < lastDistance; start += cotacolSegmentLengthM {
+		end := math.Min(start+cotacolSegmentLengthM, lastDistance)
+		for i < endIndex && distances[i+1] <= start {
+			i++
+		}
+		startElevation := interpolateElevation(distances, elevations, i, start)
+		for i < endIndex && distances[i+1] < end {
+			i++
+		}
+		endElevation := interpolateElevation(distances, elevations, i, end)
+		slope := (endElevation - startElevation) / (end - start)
+		if slope > 0 {
+			cotacol += (end - start) / 1000 * (slope * 100) * (slope * 100)
+		}
+	}
+	return cotacol
+}
+
+func interpolateElevation(distances, elevations []float64, i int, distance float64) float64 {
+	if i+1 >= len(distances) {
+		return elevations[i]
+	}
+	startDistance := distances[i]
+	endDistance := distances[i+1]
+	t := (distance - startDistance) / (endDistance - startDistance)
+	return elevations[i] + t*(elevations[i+1]-elevations[i])
+}