/*
  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.
  
  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.
  
  You should have received a copy of the GNU General Public License
  along with this program.  If not, see <http://www.gnu.org/licenses/>. 
*/

#include <SandS.h>
#include <Arduino.h>
#include <EEPROM.h>
#include <Wire.h>

#if defined(__AVR_ATmega32U4__) // SHIELD
SoftwareSerial SerialUI(UI_RX, UI_TX); // RX, TX
#endif 

SandS_Class SandS;

SandS_Class::SandS_Class() { }

// TODO: These should be flipped for v002 boards
#define PIN_ID_UP   8 // Upstream (towards host) ident/alarm pin
#define PIN_ID_DOWN 2 // Downstream (away from host) ident/alarm pin
#define PIN_ALARM  10 // Alarm HI-Z

#define ADDRESS_BROADCAST 0x00

//#define SANDS_DEBUG // Debug via Serial

#ifdef SANDS_DEBUG
#define DEBUG_BEGIN() Serial.begin(9600)
#define DEBUG_WRITE(X) Serial.write(X)
#define DEBUG_PRINT(X) Serial.print(X)
#define DEBUG_PRINTLN(X) Serial.println(X)
#else
#define DEBUG_BEGIN()
#define DEBUG_WRITE(X)
#define DEBUG_PRINT(X)
#define DEBUG_PRINTLN(X)
#endif


typedef enum {
  WRITE_NOTHING,
  WRITE_OK,
  WRITE_ERROR,
  WRITE_IDENT,
  WRITE_DEVINFO,
  WRITE_DEVICE,
} writestate_e;

typedef enum {
  CMD_DEVINFO     = 0x00,
  CMD_IDENT_START = 0x80,
  CMD_IDENT_END   = 0x81,
  CMD_IDENT_PASS  = 0x82,
  CMD_IDENT_READ  = 0x83,
  CMD_IDENT_WRITE = 0x84,
  CMD_IDENT_TEMP  = 0x85,
} command_e;

typedef enum {
  IDAL_ALARM,
  IDAL_IDENT,
  IDAL_IDENT_PASS
} idalstate_e;

#define IDENT_ACCEPT (idal_state == IDAL_IDENT && digitalRead(PIN_ID_UP) == HIGH)

#define WRITEBUF_SIZE 32

static uint8_t address = ADDRESS_BROADCAST;
static uint8_t address_temp = ADDRESS_BROADCAST;
static uint8_t write_state = WRITE_NOTHING;
static uint8_t idal_state = IDAL_ALARM;
static boolean alarm = false;

static uint8_t writebuf[WRITEBUF_SIZE];
static uint8_t *p_writebuf = writebuf;

static void (*user_onCommand)(byte, int) = NULL;
static void (*user_onReadRequest)(byte) = NULL;

static char devinfo[4];

// Load configuration data from EEPROM
bool SandS_Class::loadData(void *data, uint8_t data_size) {
	uint16_t checksum = data_size;
	int address;
	DEBUG_WRITE("Attempting to load ");
	DEBUG_PRINT(data_size);
	DEBUG_WRITE(" bytes data...\n");
	DEBUG_WRITE("Check device type...\n");
	if(EEPROM.read(2) != devinfo[0]) return false;
	DEBUG_WRITE("Check device version...\n");
	if(EEPROM.read(3) != devinfo[1]) return false;
	if(EEPROM.read(4) != devinfo[2]) return false;
	if(EEPROM.read(5) != devinfo[3]) return false;
	if(EEPROM.read(6) != data_size) return false;
	for(address = 7; address < data_size + 7; address++) {
		checksum += EEPROM.read(address);
		if(checksum >= 0x0100) checksum -= 0x00FF;
	}
	checksum = (uint8_t)~checksum;
	DEBUG_WRITE("Checksums: C/E = ");
	DEBUG_PRINT(checksum);
	DEBUG_WRITE("/");
	DEBUG_PRINT(EEPROM.read(address));
	DEBUG_WRITE("\n");
	if(EEPROM.read(address) != checksum) return false;
	DEBUG_WRITE("Reading data...\n");
	for(address = 7; address < data_size + 7; address++) {
		*(uint8_t*)data = EEPROM.read(address);
		DEBUG_PRINT(*(uint8_t*)data);
		DEBUG_WRITE(" ");
		data = ((uint8_t*)data) + 1;		
	}
	DEBUG_WRITE("\nDone!\n");
	return true;
}

// Save configuration data to EEPROM
void SandS_Class::saveData(void *data, uint8_t data_size) {
	uint16_t checksum = data_size;
	int address = 7;
	EEPROM.write(2, devinfo[0]);
	EEPROM.write(3, devinfo[1]);
	EEPROM.write(4, devinfo[2]);
	EEPROM.write(5, devinfo[3]);
	EEPROM.write(6, data_size);
	while(data_size--) {
		EEPROM.write(address++, *(uint8_t*)data);
		checksum += *(uint8_t*)data;
		if(checksum >= 0x0100) checksum -= 0x00FF;
		data = ((uint8_t*)data) + 1;
	}
	checksum = (uint8_t)~checksum;
	EEPROM.write(address, checksum);
}

// Set alarm state
void SandS_Class::setAlarm(boolean alarm) {
	pinMode(PIN_ALARM, alarm ? OUTPUT : INPUT);
    digitalWrite(PIN_ALARM, LOW);
}

// Get alarm state
boolean SandS_Class::getAlarm() {
	return digitalRead(PIN_ALARM);
}

// Updates TWI address, and enables general-call (address 0)
static void update_address(uint8_t addr) {
  addr &= 0x7F;
  if(TWAR != ((addr << 1) | 0x01)) {
	  TWAR = (addr << 1) | 0x01;
	  DEBUG_WRITE("\nID is now ");
	  DEBUG_PRINT(addr);
	}
}

// Save address to EEPROM
static boolean save_address() {
  uint8_t check;
  if(address != ADDRESS_BROADCAST && !(address & 0x80)) {
    check = (address + 0x55) ^ 0xAA;
    EEPROM.write(0, address);
    EEPROM.write(1, check);
    update_address(address);
    return true;
  }
  return false;
}

// Load address from EEPROM
static boolean load_address() {
  uint8_t check;
  address = EEPROM.read(0);
  check = (address + 0x55) ^ 0xAA;
  if((address & 0x80) || check != EEPROM.read(1)) {
    // Default addresses
	switch(devinfo[0]) {
		case 'S': address = 0x01;
		case 'R': address = 0x02;
		case 'M': address = 0x03;
		case 'I': address = 0x04;
		case 'C': address = 0x05;
		default:  address = ADDRESS_BROADCAST;
	}
	save_address();
    return false;
  }
  if(devinfo[0] == 'S') address = 0x01;
  return true;
}

// Set IDAL state
static void idal_set(uint8_t state) {
  idal_state = state;
  if(devinfo[0] == 'S') {
	idal_state = IDAL_ALARM;
	return;
  }
  DEBUG_WRITE("\nIDAL state <- ");
  switch(state) {
    case IDAL_ALARM:
      DEBUG_WRITE("ALARM MODE");
      digitalWrite(PIN_ID_DOWN, LOW);
      break;
    case IDAL_IDENT:
      DEBUG_WRITE("IDENT MODE");
      digitalWrite(PIN_ID_DOWN, LOW);
      break;
    case IDAL_IDENT_PASS:
      DEBUG_WRITE("PASSTHROUGH");
      delay(100);
      digitalWrite(PIN_ID_DOWN, HIGH);
      break;
    default:
      DEBUG_WRITE("UNKNOWN");
  }
  update_address(address);
}

// Handles commands that are common between all devices
static void twi_write(int count) {
  uint8_t cmd;
  if(count) {
    write_state = WRITE_NOTHING;  
    DEBUG_WRITE("Command: ");
    cmd = Wire.read();
    switch(cmd) {
      case CMD_IDENT_START: // Start identification mode
        if(count != 1) break;
        DEBUG_WRITE("ID Start");
        idal_set(IDAL_IDENT);
        break;
      case CMD_IDENT_END: // End indentification mode
        if(count != 1) break;
        DEBUG_WRITE("ID End");
        idal_set(IDAL_ALARM);
        break;
      case CMD_IDENT_PASS: // Pass on token
        if(count != 1) break;
        DEBUG_WRITE("ID Passthrough ");
        if(IDENT_ACCEPT) {
          DEBUG_WRITE("(accepted)");
          idal_set(IDAL_IDENT_PASS);
        } else {
          DEBUG_WRITE("(denied)");
        }
        break;
      case CMD_IDENT_READ: // Read identification
        if(count != 1) break;
        DEBUG_WRITE("ID Readout ");
        if(idal_state == IDAL_ALARM || IDENT_ACCEPT) {
          DEBUG_WRITE("(accepted)");
          write_state = WRITE_IDENT; // Write identifier on next request
        } else {
          DEBUG_WRITE("(denied)");
        }
        break;
      case CMD_IDENT_TEMP: // Set temporary identification
        if(count != 2) break;
        DEBUG_WRITE("ID Temp ");
        if(IDENT_ACCEPT) {
          DEBUG_WRITE("(accepted)");
          address_temp = Wire.read();
          update_address(address_temp);
          write_state = WRITE_OK;
        } else {
          DEBUG_WRITE("(denied)");
        }
        break;
      case CMD_IDENT_WRITE: // Write identification
        if(count != 2) break;
        DEBUG_WRITE("ID Write ");
        if(idal_state == IDAL_ALARM || IDENT_ACCEPT) {
          DEBUG_WRITE("(accepted)");
          address = Wire.read();
          if(devinfo[0] == 'S') address = 0x01;
          write_state = save_address() ? WRITE_OK : WRITE_ERROR; // Write OK/ERROR on next request
        } else {
          DEBUG_WRITE("(denied)");
        }
        break;
      case CMD_DEVINFO: // Device information
      	if(count != 1) break;
        DEBUG_WRITE("Device info ");
      	if(idal_state == IDAL_ALARM || IDENT_ACCEPT) {
          DEBUG_WRITE("(accepted)");
          write_state = WRITE_DEVINFO;
        } else {
          DEBUG_WRITE("(denied)");
      	}
      	break;
      default:
        DEBUG_WRITE("Device ");
        DEBUG_PRINT(cmd);
        if(idal_state == IDAL_ALARM || IDENT_ACCEPT) {
          DEBUG_WRITE(" (accepted)\n");
		  // Reset writebuffer
		  p_writebuf = writebuf;
          write_state = WRITE_DEVICE;
		  if(user_onCommand) user_onCommand(cmd, count - 1);
          DEBUG_WRITE("Device wrote ");
          DEBUG_PRINT(p_writebuf - writebuf);
          DEBUG_WRITE(" bytes");
        } else {
          DEBUG_WRITE(" (denied)");
        }
    }
    DEBUG_WRITE('\n');
    // Flush extra data, or Wire will lock up
    while(Wire.available()) Wire.read();
  }
}

// Handles command responses that are common between all devices
static void twi_read(void) {
	DEBUG_WRITE("Request: ");
  if(write_state == WRITE_NOTHING) return;
  switch(write_state) {
    case WRITE_OK:
    	DEBUG_WRITE("OK\n");
      Wire.write(0x01);
      break;
    case WRITE_ERROR:
    	DEBUG_WRITE("Error\n");
      Wire.write(0xFE);
      break;
    case WRITE_IDENT:
    	DEBUG_WRITE("ID\n");
      Wire.write(address);
      break;
    case WRITE_NOTHING:
    	DEBUG_WRITE("Nothing\n");
      break;
    case WRITE_DEVINFO:
    	DEBUG_WRITE("Device info\n");
    	Wire.write((const uint8_t*)devinfo, 4);
    	break;
    case WRITE_DEVICE:
    	DEBUG_WRITE("Device\n");
		if(user_onReadRequest) {
			user_onReadRequest(32);
		} else {
			Wire.write(writebuf, p_writebuf - writebuf);
		}
      break;
    default:
    	DEBUG_WRITE("UNKNOWN\n");
  }
}

// Configure environment
void SandS_Class::begin(char type, char major, char minor, char revision) {
  devinfo[0] = type;
  devinfo[1] = major;
  devinfo[2] = minor;
  devinfo[3] = revision;
  load_address();
  Wire.begin(address);
  Wire.onReceive(twi_write); 
  Wire.onRequest(twi_read);
  update_address(address);
  setAlarm(LOW);
  pinMode(PIN_ID_UP, INPUT);
  pinMode(PIN_ID_DOWN, OUTPUT);
  idal_set(IDAL_ALARM);
  DEBUG_BEGIN();
}

// Set handler for commands
void SandS_Class::onCommand(void (*function)(byte, int)) {
	user_onCommand = function;
}

// Write response data (from command handler)
size_t SandS_Class::write(uint8_t data) {
	if(p_writebuf < &writebuf[WRITEBUF_SIZE]) {
		*p_writebuf++ = data;
		return 1;
	}
	return 0;
}

// Sets up custom write
void SandS_Class::onRequest(void (*function)(byte)) {
	user_onReadRequest = function;
}

// Write response data (from command handler)
size_t SandS_Class::write(const uint8_t *data, size_t quantity) {
	if(quantity > (size_t)(&writebuf[WRITEBUF_SIZE] - p_writebuf))
		quantity = &writebuf[WRITEBUF_SIZE] - p_writebuf;
	memcpy(p_writebuf, data, quantity);
	p_writebuf += quantity;
	return quantity;
}

bool Module::send(uint8_t *data, uint8_t size) {
  if(!initialized) return false;
  Wire.begin();
  Wire.beginTransmission(address);
  Wire.write(data, size);
  Wire.endTransmission();
  return true;
}


bool Module::recv(uint8_t *data, uint8_t size) {
  if(!initialized) return false;
    Wire.begin();
  byte got = Wire.requestFrom(address, size);
  if(got < size || Wire.available() < size) {
    while(Wire.available()) Wire.read();
    return false;
  } else {
    for(uint8_t n = 0; n < size; n++) {
      data[n] = Wire.read();
    }
  }
  return true;
}

bool Module::initialize() {
  uint8_t command[] = {0x00};
  Wire.begin();
  // Issue DEVINFO command
  Wire.beginTransmission(address);
  Wire.write(command, 1);
  Wire.endTransmission();
  // Read DEVINFO response
  Wire.requestFrom(address, (uint8_t)4);
  // Make sure board is of correct type
  if(Wire.read() != type) return false;
  // Read version
  uint8_t maj = Wire.read() - '0';
  uint8_t min = Wire.read() - '0';
  uint8_t rev = Wire.read() - '0';
  // Sanity check returned data
  if(maj > 9 || min > 9 || rev > 9) return false;
  version = maj * 100 + min * 10 + rev;
  // Check version supported
  if(version > maxversion) return false;
  initialized = true;
  return true;
}

Relay::Relay(uint8_t address) {
  this->address = address;
  type = 'R';
  maxversion = 100;
}

bool Relay::initialize() {
  uint8_t command[] = {0x21};
  uint8_t response[5];
  if(Module::initialize()) {
    if(send(command, 1)) {
      if(recv(response, 5)) {
        powerupState = response[0];
        pwmPeriod = response[1];
        commTimeout = (response[2] << 8) | response[3];
		responseMode = response[4];
        return true;
      }
    }
  }
  initialized = false;
  return false;
}

void Relay::digitalWrite(uint8_t value) {
  uint8_t command[] = {0x01, value ? 0x01 : 0x00};
  send(command, 2);
  pwm = false;
}

void Relay::analogWrite(uint8_t value) {
  uint8_t command[2];
  if(!pwm) {
    command[0] = 0x01;
    command[1] = 0x80;
    send(command, 2);
  }
  command[0] = 0x03;
  command[1] = value;
  send(command, 2);
  pwm = true;
}

bool Relay::setConfiguration(uint8_t powerupState, uint8_t pwmPeriod, word commTimeout, byte responseMode) {
  uint8_t command[] = {0x20, powerupState, pwmPeriod, commTimeout >> 8, commTimeout, responseMode};
  if(powerupState != ENGAGED && powerupState != DISENGAGED && powerupState != PWM) return false;
  if(pwmPeriod < 1 || pwmPeriod > 240) return false;
  if(responseMode != 0x80 && (responseMode & 0xFE) != 0x00) return false;
  return send(command, 6);
}

Mosfet::Mosfet(uint8_t address) {
  this->address = address;
  type = 'M';
  maxversion = 100;
}

bool Mosfet::initialize() {
  uint8_t command[] = {0x21};
  uint8_t response[10];
  if(Module::initialize()) {
    if(send(command, 1)) {
      if(recv(response, 10)) {
        powerupState = response[0];
        pwmFrequency = response[1];
        commTimeout = (response[2] << 8) | response[3];
		alarmMode = response[4];
		alarmLevel = (response[5] << 8) | response[6];
		alarmDelay = (response[7] << 8) | response[8];
		responseMode = response[9];
        return true;
      }
    }
  }
  initialized = false;
  return false;
}

void Mosfet::digitalWrite(uint8_t value) {
  uint8_t command[] = {0x03, value ? 0x01 : 0x00};
  send(command, 2);
  pwm = false;
}

void Mosfet::analogWrite(uint8_t value) {
  uint8_t command[2];
  if(!pwm) {
    command[0] = 0x03;
    command[1] = 0x80;
    send(command, 2);
  }
  command[0] = 0x05;
  command[1] = value;
  send(command, 2);
  pwm = true;
}

bool Mosfet::setConfiguration(uint8_t powerupState, uint8_t pwmFrequency, word commTimeout, byte alarmMode, word alarmLevel, word alarmDelay, byte responseMode) {
  uint8_t command[] = {0x20, powerupState, pwmFrequency, commTimeout >> 8, commTimeout, alarmMode, alarmLevel >> 8, alarmLevel, alarmDelay >> 8, alarmDelay, responseMode};
  if(powerupState != ENGAGED && powerupState != DISENGAGED && powerupState != PWM) return false;
  if(pwmFrequency < 1 || pwmFrequency > 250) return false;
  if(responseMode != 0x80 && (responseMode & 0xFE) != 0x00) return false;
  if(alarmMode > 0x02) return false;
  return send(command, 11);
}




Consumption::Consumption(uint8_t address) {
  this->address = address;
  type = 'C';
  maxversion = 100;
}

bool Consumption::initialize() {
  uint8_t command[] = {0x21};
  uint8_t response[8];
  if(Module::initialize()) {
    if(send(command, 1)) {
      if(recv(response, 8)) {
        ampRange = response[0];
		commTimeout = (response[1] << 8) | response[2];
		alarmMode = response[3];
		alarmLevel = (response[4] << 8) | response[5];
		alarmDelay = (response[6] << 8) | response[7];
        return true;
      }
    }
  }
  initialized = false;
  return false;
}

word Consumption::analogRead() {
  uint8_t command[] = {0x01};
  uint8_t response[2];
  send(command, 1);
  recv(response, 2);
  return (response[0] << 8) | response[1];
}

bool Consumption::setConfiguration(uint8_t range, word commTimeout, byte alarmMode, word alarmLevel, word alarmDelay) {
  uint8_t command[] = {0x20, range, commTimeout >> 8, commTimeout, alarmMode, alarmLevel >> 8, alarmLevel, alarmDelay >> 8, alarmDelay};
  if(range < 1 || range > 50) return false;
  if(alarmMode > 0x02) return false;
  return send(command, 9);
}

Input::Input(uint8_t address) {
  this->address = address;
  type = 'I';
  maxversion = 100;
}

bool Input::initialize() {
  uint8_t command[] = {0x21};
  uint8_t response[11];
  if(Module::initialize()) {
    if(send(command, 1)) {
      if(recv(response, 11)) {
        mode = response[0];
        //bias = ((double)((response[1] << 8) | response[2])) / 310.0;
        //gain = 1 << response[3];
		commTimeout = (response[4] << 8) | response[5];
		alarmMode = response[6];
		alarmLevel = (response[7] << 8) | response[8];
		alarmDelay = (response[9] << 8) | response[10];
        command[0] = 0x04;
        if(send(command, 1)) {
          if(recv(response, 2)) {
            bias = ((double)((response[0] << 8) | response[1])) / 310.0;
            command[0] = 0x05;
            if(send(command, 1)) {
              if(recv(response, 1)) {
                gain = 1 << response[0];
                return true;
              }
            }
          }
        }
      }
    }
  }
  initialized = false;
  return false;
}

bool Input::setBias(double bias) {
  uint8_t command[] = {0x03, 0, 0};
  word nbias = bias * 310.0;
  command[1] = nbias >> 8;
  command[2] = nbias & 0xFF;    
  this->bias = ((double)nbias) / 310.0;
  return send(command, 3);
}

bool Input::setGain(word gain) {
  uint8_t command[] = {0x05, 1};
  word ngain = gain;
  while(ngain >>= 1) command[1]++;
  if((1 << command[1]) != gain) return false;
  if(send(command, 2)) {
    this->gain = gain;
    return true;
  }
  return false;
}

bool Input::setConfiguration(uint8_t mode, double bias, word gain, word commTimeout, byte alarmMode, word alarmLevel, word alarmDelay) {
  uint8_t command[] = {0x20, mode, 0, 0, 1, commTimeout >> 8, commTimeout, alarmMode, alarmLevel >> 8, alarmLevel, alarmDelay >> 8, alarmDelay};
  if(mode != RESISTANCE && (((mode & 0xF0) > 0x20) || ((mode & 0x0F > 0x02)))) return false;
  if(bias < 0.0 || bias > 3.3) return false;
  if(alarmMode > 0x02) return false;
  word ngain = gain;
  while(ngain >>= 1) command[4]++;
  if((1 << command[4]) != gain) return false;
  word nbias = bias * 310.0;
  command[2] = nbias >> 8;
  command[3] = nbias & 0xFF;
  if(send(command, 12)) {
    this->mode = mode;
    this->bias = ((double)nbias) / 310.0;
    this->gain = gain;
    return true;
  }
  return false; 
}

double Input::analogRead() {
  uint8_t command[] = {0x01};
  uint8_t response[2];
  send(command, 1);
  recv(response, 2);
  word readout = (response[0] << 8) | response[1];
  double calc = ((((double)readout) / 310.0) / ((double)gain)) + bias;
  if(mode == ANALOG12V) calc *= 6.0;
  else if(mode == ANALOG24V) calc *= 11.0;
  else if(mode == RESISTANCE) calc = -(calc - 3.3) / calc * 10000.0;
  return calc;
}

bool Input::digitalRead() {
  uint8_t command[] = {0x02};
  uint8_t response[1];
  send(command, 1);
  recv(response, 1);
  return response[0] == 0x81;
}

#if defined(__AVR_ATmega2560__) // MOTHERBOARD
#define PIN_RS485 30
#define PIN_FAST  31
#define PIN_HDPLX 32
#define PIN_SHDN  33
#define PIN_CTS   34
#define PIN_RTS   35
bool Max3160::begin(unsigned long baudrate, uint8_t format, byte mode, boolean srl) {
  MaxOnMoBo.begin(baudrate, format);
  pinMode(PIN_SHDN,  OUTPUT);
  pinMode(PIN_FAST,  OUTPUT);
  pinMode(PIN_RS485, OUTPUT);
  pinMode(PIN_HDPLX, OUTPUT);
  pinMode(PIN_RTS,   OUTPUT);
  pinMode(PIN_CTS,   INPUT);
  switch(mode) {
    case SERIAL_RS232:
      digitalWrite(PIN_RS485, LOW );
      digitalWrite(PIN_HDPLX, LOW );
      break;
    case SERIAL_RS485_FD:
      digitalWrite(PIN_RS485, HIGH);
      digitalWrite(PIN_HDPLX,  LOW);
      digitalWrite(PIN_RTS,   HIGH);
      break;
    case SERIAL_RS485_HD:
      digitalWrite(PIN_RS485, HIGH);
      digitalWrite(PIN_HDPLX, HIGH);
      digitalWrite(PIN_RTS,   LOW );
      break;
  }
  digitalWrite(PIN_SHDN, HIGH);
  digitalWrite(PIN_FAST, !srl);
  return true;
}
void Max3160::setRTS(boolean rts) {
	digitalWrite(PIN_RTS, rts);
}
Max3160 SerialPort;
#elif defined(__AVR_ATmega32U4__) // SHIELD
bool Max3160::begin(unsigned long baudrate, uint8_t format, byte mode, boolean srl) {
  uint8_t data[6] = { 0x01 };
  data[1] = (baudrate >> 8) & 0xFF;
  data[2] = baudrate & 0xFF;

  switch(format) {
    case SERIAL_5N1: data[3] = 0x00; break;
    case SERIAL_6N1: data[3] = 0x02; break;
    case SERIAL_7N1: data[3] = 0x04; break;
    case SERIAL_5N2: data[3] = 0x08; break;
    case SERIAL_6N2: data[3] = 0x0A; break;
    case SERIAL_7N2: data[3] = 0x0C; break;
    case SERIAL_8N2: data[3] = 0x0E; break;
    case SERIAL_5E1: data[3] = 0x20; break;
    case SERIAL_6E1: data[3] = 0x22; break;
    case SERIAL_7E1: data[3] = 0x24; break;
    case SERIAL_8E1: data[3] = 0x26; break;
    case SERIAL_5E2: data[3] = 0x28; break;
    case SERIAL_6E2: data[3] = 0x2A; break;
    case SERIAL_7E2: data[3] = 0x2C; break;
    case SERIAL_8E2: data[3] = 0x2E; break;
    case SERIAL_5O1: data[3] = 0x30; break;
    case SERIAL_6O1: data[3] = 0x32; break;
    case SERIAL_7O1: data[3] = 0x34; break;
    case SERIAL_8O1: data[3] = 0x36; break;
    case SERIAL_5O2: data[3] = 0x38; break;
    case SERIAL_6O2: data[3] = 0x3A; break;
    case SERIAL_7O2: data[3] = 0x3C; break;
    case SERIAL_8O2: data[3] = 0x3E; break;
    default:         data[3] = 0x06; break;
  }
  switch(mode) {
	case SERIAL_RS485_FD: data[4] = 0x02; break;
	case SERIAL_RS485_HD: data[4] = 0x03; break;
	default             : data[4] = 0x00; break;
  }
  data[5] = srl;
  Wire.begin();
  Wire.beginTransmission(0x01);
  Wire.write(data, 6);
  return Wire.endTransmission() == 0;
}
int Max3160::peek(void) {
}
int Max3160::read(void) {
}
int Max3160::available(void) {
}
size_t Max3160::write(uint8_t n) {
  uint8_t data[2] = {0x02, n};
  Wire.begin();
  Wire.beginTransmission(0x01);
  Wire.write(data, 2);
  Wire.endTransmission();
}
void Max3160::setRTS(boolean rts) {
  uint8_t data[2] = {0x03, rts};
  Wire.begin();
  Wire.beginTransmission(0x01);
  Wire.write(data, 2);
  Wire.endTransmission();
}
Max3160 SerialPort;
#endif


void WireSEI() {
	TWCR &= ~_BV(TWIE);
	sei();
}

void WireCLI() {
	cli();
	TWCR |= _BV(TWIE);
}