#include<ArduinoTrace.h>

class RemoteOutput{
  public:
  int latchPin,clockPin,dataPin;
  bool bitOrder;
  RemoteOutput::RemoteOutput(int _clockPin, int _latchPin, int _dataPin, bool _bitOrder){
    clockPin=_clockPin;
    latchPin = _latchPin;
    dataPin= _dataPin;
    bitOrder = _bitOrder;
  }
  void begin(){
    pinMode(latchPin, OUTPUT);
    pinMode(clockPin, OUTPUT);
    pinMode(dataPin, OUTPUT);   
  }
  ////////////////////////////////////////////////////////////////////////////////////////
  /*595 OUTPUT datasheet https://www.ti.com/lit/ds/symlink/sn74hc595.pdf
  Connect the data pin (SER) of the first 74HC595 to an Arduino output pin (e.g., dataPin).
  Connect the clock pin (SRCLK) of the 74HC595 to an Arduino output pin (e.g., clockPin).
  Connect the latch pin (RCLK) of the 74HC595 to an Arduino output pin (e.g., latchPin).
  Daisy-chain the Q7' pin of the first 74HC595 to the SER pin of the second 74HC595.
  
  ARDUINO  >   XX595  >   XX595
  dataPin()   SER(14)
  clockPin()  SRCLK(11)     SRCLK(11)
  latchPin()  RCLK(12)      RCLK(12)
              Q7(9)         SER(14)
  GND         /OE(13)       /OE(13)
  VCC         /SRCLR(10)    /SRCLR(10)
  */
  void shiftOut16msbf(uint16_t data) {
    digitalWrite(latchPin, LOW);  // Begin
    shiftOut(dataPin, clockPin, MSBFIRST, highByte(data)); // Send high byte
    shiftOut(dataPin, clockPin, MSBFIRST, lowByte(data));  // Send low byte
    digitalWrite(latchPin, HIGH);  // Output the data
  }
  ////////////////////////////////////////////////////////////////////////////////////////
  byte nthbyte(uint16_t data, int n){
    return 0xFF & (data >> (8*n));
  }
};

class RemoteIntput{
  public:
  int latchPin,clockPin,dataPin;
  bool bitOrder;
  RemoteIntput::RemoteIntput(int _clockPin, int _latchPin, int _dataPin, bool _bitOrder){
    clockPin=_clockPin;
    latchPin = _latchPin;
    dataPin= _dataPin;
    bitOrder = _bitOrder;
  }
  void begin(){
    pinMode(latchPin, OUTPUT);
    pinMode(clockPin, OUTPUT);
    pinMode(dataPin, INPUT);   
  }
  ////////////////////////////////////////////////////////////////////////////////////////
  /*165 INPUT  datasheet https://www.ti.com/lit/ds/symlink/sn74hc165.pdf
  Connect the serial output (Q7) of the second 74HC165 to an Arduino input pin (e.g., dataPin).
  Connect the clock pin (CP) of the 74HC165 to an Arduino output pin (e.g., clockPin).
  Connect the latch pin (PL) of the 74HC165 to an Arduino output pin (e.g., latchPin).
  Daisy-chain the Q7 pin of the first 74HC165 to the D1 pin of the second 74HC165.
  
  ARDUINO   <  XX165   <  XX165
  dataPin()   Q7(9)
  clockPin()  CP(2)         CP(2)
  latchPin()  PL(1)         PL(1)
              D1(10)        Q7(9)
  GND         /CLKINH(15)   /CLKINH(15)
  VCC         
  
  */
  uint16_t shiftIn16msbf() {
    uint16_t result = 0;

    // Latch the current state of the parallel inputs
    digitalWrite(latchPin, LOW);
    delayMicroseconds(5);
    digitalWrite(latchPin, HIGH);

    // Shift in the high byte
    result = shiftIn(dataPin, clockPin, MSBFIRST);
    result <<= 8;  // Make room for the low byte
    // Shift in the low byte and add it to result
    result |= shiftIn(dataPin, clockPin, MSBFIRST);

    return result;
  }
  ////////////////////////////////////////////////////////////////////////////////////////
};




class OutputExpander8{
  public:
  int latchPin,clockPin,dataPin;
  bool bitOrder;
  OutputExpander8::OutputExpander8(int _clockPin, int _latchPin, int _dataPin, bool _bitOrder){
    clockPin=_clockPin;
    latchPin = _latchPin;
    dataPin= _dataPin;
    bitOrder = _bitOrder;
  }
  void begin(){
    pinMode(latchPin, OUTPUT);
    pinMode(clockPin, OUTPUT);
    pinMode(dataPin, OUTPUT);   
  }
  void digitalWriteX8(byte data){
    // ST_CP LOW to keep LEDs from changing while reading serial data
    digitalWrite(latchPin, LOW);
    // Shift out the bits
    //DUMP(bitOrder);DUMP(dataPin);DUMP(clockPin);DUMP(data);
    if(bitOrder){shiftOut(dataPin, clockPin, MSBFIRST, data);TRACE();}
    if(!bitOrder){shiftOut(dataPin, clockPin, LSBFIRST, data);TRACE();}    
    // ST_CP HIGH change LEDs
    digitalWrite(latchPin, HIGH);  
  }
  void digitalWriteX16(int data){
    // ST_CP LOW to keep LEDs from changing while reading serial data
    digitalWrite(latchPin, LOW);
    // Shift out the bits
    //DUMP(bitOrder);DUMP(dataPin);DUMP(clockPin);DUMP(data);
    if(bitOrder){
      shiftOut(dataPin, clockPin, MSBFIRST, data>>8);
      shiftOut(dataPin, clockPin, MSBFIRST, data);      
      TRACE();
    }    
    if(!bitOrder){shiftOut(dataPin, clockPin, LSBFIRST, data);TRACE();}    
    // ST_CP HIGH change LEDs
    digitalWrite(latchPin, HIGH);  
  }  

////////////////////////////////////////////////////////////////////////////////////////
/*595 OUTPUT
Connect the data pin (SER) of the first 74HC595 to an Arduino output pin (e.g., dataPin).
Connect the clock pin (SRCLK) of the 74HC595 to an Arduino output pin (e.g., clockPin).
Connect the latch pin (RCLK) of the 74HC595 to an Arduino output pin (e.g., latchPin).
Daisy-chain the Q7' pin of the first 74HC595 to the SER pin of the second 74HC595.
*/
void shiftOut16msbf(uint16_t data) {
  digitalWrite(latchPin, LOW);  // Begin
  shiftOut(dataPin, clockPin, MSBFIRST, highByte(data)); // Send high byte
  shiftOut(dataPin, clockPin, MSBFIRST, lowByte(data));  // Send low byte
  digitalWrite(latchPin, HIGH);  // Output the data
}
////////////////////////////////////////////////////////////////////////////////////////
/*165 INPUT
Connect the serial output (Q7) of the second 74HC165 to an Arduino input pin (e.g., dataPin).
Connect the clock pin (CP) of the 74HC165 to an Arduino output pin (e.g., clockPin).
Connect the latch pin (PL) of the 74HC165 to an Arduino output pin (e.g., latchPin).
Daisy-chain the Q7 pin of the first 74HC165 to the D1 pin of the second 74HC165.
*/
uint16_t shiftIn16msbf() {
  uint16_t result = 0;

  // Latch the current state of the parallel inputs
  digitalWrite(latchPin, LOW);
  delayMicroseconds(5);
  digitalWrite(latchPin, HIGH);

  // Shift in the high byte
  result = shiftIn(dataPin, clockPin, MSBFIRST);
  result <<= 8;  // Make room for the low byte

  // Shift in the low byte and add it to result
  result |= shiftIn(dataPin, clockPin, MSBFIRST);

  return result;
}
////////////////////////////////////////////////////////////////////////////////////////






};
OutputExpander8 output(10,11,12,true);

//////////////////////////////////////////////////////////////////
/*
  74HC595 Shift Register Demonstration 1
  74hc595-demo.ino
  Count in Binary and display on 8 LEDs

  Modified from "Hello World" example by Carlyn Maw,Tom Igoe and David A. Mellis

  DroneBot Workshop 2020
  https://dronebotworkshop.com
*/
unsigned long t1,t2, readtime, writetime;
// Define Connections to 74HC595
const int latchPin = 11;// ST_CP pin 12
const int clockPin = 10;// SH_CP pin 11
const int dataPin = 12;// DS pin 14
//////////////
// Define Connections to 74HC165
int load = 7;// PL pin 1
int clockEnablePin = 4;// CE pin 15
int dataIn = 5;// Q7 pin 7
int clockIn = 6;// CP pin 2
/////////////////////////////////////////// 
void setup ()
{
  // Setup pins as Outputs
 Serial.begin(9600);
  delay(10); 

  output.begin();

  // Setup 74HC595 connections 
  pinMode(latchPin, OUTPUT);
  pinMode(clockPin, OUTPUT);
  pinMode(dataPin, OUTPUT);

  // Setup 74HC165 connections
  pinMode(load, OUTPUT);
  pinMode(clockEnablePin, OUTPUT);
  pinMode(clockIn, OUTPUT);
  pinMode(dataIn, INPUT);

  //digitalWrite(dataPin, HIGH);
  //delay(200);
}

void loop() {


//read from 165 and write to 595
  byte b;
  t1=micros();
  b=digitalRead8();
  t2=micros();readtime=t2-t1;
  Serial.println(b);
  t1=micros();
  digitalWrite8(b);
  //output.digitalWriteX8(b);
  t2=micros();writetime=t2-t1;
  delay(1000);
  Serial.println("Read Time "+String(readtime));
  Serial.println("Write Time "+String(writetime));

}
///////////////////////////////////////////
/////////////////////////////////////////////////
/////////////////////////////////////////////////
void digitalWrite8(byte n){
  // ST_CP LOW to keep LEDs from changing while reading serial data
    digitalWrite(latchPin, LOW);

    // Shift out the bits
    shiftOut(dataPin, clockPin, LSBFIRST, n);

    // ST_CP HIGH change LEDs
    digitalWrite(latchPin, HIGH); 
    //delay(50); 
}
byte digitalRead8(){
//pins/////////////////////
//load
//clockIn
//dataIn
//clockEnablePin
  // Write pulse to load pin
  digitalWrite(load, LOW);
  delayMicroseconds(5);
  digitalWrite(load, HIGH);
  delayMicroseconds(5);
 
  // Get data from 74HC165
  digitalWrite(clockIn, HIGH);
  digitalWrite(clockEnablePin, LOW);
  byte incoming = shiftIn(dataIn, clockIn, LSBFIRST);
  digitalWrite(clockEnablePin, HIGH);
  return incoming;

}