#include <EEPROM.h>
#include <RASCII.h>
#include <SPI.h>
#include <MFRC522.h>

#include "Test.h"
/*
 * http://playground.arduino.cc/Learning/MFRC522
 * 
 */
bool RUN  = false;
bool RFID_Send = false;

int  AnaVal[6];
byte DigVal[14];
byte RFIDVal[8][7];

// The RASCII connections.
RASCII Rascii(false);
RASCII Rascii485(true);

// Create MFRC522 instance.
MFRC522 mfrc522(SS_PIN, RST_PIN);

// The board configuration structure.
BoardSetup boardSetup;

// Check on which board we compile.
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
byte  BoardType = TYPE_MEGA;
byte  NrAnalog  = 16;
byte  NrDigital = 54;
byte APORT[16] = {A0,A1,A2,A3,A4,A5,A6,A7,A8,A9,A10,A11,A12,A13,A14,A15};
#elif defined(__AVR_ATmega328P__)
byte  BoardType = TYPE_UNO;
byte  NrAnalog  = 6;
byte  NrDigital = 14;
byte APORT[16] = {A0,A1,A2,A3,A4,A5,A6,A7};
#else
byte  BoardType = TYPE_UNO; // Default to UNO
byte  NrAnalog  = 6;
byte  NrDigital = 14;
byte APORT[16] = {A0,A1,A2,A3,A4,A5,A6,A7};
#endif

/*
 * The main setup.  
 */
void setup() {
  EEPROM.get( 0, boardSetup );  
  initIO();  
  Rascii.begin(57600);
  initRFID();
  RUN = true;
}


/*
 * The main loop. 
 */
uint32_t _previousMillis = 0;

void loop() {
  byte cmd[64];
  
  // delay of 10ms
  if (millis() - _previousMillis >= 10) {
    _previousMillis = millis();
    if( Rascii.read(cmd) ) {
      evaluateCmd(cmd);
      if( boardSetup.master && boardSetup.rs485 ) {
        // ToDo: Master.
      }
      if( !boardSetup.master && boardSetup.rs485 ) {
        // ToDo: Slave.
      }
    }
    // Only read I/O in case the RUN flag ist set.
    if( RUN ) {
      doRFID();
      doDIN();
      doAIN();
    }
  }
}


/* 
 * Initialize the RFID reader. 
 */
void initRFID() {
  SPI.begin();
  mfrc522.PCD_Init();   // Init MFRC522
  mfrc522.PCD_SetAntennaGain(mfrc522.RxGain_max);
}


/* 
 * Init the analog and digital pins. 
 */
void initIO() {
  for( byte i = 0; i < NrAnalog; i++ ) {
    AnaVal[i] = 0; 
    if( boardSetup.analogport[i] == ANA_DIN || boardSetup.analogport[i] == ANA_DSEN ) {
      pinMode(APORT[i], INPUT);
      digitalWrite(APORT[i], HIGH);  // set pullup on analog pin
    }
    if( boardSetup.analogport[i] == ANA_DOUT )
      pinMode(APORT[i], OUTPUT);
  }
  for( byte i = 0; i < NrDigital; i++ ) {
    DigVal[i] = 0; 
    if( boardSetup.digitalport[i] == PORT_OUT )
      pinMode(i, OUTPUT);
    else if( boardSetup.digitalport[i] == PORT_IN )
      pinMode(i, INPUT);
    else if( boardSetup.digitalport[i] == PORT_INPULL )
      pinMode(i, INPUT_PULLUP);
  }
  for( byte i = 0; i < 8; i++ ) {
    for( byte n = 0; i < 7; i++ )
      RFIDVal[i][n] = 0; 
  }
}


/*
 * Evaluate a host command.
 */
void evaluateCmd(byte* cmd) {
  char s[32];
  if( cmd[1] == 0xFF || cmd[1] == boardSetup.addr || cmd[0] == CMD_INFO) {
    if( cmd[0] == CMD_INFO ) {
      sprintf(s, "RocDino V0.1 addr=%d", boardSetup.addr);
      Rascii.write(s);
    }
    else if( cmd[0] == CMD_START ) {
      Rascii.write("Start reading");
      RUN = true;
      mfrc522.PCD_Init();   // Init MFRC522
      mfrc522.PCD_SetAntennaGain(mfrc522.RxGain_max);
    }
    else if( cmd[0] == CMD_STOP ) {
      Rascii.write("Stop reading");
      RUN = false;
    }
    else if( cmd[0] == CMD_DOUT ) {
      digitalOut(cmd);
    }
    else if( cmd[0] == CMD_AOUT ) {
      analogOut(cmd);
    }
    else if( cmd[0] == CMD_GETCONF ) {
      getConfig();
    }
    else if( cmd[0] == CMD_SETCONF ) {
      setConfig(cmd);
    }
    else if( cmd[0] == CMD_RESETCONF ) {
      resetConfig();
    }
    else if( cmd[0] == CMD_GETCV ) {
      getCV(cmd);
    }
    else if( cmd[0] == CMD_SETCV ) {
      setCV(cmd);
    }
    else {
      sprintf(s, "unsupported command %02X", cmd[0]);
      Rascii.write(s);
    }
  }
}


/*
 * Send the configuration to the host.
 */
void getConfig() {
  byte conf[128];
  memcpy(conf, &boardSetup, sizeof(boardSetup));
  Rascii.write(CMD_GETCONF, conf, sizeof(boardSetup), RSP);
}


/*
 * Send a CV value to the host.
 */
void getCV( byte* cmd ) {
  int  cv = cmd[2] * 256 + cmd[3];
  byte conf[32];
  byte* p = (byte*)&boardSetup;
  conf[0] = cmd[2];
  conf[1] = cmd[3];
  conf[2] = p[cv];
  Rascii.write(CMD_GETCV, conf, 3, RSP);
}


/*
 * Overwrite the complete board configuration. (Not recomendet.)
 */
void setConfig( byte* cmd ) {
  char s[32];
  boardSetup.addr = cmd[2];
  memcpy(&boardSetup, cmd+2, sizeof(boardSetup));
  sprintf(s, "EEPROM addr=%02X", boardSetup.addr);
  Rascii.write(s);
  EEPROM.put( 0, boardSetup );
  initIO();
}


/*
 * Change a CV value of the board configuration.
 */
void setCV( byte* cmd ) {
  char s[32];
  int  cv  = cmd[2] * 256 + cmd[3];
  byte val = cmd[4];
  byte* p = (byte*)&boardSetup;
  p[cv] = val;
  sprintf(s, "set cv %d to %d", cv, val);
  Rascii.write(s);
  EEPROM.write( (int)&p[cv], val );
  initIO();
}


/*
 * Reset the complete board configuration to zero.
 */
void resetConfig() {
  memset(&boardSetup, 0, sizeof(boardSetup));
  EEPROM.put( 0, boardSetup );
  initIO();
}


/*
 * Set a digital or anlaog output.
 */
void digitalOut( byte* cmd ) {
  if( cmd[2] < 100 && boardSetup.digitalport[cmd[2]] != PORT_RES) 
    digitalWrite(cmd[2], cmd[3]?HIGH:LOW); 
  else if(boardSetup.analogport[cmd[2]-100] != ANA_RES) // Analog port used as digital
    digitalWrite(APORT[cmd[2]-100], cmd[3]?HIGH:LOW); 
}


/*
 * Set a new value to a PWM output.
 */
void analogOut( byte* cmd ) {
  analogWrite(cmd[2], cmd[3]); 
}


/*
 * Check if a new RFID has been scanned.
 */
void doRFID() {
  // Check for a new ID.
  if (mfrc522.PICC_IsNewCardPresent()) {
    if (mfrc522.PICC_ReadCardSerial()) {
      byte rfidevt[10] = {boardSetup.addr,1};
      for( byte i = 0; i < mfrc522.uid.size; i++ ) {
        rfidevt[2+i] = mfrc522.uid.uidByte[i];
      }
      Rascii.write(EVT_RFID, rfidevt, 2 + mfrc522.uid.size, EVT);
    }
  }

  // Dry test.
  if( !RFID_Send ) {
    byte rfid[10] = {boardSetup.addr,1,45,13,55,123,213};
    Rascii.write(EVT_RFID, rfid, 7, EVT);
    RFID_Send = true;
    rfid[2] = 0;
    Rascii.write(EVT_RFID, rfid, 3, EVT);
  }
}


/*
 * Check all input ports if the state has been changed.
 */
void doDIN() {
  for( byte i = 0; i < NrDigital; i++ ) {
    if( boardSetup.digitalport[i] == PORT_IN || boardSetup.digitalport[i] == PORT_INPULL || boardSetup.digitalport[i] == PORT_SEN ) {
      byte val = digitalRead(i);
      if( val != DigVal[i] ) {
        byte din[3];
        DigVal[i] = val; 
        din[0] = boardSetup.addr; // address
        din[1] = i; // port
        din[2] = val;
        if( boardSetup.digitalport[i] == PORT_SEN )
          Rascii.write(EVT_DSEN, din, 3, EVT);
        else
          Rascii.write(EVT_DIN, din, 3, EVT);
      }
    }
  }
  for( byte i = 0; i < NrAnalog; i++ ) {
    if( boardSetup.analogport[i] == ANA_DIN || boardSetup.analogport[i] == ANA_DSEN ) {
      byte val = digitalRead(APORT[i]);
      if( val != AnaVal[i] ) {
        byte din[3];
        DigVal[i] = val; 
        din[0] = boardSetup.addr; // address
        din[1] = i + 100; // port
        din[2] = val;
        if( boardSetup.digitalport[i] == ANA_DSEN )
          Rascii.write(EVT_DSEN, din, 3, EVT);
        else
          Rascii.write(EVT_DIN, din, 3, EVT);
      }
    }
  }
}


/*
 * Read the analog ports and compare the new values with the old values.
 * Treshold is 100.
 */
void doAIN() {
  for( byte i = 0; i < NrAnalog; i++ ) {
    if( boardSetup.analogport[i] == ANA_IN || boardSetup.analogport[i] == ANA_SEN ) {
      int val = analogRead(i);
      if( val != AnaVal[i] && ((val - AnaVal[i]) > 100 || (AnaVal[i] - val) > 100) ) { 
        byte ain[4];
        AnaVal[i] = val; 
        ain[0] = boardSetup.addr; // address
        ain[1] = i; // port
        ain[2] = (val / 256) & 0xFF;
        ain[3] = (val & 0xFF);
        if( boardSetup.analogport[i] == ANA_IN )
          Rascii.write(EVT_AIN, ain, 4, EVT);
        else
          Rascii.write(EVT_ASEN, ain, 4, EVT);
      }
    }
  }

}


