/* DO NOT CHANGE ANYTHING IN THE FUNCTION BELOW!!!
 *
 * It was specially crafted to work with an 8Mhz Atmega328/168 (int. resonator).
 *
 * */
static inline void GCPad_send(byte *cmd, byte length) {
	byte bit = 128;
	byte high;

	pinModeFast(JOY_DATA_PIN, OUTPUT);

	loop:

	high = *cmd & bit;

	digitalWriteFast(JOY_DATA_PIN, LOW);

	if(high) {
		digitalWriteFast(JOY_DATA_PIN, HIGH);

		bit >>= 1;

		if(bit < 1) {
			bit = 128;
			cmd++;
			length--;
		} else {
			asm volatile ("nop\nnop\nnop\nnop\nnop\n");
		}

		asm volatile ("nop\nnop\nnop\nnop\n");
	} else {

static void ucg_com_arduino_send_generic_SW_SPI(ucg_t *ucg, uint8_t data)
{
  uint8_t i = 8;
  
  do
  {
    if ( data & 128 )
    {
      digitalWriteFast(ucg->pin_list[UCG_PIN_SDA], 1 );
    }
    else
    {
      digitalWriteFast(ucg->pin_list[UCG_PIN_SDA], 0 );
    }
    // no delay required, also Arduino Due is slow enough
    //delayMicroseconds(1);
    digitalWriteFast(ucg->pin_list[UCG_PIN_SCL], 1 );
    //delayMicroseconds(1);
    i--;
    digitalWriteFast(ucg->pin_list[UCG_PIN_SCL], 0 );
    //delayMicroseconds(1);
    data <<= 1;
  } while( i > 0 );
  
}

void adc0_isr(void)
{
	uint32_t tmp = ADC0_RA; // read ADC result to clear interrupt
	digitalWriteFast(3, HIGH);
	delayMicroseconds(1);
	digitalWriteFast(3, LOW);
}

extern "C" int main(void)
{
	#define ARDUINO
#if !defined(ARDUINO)

	// To use Teensy 3.0 without Arduino, simply put your code here.
	// For example:

	pinMode(13, OUTPUT);
	while (1) {
		digitalWriteFast(13, HIGH);
		delay(100);
		digitalWriteFast(13, LOW);
		delay(100);
	}


#else
	// Arduino's main() function just calls setup() and loop()....
	setup();
	while (1) {
		loop();
		yield();
	}
#endif
}

byte PS2Pad::gamepad_spi(byte send_data) {
	byte recv_data = 0;

	for(byte i = 0; i < 8; i++) {
		digitalWriteFast(CLK_PIN, LOW);

		if(send_data & (1 << i)) {
			digitalWriteFast(CMD_PIN, HIGH);
		} else {
			digitalWriteFast(CMD_PIN, LOW);
		}

		delayMicroseconds(CTRL_CLK);

		digitalWriteFast(CLK_PIN, HIGH);

		if(digitalReadFast(DAT_PIN)) {
			recv_data |= (1 << i);
		}

		delayMicroseconds(CTRL_CLK);
	}

	digitalWriteFast(CLK_PIN, HIGH);

	delayMicroseconds(CTRL_BYTE_DELAY);

	return recv_data;
}

/* Setup pins and initialize SPI hardware */
void pspad_init(void) {
	pinModeFast(pinACK, OUTPUT);
	digitalWriteFast(pinACK, HIGH);

	pinModeFast(pinATT, INPUT);
	digitalWriteFast(pinATT, HIGH);

	pinModeFast(pinCMD, INPUT);
	digitalWriteFast(pinCMD, HIGH);

	pinModeFast(pinDAT, OUTPUT);
	digitalWriteFast(pinDAT, HIGH);

	pinModeFast(pinCLK, INPUT);
	digitalWriteFast(pinCLK, HIGH);

	SPCR |= (1 << CPOL); // SCK HIGH when idle
	SPCR |= (1 << CPHA); // setup data on falling edge of CLK, read on rising edge

	// turn on SPI in slave mode
	SPCR |= _BV(SPE);

	SPCR |= (1 << DORD); // data order to LSB first

	// turn on interrupts
	SPCR |= _BV(SPIE);
}

void Zetaohm_AD5676::setVoltage(uint16_t output, uint8_t dac )
{
  uint8_t buffer[3];

  //SPI.beginTransaction(SPISettings(50000000, MSBFIRST, SPI_MODE0));
  digitalWriteFast(_cs_pin, LOW);  // Begin transmission, bring SYNC line low

  buffer[0] = (0x03 << 4) + dac;
  buffer[1] = output / 256;
  buffer[2] = output % 256;
  spi4teensy3::send(buffer, 3 );  //  FRAME 1 COMMAND BYTE - Command + DAC Address (C3 C2 C1 C0 A3 A2 A1 A0)
 // SPI.transfer(buffer, 3 );  //  FRAME 1 COMMAND BYTE - Command + DAC Address (C3 C2 C1 C0 A3 A2 A1 A0)

  digitalWriteFast(_cs_pin, HIGH);  // End transmission, bring SYNC line high
  //SPI.endTransaction();

 //     digitalWrite(_ldac_pin, HIGH); 
//
 // digitalWrite(_ldac_pin, LOW); 
 //     digitalWrite(_ldac_pin, HIGH); 

 // delay(1);
 // digitalWriteFast(_cs_pin, LOW);  // Begin transmission, bring SYNC line low
 // buffer[1] = 0xFF;
 // buffer[2] = 0xFF;
 //   buffer[0] = (0x02 << 4) + dac;
 //     spi4teensy3::send(buffer, 3 );  //  FRAME 1 COMMAND BYTE - Command + DAC Address (C3 C2 C1 C0 A3 A2 A1 A0)
//
 // digitalWriteFast(_cs_pin, HIGH);  // End transmission, bring SYNC line high

}

int SpiFlash::write(byte* _buf, long _addr, uint16_t _length)
{
	// Below if statement checks that device is ready, _address is within device memory, and there won't be a page overflow writing data
	if (!checkWriteInProgress() && _length <= PAGE_SIZE && check_address(_addr, _length) && checkPageOverflow(_addr, _length))
	{
		digitalWriteFast(SS, LOW);
		send(PAGE_PROGRAM);
		send(_addr >> 16);
		send(_addr >> 8);
		send(_addr);
		#ifdef DEBUG
			Serial.println("Writing Data. . .");
		#endif
		for (int i = 0; i < _length; i++)
		{
			send(*_buf);
			#ifdef DEBUG
				//Serial.println(*_buf, BIN);
			#endif
			_buf++;
		}
		digitalWriteFast(SS, HIGH);
		
		writeEnable();
		
		return 1; // returns 1 if ok
	}

byte SPIPump(byte data) {

	byte receivedData=0;
	
	for (int8_t i=7; i>=0; i--) {
	
		receivedData <<= 1;
	
		if (data & (1<<i)) {
			digitalWriteFast(WLAN_MOSI, HIGH);
			}
		else {
			digitalWriteFast(WLAN_MOSI, LOW);
			}
			
		digitalWriteFast(WLAN_SCK, HIGH);
		asm volatile("nop");
		asm volatile("nop");
			
		digitalWriteFast(WLAN_SCK, LOW);
		
		if (digitalReadFast(WLAN_MISO)) {
			receivedData |= 1;
			}
		
		asm volatile("nop");
		asm volatile("nop");
		
		}
			
	return(receivedData);
	}

// TODO:  Make this non-blocking
void shortBeep(){
#ifdef BEEP
        digitalWriteFast(BUZZER_PIN, HIGH);
        delay(150);
        digitalWriteFast(BUZZER_PIN, LOW);
#endif
}

void LiquidCrystalNew_SSPI::writeByte(byte cmd,byte value){
//start send
#if defined(__FASTSWRITE2__)
	BLOCK_IRQS();
	*portOutputRegister(csport) &= ~ cspin;//low
	ENABLE_IRQS();
#elif defined(__FASTSWRITE__)
	digitalWriteFast(_cs, LOW);
#else
	digitalWrite(_cs, LOW);
#endif

	altSPIwrite(_adrs << 1);//in write, in read: SPI.transfer((addr << 1) | 1);
	
	//this 2 byte blocks instruct the chip
	altSPIwrite(cmd);
	altSPIwrite(value);

	// now closing communication...
#if defined(__FASTSWRITE2__)
	BLOCK_IRQS();
	*portOutputRegister(csport) |= cspin;//hi
	ENABLE_IRQS();
#elif defined(__FASTSWRITE__)
	digitalWriteFast(_cs, HIGH);
#else
	digitalWrite(_cs, HIGH);
#endif
}

// TODO:  Make this non-blocking
void longBeep(){
#ifdef BEEP
        digitalWriteFast(BUZZER_PIN, HIGH);
        delay(350);
        digitalWriteFast(BUZZER_PIN, LOW);
#endif
}

void SDcard::init(uint8_t clock_div)
{
  //init pins
#ifdef SD_PWR_PIN
  pinMode(SD_PWR_PIN,  OUTPUT);
  digitalWriteFast(SD_PWR_PIN, LOW); //power on
#endif
  pinMode(SD_CS_PIN,  OUTPUT);
  digitalWriteFast(SD_CS_PIN, HIGH); //deselect
  pinMode(SD_MISO_PIN, INPUT);
  digitalWriteFast(SD_MISO_PIN, HIGH); //pull-up
  pinMode(SD_MOSI_PIN, OUTPUT);
  pinMode(SD_CLK_PIN, OUTPUT);

#if !defined(SD_SOFTWARE_SPI)
  //SS has to be output or input with pull-up
# if (defined(__AVR_ATmega1280__) || \
      defined(__AVR_ATmega1281__) || \
      defined(__AVR_ATmega2560__) || \
      defined(__AVR_ATmega2561__))     //--- Arduino Mega ---
#  define SD_SS_PORTBIT (0) //PB0
# elif (defined(__AVR_ATmega644__) || \
        defined(__AVR_ATmega644P__))   //--- Arduino 644 ---
#  define SD_SS_PORTBIT (4) //PB4
# else                                 //--- Arduino Uno ---
#  define SD_SS_PORTBIT (2) //PB2
# endif
  if(!(DDRB & (1<<SD_SS_PORTBIT))) //SS is input
  {
      PORTB |= (1<<SD_SS_PORTBIT); //pull-up on
  }

  //init hardware spi
  switch(clock_div)
  {
    case 2:
      SPCR = (1<<SPE)|(1<<MSTR); //enable SPI, Master, clk=Fcpu/4
      SPSR = (1<<SPI2X); //clk*2 = Fcpu/2
      break;
    case 4:
      SPCR = (1<<SPE)|(1<<MSTR); //enable SPI, Master, clk=Fcpu/4
      SPSR = (0<<SPI2X); //clk*2 = off
      break;
    case 8:
      SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR0); //enable SPI, Master, clk=Fcpu/16
      SPSR = (1<<SPI2X); //clk*2 = Fcpu/8
      break;
    case 16:
      SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR0); //enable SPI, Master, clk=Fcpu/16
      SPSR = (0<<SPI2X); //clk*2 = off
      break;
    case 32:
      SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR1); //enable SPI, Master, clk=Fcpu/64
      SPSR = (1<<SPI2X); //clk*2 = Fcpu/32
      break;
  }
#endif

  return;
}

void fcBuffers::finalizeFrame()
{
    // Called in main loop context.
    // Finalize any frames received during the course of this loop iteration,
    // and update the status LED.

    if (flags & CFLAG_NO_ACTIVITY_LED) {
        // LED under manual control
        digitalWriteFast(LED_BUILTIN, flags & CFLAG_LED_CONTROL);
    } else {
        // Use the built-in LED as a USB activity indicator.
        digitalWriteFast(LED_BUILTIN, handledAnyPacketsThisFrame);
    }
    handledAnyPacketsThisFrame = false;

    if (pendingFinalizeFrame) {
        finalizeFramebuffer();
        pendingFinalizeFrame = false;
    }

    if (pendingFinalizeLUT) {
        finalizeLUT();
        pendingFinalizeLUT = false;
    }

    // Let the USB driver know we may be able to process buffers that were previously deferred
    usb_rx_resume();
}

void hexbright::set_light_level(unsigned long level) {
  // LOW 255 approximately equals HIGH 48/49.  There is a color change.
  // Values < 4 do not provide any light.
  // I don't know about relative power draw.
  
  // look at linearity_test.ino for more detail on these algorithms.
  
#if (DEBUG==DEBUG_LIGHT)
  Serial.print("light level: ");
  Serial.println(level);
#endif
  pinModeFast(DPIN_PWR, OUTPUT);
  digitalWriteFast(DPIN_PWR, HIGH);
  if(level == 0) {
    // lowest possible power, but still running (DPIN_PWR still high)
    digitalWriteFast(DPIN_DRV_MODE, LOW);
    analogWrite(DPIN_DRV_EN, 0);
  }
  else if(level<=500) {
    digitalWriteFast(DPIN_DRV_MODE, LOW);
    analogWrite(DPIN_DRV_EN, .000000633*(level*level*level)+.000632*(level*level)+.0285*level+3.98);
  } else {
    level -= 500;
    digitalWriteFast(DPIN_DRV_MODE, HIGH);
    analogWrite(DPIN_DRV_EN, .00000052*(level*level*level)+.000365*(level*level)+.108*level+44.8);
  }
}

inline void hexbright::_led_off(unsigned char led) {
  if(led == RLED) { // DPIN_RLED_SW
    digitalWriteFast(DPIN_RLED_SW, LOW);
    pinModeFast(DPIN_RLED_SW, INPUT);
  } else { // DPIN_GLED
    digitalWriteFast(DPIN_GLED, LOW);
  }
}

void ssd1351_init_board(void) {
	pinMode(SSD1351_R, OUTPUT);
	pinMode(SSD1351_CS, OUTPUT);
	pinMode(SSD1351_DC, OUTPUT);
	digitalWriteFast(SSD1351_R, 1);
	digitalWriteFast(SSD1351_CS, 1);
	digitalWriteFast(SSD1351_DC, 1);
}

// init
// Initializes the vision chips for normal operation.  Sets vision chip pins to low outputs, clears
// chip registers, sets biases and config register.  Arduino pin numbers must be specified.  For
// the remaining parameters if no parameters are passed in, default values are used.
void Stonyman::init( char inPinRESP, char inPinINCP, char inPinRESV, char inPinINCV,
                     char inPinINPHI, char inPinANALOG1, char inPinANALOG2, short vref, short nbias,
                     short aobias, char gain, char selamp )
{
  short config;

/* TODO russ: use later
  if(Stonyman::MAX_GAIN < inGain)
  {
    return Stonyman::RC_ERROR_BADPARAM;
  }
  // TODO russ: there are likely other checks to perform (vref, nbias, aobias)
*/
  pinRESP = inPinRESP;
  pinINCP = inPinINCP;
  pinRESV = inPinRESV;
  pinINCV = inPinINCV;
  pinINPHI = inPinINPHI;
  pinANALOG1 = inPinANALOG1;
  pinANALOG2 = inPinANALOG2;

  // set all digital pins to output
  pinMode(pinRESP, OUTPUT);
  pinMode(pinINCP, OUTPUT);
  pinMode(pinRESV, OUTPUT);
  pinMode(pinINCV, OUTPUT);
  pinMode(pinINPHI, OUTPUT);
  // FIXME russ: is this necessary?
  pinMode(pinANALOG1, INPUT);
  pinMode(pinANALOG2, INPUT);

  // set all pins low
  digitalWriteFast(pinRESP, 0);
  digitalWriteFast(pinINCP, 0);
  digitalWriteFast(pinRESV, 0);
  digitalWriteFast(pinINCV, 0);
  digitalWriteFast(pinINPHI, 0);

  //clear all chip register values
  clearValues();

  //set up biases
  // TODO russ: haven't looked at what this function does
  setBiases(vref,nbias,aobias);

  // sanitize this input before use
  flagUseAmplifier=selamp ? 1:0;

  config=gain+(flagUseAmplifier*8)+(16);

  //turn chip on with config value
  setPointerValue(Stonyman::REG_CONFIG,config);

// TODO russ: use later
//  return Stonyman::RC_OK;
}

int PS2Pad::init(bool disableInt) {

	PS2Pad::_disableInt = disableInt;

	pinModeFast(DAT_PIN, INPUT);
	digitalWriteFast(DAT_PIN, HIGH);

	pinModeFast(CLK_PIN, OUTPUT);
	pinModeFast(ATT_PIN, OUTPUT);
	pinModeFast(CMD_PIN, OUTPUT);

	// Init pad

	digitalWriteFast(CLK_PIN, HIGH);
	digitalWriteFast(CMD_PIN, HIGH);

	PS2Pad::read();

	if(PS2Pad::_pad_data[1] != 0x41 && PS2Pad::_pad_data[1] != 0x73 && PS2Pad::_pad_data[1] != 0x79) {
		return 1;
	}

	PS2Pad::_read_delay = 1;

	for(byte i = 0; i <= 2; i++) {

		// Enter Config Mode
		byte enter_config_command[] = {0x01, 0x43, 0x00, 0x01, 0x00};
		PS2Pad::send_command(enter_config_command, 5);

		// Get pad type
		byte get_pad_type_command[] = {0x01, 0x45, 0x00, 0x5A, 0x5A, 0x5A, 0x5A, 0x5A, 0x5A};
		PS2Pad::send_command(get_pad_type_command, 9);

		PS2Pad::_type = get_pad_type_command[3];

		// Lock to Analog Mode on Stick
		byte lock_analog_mode_command[] = {0x01, 0x44, 0x00, 0x01, 0x03, 0x00, 0x00, 0x00, 0x00};
		PS2Pad::send_command(lock_analog_mode_command, 9);

		// Exit config mode
		byte exit_config_command[] = {0x01, 0x43, 0x00, 0x00, 0x5A, 0x5A, 0x5A, 0x5A, 0x5A};
		PS2Pad::send_command(exit_config_command, 9);

		PS2Pad::read();

		if(PS2Pad::_pad_data[1] == 0x73) {
			_analogMode = true;
			break;
		}

		PS2Pad::_read_delay++;
	}

	return 0;
}

static void prvQueueReceiveTask(void *pvParameters)
{
	while(1)
		{
				digitalWriteFast(13, HIGH);
				delay(10);
				digitalWriteFast(13, LOW);
				delay(1000);
		}
}