#include <SoftwareSerial.h>

#define TRUE 1
#define FALSE 0
#define RX 6
#define TX 9
#define LIN_BAUD_RATE 19200
#define FRAME_LENGTH 10
#define BREAK_FIELD_DOMINANT_TIME 937.5 //19200 első periódusa
#define BREAK_FIELD_RECESSIVE_TIME 103.166
#define WAKE_UP_BREAK 25
#define HEADER_DELAY 1
#define BASIC_DELAY 1
#define SYNC 0x55
#define ACQUISITION_PERIOD 1000
#define MASK_00 0x01
#define MASK_01 0x02
#define MASK_02 0x04
#define MASK_03 0x08
#define MASK_04 0x10
#define MASK_05 0x20

SoftwareSerial LIN (RX, TX);

void display_init(void) {
  Serial.begin(9600);
  while (!Serial);

  Serial.print("VW IPS Sensor. Continental GmbH");
  Serial.print("\nPeriod acqusition of 0x2F and 0x30 frames");
}

byte calculate_PID(byte ID) {
  byte PID, b0, b1, b2, b3, b4, b5, p1, p0;
  ID = ID & 0x3F;

  b0 = ID & MASK_00;
  b1 = (ID & MASK_01) >> 1;
  b2 = (ID & MASK_02) >> 2;
  b3 = (ID & MASK_03) >> 3;
  b4 = (ID & MASK_04) >> 4;
  b5 = (ID & MASK_05) >> 5;

  p0 = ( b0 ^ b1 ^ b2 ^ b4) << 6;
  p1 = ( ~(b1 ^ b3 ^ b4 ^ b5)) << 7;

  PID = ID | p0 | p1;
  return PID;
}

void generate_break_field(void) {
  digitalWrite(TX, LOW);
  delayMicroseconds(BREAK_FIELD_DOMINANT_TIME);
  digitalWrite(TX, HIGH);
  delayMicroseconds(BREAK_FIELD_RECESSIVE_TIME);
}

void close_LIN(void) {
  LIN.flush();
  LIN.end();
}

void bus_init(void) {
  pinMode(TX, OUTPUT);
  pinMode(RX, INPUT);
}

void send_header(byte ID) {
  byte PID;

  PID = calculate_PID(ID);
  generate_break_field();
  LIN.begin(LIN_BAUD_RATE);
  while (!LIN);

  LIN.write(SYNC);
  LIN.write(PID);

  digitalWrite(TX, HIGH);
  delay(HEADER_DELAY);
}

void receive_data(byte* ptr_frame_data) {
  short int idx;

  for (idx = 1; idx < FRAME_LENGTH; idx++) {
    delay(BASIC_DELAY);
    ptr_frame_data[idx] = LIN.read();
  }

  close_LIN();
}

void display_frame_data(byte* ptr_frame_data) {
  Serial.print("\n\n\nFrame 0x");
  if (ptr_frame_data[0] <= 0x0F) {
    Serial.print("0");
  }
  Serial.print(ptr_frame_data[0]);
  Serial.print("\n");

  for (int i = 0; i < FRAME_LENGTH; i++) {
    if (ptr_frame_data[i] <= 0x0F) {
      Serial.print("0");
    }
    Serial.print(ptr_frame_data[i], HEX);
    Serial.print(" ");
  }
}

byte evaluate_checksum(byte* ptr_frame_data) {
  return 1;
}

void BDM_Spannung_dyn(byte* ptr_frame_data) {
  unsigned int BDM_Spannung_dyn;
  byte volts;
  byte digit1, digit2, digit3;

  BDM_Spannung_dyn = ((unsigned int)ptr_frame_data[2] & 0x3F) << 8;
  BDM_Spannung_dyn += (unsigned int)ptr_frame_data[1];
  BDM_Spannung_dyn += 4000;

  volts = (byte)(BDM_Spannung_dyn / 1000);
  digit1 = (byte)(BDM_Spannung_dyn - (volts * 1000)) / 100;
  digit2 = (byte)(BDM_Spannung_dyn % 100) / 10;
  digit3 = (byte)(BDM_Spannung_dyn % 10);

  Serial.print("\nBDM Spannung: ");
  Serial.print(volts, DEC);
  Serial.print(".");
  Serial.print(digit1, DEC);
  Serial.print(digit2, DEC);
  Serial.print(digit3, DEC);
  Serial.print(".");
}

void BDM_Temp_dyn(byte* ptr_frame_data) {

}

void BDM_I_Bereich_and_BDM_Storm_dyn(byte* ptr_frame_data) {

}

void BZE_SOC_K20(byte* ptr_frame_data) {

}

void acquire_frame_2F() {
  byte frame_data[FRAME_LENGTH] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
  byte checksum_matches;

  send_header(0x2F);
  receive_data(frame_data);

  frame_data[0] = 0x2F;
  display_frame_data(frame_data);

  frame_data[0] = calculate_PID(0x2F);
  checksum_matches = evaluate_checksum(frame_data);

  if (checksum_matches == FALSE) {
    Serial.print("\n Checksum error!");
  } else {
    BDM_Spannung_dyn(frame_data);
    Serial.print("\nVoltage: ");
    Serial.print(getSignalPhys(frame_data, 8, 14, 4000, 1000));
    Serial.print("V");

    Serial.print("\nTemperature: ");
    Serial.print(getSignalPhys(frame_data, 40, 8, -40, 1));
    Serial.print("C");

    byte bereich = getSignalPhys(frame_data, 22, 2, 0, 1);
    Serial.print("\nStrom bereich: ");
    Serial.print(bereich);
    int strom = 0;
    strom =

    // BDM_Temp_dyn(frame_data);
    BDM_I_Bereich_and_BDM_Storm_dyn(frame_data);
  }
}

void acquire_frame_30() {
  byte frame_data[FRAME_LENGTH] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
  byte checksum_matches;

  send_header(0x30);
  receive_data(frame_data);

  frame_data[0] = 0x30;
  display_frame_data(frame_data);

  frame_data[0] = calculate_PID(0x30);
  checksum_matches = evaluate_checksum(frame_data);

  if (checksum_matches == FALSE) {
    Serial.print("\n Checksum error!");
  } else {
    BZE_SOC_K20(frame_data);
  }
}



void setup() {
  display_init();
  bus_init();
}

#define GET_BIT(INPUT, POS) ((INPUT & (1 << POS)) >> POS);

float getSignalPhys(unsigned char * frame, unsigned char position,
unsigned char length, int offset, unsigned int factor) {
  unsigned long raw_value = 0;
  unsigned char step, byte_index, bit_index;
  byte bit;

  for (step = 0; step < length; step++) {
    byte_index = (position + step) / 8;
    bit_index = (position + step) & 0x7;
    bit = GET_BIT(frame[byte_index], bit_index);
    raw_value |= bit << step;
  }
  return (((float)raw_value + (float)offset) / (float)factor);
}

void printSignalPhys() {

}

void loop() {
  acquire_frame_2F();
  delay(ACQUISITION_PERIOD);
  acquire_frame_30();
  delay(ACQUISITION_PERIOD);
}