#pragma config(Sensor, S2,     ,               sensorTouch)
#pragma config(Sensor, S3,     ,               sensorTouch)
#pragma config(Sensor, S4,     colour,         sensorCOLORFULL)
#pragma config(Motor,  motorA,          motorFeeder,   tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorB,          motorPen,      tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          motorMove,     tmotorNormal, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

const int printSpeed=40, calibrationSpeed=60, buttonError=50, pushdown = 80;
int x = 0, y = 0;
float fullWidth, fullLength;

void movepen(){
  while(nMotorEncoder(motorB)>-pushdown){
    motor(motorB)=-printSpeed;
  }
  nMotorEncoder(motorB)=0;
  motor(motorB)=0;
  while(nMotorEncoder(motorB)<pushdown){
    motor(motorB)=printSpeed;
  }
  nMotorEncoder(motorB)=0;
  motor(motorB)=0;
}
void initialization(){
  nVolume=3;
  nMotorEncoder(motorA)=0;
  nMotorEncoder(motorB)=0;
  nMotorEncoder(motorC)=0;
}

void entrance(){
  nxtDisplayTextLine(1,"Press Enter");
  nxtDisplayTextLine(2,"To Begin");

  while(nNxtButtonPressed==-1){
  }
}

void calibration(){
  nxtDisplayTextLine(1,"Wait for Beep");
  while(SensorValue(S3)==0){
    motor(motorC)=calibrationSpeed;
  }
  wait1Msec(500);
  nMotorEncoder(motorC)=0;
  while(SensorValue(S2)==0){
    motor(motorC)=-calibrationSpeed;
  }
  motor(motorC)=0;
  wait1Msec(500);
  fullWidth=nMotorEncoder(motorC)*-1-buttonError;
  nMotorEncoder(motorC)=0;
  while(nMotorEncoder(motorC)<fullWidth/2){
    motor(motorC)=calibrationSpeed;
  }
  motor(motorC)=0;
  nMotorEncoder(motorC)=0;
  PlayTone(800,200);
}

void pullforpaper(){
  while(SensorValue(S4)!=6){
  }
  while(nMotorEncoder(motorA)<1000){
    motor(motorA)=calibrationSpeed/2;
  }
  while(SensorValue(S4)==6){
    motor(motorA)=calibrationSpeed/2;
  }
  fullLength=nMotorEncoder(motorA)*6.4/10;
  nMotorEncoder(motorA)=0;
  while(nMotorEncoder(motorA)>-fullLength){
    motor(motorA)=-calibrationSpeed/2;
  }
  nMotorEncoder(motorA)=0;
  while(nMotorEncoder(motorC)>-fullWidth/2){
    motor(motorC)=-calibrationSpeed;
  }
  motor(motorA)=0;
  motor(motorB)=0;
  motor(motorC)=0;
  nMotorEncoder(motorA)=0;
  nMotorEncoder(motorC)=0;
  wait1Msec(1000);
}

void draw(){
  char incommingChar;

  byte CR = 13;
  byte LF = 10;
  byte S = 48;

  TFileHandle   hFileHandle;
  TFileIOResult nIOResult;
  string        FileName = "picture.txt";
  int           FileSize = 10000000;

  OpenRead(hFileHandle, nIOResult, FileName, FileSize);

  int Width = 160, Height = 114;

   for(x=0; x<Width; x++){
     for(y=0; y<Height; y++){      
       ReadByte(hFileHandle, nIOResult, incommingChar);
       nxtDisplayClearTextLine(1);
       nxtDisplayTextLine(1,incommingChar);
       if(incommingChar==S || incommingChar==S+1){
          if(incommingChar == S){
            motor(motorA)=0;
            motor(motorB)=0;
            motor(motorC)=0;
            movepen();                 
          }           
          while(nMotorEncoder(motorC)<(fullWidth/Width)){
            motor(motorC)=printSpeed;
          }      
          nMotorEncoder(motorC)=0;
          wait1Msec(500);
        }
        else{
          y=y-1;
          motor(motorA)=0;
          motor(motorB)=0;
          motor(motorC)=0;
        }
     }
     while(nMotorEncoder(motorA)<fullLength/Height){
       motor(motorA)=printSpeed;      
     }
     nMotorEncoder(motorA)=0;
     while(nMotorEncoder(motorC)>-fullWidth){
       motor(motorC)=-printSpeed*1.5;
     }
     motor(motorA)=0;
     motor(motorC)=0;
     nMotorEncoder(motorC)=0;
   }
}



task main(){
  initialization();
  entrance();
  calibration();
  pullforpaper();
  drawdraw();
}