void rampDrive(int driveTarget)
	{
		int direction = 1;
		if( driveTarget < 0 )
		{
			direction = -1;
			driveTarget *= -1;
		}
		nMotorEncoder[LeftMWheel] = 0;
		int distance = 0;
		int pwr = 0;
		while(distance < driveTarget )
		{
			//distance = abs ( nMotorEncoder[leftBack] + nMotorEncoder[rightBack] ) / 2;

			distance = abs(nMotorEncoder[LeftMWheel]);

			if( distance <= driveTarget / 2)
				pwr = distance / 5 + 20;
			else
				pwr = ( driveTarget - distance ) / 10 + 20;

			pwr *= direction;
			setLeft(pwr);
			setRight(pwr);
		}
		setLeft(0);
		setRight(0);
	}

	// time is in milliseconds
	// distance is in tenths of inches
	// direction is 1 or -1 -- positive is forwards
	void moveStraight(int direction, int time, int driveTarget)
	{
		nMotorEncoder[LeftMWheel] = 0;
		int distance = 0;
		float pwr = 0;
			while(distance < driveTarget )
			{

					distance = abs(nMotorEncoder[LeftMWheel]);
					//initial version
					//pwr = 64 * sin(distance * PI / driveTarget);
					//improved version

					//SHITTY PID:
					//pwr = 0.05 * (driveTarget - distance) + 0.0003 * (0.05 * (driveTarget * distance - 0.5 * (distance * distance)));

					if( distance <= driveTarget / 2)
						pwr = distance / 5 + 20; //TODO: figure out how this works
					else
						pwr = ( driveTarget - distance ) / 10 + 20;

						//pwr = -1 * (1.11 * sqrt(driveTarget) - 10) * cos(distance * 2 * PI / driveTarget) + (1.11 * sqrt(distance)) + 10;


					pwr *= direction;
					setLeft(pwr);
					setRight(pwr);
			}
			setLeft(0);
			setRight(0);
		}

// time is in milliseconds
// distance is in tenths of inches
// direction is 1 or -1 -- positive is forwards
void moveStraight(int direction, int time, int driveTarget)
{
	nMotorEncoder[LeftMWheel] = 0;
	int distance = 0;
	float pwr = 0;
	while(distance < driveTarget )
	{
		distance = abs(nMotorEncoder[LeftMWheel]);
		// works
		if( distance <= driveTarget / 2)
			pwr = distance / 5 + 20; //TODO: figure out how this works
		else
			pwr = ( driveTarget - distance ) / 10 + 20;

		// test old formula
		//pwr = oldPwr(driveTarget, distance);
		// test new formula
		//pwr = newPwr(driveTarget, distance);

		pwr *= direction;
		setLeft(pwr);
		setRight(pwr);
	} // end while loop

	setLeft(0);
	setRight(0);
}

 void PlayerInputStruct::fromGameEvent(const GameControl &control, bool state)
 {
     switch(control)
     {
         case GameControl::ACTION:
             setAction(state);
             break;
         case GameControl::UP:
             if(state)
                 setUp(100000);
             else
                 setUp(0);
             break;
         case GameControl::DOWN:
             if(state)
                 setDown(100000);
             else
                 setDown(0);
             break;
         case GameControl::LEFT:
             if(state)
                 setLeft(100000);
             else
                 setLeft(0);
             break;
         case GameControl::RIGHT:
             if(state)
                 setRight(100000);
             else
                 setRight(0);
             break;
         case GameControl::BUTTON1:
             setButton1(state);
             break;
         case GameControl::BUTTON2:
             setButton2(state);
             break;
         case GameControl::BUTTON3:
             setButton3(state);
             break;
         case GameControl::BUTTON4:
             setButton4(state);
             break;
         case GameControl::BUTTON5:
             setButton5(state);
             break;
         case GameControl::BUTTON6:
             setButton6(state);
             break;
         default:
             //Nothing to do; control is not handled by the automatic conversion.
             break;
     }
 }

		void pickUpBall(int goals)
	{
		resetValues(0);
		intake(1);
		wait10Msec(30);
		int current = 0;
		while(current < goals * 250)
		{
			setLeft(25); setRight(25);
			current = nMotorEncoder[LeftMWheel];
		}
		intake(0);
		setLeft(0); setRight(0);
	}

void blueUdit()
{
	deploy();
	intake(1);
	wait10Msec(10);
	moveStraight(1, 0, 475); //picks up
	wait10Msec(50);
	moveStraight(-1, 0, 475);//comes back
	//intake(0)();
	spin(1, 0, 400);
	lift(BUMP);
	holdArm();
	waitForButton();
	intake(-1);
	wait10Msec(100);
	resetValues(0);
	liftDown(); // added

	waitForButton();
	//liftDown();
	// end Devansh
	//waitForButton();
	intake(1);
	moveStraight(1, 0, (HALF_TILE));
	wait10Msec(50);
	pickUpBall(1);
	spin(1, 0, 200);
	//crossBump();
	lift(BUMP);
	nMotorEncoder[LeftMWheel] = 0;
	if(true)
	{
		setRight(127);
		wait10Msec(10);
		setLeft(127);
	}
	while (abs(nMotorEncoder[LeftMWheel]) < 500)
	{
		setRight(127); setLeft(127);
	}
	setRight(0); setLeft(0);
	lift(BARRIER);
	holdArm();
	wait10Msec(30);
	moveStraight(1, 0, 550);
	intake(-1);
	// end udit
	resetValues(1000);
}

task usercontrol()
{

	// Flags

	// Servo / program initialization:

	zeroEncoders();

	// Main loop:

	waitForStart();

	while (true)
	{

		getJoystickSettings(joystick);

		setLeft(100);
		setRight(100);

	}

	return;

}

ExpressionNode::ExpressionNode(const ExpressionNode & other): //copy constructor
type(other.getType()), operation(other.getOperation()), activechildren(other.getOperation()->getArity()),
right(0), firstChild(0), variable(other.getVariable()), value(other.getValue())
{
	setRight(other.getRight());
	setFirstChild(other.getFirstChild()); // automatically also sets other children
}

void Camera::updateCameraRotation() {

	double xPos, yPos;
	glfwGetCursorPos(m_window, &xPos, &yPos); // get current mouse position
	
	// if mouse still in window center, no need to recalculate view matrix
	if (xPos == m_windowWidth / 2 && yPos == m_windowHeight / 2) {
		return;
	}
	centerMouse();

	m_horizontalAngle += float(m_windowWidth / 2 - xPos) * m_mouseSpeed;
	m_verticalAngle += float(m_windowHeight / 2 - yPos) * m_mouseSpeed;

	glm::vec3 oldLookIn = getLookAt() - getCenter();
	glm::vec3 lookInHorizontal = glm::vec3(sin(m_horizontalAngle), oldLookIn.y, cos(m_horizontalAngle));
	glm::vec3 lookInVertical = glm::vec3(oldLookIn.x * cos(m_verticalAngle), sin(m_verticalAngle), oldLookIn.z * cos(m_verticalAngle));
	glm::vec3 newLookIn = glm::normalize(lookInHorizontal + lookInVertical);
	
	setLookAt(getCenter() + newLookIn);

	float rightHorizontalAngle = m_horizontalAngle - PI / 2.0f;
	setRight(glm::vec3(sin(rightHorizontalAngle), getRight().y, cos(rightHorizontalAngle)));

	setUp(glm::normalize(glm::cross(getRight(), newLookIn)));
}

Snake::Snake():AnimUT()
{
#ifdef debug
    qDebug("Snake launched");
#endif

    controls->addAnimationControl(animate, 200);

    cellSize = 15;
    nbCells = 20;

    // controls
    snakeItem = new SnakeItem(cellSize, nbCells);

    controls->addLabel("Movements");
    QPushButton *upBtn = controls->addButton("Up");
    connect(upBtn, SIGNAL(released()), snakeItem, SLOT(setUp()));
    QPushButton *downBtn = controls->addButton("Down");
    connect(downBtn, SIGNAL(released()), snakeItem, SLOT(setDown()));
    QPushButton *leftBtn = controls->addButton("Left");
    connect(leftBtn, SIGNAL(released()), snakeItem, SLOT(setLeft()));
    QPushButton *rightBtn = controls->addButton("Right");
    connect(rightBtn, SIGNAL(released()), snakeItem, SLOT(setRight()));

    controls->addDivider();

    score = controls->addLabel("Score : 0");
    connect(snakeItem, SIGNAL(updateScore(int)), this, SLOT(updateScore(int)));

    // viewer
    drawBorders();
    viewer->addItem(snakeItem);
}

bool AnchorsBase::setAnchor(const Qt::AnchorPoint &p, QWidget *target, const Qt::AnchorPoint &point)
{
    if (!target) {
        return false;
    }

    AnchorsBase *base = AnchorsBasePrivate::getWidgetAnchorsBase(target);

    if (!base) {
        base = new AnchorsBase(target);
    }

    const AnchorInfo *info = base->d_func()->getInfoByPoint(point);

    switch (p) {
    case Qt::AnchorTop:
        return setTop(info);
    case Qt::AnchorBottom:
        return setBottom(info);
    case Qt::AnchorLeft:
        return setLeft(info);
    case Qt::AnchorRight:
        return setRight(info);
    case Qt::AnchorHorizontalCenter:
        return setHorizontalCenter(info);
    case Qt::AnchorVerticalCenter:
        return setVerticalCenter(info);
    default:
        return false;
    }
}

    PixelRegion PixelRegion::globalCut(Side side, int p) {
        if (!lineIntersect(side, p)) {
            return PixelRegion({ 0, 0 }, { 0, 0 });
        }

        PixelRegion cutOff(*this);
        int cutSize = 0;
        switch (side) {
        case LEFT:
            setLeft(p);
            cutOff.setRight(p - cutSize);
            break;
        case TOP:
            setTop(p);
            cutOff.setBottom(p - cutSize);
            break;
        case RIGHT:
            setRight(p);
            cutOff.setLeft(p + cutSize);
            break;
        case BOTTOM:
            setBottom(p);
            cutOff.setTop(p + cutSize);
            break;
        }
        return cutOff;
    }

void GuiSetBorder::mousePressEvent(QMouseEvent * e)
{
	if (e->y() > e->x()) {
		if (e->y() < height() - e->x()) {
			if (left_.enabled) {
				setLeft(!left_.set);
				// emit signal
				leftSet(left_.set);
			}
		} else {
			if (bottom_.enabled) {
				setBottom(!bottom_.set);
				// emit signal
				bottomSet(bottom_.set);
			}
		}
	} else {
		if (e->y() < height() - e->x()) {
			if (top_.enabled) {
				setTop(!top_.set);
				// emit signal
				topSet(top_.set);
			}
		} else {
			if (right_.enabled) {
				setRight(!right_.set);
				// emit signal
				rightSet(right_.set);
			}
		}
	}
	update();
	// emit signal
	clicked();
}

BPlusNode::BPlusNode(IXFileHdr &hdr, PFPageHandle &page, bool load)
	: len_(hdr.len)
	, type_(hdr.type)
	, capacity_(hdr.capacity)
	, page_(page)
{
	// allocPage 用于表示这个页面是否为新分配的
	// 布局如下
	//  一共n个key,占据 n * len_字节的大小
	//  一共n个rid,占据 n * sizeof(RID) 字节的大小
	//  一个字节用于记录一共有多少个key,占据sizeof(numOfKeys_)字节的大小
	//  一个left字段,占据sizeof(Page)字节的大小
	//  一个right字段,占据了sizeof(Page)字节的大小
	Ptr addr = page.rawPtr();
	num_ = page.page();

	keys_ = addr;
	rids_ = reinterpret_cast<RID *>(addr + len_ * capacity_);
	if (load) { // 说明这个页面实际上已经初始化过了
		size_ = size();
		left_ = left();
		right_ = right();
	}
	else { // 这里需要重新初始化
		size_ = 0;
		setSize(0);
		setLeft(-1);
		setRight(-1);
		page.setDirty();
	}
}

void buildBinaryTree(pin_node root, int i)
{
	int v;
	pin_node q, p;
	p = q = root;
	while (q) {
		p = q;
		v = info(p);
		if (v == i) {
			printf("duplicate\n");
			q = NULL;
		} else if (v < i) {
			q = q->left;

		} else {
			q = q->right;
		}
		

	}

	if (p) {
		int v = p->val;
		 if (v < i) {
		 	setLeft(p, i);
		} else {
			setRight(p, i);
		}
	}
	
}

void Rect::expand( const QPoint& point )
{
	setLeft  ( qMin(point.x(), left()  ) );
	setRight ( qMax(point.x(), right() ) );
	setTop   ( qMax(point.y(), top()   ) );
	setBottom( qMin(point.y(), bottom()) ); 
};

bool
DCirfile::NextItemStr(const char *pos)
{
	CurrLeft = pos;

  for (;;) {
		CurrLeft = GetLinePos(CurrLeft);
		if (!CurrLeft)
			return false;
    //skipLine();
    if (isalnum(*CurrLeft)
     || (ispunct(*CurrLeft)
		  && ((*CurrLeft != ';')
		   || (*(CurrLeft + 1) == '=')) // deleted setting
		  && (*CurrLeft != '['))
		 || (*CurrLeft < 0)) {    // allow high-bit chars
			if (*CurrLeft == '\\')
				CurrLeft++;
      break;
		}
    if ((*CurrLeft == '\0')
     || (*CurrLeft == '[')) {
      CurrLeft = CurrRight = NULL;
      LeftLen = RightLen = 0;
      return false;
    }
  }

  setRight();
  return true;
}

void GfxCropDialog::bindEvents()
{
	// Left text box
	text_left_->Bind(wxEVT_TEXT_ENTER, &GfxCropDialog::onTextEnter, this);
	text_left_->Bind(wxEVT_KILL_FOCUS, [&](wxFocusEvent& e) {
		setLeft();
		e.Skip();
	});

	// Top text box
	text_top_->Bind(wxEVT_TEXT_ENTER, &GfxCropDialog::onTextEnter, this);
	text_top_->Bind(wxEVT_KILL_FOCUS, [&](wxFocusEvent& e) {
		setTop();
		e.Skip();
	});

	// Right text box
	text_right_->Bind(wxEVT_TEXT_ENTER, &GfxCropDialog::onTextEnter, this);
	text_right_->Bind(wxEVT_KILL_FOCUS, [&](wxFocusEvent& e) {
		setRight();
		e.Skip();
	});

	// Bottom text box
	text_bottom_->Bind(wxEVT_TEXT_ENTER, &GfxCropDialog::onTextEnter, this);
	text_bottom_->Bind(wxEVT_KILL_FOCUS, [&](wxFocusEvent& e) {
		setBottom();
		e.Skip();
	});

	// Absolute/Relative radio buttons
	rb_absolute_->Bind(wxEVT_RADIOBUTTON, [&](wxCommandEvent&) { updateValues(); });
	rb_relative_->Bind(wxEVT_RADIOBUTTON, [&](wxCommandEvent&) { updateValues(); });
}

// -1 is max left
// 0 is max forward
// 1 is max right
void MotorController::arc(double direction, double speedScale) {
    if (abs(direction) > 1)direction /= abs(direction); //makes direction between 1 & -1
    printf("Direction: %f\n", direction);

    if (direction < 0) { //left
        setLeft((int) (LEFT_MAX * (1 - abs((int) direction)) *
                       speedScale));    //sets the left motor to something less than the max speed
        setRight((int) (RIGHT_MAX * (speedScale - 0.05)));                        //sets the right motor to max speed
    } else if (direction > 0) { //right
        setLeft((int) (LEFT_MAX * speedScale));                        //sets the left motor the max speed
        setRight((int) (RIGHT_MAX * (1 - direction) *
                        speedScale));     //sets the right motor to something less than the max speed
    } else if (direction == 0) {
        moveForward();
    }

}

/*!
 
  Constructs a rectangle with \e topLeft the top-left corner location
  and \e bottomRight the bottom-right corner.

*/
vpRect::vpRect(const vpImagePoint &topLeft, const vpImagePoint &bottomRight)
{
  this->left = topLeft.get_u();
  this->top = topLeft.get_v();
  
  setBottom( bottomRight.get_v() );
  setRight( bottomRight.get_u() );
};