/*
   * Copyright 2007-2008 (c) Erik Sjodin, eriksjodin.net
   *
   * Permission is hereby granted, free of charge, to any person
   * obtaining a copy of this software and associated documentation
   * files (the "Software"), to deal in the Software without
   * restriction, including without limitation the rights to use,
   * copy, modify, merge, publish, distribute, sublicense, and/or sell
   * copies of the Software, and to permit persons to whom the
   * Software is furnished to do so, subject to the following
   * conditions:
   *
   * The above copyright notice and this permission notice shall be
   * included in all copies or substantial _portions of the Software.
   *
   * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
   * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
   * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
   * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
   * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
   * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
   * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
   * OTHER DEALINGS IN THE SOFTWARE.
   */
  
  include <ofStandardFirmata.h>
  
  // TODO thread it?
  // TODO throw event or exception if the serial port goes down...
  //---------------------------------------------------------------------------
  ofStandardFirmata::ofStandardFirmata(){
          _portStatus=-1;
          _waitForData=0;
          _analogHistoryLength = 2;
          _digitalHistoryLength = 2;
          _stringHistoryLength = 1;
          _sysExHistoryLength = 1;
  
          _majorProtocolVersion = 0;
          _minorProtocolVersion = 0;
          _majorFirmwareVersion = 0;
          _minorFirmwareVersion = 0;
          _firmwareName = "Unknown";
  
      // ports
          for(int i=0; i<ARD_TOTAL_PORTS; ++i) {
                  _digitalPortValue[i]=0;
                  _digitalPortReporting[i] = ARD_OFF;
          }
  
      // digital pins
          for(int i=0; i<16; ++i) {
                  _digitalPinValue[i] = -1;
                  _digitalPinMode[i] = ARD_OUTPUT;
                  _digitalPinReporting[i] = ARD_OFF;
          }
  
          // analog in pins
          for (int i=0; i<ARD_TOTAL_ANALOG_PINS; ++i) {
                  _analogPinReporting[i] = ARD_OFF;
                  // analog pins used as digital
                  _digitalPinMode[i]=ARD_ANALOG;
                  _digitalPinValue[i] = -1;
          }
  }
  
  ofStandardFirmata::~ofStandardFirmata() {
          _port.close();
  }
  
  int ofStandardFirmata::connect(string device, int baud){
          _initialized = false;
          _port.enumerateDevices();
          return _port.setup(device.c_str(), baud);
  }
  
  void ofStandardFirmata::setDigitalHistoryLength(int length){
          if(length>=2)
                  _digitalHistoryLength=length;
  }
  
  void ofStandardFirmata::setAnalogHistoryLength(int length){
          if(length>=2)
                  _analogHistoryLength=length;
  }
  
  void ofStandardFirmata::setSysExHistoryLength(int length){
          if(length>=1)
                  _sysExHistoryLength=length;
  }
  
  void ofStandardFirmata::setStringHistoryLength(int length){
          if(length>=1)
                  _stringHistoryLength=length;
  }
  
  void ofStandardFirmata::disconnect(){
          _port.close();
  }
  
  void ofStandardFirmata::update(){
          int dataRead=0;
          // try to empty the _port buffer
          while (dataRead<512) {
  
                  int byte = _port.readByte();
  
                  // process data....
                  if (byte!=-1) {
                          processData((char)(byte));
                          dataRead++;
                  }
                  // _port buffer is empty
                  else{
                          break;
                  }
          }
  }
  
  int ofStandardFirmata::getAnalog(int pin){
          if(_analogHistory[pin].size()>0)
                  return _analogHistory[pin].front();
          else
                  return -1;
  }
  
  int ofStandardFirmata::getDigital(int pin){
          if(_digitalPinMode[pin]==ARD_INPUT && _digitalHistory[pin].size()>0)
                  return _digitalHistory[pin].front();
          else if (_digitalPinMode[pin]==ARD_OUTPUT)
                  return _digitalPinValue[pin];
          else
                  return -1;
  }
  
  int ofStandardFirmata::getPwm(int pin){
          if(_digitalPinMode[pin]==ARD_PWM)
                  return _digitalPinValue[pin];
          else
                  return -1;
  }
  
  vector<unsigned char> ofStandardFirmata::getSysEx(){
          return _sysExHistory.front();
  }
  
  string ofStandardFirmata::getString(){
          return _stringHistory.front();
  }
  
  int ofStandardFirmata::getDigitalPinMode(int pin){
          return _digitalPinMode[pin];
  }
  
  void ofStandardFirmata::sendDigital(int pin, int value, bool force){
          if((_digitalPinMode[pin]==ARD_INPUT || _digitalPinMode[pin]==ARD_OUTPUT) && (_digitalPinValue[pin]!=value || force)){
  
                  _digitalPinValue[pin] = value;
  
                  int port=0;
                  int bit=0;
  
                  if(pin < 8 && pin >1){
                          port=0;
                          bit = pin;
                  }
                  else if(pin>7 && pin <14){
                          port = 1;
                          bit = pin-8;
                  }
                  else if(pin>15 && pin <22){
                          port = 2;
                          bit = pin-16;
                  }
  
                  // set the bit
                  if(value==1)
                          _digitalPortValue[port] |= (1 << bit);
  
                  // clear the bit
                  if(value==0)
                          _digitalPortValue[port] &= ~(1 << bit);
  
                  sendByte(FIRMATA_DIGITAL_MESSAGE+port);
                  sendValueAsTwo7bitBytes(_digitalPortValue[port]);
  
          }
  }
  
  void ofStandardFirmata::sendPwm(int pin, int value, bool force){
          if(_digitalPinMode[pin]==ARD_PWM && (_digitalPinValue[pin]!=value || force)){
                  sendByte(FIRMATA_ANALOG_MESSAGE+pin);
                  sendValueAsTwo7bitBytes(value);
                  _digitalPinValue[pin] = value;
          }
  }
  
  void ofStandardFirmata::sendSysEx(int command, vector<unsigned char> data){
          sendByte(FIRMATA_START_SYSEX);
          sendByte(command);
          vector<unsigned char>::iterator it = data.begin();
          while( it != data.end() ) {
                  sendByte(*it);
                  it++;
          }
          sendByte(FIRMATA_END_SYSEX);
  }
  
  void ofStandardFirmata::sendSysExBegin(){
          sendByte(FIRMATA_START_SYSEX);
  }
  
  void ofStandardFirmata::sendSysExEnd(){
          sendByte(FIRMATA_END_SYSEX);
  }
  
  void ofStandardFirmata::sendString(string str){
          sendByte(FIRMATA_START_SYSEX);
          sendByte(FIRMATA_SYSEX_FIRMATA_STRING);
          string::iterator it = str.begin();
          while( it != str.end() ) {
                  sendValueAsTwo7bitBytes(*it);
                  it++;
          }
          sendByte(FIRMATA_END_SYSEX);
  }
  
  void ofStandardFirmata::sendProtocolVersionRequest(){
          sendByte(FIRMATA_REPORT_VERSION);
  }
  
  void ofStandardFirmata::sendFirmwareVersionRequest(){
          sendByte(FIRMATA_START_SYSEX);
          sendByte(FIRMATA_SYSEX_REPORT_FIRMWARE);
          sendByte(FIRMATA_END_SYSEX);
  }
  
  void ofStandardFirmata::sendReset(){
          sendByte(FIRMATA_SYSTEM_RESET);
  }
  
  void ofStandardFirmata::sendAnalogPinReporting(int pin, int mode){
          int i;
          //bool send;
  
      // disable reporting for all pins on port 2
          for(i=16; i<22; ++i) {
                  if(_digitalPinReporting[i]==ARD_ON)
                          sendDigitalPinReporting(i, ARD_OFF);
          }
  
          _digitalPinMode[16+pin]=ARD_ANALOG;
  
          sendByte(FIRMATA_REPORT_ANALOG+pin);
          sendByte(mode);
          _analogPinReporting[pin] = mode;
  }
  
  void ofStandardFirmata::sendDigitalPinMode(int pin, int mode){
          sendByte(FIRMATA_SET_PIN_MODE);
          sendByte(pin); // Tx pins 0-6
          sendByte(mode);
          _digitalPinMode[pin]=mode;
  
          // turn on or off reporting on the port
          if(mode==ARD_INPUT){
                  sendDigitalPinReporting(pin, ARD_ON);
          }
          else {
                  sendDigitalPinReporting(pin, ARD_OFF);
          }
  }
  
  int ofStandardFirmata::getAnalogPinReporting(int pin){
          return _analogPinReporting[pin];
  }
  
  list<int>* ofStandardFirmata::getAnalogHistory(int pin){
          return &_analogHistory[pin];
  }
  
  list<int>* ofStandardFirmata::getDigitalHistory(int pin){
          return &_digitalHistory[pin];
  }
  
  list<vector<unsigned char> >* ofStandardFirmata::getSysExHistory(){
          return &_sysExHistory;
  }
  
  list<string>* ofStandardFirmata::getStringHistory(){
          return &_stringHistory;
  }
  
  int ofStandardFirmata::getMajorProtocolVersion(){
          return _majorFirmwareVersion;
  }
  
  int ofStandardFirmata::getMinorProtocolVersion(){
          return _minorFirmwareVersion;
  }
  
  int ofStandardFirmata::getMajorFirmwareVersion(){
          return _majorFirmwareVersion;
  }
  
  int ofStandardFirmata::getMinorFirmwareVersion(){
          return _minorFirmwareVersion;
  }
  
  string ofStandardFirmata::getFirmwareName(){
          return _firmwareName;
  }
  
  bool ofStandardFirmata::isInitialized(){
          return _initialized;
  }
  
  // ------------------------------ private functions
  
  void ofStandardFirmata::processData(unsigned char inputData){
  
          char msg[100];
          sprintf(msg, "Received Byte: \%i", inputData);
          //Logger::get("Application").information(msg);
  
          // we have command data
          if(_waitForData>0 && inputData<128) {
                  _waitForData--;
  
                  // collect the data
                  _storedInputData [waitForData] = inputData;
  
                  // we have all data executeMultiByteCommand
                  if(_waitForData==0) {
                          switch (_executeMultiByteCommand) {
                                  case FIRMATA_DIGITAL_MESSAGE:
                                          processDigitalPort(_multiByteChannel, (_storedInputData[0] << 7) | _storedInputData[1]);
                                  break;
                                  case FIRMATA_REPORT_VERSION: // report version
                                          _majorProtocolVersion = _storedInputData[1];
                                          _minorProtocolVersion = _storedInputData[0];
                                          ofNotifyEvent(EProtocolVersionReceived, _majorProtocolVersion, this);
                                  break;
                                  case FIRMATA_ANALOG_MESSAGE:
                                          int previous = _analogHistory [multiByteChannel].front();
  
                                          _analogHistory [multiByteChannel].push_front((_storedInputData[0] << 7) | _storedInputData[1]);
                                          if((int)_analogHistory [multiByteChannel].size()>_analogHistoryLength)
                                                  _analogHistory [multiByteChannel].pop_back();
  
                                          // trigger an event if the pin has changed value
                                          if(_analogHistory [multiByteChannel].front()!=previous)
                                                  ofNotifyEvent(EAnalogPinChanged, _multiByteChannel, this);
                                  break;
                          }
  
                  }
          }
          // we have SysEx command data
          else if(_waitForData<0){
                  // we have all sysex data
                  if(inputData==FIRMATA_END_SYSEX){
                          _waitForData=0;
                          processSysExData(_sysExData);
                          _sysExData.clear();
                  }
                  // still have data, collect it
                  else {
                          _sysExData.push_back((unsigned char)inputData);
                  }
          }
          // we have a command
          else{
  
                  int command;
  
                  // extract the command and channel info from a byte if it is less than 0xF0
                  if(inputData < 0xF0) {
                    command = inputData & 0xF0;
                    _multiByteChannel = inputData & 0x0F;
                  }
                  else {
                    // commands in the 0xF* range don't use channel data
                    command = inputData;
                  }
  
                  switch (command) {
                          case FIRMATA_REPORT_VERSION:
                          case FIRMATA_DIGITAL_MESSAGE:
                          case FIRMATA_ANALOG_MESSAGE:
                                  _waitForData = 2;  // 2 bytes needed
                                  _executeMultiByteCommand = command;
                          break;
                          case FIRMATA_START_SYSEX:
                                  _sysExData.clear();
                                  _waitForData = -1;  // n bytes needed, -1 is used to indicate sysex message
                                  _executeMultiByteCommand = command;
                          break;
                  }
  
          }
  }
  
  // sysex data is assumed to be 8-bit bytes split into two 7-bit bytes.
  void ofStandardFirmata::processSysExData(vector<unsigned char> data){
  
          string str;
  
          vector<unsigned char>::iterator it;
          unsigned char buffer;
          //int i = 1;
  
          // act on reserved sysEx messages (extended commands) or trigger SysEx event...
          switch(data.front()) { //first byte in buffer is command
                  case FIRMATA_SYSEX_REPORT_FIRMWARE:
                          it = data.begin();
                          it++; // skip the first byte, which is the firmware version command
                          _majorFirmwareVersion = *it;
                          it++;
                          _minorFirmwareVersion = *it;
                          it++;
  
                          while( it != data.end() ) {
                                          buffer = *it;
                                          it++;
                                          buffer += *it << 7;
                                          it++;
                                          str+=buffer;
                          }
                          _firmwareName = str;
  
                          ofNotifyEvent(EFirmwareVersionReceived, _majorFirmwareVersion, this);
  
                          // trigger the initialization event
                          ofNotifyEvent(EInitialized, _majorFirmwareVersion, this);
                          _initialized = true;
  
                  break;
                  case FIRMATA_SYSEX_FIRMATA_STRING:
                          it = data.begin();
                          it++; // skip the first byte, which is the string command
                          while( it != data.end() ) {
                                          buffer = *it;
                                          it++;
                                          buffer += *it << 7;
                                          it++;
                                          str+=buffer;
                          }
  
                          _stringHistory.push_front(str);
                          if((int)_stringHistory.size()>_stringHistoryLength)
                                          _stringHistory.pop_back();
  
                          ofNotifyEvent(EStringReceived, str, this);
                  break;
                  default: // the message isn't in Firmatas extended command set
                          _sysExHistory.push_front(data);
                          if((int)_sysExHistory.size()>_sysExHistoryLength)
                                          _sysExHistory.pop_back();
                          ofNotifyEvent(ESysExReceived, data, this);
                  break;
  
          }
  }
  
  void ofStandardFirmata::processDigitalPort(int port, unsigned char value){
  
          unsigned char mask;
          int previous;
          int i;
          int pin;
          switch(port) {
                  case 0: // pins 2-7  (0,1 are ignored as serial RX/TX)
                          for(i=2; i<8; ++i) {
                                  pin = i;
                                  if(_digitalPinMode[pin]==ARD_INPUT){
                                          previous = _digitalHistory[pin].front();
  
                                          mask = 1 << i;
                                          _digitalHistory[pin].push_front((value & mask)>>i);
  
                                          if((int)_digitalHistory[pin].size()>_digitalHistoryLength)
                                                          _digitalHistory[pin].pop_back();
  
                                          // trigger an event if the pin has changed value
                                          if(_digitalHistory[pin].front()!=previous){
                                                  ofNotifyEvent(EDigitalPinChanged, pin, this);
                                          }
                                  }
                          }
          break;
                  case 1: // pins 8-13 (14,15 are disabled for the crystal)
                   for(i=0; i<6; ++i) {
                          pin = i+8;
  
                          if(_digitalPinMode[pin]==ARD_INPUT){
                                  previous = _digitalHistory[pin].front();
  
                                  mask = 1 << i;
                                  _digitalHistory[pin].push_front((value & mask)>>i);
  
                                  if((int)_digitalHistory[pin].size()>_digitalHistoryLength)
                                          _digitalHistory[pin].pop_back();
  
                                  // trigger an event if the pin has changed value
                                  if(_digitalHistory[pin].front()!=previous){
                                          ofNotifyEvent(EDigitalPinChanged, pin, this);
                                  }
                          }
                   }
          break;
          case 2: // analog pins used as digital pins 16-21
                  for(i=0; i<6; ++i) {
                          pin = i+16;
                          if(_digitalPinMode[pin]==ARD_INPUT){
                                  previous = _digitalHistory[pin].front();
  
                                  mask = 1 << i;
                                  _digitalHistory[pin].push_front((value & mask)>>i);
  
                                  if((int)_digitalHistory[pin].size()>_digitalHistoryLength)
                                          _digitalHistory[pin].pop_back();
  
                                  // trigger an event if the pin has changed value
                                  if(_digitalHistory[pin].front()!=previous){
                                          ofNotifyEvent(EDigitalPinChanged, pin, this);
                                  }
                          }
                  }
          break;
          }
  }
  
  // port 0: pins 2-7  (0,1 are serial RX/TX, don't change their values)
  // port 1: pins 8-13 (14,15 are disabled for the crystal)
  // port 2: pins 16-21 analog pins used as digital, all analog reporting will be turned off if this is set to ARD_ON
  void ofStandardFirmata::sendDigitalPortReporting(int port, int mode){
          sendByte(FIRMATA_REPORT_DIGITAL+port);
          sendByte(mode);
          _digitalPortReporting[port] = mode;
          if(port==2 && mode==ARD_ON){ // if reporting is turned on on port 2 then ofStandardFirmata on the Arduino disables all analog reporting
  
                  for (int i=0; i<ARD_TOTAL_ANALOG_PINS; i++) {
                                  _analogPinReporting[i] = ARD_OFF;
                  }
          }
  }
  
  void ofStandardFirmata::sendDigitalPinReporting(int pin, int mode){
          _digitalPinReporting[pin] = mode;
          if(mode==ARD_ON){        // enable reporting for the port
                  if(pin<=7 && pin>=2)
                          sendDigitalPortReporting(0, ARD_ON);
                  if(pin<=13 && pin>=8)
                          sendDigitalPortReporting(1, ARD_ON);
                  if(pin<=21 && pin>=16)
                          sendDigitalPortReporting(2, ARD_ON);
          }
          else if(mode==ARD_OFF){
                  int i;
                  bool send=true;
                  if(pin<8 && pin>=2){    // check if all pins on the port are off, if so set port reporting to off..
                          for(i=2; i<8; ++i) {
                                  if(_digitalPinReporting[i]==ARD_ON)
                                                  send=false;
                          }
                          if(send)
                                  sendDigitalPortReporting(0, ARD_OFF);
                  }
                  if(pin<14 && pin>=8){
                          for(i=8; i<14; ++i) {
                                  if(_digitalPinReporting[i]==ARD_ON)
                                                  send=false;
                          }
                          if(send)
                                  sendDigitalPortReporting(1, ARD_OFF);
                  }
                  if(pin<22 && pin>=16){
                          for(i=16; i<22; ++i) {
                                  if(_digitalPinReporting[i]==ARD_ON)
                                                  send=false;
                          }
                          if(send)
                                  sendDigitalPortReporting(2, ARD_OFF);
                  }
  
          }
  }
  
  void ofStandardFirmata::sendByte(unsigned char byte){
          //char msg[100];
          //sprintf(msg, "Sending Byte: \%i", byte);
          //Logger::get("Application").information(msg);
          _port.writeByte(byte);
  }
  
  // in Firmata (and MIDI) data bytes are 7-bits. The 8th bit serves as a flag to mark a byte as either command or data.
  // therefore you need two data bytes to send 8-bits (a char).
  void ofStandardFirmata::sendValueAsTwo7bitBytes(int value)
  {
          sendByte(value & 127); // LSB
          sendByte(value >> 7 & 127); // MSB
  }
  
  // SysEx data is sent as 8-bit bytes split into two 7-bit bytes, this function merges two 7-bit bytes back into one 8-bit byte.
  int ofStandardFirmata::getValueFromTwo7bitBytes(unsigned char lsb, unsigned char msb){
     return (msb << 7) | lsb;
  }