#include <NewPing.h>

#define NUM_SENSORS 7
#define MAX_DISTANCE 200

// Pin untuk sensor ultrasonik
int TRIGPIN[NUM_SENSORS] = {2, 4, 6, 8, 10, 12, 14}; // Pin trigger
int ECHOPIN[NUM_SENSORS] = {3, 5, 7, 9, 11, 13, 15};    // Pin echo

int sensors[NUM_SENSORS]; // Array untuk objek sensor
NewPing sonar1(TRIGPIN[1 - 1], ECHOPIN[1 - 1], MAX_DISTANCE);
NewPing sonar2(TRIGPIN[2 - 1], ECHOPIN[2 - 1], MAX_DISTANCE);
NewPing sonar3(TRIGPIN[3 - 1], ECHOPIN[3 - 1], MAX_DISTANCE);
NewPing sonar4(TRIGPIN[4 - 1], ECHOPIN[4 - 1], MAX_DISTANCE);
NewPing sonar5(TRIGPIN[5 - 1], ECHOPIN[5 - 1], MAX_DISTANCE);
NewPing sonar6(TRIGPIN[6 - 1], ECHOPIN[6 - 1], MAX_DISTANCE);
NewPing sonar7(TRIGPIN[7 - 1], ECHOPIN[7 - 1], MAX_DISTANCE);

float input[NUM_SENSORS]; // Array untuk menyimpan hasil pengukuran jarak
float weightsInputHidden[NUM_SENSORS][5]; // Bobot antara input dan lapisan tersembunyi
float weightsHiddenOutput[5][4]; // Bobot antara lapisan tersembunyi dan output
float biasHidden[5]; // Bias untuk lapisan tersembunyi
float biasOutput[4]; // Bias untuk output

void setup() {
  Serial.begin(115200);
  // Inisialisasi sensor ultrasonik
  // for (int i = 0; i < NUM_SENSORS; i++) {
  //   sensors[i] = NewPing(triggerPins[i], echoPins[i], MAX_DISTANCE);
  // }
  
  // Inisialisasi bobot dan bias
  initializeWeightsAndBiases();
  
  // Inisialisasi motor penggerak dan lainnya di sini
}

void loop() {
  Serial.println("\ninput layer:_____________");
  // Mengukur jarak dari masing-masing sensor
  // for (int i = 0; i < NUM_SENSORS; i++) {
  //   input[i] = sensors[i].ping_cm();
  // }
  input[0] = sonar1.ping_cm();
  input[1] = sonar2.ping_cm();
  input[2] = sonar3.ping_cm();
  input[3] = sonar4.ping_cm();
  input[4] = sonar5.ping_cm();
  input[5] = sonar6.ping_cm();
  input[6] = sonar7.ping_cm();

  for(int i=0;i<7;i++){
    Serial.print(input[i]);Serial.print(" ");
  }Serial.println();
  
  // Feedforward
  float hiddenLayer[5];
  float output[4];
  
  // Hitung lapisan tersembunyi
  Serial.println("\nhidden layer:_________________");
  for (int i = 0; i < 5; i++) {
    hiddenLayer[i] = 0;
    for (int j = 0; j < NUM_SENSORS; j++) {
      hiddenLayer[i] += input[j] * weightsInputHidden[j][i];
      Serial.print(hiddenLayer[i]);Serial.print(" ");
    }Serial.print("\t= ");
    hiddenLayer[i] += biasHidden[i];Serial.print(hiddenLayer[i]);Serial.print(" >> ");
    hiddenLayer[i] = sigmoid(hiddenLayer[i]); // Aktivasi sigmoidal
    Serial.println(hiddenLayer[i]);
  }
  
  // Hitung output
  Serial.println("\noutputlayer:___________________");
  for (int i = 0; i < 4; i++) {
    output[i] = 0;
    for (int j = 0; j < 5; j++) {
      output[i] += hiddenLayer[j] * weightsHiddenOutput[j][i];
      Serial.print(output[i]);Serial.print(" ");
    }Serial.print("\t= ");
    output[i] += biasOutput[i];Serial.print(output[i]);Serial.print(" >> ");
    output[i] = sigmoid(output[i]); // Aktivasi sigmoidal
    Serial.println(output[i]);
  }
  Serial.print("out:");
  // Menggunakan output JST untuk mengontrol robot
  // Implementasikan logika kontrol di sini sesuai dengan output
  // Contoh:
  if (output[0] > 0.5) {
    // Maju
    Serial.print(".maju");
  } else if (output[1] > 0.5) {
    // Mundur
    Serial.print(".mundur");
  } else if (output[2] > 0.5) {
    // Belok ke kiri
    Serial.print(".kiri");
  } else if (output[3] > 0.5) {
    // Belok ke kanan
    Serial.print(".kanan");
  }
  Serial.println();
  
  // Delay atau loop rate di sini
  delay(100); // Contoh: delay 100ms
}

void initializeWeightsAndBiases() {
  // Inisialisasi bobot antara input layer dan hidden layer
  Serial.println("bobot hidden layer:");
  for (int i = 0; i < NUM_SENSORS; i++) {
    for (int j = 0; j < 5; j++) {
      // Inisialisasi bobot ke nilai acak kecil antara -0.5 dan 0.5
      weightsInputHidden[i][j] = (float)random(-50, 50) / 100.0;
      Serial.print(weightsInputHidden[i][j]);Serial.print(" ");
    }Serial.println();
  }Serial.println();

  // Inisialisasi bias untuk lapisan tersembunyi
  Serial.println("bias hidden:");
  for (int i = 0; i < 5; i++) {
    // Bias bisa diatur ke nol atau nilai awal yang sesuai
    biasHidden[i] = 0.0; // Contoh: Diatur ke nol
  }

  // Inisialisasi bobot antara hidden layer dan output layer
  Serial.println("bobot output : ");
  for (int i = 0; i < 5; i++) {
    for (int j = 0; j < 4; j++) {
      // Inisialisasi bobot ke nilai acak kecil antara -0.5 dan 0.5
      weightsHiddenOutput[i][j] = (float)random(-50, 50) / 100.0;
      Serial.print(weightsHiddenOutput[i][j]);Serial.print(" ");
    }Serial.println();
  }Serial.println();

  // Inisialisasi bias untuk output layer
  Serial.println("bias output :");
  for (int i = 0; i < 4; i++) {
    // Bias bisa diatur ke nol atau nilai awal yang sesuai
    biasOutput[i] = 0.0; // Contoh: Diatur ke nol
  }
}


float sigmoid(float x) {
  // Serial.println("sigmoid");
  // Fungsi aktivasi sigmoid
  return 1.0 / (1.0 + exp(-x));
}