const bool debugMode = false;

// INPUT pins
const unsigned char pinInputIndexes[] = {2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}; // worst case scenario

// OUTPUT pins
const unsigned char pinOutputIndexes[] = {}; // Decide pins if needed

// Time calculation
unsigned long deltaTimeMs = 20;
const bool useMicros = true;
unsigned char pinReadDelayUsec = 4;

// Signal variables
// TODO: change according to real readings
unsigned short appSignalEnableThreshold = 1500; // Min raw signal to run
unsigned short idleSignalLength = 1500; // Raw signal for NoAction (middle signal)

unsigned short minSignalLength = 1000; // Raw bottom signal
unsigned short maxSignalLength = 2000; // Raw top signal

const short minActionLength = -255;
const short maxActionLength = 255;

// Decoding speed importance
// TEMPORARY CONSTANT COEFFICIENTS, TODO: change to interpolation function
unsigned char movingCoefficient = 1;
unsigned char rotatingCoefficient = 4;

// Serial data transfer speed
const unsigned int serialBaudRate = 115200;

/***  DO NOT CHANGE  ***/
// Internal calculations
bool isEnabled = true;
const unsigned char debugLagCoefficient = 20;

unsigned long nowTime, lastTime;
unsigned short signalLength;

unsigned char movementCoefficient;
short movingSpeed, rotatingSpeed;
short leftMovementSpeed, rightMovementSpeed;

// Input pins variables
const unsigned char pinInputIndexesSize = sizeof(pinInputIndexes) / sizeof(pinInputIndexes[0]);
bool pinReadings[pinInputIndexesSize];
bool pinChangeDetected[pinInputIndexesSize];
unsigned long pinReadTimeHIGH[pinInputIndexesSize];
unsigned long pinSignalLength[pinInputIndexesSize];

// Output pins variables
const unsigned char pinOutputIndexesSize = sizeof(pinOutputIndexes) / sizeof(pinOutputIndexes[0]);


static unsigned long getTime() {
  return useMicros? micros() : millis();
}

String generateTimestamp() {
  unsigned long currentTime = getTime();

  unsigned long minutes = currentTime / (useMicros ? 60000000 : 60000);
  unsigned long seconds = (currentTime % (useMicros ? 60000000 : 60000)) / (useMicros ? 1000000 : 1000);
  unsigned long milliseconds = (currentTime % (useMicros ? 1000000 : 1000)) / (useMicros ? 1000 : 1);
  unsigned long microseconds = useMicros ? currentTime % 1000 : 0;

  // Format the timestamp with leading zeros
  String timestamp = "[";
  timestamp += (minutes < 10 ? "0" : "") + String(minutes) + ":";
  timestamp += (seconds < 10 ? "0" : "") + String(seconds) + ".";
  timestamp += (milliseconds < 10 ? "00" : (milliseconds < 100 ? "0" : "")) + String(milliseconds);

  if (useMicros) {
    timestamp += "." + String(microseconds < 10 ? "00" : (microseconds < 100 ? "0" : "")) + String(microseconds);
  }

  timestamp += "] ";

  return timestamp;
}

void setup() {

  for (unsigned char i = 0; i < pinInputIndexesSize; i++) {
    pinMode(pinInputIndexes[i], INPUT);
  }
  
  for (unsigned char i = 0; i < pinOutputIndexesSize; i++) {
    pinMode(pinOutputIndexes[i], OUTPUT);
  }

  deltaTimeMs = useMicros? deltaTimeMs * 1000 : deltaTimeMs;
  pinReadDelayUsec = useMicros? pinReadDelayUsec : pinReadDelayUsec / 1000;

  appSignalEnableThreshold = useMicros? appSignalEnableThreshold : appSignalEnableThreshold / 1000; 
  idleSignalLength = useMicros? idleSignalLength : idleSignalLength / 1000; 
  minSignalLength = useMicros? minSignalLength : minSignalLength / 1000;
  maxSignalLength = useMicros? maxSignalLength : maxSignalLength / 1000;

  // Used for debugging. Serial 1-3 used for data transfer.
  Serial.begin(serialBaudRate);
  while (!Serial) { /* Await Serial */ }
  
  if (debugMode) {
    char timeResolution[1];
    strcpy(timeResolution, useMicros? "μs" : "ms");
    Serial.println(generateTimestamp() + "[INFO] Chosen time resolution is '" + String(timeResolution) + "'");
    Serial.println(generateTimestamp() + "[INFO] Chosen time window is " + String(deltaTimeMs));
    deltaTimeMs *= debugLagCoefficient;
    Serial.println(generateTimestamp() + "[DEBUG] Changing time window to " + String(deltaTimeMs) + " ("+ String(debugLagCoefficient) +"x) due to debug lag");
    Serial.println(generateTimestamp() + "[WARNING] There still will be delays in readings from debug lag");
    Serial.println(generateTimestamp() + "[INFO] Finished initialization after ~" + String(getTime()));
  }
  lastTime = getTime();
}

static short interpolate(unsigned short signalLength) {
  return map(signalLength, minSignalLength, maxSignalLength, minActionLength, maxActionLength);
}

static void decodeSignal(unsigned char pinId) {
  signalLength = pinSignalLength[pinId];
  pinId = pinInputIndexes[pinId];

  switch (pinId) {
    case 2: // let it be the first possible pin in pinInputIndexes[]
      isEnabled = signalLength > appSignalEnableThreshold;

      // Checking if 'isEnabled' detects correctly
      if (debugMode) {
        Serial.println(generateTimestamp() + "[INFO] " + String(signalLength) + " -> " + String(isEnabled));
      }

      // Set everything to "idle" if !isEnabled
      if (!isEnabled) {
        for (unsigned char i = 0; i < pinInputIndexesSize; i++) {
          pinSignalLength[i] = idleSignalLength;
        }
        if (debugMode) {
          Serial.println(generateTimestamp() + "[DEBUG] Setting all readings to " + String(idleSignalLength));
        }
      }
      break;
    case 3:
      // handle forwards-backwards movement. It calculates first
      movingSpeed = interpolate(signalLength);
      break;
    case 4:
      // handle left-right movement. It handles after forwards-backwards.
      rotatingSpeed = interpolate(signalLength);
      break;
    default:
      // Do nothing if other pin
      break;
  }
}

// TODO: rewrite function
static unsigned char calcCoefficient(unsigned short signalLength) {
  return 0;
}

static void sendSignal(unsigned char pinId) {
  signalLength = pinSignalLength[pinId];
  pinId = pinInputIndexes[pinId];

  switch (pinId) {
    case 2: // let it be the first possible pin in pinInputIndexes[]
      // do nothing
      break;
    case 3:
      // do something, e.g. calculate common coefficient
      
      // TODO: function for coefficient
      // movingCoefficient = calcCoefficient(movingSpeed);
      // rotatingCoefficient = calcCoefficient(rotatingSpeed);
      movementCoefficient = movingCoefficient + rotatingCoefficient;

      if (debugMode) {
        Serial.println(generateTimestamp() + "[INFO] Moving coefficient is " + String(movingCoefficient));
        Serial.println(generateTimestamp() + "[INFO] Rotating coefficient is " + String(rotatingCoefficient));
        Serial.println(generateTimestamp() + "[DEBUG] Common movement coefficient is " + String(movementCoefficient));
      }

      break;
    case 4:
      // do other thing, e.g. calculate speed for each motor

      // TODO: change function for speed calculations,

      if (debugMode) {
        Serial.println(generateTimestamp() + "[INFO] Moving speed is " + String(movingSpeed));
        Serial.println(generateTimestamp() + "[INFO] Rotating speed is " + String(rotatingSpeed));
      }

      movingSpeed *= movingCoefficient;
      rotatingSpeed *= rotatingCoefficient;
      leftMovementSpeed = (movingSpeed + rotatingSpeed) / movementCoefficient;
      rightMovementSpeed = (movingSpeed - rotatingSpeed) / movementCoefficient;

      if (debugMode) {
        Serial.println(generateTimestamp() + "[DEBUG] Moving speed with coefficient is " + String(movingSpeed));
        Serial.println(generateTimestamp() + "[DEBUG] Rotating speed with coefficient is " + String(rotatingSpeed));
        Serial.println(generateTimestamp() + "[INFO] Left movement speed is " + String(leftMovementSpeed));
        Serial.println(generateTimestamp() + "[INFO] Right movement speed is " + String(rightMovementSpeed));
      }

      break;
    default:
      // Do nothing if other pin
      break;
  }
}

void loop() {
  nowTime = getTime();
  
  // Report signal type
  if (abs(nowTime - lastTime) >= deltaTimeMs) {
    // Convert readings to format
    for (unsigned char i = 0; i < pinInputIndexesSize; i++) {
      decodeSignal(i);
    }

    // Do uniform signal actions
    for (unsigned char i = 0; i < pinInputIndexesSize; i++) {
      sendSignal(i);
    }
    
    // Checking correct 'signalLength' calculation
    if (debugMode) {
      for (unsigned char i = 0; i < pinInputIndexesSize; i++) {
        Serial.println(generateTimestamp() + "[INFO] Pin " + String(pinInputIndexes[i]) + " utilization: " + String(pinSignalLength[i]) + " (" + pinSignalLength[i]*100/pinSignalLength[0] + "%)");
      }
      Serial.println(generateTimestamp() + "[INFO] Cycle " + String(nowTime/deltaTimeMs) + " finished");
    }

    // Clear readings
    for (unsigned char i = 0; i < pinInputIndexesSize; i++) {
      pinSignalLength[i] = 0;
    }
    lastTime = nowTime;

    if (debugMode) {
      Serial.println(generateTimestamp() + "[INFO] Operation loop was running for " + String(getTime() - lastTime));
    }
  }

  // Read signals
  for (unsigned char i = 0; i < pinInputIndexesSize; i++) {
    pinReadings[i] = digitalRead(pinInputIndexes[i]);
  }

  // Deprecated code //
  // Count signal length [by data]
  // for (unsigned char i = 0; i < pinInputIndexesSize; i++) {
  //   pinSignalLength[i] += pinReadings[i];
  // }
  
  // Count signal length [by time]
  for (unsigned char i = 0; i < pinInputIndexesSize; i++) {
    if (pinReadings[i] == HIGH && pinChangeDetected[i]) {
      pinChangeDetected[i] = !pinChangeDetected[i];
      pinReadTimeHIGH[i] = getTime();
    } else if (pinReadings[i] == LOW && !pinChangeDetected[i]) {
      pinChangeDetected[i] = !pinChangeDetected[i];
      pinSignalLength[i] += getTime() - pinReadTimeHIGH[i] - pinReadDelayUsec * i; // cumulative time calculation
      if (debugMode) {
        Serial.println(generateTimestamp() + "[DEBUG] Detected change at pin " + String(pinInputIndexes[i]) + ": " + String(pinSignalLength[i]));
      }
    }
  }

  if (debugMode) {
    for (unsigned char i = 0; i < pinInputIndexesSize; i++) {
      Serial.println(generateTimestamp() + "[INFO] Pin " + String(pinInputIndexes[i]) + ": " + String(pinSignalLength[i]));
    }
    Serial.println(generateTimestamp() + "[INFO] Main loop was running for " + String(getTime() - nowTime));
  }
}