//*!!Sensor,    S1,          touchSensor, sensorTouch,                           !!*//
//*!!                                                                            !!*//
//*!!Start automatically generated configuration code.                           !!*//
#pragma platform(NXT)

const tSensors touchSensor         = (tSensors) S4;   //sensorTouch - pen        //*!!!!*//
const penTime = 100;


int toX, toY;
bool headMoving=false, paperMoving=false, isPenDown;

int headSpeed=70, paperSpeed=60, penSpeed=75;

void penUp(int time) {
		motor[motorC] = -penSpeed;

		wait10MSec(time);
		motor[motorC] = 0;

		isPenDown = false;
		wait10Msec(100);
}
void penDown(int time) {

		motor[motorC] = penSpeed;
		wait10MSec(time);

		motor[motorC] = 0;
		isPenDown = true;

		wait10Msec(100);
}
task moveHead() {

	    if (nMotorEncoder[motorB] > -toX) {

	   		motor[motorB] = -headSpeed;
	   		headMoving = true;

	   		while (nMotorEncoder[motorB] >= -toX) {}

	 		}
	 		else if (nMotorEncoder[motorB] < -toX) {

	   		motor[motorB] = headSpeed;
	   		headMoving = true;

	   		while (nMotorEncoder[motorB] <= -toX) {}

   		}
   		motor[motorB] = 0;
   		headMoving = false;

}

task movePaper() {
	 if (nMotorEncoder[motorA] > -toY) {

	   motor[motorA] = -paperSpeed;
	   paperMoving = true;

	   while (nMotorEncoder[motorA] >= -toY) {

	   }
	  }
	  else if (nMotorEncoder[motorA] < -toY) {

	   motor[motorA] = paperSpeed;
	   paperMoving = true;

	   while (nMotorEncoder[motorA] <= -toY) {

	   }
	  }
    motor[motorA] = 0;

	  paperMoving = false;
}


void moveTo(int x, int y) {

		toX = x;
		toY = y;
		StartTask(moveHead);

		StartTask(movePaper);
		wait1Msec(10); // Wait to be sure tasks i running
		while (headMoving || paperMoving) {

			// Wait until head and paper has moved
	  }
}

void initiate(){
  penDown(2*penTime);

  wait10MSec(3*penTime);
	penUp(penTime);
	headMoving = true;

	while (SensorValue(touchSensor) == 0) {

	    motor[motorB] = headSpeed;
	}
	motor[motorB] = 0;

	headMoving = false;
	nMotorEncoder[motorA] = 0;

	nMotorEncoder[motorB] = 0;             //the rotation sensor for motor A is reset to 0
}

void dot(int x, int y){

//	  moveTo(0,0);
	  moveTo(x,y);
    penDown(2*penTime);

    penUp(penTime);
}

void feedPaper(){

	nNxtButtonTask = 0;
	int run=1;
  nxtDisplayTextLine(8,"  FEEDPAPER   ");

	while(run){
	  	switch (nNxtButtonPressed)
			{

				case kRightButton:
					motor[motorA]=paperSpeed;
				break;

				case kLeftButton:
				  motor[motorA]=-paperSpeed;

				break;

				case kEnterButton:
				    run=0;

				break;

				default:
				     motor[motorA]=0;

				break;
			}
   }
}

void square(int x, int y, int sizeX,int sizeY){

    //moveTo(0,0);
    moveTo(x,y);
    penDown(2*penTime);

    moveTo(x,y+sizeY);
    moveTo(x+sizeX,y+sizeY);

    moveTo(x+sizeX,y);
    moveTo(x,y);

    penUp(penTime);
}

task output() {
	  eraseDisplay();

    while(1) {
        nxtDisplayTextLine(0,"x  : %d",-nMotorEncoder[motorB]);

        nxtDisplayTextLine(1,"y  : %d",-nMotorEncoder[motorA]);
        if(isPenDown){

           nxtDisplayTextLine(2,"Pen is down");
        }
        else{
            nxtDisplayTextLine(2,"Pen is up");

        }

    }
}


task main(){

	SetSensorType(S4, sensorTouch);
	bFloatDuringInactiveMotorPWM = false;

  feedPaper();
  StartTask(output); // Start output til display
	initiate();

  while(1){
  	dot(100,100);
  	square(200,200,200,200);

  	square(300,300,300,300);
	dot(50,50);

  }

}