<commit_before>package algorithm

import RouteMetricRequest
import SonarRequest
import algorithm.geometry.Angle
import algorithm.geometry.AngleData
import algorithm.geometry.Point
import algorithm.geometry.Wall
import objects.Car
import java.util.concurrent.Exchanger

class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {

    // TODO: set to appropriate values
    override val ATTEMPTS: Int = 5
    override val SMOOTHING = SonarRequest.Smoothing.MEDIAN
    override val WINDOW_SIZE = 3

    private val MAX_DISTANCE_TO_WALL_AHEAD = 30         // reached the corner and should turn left
    private val OUTER_CORNER_DISTANCE_THRESHOLD = 60    // reached outer corner
    private val ISOSCALENESS_MIN_DIFF = 10              // have to correct alignment to the wall
    private val ISOSCALENESS_MAX_DIFF = 50              // almost reached outer corner but not yet, and 120 meas. founds far wall
    private val DISTANCE_TO_WALL_UPPER_BOUND = 50       // have to move closer to the parallel wall
    private val DISTANCE_TO_WALL_LOWER_BOUND = 25       // have to move farther to the parallel wall
    private val SPURIOUS_REFLECTION_DIFF = 70           // we're approaching corner and get spurious reflection from two walls on 60 meas. ! Should be before outer corner case !

    private var calibrateAfterRotate = false

    var carX = 0
    var carY = 0
    var carAngle = 0

    override fun getCarState(anglesDistances: Map<Angle, AngleData>): CarState {
        return CarState.WALL
    }

    private fun noParallelWallFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
        resultBuilder.setDirections(getIntArray(FORWARD))
        resultBuilder.setDistances(getIntArray(10))
        return resultBuilder.build()
    }

    private fun noOrthogonalMeasurementFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
        resultBuilder.setDirections(getIntArray(FORWARD, RIGHT, FORWARD))
        val wallAngle = calculateAngle(anglesDistances, state)
        resultBuilder.setDistances(getIntArray(15, wallAngle.degs(), 60))
        addWall(-wallAngle)
        calibrateAfterRotate = true
        return resultBuilder.build()
    }

    private fun wallAheadFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
        resultBuilder.setDirections(getIntArray(LEFT, FORWARD))
        val wallAngle = calculateAngle(anglesDistances, state)
        resultBuilder.setDistances(getIntArray(wallAngle.degs(), 15))
        addWall(wallAngle)
        calibrateAfterRotate = true
        return resultBuilder.build()
    }

    private fun alignParallelToWall(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))

        // we're sure that we have 120 and 60 degs measurements as they were checked before
        val rotationDirection = if (anglesDistances[Angle(120)]!!.distance > anglesDistances[Angle(60)]!!.distance) LEFT else RIGHT

        resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
        resultBuilder.setDistances(getIntArray(10, 7))//todo calibrate
        return resultBuilder.build()
    }

    private fun correctDistanceToParallelWall(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))

        val rotationDirection = if (anglesDistances[Angle(90)]!!.distance > 50) RIGHT else LEFT

        resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
        resultBuilder.setDistances(getIntArray(10, 10))
        return resultBuilder.build()
    }

    private fun moveForward(): RouteMetricRequest {
        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
        resultBuilder.setDirections(getIntArray(FORWARD))
        resultBuilder.setDistances(getIntArray(20))
        return resultBuilder.build()
    }

    private fun addWall(angleWithPrevWall: Angle) {
        RoomModel.updateWalls()

        val firstWall = RoomModel.walls.first()
        val lastWall = RoomModel.walls.last()
        if (firstWall == lastWall && RoomModel.walls.size > 1) {
            RoomModel.finished = true
            lastWall.pushBackPoint(firstWall.rawPoints.last())
            lastWall.markAsFinished()
            RoomModel.walls.removeAt(0)
        } else {
            RoomModel.walls.add(Wall(lastWall.wallAngleOX + angleWithPrevWall))
        }
    }

    override fun getCommand(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest? {

        val dist0 = anglesDistances[Angle(0)]!!
        val dist60 = anglesDistances[Angle(70)]!!
        val dist90 = anglesDistances[Angle(90)]!!
        val dist120 = anglesDistances[Angle(110)]!!
        val sonarAngle = carAngle - 90

        if (anglesDistances.filter { it.value.angle.degs() >= 60 && it.value.angle.degs() <= 120 && it.value.distance == -1 }.size != 0) {
            return null
        }

        if (calibrateAfterRotate) {
            if (Math.abs(dist60.distance - dist120.distance) > 4) {
                println("Flaw in align found, correcting")
                val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
                val rotationDirection = if (anglesDistances[Angle(120)]!!.distance > anglesDistances[Angle(60)]!!.distance) LEFT else RIGHT
                resultBuilder.setDirections(getIntArray(rotationDirection))
                resultBuilder.setDistances(getIntArray(5))
                return resultBuilder.build()
            } else {
                calibrateAfterRotate = false
                carAngle = RoomModel.walls.last().wallAngleOX.degs()
            }
        }
        // Check most basic measurements: 60/90/120
        if (dist90.distance == -1 || dist90.distance > OUTER_CORNER_DISTANCE_THRESHOLD) {
            println("No orthogonal measurement found")
            return noOrthogonalMeasurementFound(anglesDistances, state)
        }

        // Add point to room map<commit_msg>add dynamic rotation after corner<commit_after>package algorithm

import RouteMetricRequest
import SonarRequest
import algorithm.geometry.Angle
import algorithm.geometry.AngleData
import algorithm.geometry.Point
import algorithm.geometry.Wall
import objects.Car
import java.util.concurrent.Exchanger

class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {

    // TODO: set to appropriate values
    override val ATTEMPTS: Int = 5
    override val SMOOTHING = SonarRequest.Smoothing.MEDIAN
    override val WINDOW_SIZE = 3

    private val MAX_DISTANCE_TO_WALL_AHEAD = 30         // reached the corner and should turn left
    private val OUTER_CORNER_DISTANCE_THRESHOLD = 60    // reached outer corner
    private val ISOSCALENESS_MIN_DIFF = 10              // have to correct alignment to the wall
    private val ISOSCALENESS_MAX_DIFF = 50              // almost reached outer corner but not yet, and 120 meas. founds far wall
    private val DISTANCE_TO_WALL_UPPER_BOUND = 50       // have to move closer to the parallel wall
    private val DISTANCE_TO_WALL_LOWER_BOUND = 25       // have to move farther to the parallel wall
    private val SPURIOUS_REFLECTION_DIFF = 70           // we're approaching corner and get spurious reflection from two walls on 60 meas. ! Should be before outer corner case !

    private var calibrateAfterRotate = false

    var carX = 0
    var carY = 0
    var carAngle = 0

    override fun getCarState(anglesDistances: Map<Angle, AngleData>): CarState {
        return CarState.WALL
    }

    private fun noParallelWallFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
        resultBuilder.setDirections(getIntArray(FORWARD))
        resultBuilder.setDistances(getIntArray(10))
        return resultBuilder.build()
    }

    private fun noOrthogonalMeasurementFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
        resultBuilder.setDirections(getIntArray(FORWARD, RIGHT, FORWARD))
        val wallAngle = calculateAngle(anglesDistances, state)
        resultBuilder.setDistances(getIntArray(15, wallAngle.degs(), 60))
        addWall(-wallAngle)
        calibrateAfterRotate = true
        return resultBuilder.build()
    }

    private fun wallAheadFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
        resultBuilder.setDirections(getIntArray(LEFT, FORWARD))
        val wallAngle = calculateAngle(anglesDistances, state)
        resultBuilder.setDistances(getIntArray(wallAngle.degs(), 15))
        addWall(wallAngle)
        calibrateAfterRotate = true
        return resultBuilder.build()
    }

    private fun alignParallelToWall(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))

        // we're sure that we have 120 and 60 degs measurements as they were checked before
        val rotationDirection = if (anglesDistances[Angle(120)]!!.distance > anglesDistances[Angle(60)]!!.distance) LEFT else RIGHT

        resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
        resultBuilder.setDistances(getIntArray(10, 7))//todo calibrate
        return resultBuilder.build()
    }

    private fun correctDistanceToParallelWall(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))

        val rotationDirection = if (anglesDistances[Angle(90)]!!.distance > 50) RIGHT else LEFT

        resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
        resultBuilder.setDistances(getIntArray(10, 10))
        return resultBuilder.build()
    }

    private fun moveForward(): RouteMetricRequest {
        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
        resultBuilder.setDirections(getIntArray(FORWARD))
        resultBuilder.setDistances(getIntArray(20))
        return resultBuilder.build()
    }

    private fun addWall(angleWithPrevWall: Angle) {
        RoomModel.updateWalls()

        val firstWall = RoomModel.walls.first()
        val lastWall = RoomModel.walls.last()
        if (firstWall == lastWall && RoomModel.walls.size > 1) {
            RoomModel.finished = true
            lastWall.pushBackPoint(firstWall.rawPoints.last())
            lastWall.markAsFinished()
            RoomModel.walls.removeAt(0)
        } else {
            RoomModel.walls.add(Wall(lastWall.wallAngleOX + angleWithPrevWall))
        }
    }

    override fun getCommand(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest? {

        val dist0 = anglesDistances[Angle(0)]!!
        val dist60 = anglesDistances[Angle(70)]!!
        val dist90 = anglesDistances[Angle(90)]!!
        val dist120 = anglesDistances[Angle(110)]!!
        val sonarAngle = carAngle - 90

        if (anglesDistances.filter { it.value.angle.degs() >= 60 && it.value.angle.degs() <= 120 && it.value.distance == -1 }.size != 0) {
            return null
        }

        if (calibrateAfterRotate) {
            if (Math.abs(dist60.distance - dist120.distance) > 4) {
                println("Flaw in align found, correcting")
                val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
                val rotationDirection = if (anglesDistances[Angle(120)]!!.distance > anglesDistances[Angle(60)]!!.distance) LEFT else RIGHT
                resultBuilder.setDirections(getIntArray(rotationDirection))
                resultBuilder.setDistances(getIntArray(Math.min(Math.abs(dist60.distance - dist120.distance), 20)))
                return resultBuilder.build()
            } else {
                calibrateAfterRotate = false
                carAngle = RoomModel.walls.last().wallAngleOX.degs()
            }
        }
        // Check most basic measurements: 60/90/120
        if (dist90.distance == -1 || dist90.distance > OUTER_CORNER_DISTANCE_THRESHOLD) {
            println("No orthogonal measurement found")
            return noOrthogonalMeasurementFound(anglesDistances, state)
        }

        // Add point to room map
{'fraction_non_alphanumeric': 0.0858942256620381, 'fraction_numerical': 0.0186660596403369, 'mean_word_length': 3.272285251215559, 'pattern_counts': {'":': 0, '<': 21, '<?xml version=': 0, '>': 33, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '6015668', 'n_tokens_mistral': 3828, 'n_tokens_neox': 3528, 'n_words': 843}