#include <AES.h>
//#include "./printf.h"
//#include <ESP8266WiFi.h>

typedef byte   uchar;
const int size_mesg=256;   // size of plain-message
const int sleepTimeS = 10;
const int h=4;  // size of each block
const int nblocks=size_mesg/h; // number of blocks
int rp=1;


byte DK[64];
int PboxRM[h];
uchar Sbox1[256];
  

//--------------------------------------------------------------------
//Function: KSA algorithm of RC4
//--------------------------------------------------------------------
void rc4key(uchar *key, uchar *sc, int size_DK) {

  for(int i=0;i<256;i++) {
    sc[i]=i;
  }


  uchar j0 = 0;
  for(int i0=0; i0<256; i0++) {
    j0 = (j0 + sc[i0] + key[i0%size_DK] )&0xFF;
    uchar tmp = sc[i0];
    sc[i0] = sc[j0 ];
    sc[j0] = tmp;
  }
}


//--------------------------------------------------------------------
//Function: Modified KSA of RC4
//--------------------------------------------------------------------
void rc4keyperm(uchar *key,int nblocks, int rp,int *sc, int size_DK) {

  //sc=1:nblocks; 
  for (int i=0;i<nblocks;i++) {
    sc[i]=i;
  }
  for (int it = 0; it < rp; it++) {
    int j0 = 1;
    for(int i0 = 0; i0<nblocks; i0++) {
      j0 = (j0 + sc[i0] + sc[j0] + key[i0%size_DK] )% nblocks;
      int tmp = sc[i0];
      sc[i0] = sc[j0];
      sc[j0] = tmp;
    }

  }
}

//--------------------------------------------------------------------
//Function: PRGA of RC4
//--------------------------------------------------------------------
void prga(uchar *sc, int ldata, uchar *r) {
  uchar i0=0;
  uchar j0=0;

  for (int it=0; it<ldata; it++) {
    i0 = ((i0+1)&0xFE); //%255);
    j0 = (j0 + sc[i0])&0xFF;
    uchar tmp = sc[i0];
    sc[i0] = sc[j0];
    sc[j0] = tmp;
    r[it]=sc[(sc[i0]+sc[j0])&0xFF];
  }
}

//--------------------------------------------------------------------
//Function: Binary diffusion operation (h to h)
//--------------------------------------------------------------------
void diff(uchar *Y, uchar *X, int h) { 

  if(h==4) {
    Y[0] = X[1]^X[2]^X[3];
    Y[1] = X[0]^X[2]^X[3];
    Y[2] = X[0]^X[1]^X[3];
    Y[3] = X[0]^X[1]^X[2];
  }
  else if(h==8) {
    Y[0] = X[0]^X[2]^X[3]^X[5]^X[6]^X[7];
    Y[1] = X[0]^X[1]^X[3]^X[4]^X[6]^X[7];
    Y[2] = X[0]^X[1]^X[2]^X[4]^X[5]^X[7];
    Y[3] = X[1]^X[2]^X[3]^X[4]^X[5]^X[6];
    Y[4] = X[0]^X[1]^X[5]^X[6]^X[7];
    Y[5] = X[1]^X[2]^X[4]^X[6]^X[7];
    Y[6] = X[2]^X[3]^X[4]^X[5]^X[7];
    Y[7] = X[0]^X[3]^X[4]^X[5]^X[6];
  }
  else if(h==16) {
  
  Y[0] = X[3] ^ X[4] ^ X[6] ^ X[8] ^ X[9] ^ X[13] ^ X[14];
  Y[1] = X[2] ^ X[5] ^ X[7] ^ X[8] ^ X[9] ^ X[12] ^ X[15]; 
  Y[2] = X[1] ^ X[4] ^ X[6] ^ X[10] ^ X[11] ^ X[12] ^ X[15]; 
  Y[3] = X[0] ^ X[5] ^ X[7] ^ X[10] ^ X[11] ^ X[13] ^ X[14];
  Y[4] = X[0] ^ X[2] ^ X[5] ^ X[8] ^ X[11] ^ X[14] ^ X[15]; 
  Y[5] = X[1] ^ X[3] ^ X[4] ^ X[9] ^ X[10] ^ X[14] ^ X[15]; 
  Y[6] = X[0] ^ X[2] ^ X[7] ^ X[9] ^ X[10] ^ X[12] ^ X[13]; 
  Y[7] = X[1] ^ X[3] ^ X[6] ^ X[8] ^ X[11] ^ X[12] ^ X[13]; 
  Y[8] = X[0] ^ X[1] ^ X[4] ^ X[7] ^ X[10] ^ X[13] ^ X[15]; 
  Y[9] = X[0] ^ X[1] ^ X[5] ^ X[6] ^ X[11] ^ X[12] ^ X[14]; 
  Y[10] = X[2] ^ X[3] ^ X[5] ^ X[6] ^ X[8] ^ X[13] ^ X[15]; 
  Y[11] = X[2] ^ X[3] ^ X[4] ^ X[7] ^ X[9] ^ X[12] ^ X[14];
  Y[12] = X[1] ^ X[2] ^ X[6] ^ X[7] ^ X[9] ^ X[11] ^ X[12]; 
  Y[13] = X[0] ^ X[3] ^ X[6] ^ X[7] ^ X[8] ^ X[10] ^ X[13]; 
  Y[14] = X[0] ^ X[3] ^ X[4] ^ X[5] ^ X[9] ^ X[11] ^ X[14]; 
  Y[15] = X[1] ^ X[2] ^ X[4] ^ X[5] ^ X[8] ^ X[10] ^ X[15]; 
  }
  else if(h==32) {

    
    Y[0]=X[0]^X[1]^X[2]^X[3]^X[4]^X[7]^X[8]^X[10]^X[12]^X[15]^X[16]^X[17]^X[18]^X[20]^X[21]^X[24]^X[25]^X[28]^X[30];
    Y[1]=X[0]^ X[1]^X[2]^X[3]^X[5]^X[6]^X[9]^X[11]^X[13]^X[14]^X[16]^X[17]^X[19]^X[20]^X[21]^ X[24]^X[25]^X[29]^X[31];
    Y[2]=X[0]^X[1]^X[2]^X[3]^X[5]^X[6]^X[8]^X[10]^X[13]^X[14]^X[16]^X[18]^X[19]^X[22]^X[23]^X[26]^X[27]^X[28]^X[30];
    Y[3]=X[0]^X[1]^X[2]^X[3]^X[4]^X[7]^X[9]^X[11]^X[12]^X[15]^X[17]^X[18]^X[19]^X[22]^X[23]^X[26]^X[27]^X[29]^X[31];
    Y[4]=X[0]^X[3]^X[5]^X[6]^X[7]^X[10]^X[11]^ X[12]^X[13]^X[14]^ X[15]^X[16]^X[19]^X[21]^X[23]^  X[25]^X[27]^X[30]^X[31];  
    Y[5]=X[1]^X[2]^X[4]^X[6]^X[7]^X[10]^X[11]^X[12]^X[13]^X[14]^X[16 ]^X[17]^X[18]^X[20]^X[22]^X[24]^X[26]^X[30]^X[31];
    Y[6]=X[1]^X[2]^X[4]^X[5]^X[7]^X[8]^X[9]^X[12]^X[13]^X[14]^X[15]^ X[17]^X[18]^X[21]^X[23]^X[25]^X[27]^X[28]^X[29];
    Y[7]=X[0]^X[3]^X[4]^X[5]^X[6]^X[9 ]^X[9]^X[12]^X[13]^X[14]^X[15]^X[16]^X[19]^X[20]^X[22]^X[24]^X[26]^X[28]^X[29];
    Y[8]=X[0]^X[2]^X[6]^X[7]^X[8]^X[10]^X[11]^X[14]^X[15]^X[16]^X[18]^X[21]^X[22]^X[25]^X[26];
    Y[9]=X[1]^ X[3]^X[6]^X[7]^X[9]^X[10]^X[11]^X[14]^X[15]^X[17]^X[19]^X[20]^X[23]^X[24]^X[27];
    Y[10]=X[0]^X[2]^X[4]^X[5]^X[8]^X[9]^X[10]^X[12]^X[13]^X[16]^X[18]^X[20]^X[23]^ X[24]^X[27];
    Y[11]=X[1]^X[3]^X[4]^X[5]^X[8]^X[9]^X[11]^X[12]^X[13]^X[17]^X[19]^X[21]^X[22]^X[25]^X[26];
    Y[12]=X[0]^X[3]^X[4]^X[5]^X[6]^X[7]^X[10]^X[11]^X[13]^X[14]^X[15]^X[16]^X[19]^X[21]^X[23]^X[25]^X[27]^X[30]^X[31];
    Y[13]=X[1]^X[2]^X[4]^X[5]^X[6]^X[7]^X[10]^X[11]^X[12]^X[14]^X[15]^X[17]^ X[18]^X[20]^X[22]^X[24]^X[26]^X[30]^X[31];
    Y[14]=X[1]^X[2]^X[4]^X[5]^X[6]^X[7]^X[8]^X[9]^X[12]^X[13]^X[15]^X[17]^X[18]^X[21]^X[23]^X[25]^X[27]^X[28]^X[29];
    Y[15]=X[0]^X[3]^X[4]^X[5]^X[6]^X[7]^X[8]^X[9]^X[12]^X[13]^X[14]^X[16]^X[19]^X[20]^X[22]^ X[24]^X[26]^X[28]^X[29];
    Y[16]=X[0]^X[1]^X[2]^X[4]^X[8 ]^X[8]^X[10]^X[13 ]^X[15]^X[16]^X[17]^X[18]^X[19]^X[20]^X[21]^X[24]^X[25]^X[28]^X[30];
    Y[17]=X[0]^X[1]^X[3]^X[5]^X[6]^X[9]^X[11]^X[13]^X[14]^X[16]^X[17]^X[18]^X[19]^X[20]^X[21]^X[24]^X[25]^X[29]^X[31];
    Y[18]=X[0]^X[2]^X[3]^X[5]^X[6]^X[8]^X[10]^X[13]^X[14]^X[16]^X[17]^X[18]^X[19]^X[22]^X[23]^X[26]^X[27]^X[28]^X[30];
    Y[19]=X[1]^X[2]^X[3]^X[4]^X[7]^X[9]^X[11]^X[12]^X[15]^X[16]^X[17]^X[18]^X[19]^X[22]^X[23]^X[26]^X[28 ]^X[29]^X[31];
    Y[20]=X[0]^X[1]^X[5]^X[7]^X[10 ]^X[10]^X[13]^X[15]^X[16]^X[17]^X[20]^X[21]^X[23]^X[29]^X[30];
    Y[21]=X[0]^X[1]^X[4]^X[6]^X[8]^X[11]^X[12]^X[14]^X[16]^X[17]^X[20]^X[21]^X[22]^X[28]^X[31];
    Y[22]=X[2]^X[3]^X[5]^X[7]^X[8]^X[11]^X[13]^X[15]^X[18]^X[19]^X[21]^X[22]^X[23]^X[28]^X[31];
    Y[23]=X[2]^X[3]^X[4]^X[6]^X[9]^X[10]^X[12]^X[14]^ X[18]^X[19]^X[20]^X[22]^X[23]^X[29]^X[30];
    Y[24]=X[0]^X[1]^X[5]^X[7]^X[9]^X[10]^X[13]^X[15]^X[16]^X[17]^X[24]^X[25]^X[27]^X[29]^X[30];
    Y[25]=X[0]^X[1]^X[4]^X[6]^X[8]^X[11]^X[12]^X[14]^X[16]^X[17]^X[24]^X[25]^X[26]^X[28]^X[31];
    Y[26]=X[2]^X[3]^X[5]^X[7]^X[8]^X[11]^X[13]^X[15]^X[18]^X[19]^X[25]^X[26]^X[27]^X[28]^ X[31];
    Y[27]=X[2]^X[3]^X[4]^X[6]^X[9]^X[10]^X[12]^X[14]^X[18]^X[19]^X[24]^X[26]^X[27]^X[29]^X[30];
    Y[28]=X[0]^X[2]^X[6]^X[7]^X[14]^X[15]^X[16]^X[18]^X[21]^X[22]^X[25]^X[26]^X[28]^X[30]^X[31];
    Y[29]=X[2]^X[3]^X[6]^X[7]^X[14]^X[15]^X[17]^X[19]^X[20]^X[23]^X[24]^X[27]^X[29]^X[30]^X[31];
    Y[30]=X[1]^X[2]^X[4]^X[5]^X[12]^X[13]^X[16]^X[18]^X[20]^X[23]^X[24]^X[27]^X[28]^X[29]^X[30];
    Y[31]=X[2]^X[3]^X[4]^X[5]^X[12]^X[13]^X[17]^X[19]^X[21]^X[22]^X[25]^X[26]^X[28]^X[29]^X[31];
  }

}


//--------------------------------------------------------------------
//Function: Proposed hash function
//--------------------------------------------------------------------
void hash_DSD_BIN(uchar* seq_in, uchar* Hashval,int nblocks, int *PboxRM, uchar *Sbox1,  int h) {
  // Goal: Calculate the hash value
  // Output: RM (hash value)
  uchar X[h];
  uchar fX[h];
  uchar fX2[h];
  int ind2; 
  for(int it=0;it<nblocks;it++) {
   
    ind2=it*h;

    // Mix with previous hash val
    
    for(int a=0;a<h;a+=4) {
      fX[a]=Hashval[a]^seq_in[ind2+a];
      fX[a+1]=Hashval[a+1]^seq_in[ind2+a+1];
      fX[a+2]=Hashval[a+2]^seq_in[ind2+a+2];
      fX[a+3]=Hashval[a+3]^seq_in[ind2+a+3];
    }

    // First Diffusion Operation
    diff(fX2,fX,h);
    
    
    // Substitution  Operation
    for(int a=0;a<h;a+=4) {
      fX[a]=Sbox1[fX2[a]];           //Warning according to the size of h2, we can be outsize of Sbox1[a]
      fX[a+1]=Sbox1[fX2[a+1]];
      fX[a+2]=Sbox1[fX2[a+2]];
      fX[a+3]=Sbox1[fX2[a+3]];           
    }
    
    // Second Diffusion Operation
    
    diff(fX2,fX,h);
    
// update RM and mix it with hashed block
    for(int a=0;a<h;a+=4) {
      Hashval[a]=fX2[a]^Hashval[PboxRM[a]];
      Hashval[a+1]=fX2[a+1]^Hashval[PboxRM[a+1]];
      Hashval[a+2]=fX2[a+2]^Hashval[PboxRM[a+2]];
      Hashval[a+3]=fX2[a+3]^Hashval[PboxRM[a+3]];
    }
    
  }


}



//--------------------------------------------------------------------
//Function: to get the mean of a vector
//--------------------------------------------------------------------
float mean(int m, int a[]) {
    int sum=0, i;
    for(i=0; i<m; i++)
        sum+=a[i];
    return((float)sum/m);
}


//--------------------------------------------------------------------
//Function: to print a  vector of bytes
//--------------------------------------------------------------------
void printArray(byte *mes, int n) {
  for (byte i = 0; i < n; i++) {
    Serial.print(mes[i]);
    Serial.print(" ");
  }
  Serial.println();
}

//--------------------------------------------------------------------
//Function:  Construction of initial layer
//--------------------------------------------------------------------

void setup ()
{
  Serial.begin (57600) ;
//  printf_begin();
  delay(500);
  printf("\n===testng mode\n") ;

  uchar sc[256];  
  uchar Hashval[h];
  randomSeed(134);
  for(byte i=0;i<64;i++) {
    DK[i]=random(255);
  }
  
// creation of S-box
rc4key(DK, Sbox1, 8);
// Creation  of initial hash value (keyed hash), equals to zero if it is unkeyed
rc4key(&DK[8], sc, 8);
prga(sc, h, Hashval);

rc4keyperm(&DK[16], h, rp, PboxRM, 8); 
Serial.println("END OF INIT"); 

//rc4key(&DK[16], sc, 8);
//prga(sc, h, Hashval2);

}

//--------------------------------------------------------------------
//Function: 
//--------------------------------------------------------------------

void loop () 
{
  prekey_test () ;
  delay(1000);

//  Serial.println("ESP8266 in sleep mode");
 // ESP.deepSleep(sleepTimeS * 1000000, WAKE_RF_DISABLED );
  delay(1000);
//  Serial.println("ESP8266 in normal mode");
}

//--------------------------------------------------------------------
//Function: 
//--------------------------------------------------------------------
//mean(float m, int a[])
void prekey (int bits)
{
 
byte plain[size_mesg];
//byte plain2[size_mesg];
byte  Hashval[h];
byte  Hashval2[h];

  //%%%%%%%%%%%%% Initialization of seed%%%%%%%%%%%%%% 
  int seed=random(2^32);
  randomSeed(seed);

  
  //%%%%%%%%%%%%% Generation of plain message %%%%%%%%%%%%%% 
  for(int i=0;i<size_mesg;i++) {
    plain[i]=random(255);
  }

// copy of hashval to hashval2 (it is the output of the  hash function)
   for(int i=0;i<h;i++){
    Hashval2[i]=Hashval[i];
  }
  
  //printf("\n\nKey after copy :");
 //printArray(Hashval2,h);
 
  unsigned long ms1 = micros ();
  hash_DSD_BIN(plain, Hashval,nblocks,PboxRM,Sbox1,h);
  Serial.print("Hash time: ");
  Serial.println(micros() - ms1);

  printf("\n\nOriginal PLAIN :");
  printArray(plain,16*2);
  printf("\n Original Hash value:");
  printArray(Hashval,h);

// Modify message with a random index


  int indx=random(size_mesg);
  Serial.println("Index");
  Serial.println(indx);
  plain[indx]=plain[indx]+1;
  unsigned long ms3 = micros ();
  hash_DSD_BIN(plain, Hashval2,nblocks,PboxRM,Sbox1,h);
  Serial.print("Hash time: ");
  Serial.println(micros() - ms3);

  printf("\n\nModify PLAIN :");
  printArray(plain,16*2);
  printf("\n Modify Hash value:");
  printArray(Hashval2,h);


  bool equal=true;
  for(int i=0;i<size_mesg;i++) {
      if(Hashval2[i]!=Hashval[i]) {
        equal=false;
      }
    }
 
  Serial.print("CHECK ");
  Serial.println(equal);
  


/*}
timee=mean(itk,  timecompute) ;
Serial.println ("Mean Time");
  Serial.println(timee);
    delay(500);
    */
    
  /*


 
  //printf("\nCHECK :");
  //printArray(check,16*2);
  
 */
  
}
//--------------------------------------------------------------------
//Function: 
//--------------------------------------------------------------------
void prekey_test ()
{
  prekey (128) ;
}