#define DHT22_PIN 2
#define LIGHTING_PIN 3

#define LCD_RS 8
#define LCD_EN 9
#define LCD_D4 4
#define LCD_D5 5
#define LCD_D6 6
#define LCD_D7 7

#define POTENTIOEMETER_ANALOG 1

#define RED_LED 10
#define YELLOW_LED 11
#define GREEN_LED 12

namespace manual {
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))

  uint8_t digital_pin_to_bit_mask_fn(uint8_t pin) {
    // Define a lookup table for the Nano board
    const uint8_t digital_pin_to_bit_mask[] = {
      _BV(0), /* 0, port D */
      _BV(1),
      _BV(2),
      _BV(3),
      _BV(4),
      _BV(5),
      _BV(6),
      _BV(7),
      _BV(0), /* 8, port B */
      _BV(1),
      _BV(2),
      _BV(3),
      _BV(4),
      _BV(5)
    };

    if (pin >= sizeof(digital_pin_to_bit_mask)) {
        return 0;
    }

    return digital_pin_to_bit_mask[pin];
  }

  uint8_t digital_pin_to_timer_fn(uint8_t pin) {
    // Define a lookup table for the Nano board
    const uint8_t digital_pin_to_timer[] = {
      NOT_ON_TIMER, /* 0 - port D */
      NOT_ON_TIMER,
      NOT_ON_TIMER,
      TIMER2B,
      NOT_ON_TIMER,
      TIMER0B,
      TIMER0A,
      NOT_ON_TIMER,
      NOT_ON_TIMER, /* 8 - port B */
      TIMER1A,
      TIMER1B,
      TIMER2A,
      NOT_ON_TIMER,
      NOT_ON_TIMER
    };

    if (pin >= sizeof(digital_pin_to_timer)) {
        return NOT_ON_TIMER;
    }

    return digital_pin_to_timer[pin];
  }

  uint8_t digital_pin_to_port_fn(uint8_t pin) {
    // TODO: Rewrite
    return digitalPinToPort(pin);
  }

  void turn_off_pwm(uint8_t timer) {
      switch (timer)
      {
          case TIMER1A:   cbi(TCCR1A, COM1A1);    break;
          case TIMER1B:   cbi(TCCR1A, COM1B1);    break;
          case  TIMER0A:  cbi(TCCR0A, COM0A1);    break;
          case  TIMER0B:  cbi(TCCR0A, COM0B1);    break;
          case  TIMER2A:  cbi(TCCR2A, COM2A1);    break;
          case  TIMER2B:  cbi(TCCR2A, COM2B1);    break;
      }
  }

  void digital_write(uint8_t pin, uint8_t val) {
    uint8_t timer = digital_pin_to_timer_fn(pin);
    uint8_t bt = digital_pin_to_bit_mask_fn(pin);
    uint8_t port = digital_pin_to_port_fn(pin);
    volatile uint8_t *out;

    if (port == NOT_A_PIN) return;

    // If the pin that support PWM output, we need to turn it off
    // before doing a digital write.
    if (timer != NOT_ON_TIMER) turn_off_pwm(timer);

    out = portOutputRegister(port);

    uint8_t oldSREG = SREG;
    cli();

    if (val == LOW) {
      *out &= ~bt;
    } else {
      *out |= bt;
    }

    SREG = oldSREG;
  }
}

void sendCommand(byte command) {
  manual::digital_write(LCD_RS, LOW);

  manual::digital_write(LCD_D4, (command >> 4) & 0x01);
  manual::digital_write(LCD_D5, (command >> 5) & 0x01);
  manual::digital_write(LCD_D6, (command >> 6) & 0x01);
  manual::digital_write(LCD_D7, (command >> 7) & 0x01);

  manual::digital_write(LCD_EN, HIGH);
  delayMicroseconds(1);
  manual::digital_write(LCD_EN, LOW);

  manual::digital_write(LCD_D4, (command >> 0) & 0x01);
  manual::digital_write(LCD_D5, (command >> 1) & 0x01);
  manual::digital_write(LCD_D6, (command >> 2) & 0x01);
  manual::digital_write(LCD_D7, (command >> 3) & 0x01);

  manual::digital_write(LCD_EN, HIGH);
  delayMicroseconds(1);
  manual::digital_write(LCD_EN, LOW);

  delayMicroseconds(50); // Give the LCD time to execute the command
}

void sendData(byte data) {
  manual::digital_write(LCD_RS, HIGH);

  manual::digital_write(LCD_D4, (data >> 4) & 0x01);
  manual::digital_write(LCD_D5, (data >> 5) & 0x01);
  manual::digital_write(LCD_D6, (data >> 6) & 0x01);
  manual::digital_write(LCD_D7, (data >> 7) & 0x01);

  manual::digital_write(LCD_EN, HIGH);
  delayMicroseconds(1);
  manual::digital_write(LCD_EN, LOW);

  manual::digital_write(LCD_D4, (data >> 0) & 0x01);
  manual::digital_write(LCD_D5, (data >> 1) & 0x01);
  manual::digital_write(LCD_D6, (data >> 2) & 0x01);
  manual::digital_write(LCD_D7, (data >> 3) & 0x01);

  manual::digital_write(LCD_EN, HIGH);
  delayMicroseconds(1);
  manual::digital_write(LCD_EN, LOW);

  delayMicroseconds(50); // Give the LCD time to display the character
}

void lcd_setup() {
  pinMode(LCD_RS, OUTPUT);
  pinMode(LCD_EN, OUTPUT);
  pinMode(LCD_D4, OUTPUT);
  pinMode(LCD_D5, OUTPUT);
  pinMode(LCD_D6, OUTPUT);
  pinMode(LCD_D7, OUTPUT);

  // Initialize the LCD
  sendCommand(0x33);
  sendCommand(0x32);
  sendCommand(0x28);
  sendCommand(0x0C);
  sendCommand(0x06);
  sendCommand(0x01);
}

void sendData(const char* data, bool secondLine = false) {
  if (secondLine) {
    sendCommand(0xc0);

    // give it a moment now yeah?  
    delayMicroseconds(2000);
  }

  for (auto i = 0; i < strlen(data); i++) {
    sendData(data[i]);
  }
}

void clearScreen() {
  sendCommand(0x01);

  delayMicroseconds(2000);
}

float readCelsius(int pin, float* hum_out) {
  // Send start signal
  pinMode(pin, OUTPUT);
  manual::digital_write(pin, LOW);
  delayMicroseconds(500);
  manual::digital_write(pin, HIGH);
  delayMicroseconds(40);
  pinMode(pin, INPUT_PULLUP);

  // Wait for response signal
  unsigned long timeout = micros() + 1000;
  while (digitalRead(pin) == LOW && micros() < timeout);

  timeout = micros() + 1000;
  while (digitalRead(pin) == HIGH && micros() < timeout);


  // Read data bits
  int data[40];
  memset(data, 0, sizeof(data));
  for (int i = 0; i < 40; i++) {
    timeout = micros() + 1000;
    while (digitalRead(pin) == LOW && micros() < timeout);
    
    unsigned long start = micros();
    timeout = micros() + 1000;
    while (digitalRead(pin) == HIGH && micros() < timeout);
    
    if (micros() - start > 40) {
      data[i / 8] |= 1 << (7 - i % 8);
    }
  }

  // Verify checksum
  if (data[4] == ((data[0] + data[1] + data[2] + data[3]) & 0xFF)) {
    float temperature = (data[2] & 0x7F) * 256.0f + data[3];
    temperature /= 10.0f;
    if (data[2] & 0x80) {
      temperature *= -1.0f;
    }

    float humidity = data[0] * 256.0f + data[1];
    humidity /= 10.0f;

    if (hum_out) {
      *hum_out = humidity;
    }

    return temperature;
  }


  return -1000.0f;
}

void setLedState(int pin, bool state) {
  pinMode(pin, OUTPUT);
  manual::digital_write(pin, state ? HIGH : LOW);
}

bool getLedState(int pin) {
  pinMode(pin, INPUT);
  return digitalRead(pin) == HIGH ? true : false;
}

// returns a percentage
float getWaterLevel(int pin) {
  // Set the analog pin as input
  pinMode(pin, INPUT);

  // Read the analog value
  int value = analogRead(pin);

  return (value * 100.f) / 1023.f;
}


void setup() {
  Serial.begin(9600);

  lcd_setup();
}

void updateScreenData(float h, float v, float w) {
  clearScreen();
  sendData("T");
  char tAsString[10];
  char hAsString[10];
  char wAsString[10];
  dtostrf(v, 5, 1, tAsString);
  dtostrf(h, 5, 1, hAsString);
  dtostrf(w, 5, 1, wAsString);
  sendData(tAsString);
  sendData("C H");
  sendData(hAsString);
  sendData("%");
  sendData("WL", true);
  sendData(wAsString);
}

void loop() {
  float h = -1000.f;
  float v = readCelsius(DHT22_PIN, &h);

  if (h == -1000.f || v == -1000.f) {
    return;
  }

  float waterLevel = getWaterLevel(POTENTIOEMETER_ANALOG);

  setLedState(LIGHTING_PIN, !getLedState(LIGHTING_PIN));
  setLedState(RED_LED, !getLedState(RED_LED));
  setLedState(YELLOW_LED, !getLedState(YELLOW_LED));
  setLedState(GREEN_LED, !getLedState(GREEN_LED));

  updateScreenData(h, v, waterLevel);

  delay(2000);
}