//pins
int indexPin = 2; //8 on the drive INDEX
int track0Pin = 3; //26 on the drive. TRACK 0
int dirPin = 6; //18 on the drive. DIRECTION
int stepPin = 7; //20 on the drive. STEP
int motorEnableBPin = 9; //16 on the drive. MOTOR ENABLE B
int readPin = 5; // 30 on floppy. READ DATA
int writePin = 10; // 30 on floppy. WRITE DATA
int readWrite = 4;
String wynik = "";

int stepValue = 0;

void setup() {

  //initial delay
  delay(3000);

  Serial.begin(9600);  
  Serial.println("> Starting");  

  pinMode(dirPin, OUTPUT);
  pinMode(stepPin, OUTPUT);
  pinMode(motorEnableBPin, OUTPUT);
  pinMode(indexPin, INPUT_PULLUP);
  pinMode(track0Pin, INPUT_PULLUP);
  pinMode(readPin, INPUT_PULLUP);
  pinMode(readWrite, OUTPUT);

  //turn the motor off initially
  digitalWrite(motorEnableBPin, HIGH);
  digitalWrite(dirPin, HIGH);
  
  Serial.println("> Head possitioning");
  Serial.println(digitalRead(track0Pin));
  digitalWrite(stepPin, HIGH);
  //headGo();
  headReset();
  
  motorReset(); 
  Serial.println("> Motor starting"); 
  digitalWrite(motorEnableBPin, LOW);
  Serial.println("> Starting reading"); 
  delay(1000);
  digitalWrite(dirPin, LOW);
}

void loop()
{
   
   if(digitalRead(indexPin) == 0)
   {
     Serial.print(digitalRead(readPin));
     delay(5);
     //headGo(); 
   }
   
  /*
  if(digitalRead(indexPin) == 1)
  {
    readSector();
    
  }else{
      Serial.println(wynik);
      wynik = "";
      //headGo(); 
      while(digitalRead(indexPin) == 1){}
      delay(1);
  }
  */
  
}

void headGo()
{
  digitalWrite(readWrite, HIGH);
  digitalWrite(stepPin, HIGH);
  delay(1);
  digitalWrite(stepPin, LOW);
  stepValue ++;
  digitalWrite(readWrite, LOW);
  if(stepValue == 32) headReset();
}

void headReset()
{
  
  digitalWrite(dirPin, HIGH);
  Serial.println("> Head direction");
  while(digitalRead(track0Pin) == 1) 
  {
    //Serial.println("> Head stepping");
    digitalWrite(readWrite, HIGH);
    digitalWrite(stepPin, HIGH);
    delay(10);
    digitalWrite(stepPin, LOW);
    digitalWrite(readWrite, LOW);
  }
  stepValue = 0;
  digitalWrite(dirPin, LOW);
}

void motorReset()
{
   while(digitalRead(indexPin) == 1) 
  {
    digitalWrite(motorEnableBPin, LOW);
  }
  digitalWrite(motorEnableBPin, HIGH);
}

void motorGo()
{
  digitalWrite(motorEnableBPin, LOW);
  delay(40);
  digitalWrite(motorEnableBPin, HIGH);
}

void readSector()
{
  wynik += digitalRead(readPin);
  //Serial.println(" ");
}

void writeSector(int a)
{
  digitalWrite(readWrite, HIGH);
  digitalWrite(writePin, a);
  digitalWrite(readWrite, LOW);
}