/* Copyright (C) 2006 Hans-Christoph Steiner 
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library 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
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General
 * Public License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place, Suite 330,
 * Boston, MA  02111-1307  USA
 *
 * -----------------------------
 * Firmata, the Arduino firmware
 * -----------------------------
 * 
 * Firmata turns the Arduino into a Plug-n-Play sensorbox, servo
 * controller, and/or PWM motor/lamp controller.
 *
 * It was originally designed to work with the Pd object [arduino]
 * which is included in Pd-extended.  This firmware is intended to
 * work with any host computer software package.  It can easily be
 * used with other programs like Max/MSP, Processing, or whatever can
 * do serial communications.
 *
 * @authors: Hans-Christoph Steiner <hans@at.or.at>
 *   help with protocol redesign: Jamie Allen <jamie@heavyside.net>
 *   key bugfixes: Georg Holzmann <grh@mur.at>
 *                 Gerda Strobl <gerda.strobl@student.tugraz.at>
 * @date: 2006-05-19
 * @locations: STEIM, Amsterdam, Netherlands
 *             IDMI/Polytechnic University, Brookyn, NY, USA
 *             Electrolobby Ars Electronica, Linz, Austria
 */

/* 
 * TODO: add pulseOut functionality
 * TODO: add software PWM for servos, etc (servo.h or pulse.h)
 * TODO: redesign protocol to accomodate boards with more I/Os
 * TODO: 
 * TODO: add "pinMode all 0/1" command
 * TODO: add cycle markers to mark start of analog, digital, pulseIn, and PWM
 */

/* firmware version numbers.  The protocol is still changing, so these version
 * numbers are important */
#define MAJOR_VERSION 0
#define MINOR_VERSION 1

/* firmata protocol
 * =============== 
 * data: 0-127   
 * control: 128-255
 */
  
/* computer<->Arduino commands
 * -------------------- */
/* 128-129 // UNUSED */
#define SET_PIN_ZERO_TO_IN      130 // set digital pin 0 to INPUT
#define SET_PIN_ONE_TO_IN       131 // set digital pin 1 to INPUT
#define SET_PIN_TWO_TO_IN       132 // set digital pin 2 to INPUT
#define SET_PIN_THREE_TO_IN     133 // set digital pin 3 to INPUT
#define SET_PIN_FOUR_TO_IN      134 // set digital pin 4 to INPUT
#define SET_PIN_FIVE_TO_IN      135 // set digital pin 5 to INPUT
#define SET_PIN_SIX_TO_IN       136 // set digital pin 6 to INPUT
#define SET_PIN_SEVEN_TO_IN     137 // set digital pin 7 to INPUT
#define SET_PIN_EIGHT_TO_IN     138 // set digital pin 8 to INPUT
#define SET_PIN_NINE_TO_IN      139 // set digital pin 9 to INPUT
#define SET_PIN_TEN_TO_IN       140 // set digital pin 10 to INPUT
#define SET_PIN_ELEVEN_TO_IN    141 // set digital pin 11 to INPUT
#define SET_PIN_TWELVE_TO_IN    142 // set digital pin 12 to INPUT
#define SET_PIN_THIRTEEN_TO_IN  143 // set digital pin 13 to INPUT
/* 144-149 // UNUSED */
#define DISABLE_DIGITAL_INPUTS  150 // disable reporting of digital inputs
#define ENABLE_DIGITAL_INPUTS   151 // enable reporting of digital inputs
/* 152-159 // UNUSED */
#define ZERO_ANALOG_INS         160 // disable reporting on all analog ins
#define ONE_ANALOG_IN           161 // enable reporting for 1 analog in (0)
#define TWO_ANALOG_INS          162 // enable reporting for 2 analog ins (0,1)
#define THREE_ANALOG_INS        163 // enable reporting for 3 analog ins (0-2)
#define FOUR_ANALOG_INS         164 // enable reporting for 4 analog ins (0-3)
#define FIVE_ANALOG_INS         165 // enable reporting for 5 analog ins (0-4)
#define SIX_ANALOG_INS          166 // enable reporting for 6 analog ins (0-5)
#define SEVEN_ANALOG_INS        167 // enable reporting for 6 analog ins (0-6)
#define EIGHT_ANALOG_INS        168 // enable reporting for 6 analog ins (0-7)
#define NINE_ANALOG_INS         169 // enable reporting for 6 analog ins (0-8)
/* 167-199 // UNUSED */
#define SET_PIN_ZERO_TO_OUT     200 // set digital pin 0 to OUTPUT
#define SET_PIN_ONE_TO_OUT      201 // set digital pin 1 to OUTPUT
#define SET_PIN_TWO_TO_OUT      202 // set digital pin 2 to OUTPUT
#define SET_PIN_THREE_TO_OUT    203 // set digital pin 3 to OUTPUT
#define SET_PIN_FOUR_TO_OUT     204 // set digital pin 4 to OUTPUT
#define SET_PIN_FIVE_TO_OUT     205 // set digital pin 5 to OUTPUT
#define SET_PIN_SIX_TO_OUT      206 // set digital pin 6 to OUTPUT
#define SET_PIN_SEVEN_TO_OUT    207 // set digital pin 7 to OUTPUT
#define SET_PIN_EIGHT_TO_OUT    208 // set digital pin 8 to OUTPUT
#define SET_PIN_NINE_TO_OUT     209 // set digital pin 9 to OUTPUT
#define SET_PIN_TEN_TO_OUT      210 // set digital pin 10 to OUTPUT
#define SET_PIN_ELEVEN_TO_OUT   211 // set digital pin 11 to OUTPUT
#define SET_PIN_TWELVE_TO_OUT   212 // set digital pin 12 to OUTPUT
#define SET_PIN_THIRTEEN_TO_OUT 213 // set digital pin 13 to OUTPUT
/* 214-228 // UNUSED */
#define OUTPUT_TO_DIGITAL_PINS  229 // next two bytes set digital output data 
/* 230-239 // UNUSED */
#define REPORT_VERSION          240 // return the firmware version
/* 240-249 // UNUSED */
#define DISABLE_PWM             250 // next byte sets pin # to disable
#define ENABLE_PWM              251 // next two bytes set pin # and duty cycle
#define DISABLE_SOFTWARE_PWM    252 // next byte sets pin # to disable
#define ENABLE_SOFTWARE_PWM     253 // next two bytes set pin # and duty cycle
#define SET_SOFTWARE_PWM_FREQ   254 // set master frequency for software PWMs
/* 255 // UNUSED */

 
/* two byte digital output data format
 * ----------------------
 * 0  get ready for digital input bytes (229)
 * 1  digitalOut 7-13 bitmask 
 * 2  digitalOut 0-6 bitmask
 */

/* two byte PWM data format
 * ----------------------
 * 0  get ready for digital input bytes (ENABLE_SOFTWARE_PWM/ENABLE_PWM)
 * 1  pin #
 * 2  duty cycle expressed as 1 byte (255 = 100%)
 */

/* digital input message format
 * ----------------------
 * 0   digital input marker (255/11111111)
 * 1   digital read from Arduino // 7-13 bitmask
 * 2   digital read from Arduino // 0-6 bitmask
 */

/* analog input message format
 * ----------------------
 * 0   analog input marker (160 + number of pins to report)
 * 1   high byte from analog input pin 0 
 * 2   low byte from analog input pin 0
 * 3   high byte from analog input pin 1 
 * 4   low byte from analog input pin 1
 * 5   high byte from analog input pin 2 
 * 6   low byte from analog input pin 2
 * 7   high byte from analog input pin 3 
 * 8   low byte from analog input pin 3
 * 9   high byte from analog input pin 4 
 * 10  low byte from analog input pin 4
 * 11  high byte from analog input pin 5 
 * 12  low byte from analog input pin 5
 */

#define TOTAL_DIGITAL_PINS 14

// for comparing along with INPUT and OUTPUT
#define PWM 2

// maximum number of post-command data bytes
#define MAX_DATA_BYTES 2
// this flag says the next serial input will be data
byte waitForData = 0;
byte executeMultiByteCommand = 0; // command to execute after getting multi-byte data
byte storedInputData[MAX_DATA_BYTES] = {0,0}; // multi-byte data

// this flag says the first data byte for the digital outs is next
boolean firstInputByte = false;

/* this int serves as a bit-wise array to store pin status
 * 0 = INPUT, 1 = OUTPUT
 */
int digitalPinStatus;

/* this byte stores the status off whether PWM is on or not
 * bit 9 = PWM0, bit 10 = PWM1, bit 11 = PWM2
 * the rest of the bits are unused and should remain 0
 */
int pwmStatus;

/* this byte stores the status of whether software PWM is on or not */
/* 00000010 00000000 means bit 10 is softWarePWM enabled */
int softPwmStatus;

boolean digitalInputsEnabled = true;
byte analogInputsEnabled = 6;

byte analogPin;
int analogData;

// -------------------------------------------------------------------------
void transmitDigitalInput(byte startPin) {
  byte i;
  byte digitalPin;
//  byte digitalPinBit;
  byte transmitByte = 0;
  byte digitalData;

  for(i=0;i<7;++i) {
    digitalPin = i+startPin;
/*    digitalPinBit = OUTPUT << digitalPin;
// only read the pin if its set to input
if(digitalPinStatus & digitalPinBit) {
digitalData = 0; // pin set to OUTPUT, don't read
}
else if( (digitalPin >= 9) && (pwmStatus & (1 << digitalPin)) ) {
digitalData = 0; // pin set to PWM, don't read
}*/
    if( !(digitalPinStatus & (1 << digitalPin)) ) {
      digitalData = (byte) digitalRead(digitalPin);
      transmitByte = transmitByte + ((1 << i) * digitalData);
    }
  }
  printByte(transmitByte);
}



// -------------------------------------------------------------------------
/* this function sets the pin mode to the correct state and sets the relevant
 * bits in the two bit-arrays that track Digital I/O and PWM status
 */
void setPinMode(int pin, int mode) {
  if(mode == INPUT) {
    digitalPinStatus = digitalPinStatus &~ (1 << pin);
    pwmStatus = pwmStatus &~ (1 << pin);
    pinMode(pin,INPUT);
  }
  else if(mode == OUTPUT) {
    digitalPinStatus = digitalPinStatus | (1 << pin);
    pwmStatus = pwmStatus &~ (1 << pin);
    pinMode(pin,OUTPUT);
  }
  else if( (mode == PWM) && (pin >= 9) && (pin <= 11) ) {
    digitalPinStatus = digitalPinStatus | (1 << pin);
    pwmStatus = pwmStatus | (1 << pin);
    pinMode(pin,OUTPUT);
  }
}

void setSoftPwm (int pin, byte pulsePeriod) {
  byte i;
  /*    for(i=0; i<7; ++i) {
      mask = 1 << i;
      if(digitalPinStatus & mask) {
      digitalWrite(i, inputData & mask);
      } 
      }
  */    
  //read timer type thing

  //loop through each pin, turn them on if selected
  //softwarePWMStatus
  //check timer type thing against pulsePeriods for each pin 
  //throw pin low if expired
}

void setSoftPwmFreq(byte freq) {
}


void disSoftPwm(int pin) {
  //throw pin low
    
}


/* -------------------------------------------------------------------------
 * this function checks to see if there is data waiting on the serial port 
 * then processes all of the stored data
 */
void checkForInput() {
  if(Serial.available()) {  
    while(Serial.available()) {
      processInput( (byte)Serial.read() );
    }
  }
}

/* -------------------------------------------------------------------------
 * processInput() is called whenever a byte is available on the
 * Arduino's serial port.  This is where the commands are handled.
 */
void processInput(byte inputData) {
  int i;
  int mask;
  
  // a few commands have byte(s) of data following the command
  if( waitForData > 0) {  
    waitForData--;
    storedInputData[waitForData] = inputData;

    if(executeMultiByteCommand && (waitForData==0)) {
      //we got everything
      switch(executeMultiByteCommand) {
      case ENABLE_PWM:
        setPinMode(storedInputData[1],PWM);
        analogWrite(storedInputData[1], storedInputData[0]);
        break;
      case DISABLE_PWM:
        setPinMode(storedInputData[0],INPUT);
        break;
      case ENABLE_SOFTWARE_PWM:
        setPinMode(storedInputData[1],PWM);
        setSoftPwm(storedInputData[1], storedInputData[0]);     
        break; 
      case DISABLE_SOFTWARE_PWM:
        disSoftPwm(storedInputData[0]);
        break;
      case SET_SOFTWARE_PWM_FREQ:
        setSoftPwmFreq(storedInputData[0]);
        break;
      }
      executeMultiByteCommand = 0;
    }
  }
  else if(inputData < 128) {
    if(firstInputByte) {
      // output data for pins 7-13
      for(i=7; i<TOTAL_DIGITAL_PINS; ++i) {
        mask = 1 << i;
        if( (digitalPinStatus & mask) && !(pwmStatus & mask) ) {
          // inputData is a byte and mask is an int, so align the high part of mask
          digitalWrite(i, inputData & (mask >> 7));
        }        
      }
      firstInputByte = false;
    }
    else { //
      for(i=0; i<7; ++i) {
        mask = 1 << i;
        if( (digitalPinStatus & mask) && !(pwmStatus & mask) ) {
          digitalWrite(i, inputData & mask);
        } 
      }
    }
  }
  else {
    switch (inputData) {
    case SET_PIN_ZERO_TO_IN:       // set digital pins to INPUT
    case SET_PIN_ONE_TO_IN:
    case SET_PIN_TWO_TO_IN:
    case SET_PIN_THREE_TO_IN:
    case SET_PIN_FOUR_TO_IN:
    case SET_PIN_FIVE_TO_IN:
    case SET_PIN_SIX_TO_IN:
    case SET_PIN_SEVEN_TO_IN:
    case SET_PIN_EIGHT_TO_IN:
    case SET_PIN_NINE_TO_IN:
    case SET_PIN_TEN_TO_IN:
    case SET_PIN_ELEVEN_TO_IN:
    case SET_PIN_TWELVE_TO_IN:
    case SET_PIN_THIRTEEN_TO_IN:
      setPinMode(inputData - SET_PIN_ZERO_TO_IN, INPUT);
      break;
    case SET_PIN_ZERO_TO_OUT:      // set digital pins to OUTPUT
    case SET_PIN_ONE_TO_OUT:
    case SET_PIN_TWO_TO_OUT:
    case SET_PIN_THREE_TO_OUT:
    case SET_PIN_FOUR_TO_OUT:
    case SET_PIN_FIVE_TO_OUT:
    case SET_PIN_SIX_TO_OUT:
    case SET_PIN_SEVEN_TO_OUT:
    case SET_PIN_EIGHT_TO_OUT:
    case SET_PIN_NINE_TO_OUT:
    case SET_PIN_TEN_TO_OUT:
    case SET_PIN_ELEVEN_TO_OUT:
    case SET_PIN_TWELVE_TO_OUT:
    case SET_PIN_THIRTEEN_TO_OUT:
      setPinMode(inputData - SET_PIN_ZERO_TO_OUT, OUTPUT);
      break;
    case DISABLE_DIGITAL_INPUTS:   // all digital inputs off
      digitalInputsEnabled = false;
      break;
    case ENABLE_DIGITAL_INPUTS:    // all digital inputs on
      digitalInputsEnabled = true;
      break;
    case ZERO_ANALOG_INS:   // analog input off
    case ONE_ANALOG_IN:     // analog 0 on  
    case TWO_ANALOG_INS:    // analog 0,1 on  
    case THREE_ANALOG_INS:  // analog 0-2 on  
    case FOUR_ANALOG_INS:   // analog 0-3 on  
    case FIVE_ANALOG_INS:   // analog 0-4 on  
    case SIX_ANALOG_INS:    // analog 0-5 on  
    case SEVEN_ANALOG_INS:  // analog 0-6 on  
    case EIGHT_ANALOG_INS:  // analog 0-7 on  
    case NINE_ANALOG_INS:   // analog 0-8 on  
      analogInputsEnabled = inputData - ZERO_ANALOG_INS;
      break;
    case ENABLE_PWM:
      waitForData = 2;  // 2 bytes needed (pin#, dutyCycle) 
      executeMultiByteCommand = inputData;
      break;
    case DISABLE_PWM:
      waitForData = 1;  // 1 byte needed (pin#)
      executeMultiByteCommand = inputData;
      break;      
    case SET_SOFTWARE_PWM_FREQ:
      waitForData = 1;  // 1 byte needed (pin#)
      executeMultiByteCommand = inputData;
      break;
    case ENABLE_SOFTWARE_PWM:
      waitForData = 2;  // 2 bytes needed (pin#, dutyCycle) 
      executeMultiByteCommand = inputData;
      break;
    case DISABLE_SOFTWARE_PWM:
      waitForData = 1;  // 1 byte needed (pin#)
      executeMultiByteCommand = inputData;
      break;
    case OUTPUT_TO_DIGITAL_PINS:   // bytes to send to digital outputs
      firstInputByte = true;
      break;
    case REPORT_VERSION:
      printByte(REPORT_VERSION);
      printByte(MAJOR_VERSION);
      printByte(MINOR_VERSION);
      break;
    }
  }
}


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

// -------------------------------------------------------------------------
void setup() {
  byte i;

  Serial.begin(19200);
  for(i=0; i<TOTAL_DIGITAL_PINS; ++i) {
    setPinMode(i,INPUT);
  }
}

// -------------------------------------------------------------------------
void loop() {
  checkForInput();  
  
  // read all digital pins, in enabled
  if(digitalInputsEnabled) {
    printByte(ENABLE_DIGITAL_INPUTS);
    transmitDigitalInput(7);
    checkForInput();
    transmitDigitalInput(0);
    checkForInput();
  }

  /* get analog in, for the number enabled */
  for(analogPin=0; analogPin<analogInputsEnabled; ++analogPin) {
    analogData = analogRead(analogPin);
    // these two bytes get converted back into the whole number in Pd
    // the higher bits should be zeroed so that the 8th bit doesn't get set
    printByte(ONE_ANALOG_IN + analogPin); 
    printByte(analogData >> 7);  // bitshift the big stuff into the output byte
    printByte(analogData % 128); // mod by 32 for the small byte
    checkForInput();
  }
}

