#include <Arduino.h>
#include <stdio.h>
#include <WString.h>
#include <Print.h>
#include <HardwareSerial.h>
#include "meterBusMaster.h"
// #include "config.h"
#include "fatal.h"
#include <Streaming.h>



uint8_t charToNibble(char i) {
  uint8_t r = 99;
  if ((i >= '0') && (i <= '9')) {
    r = i - '0';
  } else if ((i >= 'a') && (i <= 'f')) {
    r = i - 'a' + 10;
  } else if ((i >= 'A') && (i <= 'F')) {
    r = i - 'A' + 10;
  }
  return r;
}

uint16_t stringToUInt8(String i, uint8_t index) {
  uint16_t r = 999;
  char b0 = i[index];
  uint8_t c0 = charToNibble(b0);
  char b1 = i[index + 1];
  uint8_t c1 = charToNibble(b1);
  if ((c0 <= 15) && (c1 <= 15)) {
    r = (c0 << 4) | c1;
  }
  return r;
}


// =====================================================================================================



String CalibrationSupport::exec(String params) {
  String res = "failed";

  MeterBusMaster *mbm = (MeterBusMaster*)m_meterBusMaster;
  if (params.equalsIgnoreCase("sample") && enable) {
    mbm->sample();
    res = "done";
  } else if (params.equalsIgnoreCase("hold") && enable) {
    mbm->hold();
    res = "done";
  } else if (params.equalsIgnoreCase("enable") && ! enable) {
    enable = true;
    mbm->m_calibration = true;
    res = "enabled";
  } else if (params.equalsIgnoreCase("disable") && enable) {
    enable = false;
    mbm->m_calibration = false;
    res = "disabled";
  }
  return res;
}

String MeasureCurrent::exec(String params) {
  int currentInVal = analogRead(CURRENT_IN);

  int current = ((double)currentInVal * U_UNIT) / R_SHUNT;

  return String() + current + String("mA");
}



String SendOctets::exec(String params) {
  bool err = false;;
  uint16_t sendBufLen = 0;

  uint8_t *sendBuffer = m_meterBusMaster->getSendBuffer();

  if (sendBuffer == 0) {
    err = true;
  } else {
    uint16_t n;
    for (uint16_t j = 0; j < params.length(); j+=3) {
      n = stringToUInt8(params, j);
      if (n == 999) {
        err = true;
        break;
      }

      if (sendBufLen >= SEND_BUFFER_SIZE)
        fatal(FATAL_BUFFER_OVERFLOW + 2);

      *(sendBuffer+sendBufLen) = n;
      sendBufLen++;
    }
  }

  if (err) {
    return "error";
  } else {
    m_meterBusMaster->sendBufferReady(sendBufLen, 1, this);
    return "success";
  }
}

void SendOctets::sendResponse(uint8_t *responseBuffer, uint16_t responseBufferLength, uint8_t token) {
  Print *out = m_server;
  out->print("SO RESP: ");
  uint16_t i = 0;
  while (true) {
    if (responseBuffer[i] <= 0x0f) {
      out->print("0");
    }
    out->print(responseBuffer[i], HEX);
    out->print(" ");
    i++;
    if (i == responseBufferLength) {
      break;
    }
  }
  out->println("");
}

void SendOctets::sendError(uint8_t code, uint8_t token) {
  switch (code) {
  case 1:
    m_server->println("SO RESP: no resp.");
    break;
  }
}




// =====================================================================================================


MeterBusMaster::MeterBusMaster() : m_sendOctets(this), m_measureCurrent(this),
    m_calibrationSupport(this),
    m_cmdReadyToSend(false), m_cmdReadyFromRecv(false), m_expectResponse(false),
    m_sendBufLen(0), m_recvBufLen(0), m_retransmitCount(0), m_token(0),
    m_responseCallback(0), m_sampling(true), m_calibration(false) {
    pinMode(RX_EN_PIN, OUTPUT);
    digitalWrite(RX_EN_PIN, RX_DISABLE);
    Serial3.begin(2400);
    UART2_C1 |= UART_C1_PE | UART_C1_M;


}

void MeterBusMaster::begin(CmdServer *cmdServer) {
  m_sendOctets.registerYourself(cmdServer);
  m_measureCurrent.registerYourself(cmdServer);
  m_calibrationSupport.registerYourself(cmdServer);
}


uint8_t *MeterBusMaster::getSendBuffer() {
  return m_expectResponse ? 0 : m_sendBuffer;
}

void MeterBusMaster::sendBufferReady(uint16_t sendBufLen, uint8_t token, ResponseCallback *responseCallback) {
  m_cmdReadyToSend = true;
  m_retransmitCount = 0;
  m_sendBufLen = sendBufLen;
  m_responseCallback = responseCallback;
  m_token = token;
}

void MeterBusMaster::prepareResponse(bool err, uint8_t in) {
  //Serial << "resp in, err: " << err << endl;
  static int16_t expectedChars = -1;
  static uint8_t state = 0;

  //Serial << "r0" << endl;
  if (err) {
    //Serial << "r1" << endl;
    if (m_responseCallback != 0) {
      m_responseCallback->sendError(1, m_token);
    }
    expectedChars = 0;
    state = 0;
    m_expectResponse = false;
    m_responseCallback = 0;
  } else {
    //Serial << "r2" << endl;
    switch (state) {
    case 0:
      //Serial << "r3" << endl;
      m_recvBufLen = 0;
      if (in == 0xe5) {
        state = 2;
        expectedChars = 0;
      } else if (in == 0x10) {
        state = 2;
        expectedChars = 4;
      } else if (in == 0x68) {
        state = 1;
        expectedChars = -1;
      }
      break;
    case 1:
      //Serial << "r4" << endl;
      expectedChars = (int16_t)in;
      expectedChars += 4;
      state = 2;
      break;
    case 2:
      //Serial << "r5" << endl;
      expectedChars--;
      break;
    }

    if (m_recvBufLen >= RECEIVE_BUFFER_SIZE) {
      //Serial << "r6, " << m_recvBufLen << ", " << RECEIVE_BUFFER_SIZE << endl;
      fatal(FATAL_BUFFER_OVERFLOW + 1);
    }

    //Serial << "r7" << endl;
    m_recvBuffer[m_recvBufLen] = in;
    m_recvBufLen++;

    //Serial << "r8" << endl;
    if (expectedChars == 0) {
      //Serial << "r9" << endl;
      if (m_responseCallback != 0) {
        m_responseCallback->sendResponse(m_recvBuffer, m_recvBufLen, m_token);
      }
      m_expectResponse = false;
      m_token = 0;
      state = 0;
      m_responseCallback = 0;
    }
  }

  //Serial << "resp out" << endl;
}

void MeterBusMaster::sample() {
  if (! m_sampling) {
    m_sampling = true;
    digitalWrite(RX_EN_PIN, RX_DISABLE);
  }
}

void MeterBusMaster::hold() {
  if (m_sampling) {
    m_sampling = false;
    digitalWrite(RX_EN_PIN, RX_ENABLE);
  }
}

void MeterBusMaster::exec() {
  if (! m_calibration) {
    static unsigned long cmdSendTime = 0;

    if (m_cmdReadyToSend) {
      sample();
      //Serial << "MeterBusMaster: sending " << m_sendBufLen << " octets." << endl;
      Serial3.write(m_sendBuffer, m_sendBufLen);
      Serial3.flush();
      hold();
      m_cmdReadyToSend = false;
      m_expectResponse = true;
      cmdSendTime = millis();
    }

    if (! m_expectResponse) {
      sample();
    }

    // timeout
    if (m_expectResponse && ((millis() - cmdSendTime) > RESPONSE_TIMEOUT)) {
      m_retransmitCount++;
      if (m_retransmitCount > 2) {
        m_expectResponse = false;
        prepareResponse(true, 0);
      } else {
        m_cmdReadyToSend = true;
      }
    }

    int serialInChar = Serial3.read();
    if (serialInChar != -1) {
      //Serial << "Got: " << _HEX(serialInChar) << endl;
    }
    if ((serialInChar != -1) && m_expectResponse) {
      prepareResponse(false, (uint8_t)serialInChar);
    }
  }
}


// =====================================================================================================