#include <Arduino.h>
#include<Wire.h>
#include <LiquidCrystal_I2C.h>
#include<math.h>

int const PBNL= 2;
int const PBHS=3;
int const PBHSM=4;
int const PBBLC=5;
int const PBOUTER=6;
int const PBINNER=7;
int const PBSNUBBER=8;
int const PBNEW=9;
int const LEDG=10;
int const LEDY=11;
int const LEDO=12;
int const LEDW=13;
int const LEDR=A1;
int const LVDT=A0;

LiquidCrystal_I2C lcd(0x27, 16, 2);

float const Ho [4][3][6]={
		{{260,257,254,251,248,245},{262,259,256,253,250,247},{294,291,288,285,282,279}},
		{{260,257,254,251,248,245},{243,240,237,234,231,228},{293,290,287,284,281,278}},
		{{253,250,247,244,241,238},{225,222,219,216,213,210},{304,301,298,295,292,289}},
    {{260,257,254,251,248,245},{243,240,237,234,231,228},{288,285,282,279,276,273}}
	  };

float const Hn [4][3][4]={
    {{263,261,259,257},{265,263,261,259},{297,295,293,291}},
    {{263,261,259,257},{243,241,239,237},{293,291,289,287}},
    {{256,254,252,250},{228,226,224,222},{307,305,309,301}},
    {{263,261,259,257},{243,241,239,237},{288,286,284,282}}
  };
byte LEDArray[5] = {LEDG,LEDY,LEDO,LEDW,LEDR};
byte LEDArrayN[3] = {LEDG,LEDY,LEDR};
byte PBARRAY1[4]={PBNL,PBHS,PBHSM,PBBLC};
byte PBARRAY2[3]={PBOUTER,PBINNER,PBSNUBBER};
   
float Height=0;
int LEDStatus=0;



void setup() {
pinMode(PBNL,INPUT_PULLUP);
pinMode(PBHS,INPUT_PULLUP);
pinMode(PBHSM,INPUT_PULLUP);
pinMode(PBBLC, INPUT_PULLUP);
pinMode(PBNEW,INPUT_PULLUP); 
pinMode(PBOUTER,INPUT_PULLUP); 
pinMode(PBINNER,INPUT_PULLUP);
pinMode(PBSNUBBER,INPUT_PULLUP);
pinMode(LVDT,INPUT);

pinMode(LEDG,OUTPUT);
pinMode(LEDY,OUTPUT);
pinMode(LEDO,OUTPUT);
pinMode(LEDW,OUTPUT);
pinMode(LEDR,OUTPUT);

lcd.begin(16,2);
lcd.backlight();

Serial.begin(9600);
}

int CompareO(float x,int i,int j){
  int LED=0;
  for(int k=0;k<6;k++){
    LED=LED+1;
    if(x<=Ho[i-1][j-1][k] && x>Ho[i-1][j-1][k+1])
    {
      break;
    } 
  }
  if(x<Ho[i-1][j-1][5] || x>Ho[i-1][j-1][0])
    {
      LED=7;
    }  
   return LED;
}

int CompareN(float x,int i,int j){
  int LED=0;
  for(int k=0;k<4;k++){
    LED=LED+1;
    if(x<=Hn[i-1][j-1][k] && x>Hn[i-1][j-1][k+1])
    {
      break;
    } 
  }
  if(x<Hn[i-1][j-1][3] || x>Hn[i-1][j-1][0])
    {
      LED=7;
    }  
   return LED;
}

void loop() {
  int i=0,j=0;
  Height= map(analogRead(LVDT),0,1023,10000,0);
  Height= Height/100+204;
  Serial.print("PBwagon=");
  Serial.println(digitalRead(PBNL)+digitalRead(PBHS)+digitalRead(PBHSM)+digitalRead(PBBLC));
  delay(400);
  Serial.print("PBtype=");
  Serial.println(digitalRead(PBOUTER)+digitalRead(PBINNER)+digitalRead(PBSNUBBER));
  delay(400);
  if (((digitalRead(PBNL)+digitalRead(PBHS)+digitalRead(PBHSM)+digitalRead(PBBLC))==3)&&((digitalRead(PBOUTER)+digitalRead(PBINNER)+digitalRead(PBSNUBBER))==2))
  {
    for(int count=0;count<4;count++)
    {
      i=i+1;
      if(digitalRead(PBARRAY1[count])==LOW )
      {
        break;
      }      
    }
    
    for(int count=0;count<3;count++)
    {
      j=j+1;
      if(digitalRead(PBARRAY2[count])==LOW )
      {
        break;
      }       
    }
    
    if(digitalRead(PBNEW)==LOW)
    {
       LEDStatus= CompareN(Height,i,j);

      for (int count=0;count<5;count++)
       {
         digitalWrite(LEDArray[count],LOW);
       }

     if(LEDStatus<7)
       {
        digitalWrite(LEDArrayN[LEDStatus-1],HIGH);
        }
      else
        {
         for(int count=0;count<6;count++){
         digitalWrite(LEDArray[count],HIGH);
        }

        for(int count=0;count<6;count++){
        digitalWrite(LEDArray[count],LOW);
        }
        lcd.setCursor(0,1);
        lcd.print("Condemn");
        }
    }
    else
    {
      LEDStatus= CompareO(Height,i,j);

      for (int count=0;count<5;count++)
       {
         digitalWrite(LEDArray[count],LOW);
       }

     if(LEDStatus<7)
       {
        digitalWrite(LEDArray[LEDStatus-1],HIGH);
        }
      else
        {
         for(int count=0;count<6;count++){
         digitalWrite(LEDArray[count],HIGH);
        }

        for(int count=0;count<6;count++){
        digitalWrite(LEDArray[count],LOW);
        }
        lcd.setCursor(0,1);
        lcd.print("Condemn");
        }
    }
    //Serial.println(Height);
    lcd.setCursor(4,0);
    lcd.print(Height);
  }
  else{
    //Serial.println("Irregular Command"); 
    lcd.print("Irregular Command"); 

  }  
  delay(500);
  lcd.clear();
}