bool Buffer::find(const QString& findText, int flags, bool forward,
                  bool wrap, bool *searchWrapped) {
    if (findText.isEmpty()) {
        return false;
    }
    if (searchWrapped) {
        *searchWrapped = false;
    }
    // Perform the search
    setSearchFlags(flags);
    setTargetStart(forward ? currentPos() : currentPos() - 1);
    setTargetEnd(forward ? length() : 0);
    QByteArray findArray = findText.toUtf8();
    int findPos = searchInTarget(findArray.length(), findArray);
    // If the search should wrap, perform the search again.
    if (findPos == -1 && wrap) {
        setTargetStart(forward ? 0 : length());
        setTargetEnd(forward ? currentPos() : currentPos() - 1);
        findPos = searchInTarget(findArray.length(), findArray);
        if (searchWrapped) {
            *searchWrapped = true;
        }
    }
    if (findPos != -1)  {
        setSel(targetStart(), targetEnd());
        scrollRange(targetStart(), targetEnd());
    }
    return findPos != -1;
}
void OneRobotOffensePlay::Execute() {
  for(size_t i = 0; i < 6; i++) {
    // State machine to kick towards goal
    if(assignments[i] == 1) { // The offensive robot
      fprintf(stderr, "OffensiveRobot state %d\n", states[i]);
      if(states[i] == 0) { // Calculating position to kick state
        // needs to be tested
        int robotId = i;
        if (!_isYellowTeam) {
          i +=6;
        }

        // Sets the location of the goals
        vector<double> targetStart(2, 0);
        vector<double> targetEnd(2, 0);
        targetStart[1] = 0.35 * 1000;
        targetEnd[1] = -0.35 * 1000;
        // Only x coords of goals change
        if (_isYellowTeam) {
          targetStart[0] = -3 * 1000;
          targetEnd[0] = -3 * 1000;
        } else {
          targetStart[0] = 3 * 1000;
          targetEnd[0] = 3 * 1000;
        }

        MatrixXd ownTeamPos(6, 2);
        MatrixXd otherTeamPos(6, 2);
        
        // TODO botstates
        std::vector<Robot*>* otherTeamBots;
        if (_isYellowTeam) {
          otherTeamBots = SoccerFieldInfo::Instance()->blue_bots;
        } else {
          otherTeamBots = SoccerFieldInfo::Instance()->yellow_bots;
        }
       
        // Fills in matrix from bot states
        for (int k = 0; k<6; k++){
          ownTeamPos(k, 0) = _robots->at(k)->CurrentState()[0];
          ownTeamPos(k, 1) = _robots->at(k)->CurrentState()[1];
            
          otherTeamPos(k, 0) = otherTeamBots->at(k)->_currentPosition[0];
          otherTeamPos(k, 1) = otherTeamBots->at(k)->_currentPosition[1];
        }
        // TODO Could be an eigen vector? 
        std::vector<double> ballPos(3, 0);
        ballPos[0] = SoccerFieldInfo::Instance()->ball[0];
        ballPos[1] = SoccerFieldInfo::Instance()->ball[1];
        ballPos[2] = SoccerFieldInfo::Instance()->ball[2];
	
	Eigen::MatrixXd shootingPosMatrix= Evaluation::openAngleFinder(ballPos, robotId, targetStart, targetEnd, ownTeamPos, otherTeamPos);
	if(shootingPosMatrix.rows() == 0) {
	  fprintf(stderr, "No valid shot\n");
	  continue;
	}
	// TODO Why?
	for(int i = 0; i < shootingPosMatrix.rows(); ++i) {
	  Eigen::VectorXd shootingPos =  shootingPosMatrix.row(i);
	  fprintf(stderr, "Shooting Pos %d: %f, %f\n", i, shootingPos[0], shootingPos[1]);
	}
        Eigen::VectorXd shootingPos = shootingPosMatrix.row(0);
	
        fprintf(stderr, "Calculating shooting angle!\n");
        Eigen::Vector3d goal = _robots->at(i)->CurrentState();
        fprintf(stderr, "Robot State: %f  %f %f\n", goal[0], goal[1], goal[2]);

// 	double changeInAngle = (goal[2] - _isYellowTeam->at(i)->CurrentState()[2]) / 2;
//         goal[0] = 2 * 89 * sin(changeInAngle) * cos(changeInAngle);
//         goal[1] = 2 * 89 * sin(changeInAngle) * sin(changeInAngle);
// 	
	double desiredShotAngle = (shootingPos[0] + shootingPos[1]) / 2;
        Eigen::Vector3d goalStateToKickFrom = Evaluation::GetGoalPositionToBall(desiredShotAngle);
        wayPointsToGoalState.push_back(goalStateToKickFrom);
        fprintf(stderr, "Final goal: %f, %f, %f\n", goalStateToKickFrom[0], goalStateToKickFrom[1], goalStateToKickFrom[2] * 180 / M_PI);

        Eigen::Vector2d robotAxis(cos(_robots->at(i)->CurrentState()[2]), sin(_robots->at(i)->CurrentState()[2]));
        Eigen::Vector2d goalStateAxis(cos(goalStateToKickFrom[2]), sin(goalStateToKickFrom[2]));

        if(robotAxis.dot(goalStateAxis) > 0) {
          //Go Directly after backing up
          fprintf(stderr, "robotAxis%f %f  %f\n", robotAxis[0], robotAxis[1], (double)robotAxis.dot(goalStateAxis));
        } else { 
          fprintf(stderr, "robotAxis angle>90%f %f  %f\n", robotAxis[0], robotAxis[1], (double)robotAxis.dot(goalStateAxis));
          
	  //Decide how to go?

	  //Rotate robot axis by 90 degrees
          double temp = robotAxis[0]; //-sin(theta)
          robotAxis[0] = -robotAxis[1];//cos(theta)
          robotAxis[1] = temp;
	  
          Eigen::Vector3d wayPoint;
          if(robotAxis.dot(goalStateAxis) >= 0) {
            wayPoint = Evaluation::GetGoalPositionToBall(_robots->at(i)->CurrentState()[2] + M_PI/2);
          } else {
            wayPoint = Evaluation::GetGoalPositionToBall(_robots->at(i)->CurrentState()[2] - M_PI/2);
          }
          wayPoint[0] -= 400 * cos(wayPoint[2]);
          wayPoint[1] -= 400 * sin(wayPoint[2]);
          fprintf(stderr, "Waypoint: %f, %f, %f\n", wayPoint[0], wayPoint[1], wayPoint[2] * 180 / M_PI);
          wayPointsToGoalState.push_back(wayPoint);
        }
	
	_robots->at(i)->setSpinner(false);
        
        //Go back along current axis
        goal = _robots->at(i)->CurrentState();
        goal[0] -= 90 * cos(goal[2]);
        goal[1] -= 90 * sin(goal[2]);
	
        _robots->at(i)->goToLocation(1, goal);
        states[i] = 1;

      } else if (states[i] == 1) { // Going to location state

        if (_robots->at(i)->execute() == 1) {
          if(wayPointsToGoalState.size() > 0) {
	    Eigen::Vector3d intermediateGoal = wayPointsToGoalState.back();
            fprintf(stderr, "Going to waypoint: %f, %f, %f\n", intermediateGoal[0], intermediateGoal[1], intermediateGoal[2]);
            _robots->at(i)->goToLocation(1, intermediateGoal);
            wayPointsToGoalState.pop_back();
         } else {
           states[i] = 2;
         }
        }
      } else if (states[i] == 2) {
        _robots->at(i)->setSpinner(true);
        if (_robots->at(i)->execute() == 1) {
          fprintf(stderr, "OneRobotOffensePlay::Execute  kicking %f, %f, %f\n", _robots->at(i)->CurrentState()[0], _robots->at(i)->CurrentState()[1], _robots->at(i)->CurrentState()[2]);
          Eigen::Vector2d r = Eigen::Vector2d(_robots->at(i)->CurrentState()[0], _robots->at(i)->CurrentState()[1]);
          Eigen::Vector2d loc = Eigen::Vector2d(SoccerFieldInfo::Instance()->ball[0], SoccerFieldInfo::Instance()->ball[1]);
          double distance = (r - loc).norm();
          fprintf(stderr, "Distance: %f\n", distance);
          if(distance > 120) {
            states[i]++;
            fprintf(stderr, "Last State\n");
          } else {
            // Call Kick
            fprintf(stderr, "Kicking\n");
            _robots->at(i)->setKickSpeed(2, 0);
            _robots->at(i)->sendVelocityCommands();
          }
        }
      } else if (states[i] == 3) {
        continue;
        //_complete = true; // _complete should never be set true by a state machine
      } else {
        fprintf(stderr, "OneRobotOffensePlay::Execute Invalid state %d", states[i]);
      }
    }
  }
}