// Rotary encoder and LCD demo

#define runEvery(t) for (static typeof(t) _lasttime;\
                         (typeof(t))((typeof(t))millis() - _lasttime) > (t);\
                         _lasttime += (t))

#define CLK 7
#define DT 8
#define SW 9

#define startInterruptPin 2  // Use pin 8 for start interrupt
#define stopInterruptPin 3  // Use pin 9 for stop interrupt

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


//Crear el objeto lcd  dirección  0x27 y 16 columnas x 2 filas
LiquidCrystal_I2C lcd(0x27,16,2);  //

uint8_t arrow[8] = {0x00, 0x04 ,0x06, 0x1f, 0x06, 0x04, 0x00};

uint8_t downarrow[8] = {
	0b00000,
	0b00000,
	0b00000,
	0b11111,
	0b01110,
	0b00100,
	0b00000,
	0b00000
};

byte battery_100[] = {
  B00000,
  B01110,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111
};
byte battery_80[] = {
  B00000,
  B01110,
  B10001,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111
};
byte battery_60[] = {
  B00000,
  B01110,
  B10001,
  B10001,
  B11111,
  B11111,
  B11111,
  B11111
};
byte battery_40[] = {
  B00000,
  B01110,
  B10001,
  B10001,
  B10001,
  B11111,
  B11111,
  B11111
};
byte battery_20[] = {
  B00000,
  B01110,
  B10001,
  B10001,
  B10001,
  B10001,
  B11111,
  B11111
};

byte battery_charge[] = {
  B00000,
  B01010,
  B01010,
  B11111,
  B11111,
  B01110,
  B00100,
  B00100
};

byte battery_count = 0;
int counter = 0; int currentCLK; int previousCLK; int currentSW; int previousSW = HIGH;
String encdir = "";


bool blink = 0;
bool mass_editing = 0;

// Default mass value (empirical system standards)
short int mass10 = 1;
short int mass1 = 6;
short int mass_1 = 1;
float mass = mass10 * 10.0 + mass1 + mass_1/10.0; // Grains.

float gram2grain = 15.4324;
float mps2fps = 3.281;

bool toggle_grams = 0;
bool toggle_metric = 0;

// States
  // Main menu: State 0, Start: Substate 0, Config: Substate 1.
  // Config: State 1, Back: Substete 0, Weight: Substate 1, Units: Substate 2.
  // Weight: State 2
  // Units: State 3
int state = 0;
int substate = 0;

// Variables to store time
volatile unsigned long int1time = 0;
volatile bool int1flag = false;
volatile unsigned long int2time = 0;
volatile bool int2flag = false;
volatile unsigned long elapsedTime = 0;
volatile bool speederrorflag = false;
volatile bool timerRunning = false;

// Variables to measure speed

float distance = 10; // Meters
float speed = 0;

bool ledstate = HIGH;
int countertest = 0;

void setup() {


  
  pinMode(startInterruptPin, INPUT);
  pinMode(stopInterruptPin, INPUT);
  pinMode(13, OUTPUT);


  digitalWrite(13, ledstate);
  // Configure external interrupts to start/stop Timer1
  attachInterrupt(digitalPinToInterrupt(startInterruptPin), startTimer1, RISING);
  attachInterrupt(digitalPinToInterrupt(stopInterruptPin), stopTimer1, RISING);



  pinMode(CLK,INPUT); pinMode(DT,INPUT); pinMode(SW,INPUT_PULLUP);
  previousCLK = digitalRead(CLK);
  //Serial.begin(9600);
  // Inicializar el LCD
  lcd.init();
  lcd.createChar(0, arrow);
  lcd.createChar(1, downarrow);
  lcd.createChar(2, battery_100);
  lcd.createChar(3, battery_80);
  lcd.createChar(4, battery_60);
  lcd.createChar(5, battery_40);
  lcd.createChar(6, battery_20);
  lcd.createChar(7, battery_charge);
  //Encender la luz de fondo.
  lcd.backlight();
  lcd.setCursor(0, 0);
  // Escribimos el Mensaje en el LCD.
  lcd.print("Totograph");
  //char strmass[7];
  //sprintf(strmass, "%.1f", 1.0);
  //lcd.setCursor(8, 1);
  //lcd.print(strmass);
  delay(1000);
  draw_mainmenu();
}

void loop() {
   // Rotary
  currentCLK = digitalRead(CLK);
  if(currentCLK != previousCLK && currentCLK == 1)
  {
    if(digitalRead(DT) != currentCLK)
    {
      switch(state)
      {
        case 0:
          move_arrow(1);
          substate = 1;
          break;

        case 1:
          substate++;
          if(substate > 2)
            substate = 2;
          move_arrow(substate);
          break;
        
        case 2:
          if(!mass_editing)
          {
            substate++;
            if(substate > 3)
              substate = 3;
            move_arrow(substate);
          }
          else
          {
            if(substate == 1)
              mass += 10;
            if(substate == 2)
              mass += 1;
            if(substate == 3)
              mass += 0.1;

            lcd.setCursor(8, 1);
            if(mass < 10)
              lcd.print(0);
            lcd.print(mass,1);
          }
          break;

        case 3:
          substate++;
          if(substate > 2)
            substate = 2;
          move_arrow(substate);
      }
    }
      
    else
    {
      switch(state)
      {
        case 0:
          move_arrow(0);
          substate = 0;
          break;

        case 1:
          substate--;
          if(substate < 0)
            substate = 0;
          move_arrow(substate);
          break;

        case 2:
          if(!mass_editing)
          {
            substate--;
            if(substate < 0)
              substate = 0;
            move_arrow(substate);
          }
          else
          {
            if(substate == 1)
            {
              mass -= 10;
              if(mass < 0)
                mass += 10;
            }
            if(substate == 2)
            {
              mass -= 1;
              if(mass < 0)
                mass += 1;
            }
            if(substate == 3)
            {
              mass -= 0.1;
              if(mass < 0)
                mass += 0.1;
            }

            lcd.setCursor(8, 1);
            if(mass < 10)
              lcd.print(0);
            lcd.print(mass,1);
          }
          break;

        case 3:
          substate--;
          if(substate < 0)
            substate = 0;
          move_arrow(substate);  
      }
    }
      
    //disp_LCD();
  }
  previousCLK = currentCLK;
  

  // Button press


  currentSW = digitalRead(SW);
  if(currentSW == LOW)
  {
    if(currentSW != previousSW)
    {
      switch(state)
      {
        case 0:
          if(substate == 1)
          {
            state = 1; // Go to config screen
            substate = 0; // Set substate
          }
          else
            state = 5; // Start measuring
          break;
        
        case 1:
          if(substate == 0)
          {
            state = 0; // Go to main menu screen
            substate = 0; // Set substate
          }

          if(substate == 1)
          {
            state = 2; // Go to weight setting screen
            substate = 0; // Set substate
          }

          if(substate == 2)
          {
            state = 3; // Go to weight setting screen
            substate = 0; // Set substate
          }

          break;

        case 2:
          if(substate == 0)
          {
            state = 1; // Go to main menu screen
            substate = 0; // Set substate
          }

          else
            mass_editing = !mass_editing;
            //blink = !blink;

          break;
        
        case 3:
          if(substate == 0)
          {
            state = 1; // Go to main menu screen
            substate = 0; // Set substate
          }

          if(substate == 1)
          {
            toggle_metric = !toggle_metric;
          }

          if(substate == 2)
          {
            toggle_grams = !toggle_grams;
          }
          break; 

    

        case 5:
          state = 0;


        default:
          break;

      }
      disp_LCD();
    }
    
  }
  previousSW = currentSW;
  
  if(state == 5 && int2flag)
  {
    draw_measure();
    int2flag = false;
  }
  
  runEvery(200){
    uint16_t bat = map(analogRead(0),0,1023,0,100);
    lcd.setCursor(14,1);
    if(digitalRead(8))
    {
      lcd.write(7);
    }
    else
    {
      if(bat > 80)
      lcd.write(2);
      if((bat >= 80) | (bat > 60))
        lcd.write(3);
      else if((bat >= 60) | (bat > 40))
        lcd.write(4);
      else if((bat >= 40) | (bat > 20))
        lcd.write(5);
      else
        lcd.write(6);
    }

  }
}

void disp_LCD()
{
  lcd.clear();
  switch(state)
  {
    case 0:
      draw_mainmenu();
      break;
    case 1:
      draw_config();
      break;
    case 2:
      draw_mass();
      break;
    
    case 3:
      draw_units();
      break;
    
    case 5:
      draw_measure();
      break;


  }
  //delay(100);
}


void draw_mainmenu()
{
  lcd.clear();
  lcd.write(0);
  lcd.print("STATS");

  lcd.setCursor(9, 0);
  // lcd.write(0);
  lcd.print("START");
  lcd.setCursor(1, 1);
  lcd.print("CONFIG");
  lcd.setCursor(9, 1);
  lcd.print("BATT:");
}

void draw_config()
{
  lcd.clear();
  lcd.print("Config");
  lcd.setCursor(8, 0);
  lcd.write(0);
  lcd.print("BACK");
  lcd.setCursor(1, 1);
  
  lcd.print("MASS");
  lcd.setCursor(9, 1);
  lcd.print("UNITS");
}

void draw_mass()
{
  lcd.clear();
  move_arrow(substate);
  lcd.setCursor(1,0);
  lcd.print("BACK");
  //lcd.setCursor(8, 0);
  //lcd.write(0);
  //lcd.print("BACK");
  lcd.setCursor(0, 1);
  if(toggle_grams)
  {
    lcd.print("Grams:  ");
  }
  else
  {
    lcd.print("Grains: ");
  }
  lcd.setCursor(8, 1);
  if(mass < 10)
    lcd.print(0);
  lcd.print(mass,1);
}
    

int change_mass(int value, int unit, bool op)
{
  if(op)
    value += 1;
  else
    value -= 1;
  
  value = (value + 10) % 10;

  switch(unit)
  {
    case -1:
      lcd.setCursor(11, 1);
      break;
    
    case 0:
      lcd.setCursor(9, 1);
      break;
    
    case 1:
      lcd.setCursor(8, 1);
      break;
  }
  
  lcd.print(value);
  return value;
}

void draw_units()
{
  lcd.clear();
  move_arrow(substate);
  lcd.setCursor(1,0);
  lcd.print("BACK");

  lcd.setCursor(1, 1);
  if(toggle_metric)
    lcd.print("Metric");
  else
    lcd.print("Imperial");
  
  lcd.setCursor(10, 1);

  if(toggle_grams)
    lcd.print("Grams");
  else
    lcd.print("Grains");
}

void draw_measure()
{
  lcd.clear();

  elapsedTime = int2time - int1time;
  speed = distance / elapsedTime * 1e6;
  float vel = 323; // mps
  float energy;
  if(toggle_metric)
    lcd.print("Speed: " + String(speed,0) + " m/s");
  else
    lcd.print("Speed: " + String(speed*mps2fps,0) + " FPS");
  
  //lcd.print("Speed: " + String(vel*mps2fps,0) + " FPS");
  lcd.setCursor(0, 1);
  
  if(toggle_grams)
    energy = 0.5 * speed * speed * mass / 1e3;
  else
    energy = 0.5 * speed * speed * mass / gram2grain / 1e3;
  
  lcd.print("Energy: ");
  lcd.print(energy,1);
  lcd.print(" J");
}

void move_arrow(int pos)
{
  switch(state)
  {
    case 0:
      lcd.setCursor(0, 1);
      if(pos)
        lcd.print(" ");
      else
        lcd.write(0);

      lcd.setCursor(8, 1);
      if(pos)
        lcd.write(0);
      else
        lcd.print(" ");
      break;
      
    case 1:
      lcd.setCursor(8, 0);
      if(pos == 0)
        lcd.write(0);
      else
        lcd.print(" ");

      lcd.setCursor(0, 1);
      if(pos == 1)
        lcd.write(0);
      else
        lcd.print(" ");
      
      lcd.setCursor(8, 1);
      if(pos == 2)
        lcd.write(0);
      else
        lcd.print(" ");
      break;

    case 2:
      lcd.setCursor(0, 0);
      if(pos == 0)
        lcd.write(0);
      else
        lcd.print(" ");

      lcd.setCursor(8, 0);
      if(pos == 1)
        lcd.write(1);
      else
        lcd.print(" ");
      
      lcd.setCursor(9, 0);
      if(pos == 2)
        lcd.write(1);
      else
        lcd.print(" ");

      lcd.setCursor(11, 0);
      if(pos == 3)
        lcd.write(1);
      else
        lcd.print(" ");
      break;

    case 3:
      lcd.setCursor(0, 0);
      if(pos == 0)
        lcd.write(0);
      else
        lcd.print(" ");

      lcd.setCursor(0, 1);
      if(pos == 1)
        lcd.write(0);
      else
        lcd.print(" ");
      
      lcd.setCursor(9, 1);
      if(pos == 2)
        lcd.write(0);
      else
        lcd.print(" ");
      break;
  }
}


void startTimer1() {

  //int1flag = true;
  int1time = micros();
  //timerRunning = true;
  

  digitalWrite(13, LOW);
}

void stopTimer1() {

  
  int2flag = true;
  int2time = micros();
  
  
  // Reset the timerRunning flag
  //timerRunning = false;
  digitalWrite(13, HIGH);

  // Stop Timer1
  //TCCR1B &= ~(1 << CS10);  // Clear prescaler bits to stop the timer
}