Wall Smoothing/Exact

  public double smoothHeading(double absoluteHeading, @Output Trig headingTrig, double x, double y, int latDir) {
    double stickBearingSin = +headingTrig.cos * latDir;
    double stickBearingCos = -headingTrig.sin * latDir;

    double stickX = x + INNER_R * stickBearingSin + 4 * headingTrig.sin;
    double stickY = y + INNER_R * stickBearingCos + 4 * headingTrig.cos;

    double stickDistW = stickX - MIN_X;
    double stickDistE = MAX_X - stickX;
    double stickDistN = MAX_Y - stickY;
    double stickDistS = stickY - MIN_Y;

    double distW = x - MIN_X;
    double distE = MAX_X - x;
    double distN = MAX_Y - y;
    double distS = y - MIN_Y;

    double stickDistH;
    WALL wallH = null;
    if (stickDistW < stickDistE) {
      stickDistH = stickDistW;
      if (stickDistH < OUTER_R - 0.1) {
        wallH = WALL.W;
      }
    } else {
      stickDistH = stickDistE;
      if (stickDistH < OUTER_R - 0.1) {
        wallH = WALL.E;
      }
    }

    double stickDistV;
    WALL wallV = null;
    if (stickDistS < stickDistN) {
      stickDistV = stickDistS;
      if (stickDistV < OUTER_R - 0.1) {
        wallV = WALL.S;
      }
    } else {
      stickDistV = stickDistN;
      if (stickDistV < OUTER_R - 0.1) {
        wallV = WALL.N;
      }
    }

    WALL forwardHWall;
    if (headingTrig.sin < 0) {
      forwardHWall = WALL.W;
    } else {
      forwardHWall = WALL.E;
    }

    WALL forwardVWall;
    if (headingTrig.cos < 0) {
      forwardVWall = WALL.S;
    } else {
      forwardVWall = WALL.N;
    }

    WALL firstWall = null;
    WALL secondWall = null;
    double firstDist = Double.NaN;
    double secondDist = Double.NaN;

    if (forwardHWall == wallH && forwardVWall == wallV) {
      if (latDir > 0) {
        if (forwardHWall == WALL.W) {
          if (forwardVWall == WALL.N) {
            firstWall = WALL.W;
            firstDist = distW;
            secondWall = WALL.N;
            secondDist = distN;
          } else {
            firstWall = WALL.S;
            firstDist = distS;
            secondWall = WALL.W;
            secondDist = distW;
          }
        } else {
          if (forwardVWall == WALL.N) {
            firstWall = WALL.N;
            firstDist = distN;
            secondWall = WALL.E;
            secondDist = distE;
          } else {
            firstWall = WALL.E;
            firstDist = distE;
            secondWall = WALL.S;
            secondDist = distS;
          }
        }
      } else {
        if (forwardHWall == WALL.W) {
          if (forwardVWall == WALL.N) {
            firstWall = WALL.N;
            firstDist = distN;
            secondWall = WALL.W;
            secondDist = distW;
          } else {
            firstWall = WALL.W;
            firstDist = distW;
            secondWall = WALL.S;
            secondDist = distS;
          }
        } else {
          if (forwardVWall == WALL.N) {
            firstWall = WALL.E;
            firstDist = distE;
            secondWall = WALL.N;
            secondDist = distN;
          } else {
            firstWall = WALL.S;
            firstDist = distS;
            secondWall = WALL.E;
            secondDist = distE;
          }
        }
      }
    } else if (forwardHWall == wallH) {
      firstWall = wallH;
      firstDist = wallH == WALL.W ? distW : distE;
    } else if (forwardVWall == wallV) {
      firstWall = wallV;
      firstDist = wallV == WALL.S ? distS : distN;
    }

    WALL smoothWall = firstWall;
    double smoothDist = firstDist;

    while (smoothWall != null) {
      double wallSin = (OUTER_R - smoothDist) / OUTER_R;
      if (wallSin > 1) {
        wallSin = 1;
      }

      wallSin *= latDir;

      double wallCos = Math.sqrt(1 - wallSin * wallSin);

      double newWallSin = wallSin * DELTA_COS + wallCos * DELTA_SIN * latDir;
      double newWallCos = wallCos * DELTA_COS - wallSin * DELTA_SIN * latDir;

      if (newWallCos > 0) {
        wallSin = newWallSin;
        wallCos = newWallCos;
      }

      double newSin;
      double newCos;
      if (smoothWall == WALL.N) {
        newSin = +wallSin;
        newCos = +wallCos * +1;
      } else if (smoothWall == WALL.E) {
        newSin = +wallCos * +1;
        newCos = -wallSin;
      } else if (smoothWall == WALL.S) {
        newSin = -wallSin;
        newCos = -wallCos * +1;
      } else {
        newSin = -wallCos * +1;
        newCos = +wallSin;
      }

      double newHeading = Math.atan2(newSin, newCos);

      if (Math.signum(Utils.normalRelativeAngle(newHeading - absoluteHeading)) == latDir) {
        headingTrig.sin = newSin;
        headingTrig.cos = newCos;

        absoluteHeading = newHeading;

        if (debugSticks != null || debugHeadings != null) {
          stickBearingSin = +newCos * latDir;
          stickBearingCos = -newSin * latDir;

          double imagineStickX = x + INNER_R * stickBearingSin + 4 * newSin;
          double imagineStickY = y + INNER_R * stickBearingCos + 4 * newCos;
        }
      }

      if (smoothWall == firstWall) {
        smoothWall = secondWall;
        smoothDist = secondDist;
      } else {
        break;
      }
    }
    return absoluteHeading;
  }