// Kode 3.3 : Pengukuran Frekuensi Teknik Pooling

#define PERIODE_DISPLAY 5
#define PERIODE_GATE 1000

// unit menghitung frekuensi dalam RPS (1000 ms)
#define F_UNIT 1000L

// Pemetaan Port arduino
#define BUTTON1 A2
#define OPTO 2

// Pemetaan Port untuk memutar motor seperti praktikum sebelumnya
#define BCD1 8
#define T1 12
#define T2 13
#define AIN A0
#define ASENSE A5
#define MOTOR_INPUT1 3
#define MOTOR_INPUT2 A1

// fungsi-fungsi dari modul sebelumnya
boolean s1IsPressed() {
  static boolean b1_old = 0;
  boolean b1 = digitalRead (BUTTON1);
  boolean s1 = (b1 && !b1_old);
  b1_old = b1;
  return s1;
}

void writeDigit(int bcd){
  for (int i=0; i<4; i++){
    digitalWrite(BCD1 + i, bcd & 0x01);
    bcd = bcd >> 1;
  }
}

int digits[3];
void writeDigit3(int data) {
  digits[0] = data%10;
  data = data/10;
  digits[1] = data % 10;
  data = data / 10;
  digits[2] = data %10;
}

int index = 0;
void shiftDigit() {
  writeDigit(digits[index]);
  digitalWrite(T1, index & 0x02);
  digitalWrite(T2, index & 0x01);
  index = (index + 1) % 3;
}

// fungsi mengecek sinyal opto coupler
boolean optoIsFired() {
  static boolean b1_old = 1;
  boolean b1 = digitalRead(OPTO);
  boolean s1 = (b1 && !b1_old);
  b1_old = b1;
  return s1;
}

// Inisialisasi variabel global
long tdisplay, tgate;
long pulse;
int pwm;

void setup() {
  Serial.begin(115200);
  Serial.println("Pengukuran Frekuensi teknik pooling");
  // inisialisasi mode pin
    for(int pin=BCD1;pin<=T2;pin++){
      pinMode(pin, OUTPUT);
    }

  // Inisialisasi shift register
  pinMode(T1, OUTPUT);
  pinMode(T2, OUTPUT);
  digitalWrite(T1,LOW);
  digitalWrite(T2,LOW);

  // Inisialisasi INPUT1 dan INPUT2 motor driver
  pinMode(MOTOR_INPUT1, OUTPUT);
  pinMode(MOTOR_INPUT2, OUTPUT);
  analogWrite(MOTOR_INPUT1, 0);
  digitalWrite(MOTOR_INPUT2, LOW);

  // Inisialisasi Button1
  pinMode(BUTTON1,INPUT);
  digitalWrite(BUTTON1, HIGH);

  // inisialisasi variabel global
  tgate = tdisplay = millis();
  pulse = 0;
  pwm = 255;
}

// TaskButton(), bila S1 ditekan pwm turun secara step
void taskButton() {
  pwm = pwm - (256/16);
  if (pwm < 0) pwm = 255;
  analogWrite(MOTOR_INPUT1, pwm);
}

// TaskOpto(), bila opto terdeteksi, cacah pulse
void taskOpto() {
  pulse++;
}

// TaskFrequency(), memanggil setiap Tgate untuk menghitung frekuensi
void taskFrequency(long ms) {
  // hitung selang waktu tw, yang ms selalu sama dengan tgate, karena bisa terlambat, sehingga perlu dihitung lagi
  long tw = PERIODE_GATE + (ms - tgate);

  // hitung frekuensi
  int freq = pulse * F_UNIT / tw ; // lihat persamaan (3.1)

  // tampilkan ke 7 segment
  writeDigit3(freq);

  // untuk membandingkan, kirim ke serial
  // pwm tw pulse freq
  Serial.print(pwm);
  Serial.print(" ");
  Serial.print(tw);
  Serial.print(" ");
  Serial.print(pulse);
  Serial.print(" ");
  Serial.println(freq);

  // reset cacahan
  pulse = 0;
}

// task menampilkan bacaan 7 segment
void taskDisplay() {
 shiftDigit();
}

// loop utama
void loop() {
  long ms = millis();
  if (optoIsFired()) {
    taskOpto();
  }
  else if (ms >= tgate) {
    taskFrequency(ms);
    tgate = tgate + PERIODE_GATE;
  }
  else if (s1IsPressed()) {
    taskButton();
  }
  else if (ms >= tdisplay) {
    taskDisplay();
    tdisplay = tdisplay + PERIODE_DISPLAY;
  }
}