#define FIS_TYPE float
#define FIS_RESOLUSION 101
#define FIS_MIN -3.4028235E+38
#define FIS_MAX 3.4028235E+38
typedef FIS_TYPE(*_FIS_MF)(FIS_TYPE, FIS_TYPE*);
typedef FIS_TYPE(*_FIS_ARR_OP)(FIS_TYPE, FIS_TYPE);
typedef FIS_TYPE(*_FIS_ARR)(FIS_TYPE*, int, _FIS_ARR_OP);

const int fis_gcI = 2;
const int fis_gcO = 1;
const int fis_gcR = 9;

FIS_TYPE g_fisInput[fis_gcI];
FIS_TYPE g_fisOutput[fis_gcO];

#include <elapsedMillis.h>
#include <Wire.h> //Libary I2C
#include <LiquidCrystal_I2C.h>
#include <Keypad.h> //Libary Keypad
#include "HX711.h"
#include <SPI.h>
#include <SD.h>

elapsedSeconds TimerGenggaman;
elapsedMillis MillisTampil;

/*SD CARD*/
File sdcard_file;
const int CS_pin = 4;
String dataMessage;

/*LOAD CELL*/
#define DOUT  A1  //PIN A1
#define CLK   A0  //PIN A0
HX711 scale;
float calibration_factor = 1000; //Hasil Kalibrasi
float  berat = 19.34;
float  BERATMAX = 19.34;
float units;
float ounces;

/*DEKLARASI KEYPAD*/
const int ROW_NUM    = 4;  //Deklarasi Baris sebanyak 4 baris
const int COLUMN_NUM = 4;  //Deklarasi Kolom sebanyak 4 Kolom

char keys[ROW_NUM][COLUMN_NUM] = { //Deklarasi Array untuk penataan posisi tombol pada keypad
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};
byte pin_rows[ROW_NUM] = {12, 11, 10, 9}; //Deklarasi Tombol Baris pada masing-masing PIN Arduino
byte pin_column[COLUMN_NUM] = {8, 7, 6, 5}; //Deklarasi Tombol Kolom pada masing-masing PIN Arduino

Keypad keypad = Keypad( makeKeymap(keys), pin_rows, pin_column, ROW_NUM, COLUMN_NUM ); //Deklarasi Keypad

//DEKLARASI LCD
LiquidCrystal_I2C lcd(0x27, 16, 2);

//DEKLARASI GPIO
String        breakloop;
char          key;
String        input_angka;

int           USIA              = 25;
int           sisaWaktu         = 0;
float         PERSENTASE_STROKE = 0;
int           stateLCD          = 1;
String        HASIL_STRING      = "-";

void setup() {
  Serial.begin(115200);
  input_angka.reserve(32);    //Deklarasi Kapasitas Input Password sebesar 32 bit
  scale.begin(DOUT, CLK);
  scale.set_scale();
  scale.tare();  //Reset the scale to 0
  long zero_factor = scale.read_average(); //Get a baseline reading
  Serial.print("Zero factor: "); //This can be used to remove the need to tare the scale. Useful in permanent scale projects.
  Serial.println(zero_factor);

  pinMode(CS_pin, OUTPUT);
  SD.begin();

  breakloop = "HOME";

  /*INISIALISASI LCD*/
  //lcd.begin(); //Deklarasi LCD untuk memulai LCD
  lcd.init(); //Deklarasi LCD untuk memulai LCD
  lcd.setCursor(0, 0);
  lcd.print("ALAT PENGECEKAN");
  lcd.setCursor(0, 1);
  lcd.print("KEKUATAN TANGAN");
  delay(2000);
  lcd.clear();
  lcd.setCursor(2, 0);
  lcd.print("BAGI PASIEN");
  lcd.setCursor(1, 1);
  lcd.print("GEJALA STROKE");
  delay(2000);
  lcd.clear();
  lcd.setCursor(1, 0);
  lcd.print("DENGAN METODE");
  lcd.setCursor(2, 1);
  lcd.print("FUZZY LOGIC");
  delay(2000);
  lcd.clear();
}

void loop() {
  HOME();
}

void HOME() {
  while (breakloop == "HOME") {
    readGram();

    /*MENAMPILKAN LCD*/
    lcd.setCursor(0, 0);
    lcd.print("TEKAN '*' UNTUK ");
    lcd.setCursor(0, 1);
    lcd.print("MEMASUKAN USIA..");

    Serial.print(breakloop); Serial.print("\t");
    Serial.print(berat); Serial.print("\t");
    Serial.println();

    key = keypad.getKey();
    if (key) { //Jika tombol ditekan
      Serial.println(key); //Menampilkan tombol yang ditekan
      if (key == '*') { //Jika tombol '*' ditekan
        lcd.clear(); //LCD dibersihkan
        lcd.setCursor(0, 0);
        lcd.print("SILAHKAN MASUKAN");
        lcd.setCursor(0, 1);
        lcd.print("USIA PASIEN.....");
        delay(2000);
        lcd.clear(); //LCD dibersihkan
        breakloop = "INPUT_USIA";
        break;
      }
      else {
        input_angka += key; // Melakukan Input karakter baru pada keypad ketika ditekan
      }
    }
  }
  if (breakloop == "INPUT_USIA") {
    input_angka = ""; //Mereset Jawaban
    lcd.clear(); //LCD dibersihkan
    INPUT_USIA();
  }
}

void INPUT_USIA() {
  while (breakloop == "INPUT_USIA") {

    /*MENAMPILKAN LCD*/
    lcd.setCursor(0, 0);
    lcd.print("MASUKAN USIA :  ");
    lcd.setCursor(0, 1);
    lcd.print(input_angka); lcd.print("  ");
    lcd.setCursor(11, 1);
    lcd.print("TAHUN");

    Serial.print(breakloop); Serial.print("\t");
    Serial.print(berat); Serial.print("\t");
    Serial.println();

    key = keypad.getKey();
    if (key) { //Jika tombol ditekan
      Serial.println(key); //Menampilkan tombol yang ditekan
      if (key == '*') { //Jika tombol '*' ditekan
        lcd.clear(); //LCD dibersihkan
        lcd.setCursor(0, 0);
        lcd.print("USIA PASIEN     ");
        lcd.setCursor(0, 1);
        lcd.print("TELAH TERSIMPAN.");
        USIA = input_angka.toInt();
        delay(2000);
        lcd.clear(); //LCD dibersihkan
        breakloop = "INPUT_BEBAN";
        break;
      }
      else if (key == '#')  {
        input_angka = "";
        lcd.clear();
      }
      else {
        input_angka += key; // Melakukan Input karakter baru pada keypad ketika ditekan
      }
    }
  }
  if (breakloop == "INPUT_BEBAN") {
    input_angka = ""; //Mereset Jawaban
    lcd.clear(); //LCD dibersihkan
    INPUT_BEBAN();
  }
}

void INPUT_BEBAN() {
  while (breakloop == "INPUT_BEBAN") {
    readGram();

    /*MENAMPILKAN LCD*/
    lcd.setCursor(0, 0);
    lcd.print("MULAI GENGGAMAN?");
    lcd.setCursor(0, 1);
    lcd.print("TEKAN '*'       ");

    Serial.print(breakloop); Serial.print("\t");
    Serial.print(berat); Serial.print("\t");
    Serial.println();

    key = keypad.getKey();
    if (key) { //Jika tombol ditekan
      Serial.println(key); //Menampilkan tombol yang ditekan
      if (key == '*') { //Jika tombol '*' ditekan
        lcd.clear(); //LCD dibersihkan
        lcd.setCursor(0, 0);
        lcd.print("REKAM GENGGAMAN  ");
        lcd.setCursor(0, 1);
        lcd.print("DIMULAI...       ");
        delay(2000);
        lcd.clear(); //LCD dibersihkan
        TimerGenggaman = 0;
        breakloop = "PEREKAMAN_GENGGAMAN";
        break;
      }
      else {
        input_angka += key; // Melakukan Input karakter baru pada keypad ketika ditekan
      }
    }
  }
  if (breakloop == "PEREKAMAN_GENGGAMAN") {
    input_angka = ""; //Mereset Jawaban
    lcd.clear(); //LCD dibersihkan
    PEREKAMAN_GENGGAMAN();
  }
}

void PEREKAMAN_GENGGAMAN() {
  while (breakloop == "PEREKAMAN_GENGGAMAN") {
    readGram();

    TimerGenggaman;
    if (TimerGenggaman <= 10)  {
      /*PENGAMBILAN NILAI BERAT MAKSIMUM*/
      if (berat > BERATMAX) {
        BERATMAX = berat;
      }

      /*MENAMPILKAN LCD*/
      sisaWaktu = 10 - TimerGenggaman;
      lcd.setCursor(0, 0);
      lcd.print("LAKUKAN GENGGAM ");
      lcd.setCursor(0, 1);
      lcd.print("DALAM " + String (sisaWaktu) + " DETIK");
      lcd.print("  ");
    }
    else {
      /*MENAMPILKAN LCD*/
      lcd.clear(); //LCD dibersihkan
      lcd.setCursor(0, 0);
      lcd.print("GENGGAMAN TELAH  ");
      lcd.setCursor(0, 1);
      lcd.print("TERSIMPAN...     ");

      /*PROSES FUZIFIKASI*/
      g_fisInput[0] = USIA; // Read Input: Usia
      g_fisInput[1] = BERATMAX; // Read Input: Kekuatan_Genggaman

      g_fisOutput[0] = 0;
      fis_evaluate();
      PERSENTASE_STROKE = g_fisOutput[0];
      
      /*PROSES KLASIFIKASI*/
      if(PERSENTASE_STROKE <= 30){
        HASIL_STRING  = "RINGAN";
      }
      else if(PERSENTASE_STROKE > 30 && PERSENTASE_STROKE <= 80){
        HASIL_STRING  = "SEDANG";
      }
      else if(PERSENTASE_STROKE > 80 && PERSENTASE_STROKE <= 100){
        HASIL_STRING  = "BERAT";
      }

      /*PROSES PENYIMPANAN DI SD CARD*/
      logSDCard();

      delay(2000);
      lcd.clear(); //LCD dibersihkan
      breakloop = "HASIL";
      break;
    }

    Serial.print(breakloop); Serial.print("\t");
    Serial.print(berat); Serial.print("\t");
    Serial.println();
  }
  if (breakloop == "HASIL") {
    input_angka = ""; //Mereset Jawaban
    lcd.clear(); //LCD dibersihkan
    HASIL();
  }
}

void HASIL() {
  while (breakloop == "HASIL") {
    /*MENAMPILKAN LCD*/
    if (MillisTampil > 2000) {
      stateLCD++;
      if (stateLCD > 2) {
        stateLCD = 0;
      }
      lcd.clear();
      MillisTampil = 0;
    }

    if (stateLCD == 0) {
      lcd.setCursor(0, 0);
      lcd.print("USIA  : " + (String) USIA);
      lcd.setCursor(11, 0);
      lcd.print("TAHUN");

      lcd.setCursor(0, 1);
      lcd.print("BERAT : " + (String) BERATMAX);
      lcd.setCursor(14, 1);
      lcd.print("KG");
    }
    else if (stateLCD == 1) {
      lcd.setCursor(0, 0);
      lcd.print("RESIKO  : ");
      lcd.print(PERSENTASE_STROKE, 1);
      lcd.print(" %");
      lcd.setCursor(0, 1);
      lcd.print("KONDISI : " + HASIL_STRING);
    }
    else if (stateLCD == 2) {
      lcd.setCursor(0, 0);
      lcd.print("TEKAN '*' UNTUK ");
      lcd.setCursor(0, 1);
      lcd.print("KE MENU UTAMA.  ");
    }

    Serial.print(breakloop); Serial.print("\t");
    Serial.print(berat); Serial.print("\t");
    Serial.println();

    key = keypad.getKey();
    if (key) { //Jika tombol ditekan
      Serial.println(key); //Menampilkan tombol yang ditekan
      if (key == '*') { //Jika tombol '*' ditekan
        lcd.clear(); //LCD dibersihkan
        lcd.setCursor(0, 0);
        lcd.print("KEMBALI KE MENU ");
        lcd.setCursor(0, 1);
        lcd.print("UTAMA...        ");

        TimerGenggaman = 0;
        PERSENTASE_STROKE = 0;
        berat = 0;
        BERATMAX = 0;
        USIA = 0;
        sisaWaktu = 0;

        delay(2000);
        lcd.clear(); //LCD dibersihkan
        breakloop = "HOME";
        break;
      }
      else {
        input_angka += key; // Melakukan Input karakter baru pada keypad ketika ditekan
      }
    }
  }
  if (breakloop == "HOME") {
    input_angka = ""; //Mereset Jawaban
    lcd.clear(); //LCD dibersihkan
    HOME();
  }
}

void readGram() {
  // scale.set_scale(calibration_factor); //Adjust to this calibration factor
  // units = scale.get_units(), 1;
  // if (units < 0)  {
  //   units = 0;
  // }
  // berat = units / 10;
}

void logSDCard() {
  Serial.println("Menyimpan Data");
  dataMessage =  "Data Tenaga: " + String(BERATMAX) + " Kg";

  sdcard_file = SD.open("/data.csv", FILE_WRITE);
  if (sdcard_file) {
    Serial.println("Penyimpanan berhasil");
    sdcard_file.print (dataMessage);
    sdcard_file.close();
  }
  else {
    Serial.println("error opening data.csv");
  }
}


//***********************************************************************
// Support functions for Fuzzy Inference System
//***********************************************************************
// Trapezoidal Member Function
FIS_TYPE fis_trapmf(FIS_TYPE x, FIS_TYPE* p)
{
  FIS_TYPE a = p[0], b = p[1], c = p[2], d = p[3];
  FIS_TYPE t1 = ((x <= c) ? 1 : ((d < x) ? 0 : ((c != d) ? ((d - x) / (d - c)) : 0)));
  FIS_TYPE t2 = ((b <= x) ? 1 : ((x < a) ? 0 : ((a != b) ? ((x - a) / (b - a)) : 0)));
  return (FIS_TYPE) min(t1, t2);
}

FIS_TYPE fis_min(FIS_TYPE a, FIS_TYPE b)
{
  return min(a, b);
}

FIS_TYPE fis_max(FIS_TYPE a, FIS_TYPE b)
{
  return max(a, b);
}

FIS_TYPE fis_array_operation(FIS_TYPE *array, int size, _FIS_ARR_OP pfnOp)
{
  int i;
  FIS_TYPE ret = 0;

  if (size == 0) return ret;
  if (size == 1) return array[0];

  ret = array[0];
  for (i = 1; i < size; i++)
  {
    ret = (*pfnOp)(ret, array[i]);
  }

  return ret;
}


//***********************************************************************
// Data for Fuzzy Inference System
//***********************************************************************
// Pointers to the implementations of member functions
_FIS_MF fis_gMF[] =
{
  fis_trapmf
};

// Count of member function for each Input
int fis_gIMFCount[] = { 3, 3 };

// Count of member function for each Output
int fis_gOMFCount[] = { 3 };

// Coefficients for the Input Member Functions
FIS_TYPE fis_gMFI0Coeff1[] = { 0, 0, 12, 25 };
FIS_TYPE fis_gMFI0Coeff2[] = { 23, 32, 38, 45 };
FIS_TYPE fis_gMFI0Coeff3[] = { 42, 55, 65, 70 };
FIS_TYPE* fis_gMFI0Coeff[] = { fis_gMFI0Coeff1, fis_gMFI0Coeff2, fis_gMFI0Coeff3 };
FIS_TYPE fis_gMFI1Coeff1[] = { 0, 0, 10, 16 };
FIS_TYPE fis_gMFI1Coeff2[] = { 14, 20, 28, 32 };
FIS_TYPE fis_gMFI1Coeff3[] = { 30, 42, 50, 55 };
FIS_TYPE* fis_gMFI1Coeff[] = { fis_gMFI1Coeff1, fis_gMFI1Coeff2, fis_gMFI1Coeff3 };
FIS_TYPE** fis_gMFICoeff[] = { fis_gMFI0Coeff, fis_gMFI1Coeff };

// Coefficients for the Output Member Functions
FIS_TYPE fis_gMFO0Coeff1[] = { 0, 0, 20, 40 };
FIS_TYPE fis_gMFO0Coeff2[] = { 30, 45, 65, 80 };
FIS_TYPE fis_gMFO0Coeff3[] = { 70, 85, 100, 105 };
FIS_TYPE* fis_gMFO0Coeff[] = { fis_gMFO0Coeff1, fis_gMFO0Coeff2, fis_gMFO0Coeff3 };
FIS_TYPE** fis_gMFOCoeff[] = { fis_gMFO0Coeff };

// Input membership function set
int fis_gMFI0[] = { 0, 0, 0 };
int fis_gMFI1[] = { 0, 0, 0 };
int* fis_gMFI[] = { fis_gMFI0, fis_gMFI1};

// Output membership function set
int fis_gMFO0[] = { 0, 0, 0 };
int* fis_gMFO[] = { fis_gMFO0};

// Rule Weights
FIS_TYPE fis_gRWeight[] = { 1, 1, 1, 1, 1, 1, 1, 1, 1 };

// Rule Type
int fis_gRType[] = { 1, 1, 1, 1, 1, 1, 1, 1, 1 };

// Rule Inputs
int fis_gRI0[] = { 1, 1 };
int fis_gRI1[] = { 1, 2 };
int fis_gRI2[] = { 1, 3 };
int fis_gRI3[] = { 2, 1 };
int fis_gRI4[] = { 2, 2 };
int fis_gRI5[] = { 2, 3 };
int fis_gRI6[] = { 3, 1 };
int fis_gRI7[] = { 3, 2 };
int fis_gRI8[] = { 3, 3 };
int* fis_gRI[] = { fis_gRI0, fis_gRI1, fis_gRI2, fis_gRI3, fis_gRI4, fis_gRI5, fis_gRI6, fis_gRI7, fis_gRI8 };

// Rule Outputs
int fis_gRO0[] = { 2 };
int fis_gRO1[] = { 1 };
int fis_gRO2[] = { 1 };
int fis_gRO3[] = { 2 };
int fis_gRO4[] = { 1 };
int fis_gRO5[] = { 1 };
int fis_gRO6[] = { 3 };
int fis_gRO7[] = { 2 };
int fis_gRO8[] = { 1 };
int* fis_gRO[] = { fis_gRO0, fis_gRO1, fis_gRO2, fis_gRO3, fis_gRO4, fis_gRO5, fis_gRO6, fis_gRO7, fis_gRO8 };

// Input range Min
FIS_TYPE fis_gIMin[] = { 0, 0 };

// Input range Max
FIS_TYPE fis_gIMax[] = { 65, 50 };

// Output range Min
FIS_TYPE fis_gOMin[] = { 0 };

// Output range Max
FIS_TYPE fis_gOMax[] = { 100 };

//***********************************************************************
// Data dependent support functions for Fuzzy Inference System
//***********************************************************************
FIS_TYPE fis_MF_out(FIS_TYPE** fuzzyRuleSet, FIS_TYPE x, int o)
{
  FIS_TYPE mfOut;
  int r;

  for (r = 0; r < fis_gcR; ++r)
  {
    int index = fis_gRO[r][o];
    if (index > 0)
    {
      index = index - 1;
      mfOut = (fis_gMF[fis_gMFO[o][index]])(x, fis_gMFOCoeff[o][index]);
    }
    else if (index < 0)
    {
      index = -index - 1;
      mfOut = 1 - (fis_gMF[fis_gMFO[o][index]])(x, fis_gMFOCoeff[o][index]);
    }
    else
    {
      mfOut = 0;
    }

    fuzzyRuleSet[0][r] = fis_min(mfOut, fuzzyRuleSet[1][r]);
  }
  return fis_array_operation(fuzzyRuleSet[0], fis_gcR, fis_max);
}

FIS_TYPE fis_defuzz_centroid(FIS_TYPE** fuzzyRuleSet, int o)
{
  FIS_TYPE step = (fis_gOMax[o] - fis_gOMin[o]) / (FIS_RESOLUSION - 1);
  FIS_TYPE area = 0;
  FIS_TYPE momentum = 0;
  FIS_TYPE dist, slice;
  int i;

  // calculate the area under the curve formed by the MF outputs
  for (i = 0; i < FIS_RESOLUSION; ++i) {
    dist = fis_gOMin[o] + (step * i);
    slice = step * fis_MF_out(fuzzyRuleSet, dist, o);
    area += slice;
    momentum += slice * dist;
  }

  return ((area == 0) ? ((fis_gOMax[o] + fis_gOMin[o]) / 2) : (momentum / area));
}

//***********************************************************************
// Fuzzy Inference System
//***********************************************************************
void fis_evaluate()
{
  FIS_TYPE fuzzyInput0[] = { 0, 0, 0 };
  FIS_TYPE fuzzyInput1[] = { 0, 0, 0 };
  FIS_TYPE* fuzzyInput[fis_gcI] = { fuzzyInput0, fuzzyInput1, };
  FIS_TYPE fuzzyOutput0[] = { 0, 0, 0 };
  FIS_TYPE* fuzzyOutput[fis_gcO] = { fuzzyOutput0, };
  FIS_TYPE fuzzyRules[fis_gcR] = { 0 };
  FIS_TYPE fuzzyFires[fis_gcR] = { 0 };
  FIS_TYPE* fuzzyRuleSet[] = { fuzzyRules, fuzzyFires };
  FIS_TYPE sW = 0;

  // Transforming input to fuzzy Input
  int i, j, r, o;
  for (i = 0; i < fis_gcI; ++i)
  {
    for (j = 0; j < fis_gIMFCount[i]; ++j)
    {
      fuzzyInput[i][j] =
        (fis_gMF[fis_gMFI[i][j]])(g_fisInput[i], fis_gMFICoeff[i][j]);
    }
  }

  int index = 0;
  for (r = 0; r < fis_gcR; ++r)
  {
    if (fis_gRType[r] == 1)
    {
      fuzzyFires[r] = FIS_MAX;
      for (i = 0; i < fis_gcI; ++i)
      {
        index = fis_gRI[r][i];
        if (index > 0)
          fuzzyFires[r] = fis_min(fuzzyFires[r], fuzzyInput[i][index - 1]);
        else if (index < 0)
          fuzzyFires[r] = fis_min(fuzzyFires[r], 1 - fuzzyInput[i][-index - 1]);
        else
          fuzzyFires[r] = fis_min(fuzzyFires[r], 1);
      }
    }
    else
    {
      fuzzyFires[r] = FIS_MIN;
      for (i = 0; i < fis_gcI; ++i)
      {
        index = fis_gRI[r][i];
        if (index > 0)
          fuzzyFires[r] = fis_max(fuzzyFires[r], fuzzyInput[i][index - 1]);
        else if (index < 0)
          fuzzyFires[r] = fis_max(fuzzyFires[r], 1 - fuzzyInput[i][-index - 1]);
        else
          fuzzyFires[r] = fis_max(fuzzyFires[r], 0);
      }
    }

    fuzzyFires[r] = fis_gRWeight[r] * fuzzyFires[r];
    sW += fuzzyFires[r];
  }

  if (sW == 0)
  {
    for (o = 0; o < fis_gcO; ++o)
    {
      g_fisOutput[o] = ((fis_gOMax[o] + fis_gOMin[o]) / 2);
    }
  }
  else
  {
    for (o = 0; o < fis_gcO; ++o)
    {
      g_fisOutput[o] = fis_defuzz_centroid(fuzzyRuleSet, o);
    }
  }
}