#include "Display_Setting.h"
#include "Display_Mapping.h"

#define SELF_TEST 1
#define DEBUG 1
#define DISPLAY_COLOR 1
#define THRESHOLD 2048


#define ANALOG_IN 33
#define ANALOG_OUT 5
#define MUX0 14
#define MUX1 27
#define MUX2 26
#define MUX3 25

#define CHAR_LENGTH 11

#define PWM_CH 0
#define PWM_FREQ 5000
#define PWM_RES 8
const int SCREEN_W = 240;
const int SCREEN_H = 320;

// Store ON/OFF state
int analog_data[16] = {0};
uint16_t digital_data = 0;

SegGeom g = {20, 20, 200, 300, 12};


uint16_t valueToColor(int value)
{
  value = constrain(value, 0, 4095);

  uint8_t r = 0, g = 0, b = 0;

  uint16_t segment = value / 683;   // 4096 / 6 ≈ 683
  uint16_t offset  = value % 683;

  uint8_t t = map(offset, 0, 682, 0, 255);

  switch (segment) {
    case 0: // Blue → Cyan
      r = 0;
      g = t;
      b = 255;
      break;

    case 1: // Cyan → Green
      r = 0;
      g = 255;
      b = 255 - t;
      break;

    case 2: // Green → Yellow
      r = t;
      g = 255;
      b = 0;
      break;

    case 3: // Yellow → Orange
      r = 255;
      g = 255 - (t / 2); // slower green drop
      b = 0;
      break;

    case 4: // Orange → Red
      r = 255;
      g = 165 - map(t, 0, 255, 0, 165);
      b = 0;
      break;

    default: // Red
      r = 255;
      g = 0;
      b = 0;
      break;
  }

  // Convert RGB888 → RGB565
  return ((r & 0xF8) << 8) |
         ((g & 0xFC) << 3) |
         (b >> 3);
}

void  write_test_data(int i, int CHAR_NO) {
    uint8_t analog_data_out = 250 * (1&&((1<<i) & digitMask[CHAR_NO]));

    //Serial.println("Write called");
    analogWrite(ANALOG_OUT,analog_data_out);

    CHAR_NO++;
}


void read_analog_data() {
  digital_data = 0;
  static int CHAR_NO = 0;
  delay(500);
  for(int i=0; i<16; i++) {
    digitalWrite(MUX0,i & 0x01);
    digitalWrite(MUX1,(i >> 1) & 0x01);
    digitalWrite(MUX2,(i >> 2) & 0x01);
    digitalWrite(MUX3,(i >> 3) & 0x01);
    if(SELF_TEST) {
          if(CHAR_NO >= CHAR_LENGTH) {  // Reset the display buffer
            CHAR_NO = 0;
          }
          //Serial.print("Data sent "); Serial.print(i); Serial.print(" "); Serial.println((1&&((1<<i) & digitMask[CHAR_NO])),BIN);
          write_test_data(i,CHAR_NO);
            
    }
      
    delay(20); // Settling delay
    analog_data[i] = analogRead(ANALOG_IN);
    delay(20);
    digital_data = (digital_data) | ((analog_data[i] > THRESHOLD)<<i);
    
    if(DEBUG) {
      ;
      //Serial.print("Data read "); Serial.print(i); Serial.print(" "); Serial.println(digital_data,BIN);
      //Serial.print("Data "); Serial.print(i); Serial.print(" "); Serial.println(analog_data[i]);
    }
  }
  if(digital_data != digitMask[CHAR_NO]) {
    Serial.print("ERROR!! Char No:"); Serial.print(CHAR_NO); Serial.print(" Data sent: "); Serial.print(digitMask[CHAR_NO],BIN); Serial.print(" Data Rx:"); Serial.println(digital_data,BIN);
  }
  CHAR_NO++;
}


void setup() {
  
  analogReadResolution(12);
  //pinMode(ANALOG_OUT, OUTPUT);
  pinMode(ANALOG_IN, INPUT);
  pinMode(MUX0, OUTPUT); pinMode(MUX1, OUTPUT); pinMode(MUX2, OUTPUT); pinMode(MUX3, OUTPUT);
  tft.begin();
  tft.setRotation(0);  
  tft.fillScreen(ILI9341_BLACK);

  if(DEBUG)
    Serial.begin(9600);

  
  //drawChar(g, DISP_CHAR_8, ILI9341_RED, ILI9341_BLACK);

}

void loop() {
  
  delay(50);
  read_analog_data();
  delay(10);
  drawChar(g, display_map(digital_data), ILI9341_RED, ILI9341_BLACK);

}