#include <Wire.h>
#include <Adafruit_GFX.h> 
#include <Adafruit_SSD1306.h>
//#include <MCUFRIEND_kbv.h> 

const int inputPin = 2;

float actualDegree;
float mappedAngle;
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT);
//MCUFRIEND_kbv tft;
float mapAngle(float angle) 
{
      if (angle > 180) {
          angle -= (360+180*-1);
            }
              return angle;
              }
void setup() {
 
 pinMode(inputPin, INPUT);

  Serial.begin(9600);

      /*tft.begin(0x9486); 
      tft.setRotation(1); 
      tft.fillScreen(TFT_BLACK);
      tft.fillRect(0, 0, 480, 90, TFT_CYAN);
      tft.setTextSize(4);
      tft.setTextColor(TFT_BLACK);
      tft.setCursor(70, 30);
      tft.print("ANGLE INDICATOR");
      tft.setTextSize(3);
      tft.setTextColor(TFT_CYAN);
      tft.setCursor(90,280);
      tft.print("DESIGN BY: ");
      tft.setTextColor(TFT_PINK);
      tft.print("MAHFUD");*/
  oled.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  oled.clearDisplay();  
  oled.clearDisplay();
  oled.setTextSize(1);
  oled.setTextColor(WHITE);
  oled.setCursor(5, 10);
  oled.print("ANGLE POS. INDICATOR");
  oled.setTextSize(2);
  oled.setTextColor(WHITE);
  oled.setCursor(5, 27);
  oled.print("BY MAHFUD");
  oled.setTextSize(1);
  oled.setTextColor(WHITE);
  oled.setCursor(5, 50);
  oled.print("    083849413001");
  oled.display();
  delay(5000);
  oled.clearDisplay();

}

void functionA() {
  // Your code for function A goes here

  if (actualDegree > 180){
     mappedAngle = mappedAngle*-1;}
     Serial.println("Function A is active.");
}

// Function B
void functionB() {
  // Your code for function B goes here
  
  if (actualDegree < 180){
      mappedAngle = (mappedAngle-180);}
      Serial.println("Function B is active.");
}

void loop() {
   int state = digitalRead(inputPin);
    if (state == HIGH) {
    functionA();
  } 
    else {
    functionB();
  }
  // Read the 14-bit binary string from pins 22 to 35
  String binaryString = "";
  for (int pin = 22; pin <= 35; pin++) {
    int bitValue = digitalRead(pin);
    binaryString += String(bitValue);
  }

  // Convert binary to decimal
  long decimalNumber = convertBinaryToDecimal(binaryString);
  actualDegree = (decimalNumber*359.99)/16383.00;

  //Mapping degree
   float mappedAngle = mapAngle(actualDegree);
   //Fungsi Meriam Haluan
     //if (actualDegree > 180){
     //    mappedAngle = mappedAngle*-1;}
     //Fungsi Meriam Buritan
    //if (actualDegree < 180){
    //mappedAngle = (mappedAngle-180);}

  // Print results
  Serial.print("Bin: ");
  Serial.print(binaryString);
  Serial.print("  ");
  Serial.print("Dec: ");
  Serial.print(decimalNumber);
  Serial.print("  ");
  Serial.print("Deg: ");
  Serial.println(actualDegree);
  Serial.print("  ");
  Serial.print("Pos: ");
  Serial.println(mappedAngle);

  delay(1000); 

  oled.clearDisplay();
  oled.setTextSize(1);
  oled.setTextColor(WHITE);
  oled.setCursor(5, 5);
  oled.print("BIN:");
  oled.println(binaryString);

  oled.setCursor(5, 20);
  oled.print("DEC:");
  oled.println(decimalNumber);

  //Prints the Actual Degree on the OLED Display
  oled.setTextSize(3);
  oled.setTextColor(WHITE);
  oled.setCursor(5, 35);
  oled.print("");
  oled.println(actualDegree);
  oled.display();
/*
        tft.fillRect(65, 115, 370, 80, TFT_BLACK);
        tft.setTextSize(10);
        tft.setTextColor(TFT_RED);
        tft.setCursor(70, 120);
        tft.print(actualDegree, 2);
        tft.fillRect(65, 210, 365, 50, TFT_BLACK);
        tft.setTextSize(4);
        tft.setTextColor(TFT_WHITE);
        tft.setCursor(75, 220);
        tft.print(binaryString); 
        delay(200);
*/

}

long convertBinaryToDecimal(String binaryString) {
  long decimalVal = 0;
  for (int i = 0; i < binaryString.length(); i++) {
    int bit = binaryString.charAt(i) - '0'; // Convert char to int
    decimalVal = decimalVal * 2 + bit;
  }
  return decimalVal;
}