From f8d16759730b175ba9b85f195de2ec3dcc5f89aa Mon Sep 17 00:00:00 2001
From: Leijurv <leijurv@gmail.com>
Date: Sat, 11 Aug 2018 21:27:46 -0700
Subject: [PATCH] pushing around corners

---
 .../bot/pathing/movement/Movement.java        |  2 +-
 .../movement/movements/MovementDiagonal.java  | 37 ++++++++++++++++++-
 2 files changed, 36 insertions(+), 3 deletions(-)

diff --git a/src/main/java/baritone/bot/pathing/movement/Movement.java b/src/main/java/baritone/bot/pathing/movement/Movement.java
index 7da5c2b35..cb9b30673 100644
--- a/src/main/java/baritone/bot/pathing/movement/Movement.java
+++ b/src/main/java/baritone/bot/pathing/movement/Movement.java
@@ -107,7 +107,7 @@ public abstract class Movement implements Helper, MovementHelper {
         return currentState.getStatus();
     }
 
-    private boolean prepared(MovementState state) {
+    protected boolean prepared(MovementState state) {
         if (state.getStatus() == MovementStatus.WAITING)
             return true;
 
diff --git a/src/main/java/baritone/bot/pathing/movement/movements/MovementDiagonal.java b/src/main/java/baritone/bot/pathing/movement/movements/MovementDiagonal.java
index 289b31d6a..23aa383f4 100644
--- a/src/main/java/baritone/bot/pathing/movement/movements/MovementDiagonal.java
+++ b/src/main/java/baritone/bot/pathing/movement/movements/MovementDiagonal.java
@@ -27,6 +27,8 @@ import net.minecraft.util.math.BlockPos;
 
 public class MovementDiagonal extends Movement {
 
+    private static final double SQRT_2 = Math.sqrt(2);
+
     public MovementDiagonal(BlockPos start, EnumFacing dir1, EnumFacing dir2) {
         this(start, start.offset(dir1), start.offset(dir2), dir2);
         // super(start, start.offset(dir1).offset(dir2), new BlockPos[]{start.offset(dir1), start.offset(dir1).up(), start.offset(dir2), start.offset(dir2).up(), start.offset(dir1).offset(dir2), start.offset(dir1).offset(dir2).up()}, new BlockPos[]{start.offset(dir1).offset(dir2).down()});
@@ -67,12 +69,43 @@ public class MovementDiagonal extends Movement {
 
     @Override
     protected double calculateCost(CalculationContext context) {
-        if (getTotalHardnessOfBlocksToBreak(context.getToolSet()) != 0) {
+        double lastPos = MovementHelper.getMiningDurationTicks(context.getToolSet(), positionsToBreak[4]) + MovementHelper.getMiningDurationTicks(context.getToolSet(), positionsToBreak[5]);
+        if (lastPos != 0) {
             return COST_INF;
         }
         if (!MovementHelper.canWalkOn(positionsToPlace[0])) {
             return COST_INF;
         }
-        return Math.sqrt(2) * (BlockStateInterface.isWater(src) || BlockStateInterface.isWater(dest) ? WALK_ONE_IN_WATER_COST : WALK_ONE_BLOCK_COST);
+        double optionA = MovementHelper.getMiningDurationTicks(context.getToolSet(), positionsToBreak[0]) + MovementHelper.getMiningDurationTicks(context.getToolSet(), positionsToBreak[1]);
+        double optionB = MovementHelper.getMiningDurationTicks(context.getToolSet(), positionsToBreak[2]) + MovementHelper.getMiningDurationTicks(context.getToolSet(), positionsToBreak[3]);
+        if (optionA != 0 && optionB != 0) {
+            return COST_INF;
+        }
+        if (optionA == 0) {
+            if (MovementHelper.avoidWalkingInto(BlockStateInterface.getBlock(positionsToBreak[2]))) {
+                return COST_INF;
+            }
+            if (MovementHelper.avoidWalkingInto(BlockStateInterface.getBlock(positionsToBreak[3]))) {
+                return COST_INF;
+            }
+        }
+        if (optionB == 0) {
+            if (MovementHelper.avoidWalkingInto(BlockStateInterface.getBlock(positionsToBreak[0]))) {
+                return COST_INF;
+            }
+            if (MovementHelper.avoidWalkingInto(BlockStateInterface.getBlock(positionsToBreak[1]))) {
+                return COST_INF;
+            }
+        }
+        double multiplier = 1;
+        if (optionA != 0 || optionB != 0) {
+            multiplier = 1.5;
+        }
+        return multiplier * SQRT_2 * (BlockStateInterface.isWater(src) || BlockStateInterface.isWater(dest) ? WALK_ONE_IN_WATER_COST : WALK_ONE_BLOCK_COST);
+    }
+
+    @Override
+    protected boolean prepared(MovementState state) {
+        return true;
     }
 }