#ifndef __icsp_h_included__
#define __icsp_h_included__

#ifndef ESP8266
#include <util/delay.h>
#include <avr/io.h>
#else
//#include "Arduino.h"
#define _delay_ms(a) delay(a)
#endif
#include <stdio.h>

#if defined(ESP8266)
  #include "SPI.h"
  #define ICSP_SS                 16
  #define ICSP_MOSI               13
  #define ICSP_MISO               12
  #define ICSP_SCK                14
  #define ICSP_SPI_SPEED_SLOW     125000
  #define ICSP_SPI_SPEED_FAST     2000000
  #define ICSP_SPI_SPEED(d)       SPI.setFrequency(d);
  #define ICSP_SPI_START()        SPI.begin(); ICSP_SPI_SPEED(ICSP_SPI_SPEED_FAST);
  #define ICSP_SPI_STOP()         SPI.end();
  #define ICSP_SPI_TRANSFER(b)    b = SPI.transfer(b);
  #define ICSP_RESET_START()      pinMode(ICSP_SS, OUTPUT)
  #define ICSP_RESET_STOP()       pinMode(ICSP_SS, INPUT_PULLUP)
  #define ICSP_RESET_HIGH()       digitalWrite(ICSP_SS, HIGH)
  #define ICSP_RESET_LOW()        digitalWrite(ICSP_SS, LOW)
#else //ATmegaX8
  #define ICSP_PIN                PINB
  #define ICSP_PORT               PORTB
  #define ICSP_DDR                DDRB
  #define ICSP_SS                 0x04
  #define ICSP_MOSI               0x08
  #define ICSP_MISO               0x10
  #define ICSP_SCK                0x20
  #define ICSP_SPI_SPEED_SLOW     0x03  // 125KHz@16MHz
  #define ICSP_SPI_SPEED_FAST     0x11  // 2MHz@16MHz
  #define ICSP_SPI_SPEED(d)       SPCR = (SPCR & ~3) | ((d) & 3); if((d) & 0x10) SPSR |= 1; else SPSR &= ~1;
  #define ICSP_SPI_START()        ICSP_DDR |= (ICSP_SCK | ICSP_MOSI); ICSP_DDR &= ~ICSP_MISO; SPCR = (1 << MSTR) | (1 << SPE); ICSP_SPI_SPEED(ICSP_SPI_SPEED_FAST);
  #define ICSP_SPI_STOP()         SPCR = 0; SPSR = 0; ICSP_DDR &= ~(ICSP_MOSI | ICSP_SCK);
  #define ICSP_SPI_TRANSFER(b)    SPDR = b; while (!(SPSR & (1 << SPIF))); b = SPDR;
  #define ICSP_RESET_START()      ICSP_DDR |= ICSP_SS;
  #define ICSP_RESET_STOP()       ICSP_DDR &= ~ICSP_SS
  #define ICSP_RESET_HIGH()       ICSP_PORT |= ICSP_SS;
  #define ICSP_RESET_LOW()        ICSP_PORT &= ~ICSP_SS;
#endif

#define ICSP_SEND_FAST(a,b,c,d) (__icsp_transaction(a,b,c,d))
#define ICSP_SEND_SLOW(a,b,c,d) (__icsp_slow_transaction(a,b,c,d))
#define ICSP_SEND_BUSY(a,b,c,d) (__icsp_busy_transaction(a,b,c,d))

#define ICSP_BUSY()             (ICSP_SEND_FAST(0xF0, 0x00, 0x00, 0x00) & 0x01)
#define ICSP_GET_SIG0()         (ICSP_SEND_FAST(0x30, 0x00, 0x00, 0x00))
#define ICSP_GET_SIG1()         (ICSP_SEND_FAST(0x30, 0x00, 0x01, 0x00))
#define ICSP_GET_SIG2()         (ICSP_SEND_FAST(0x30, 0x00, 0x02, 0x00))
#define ICSP_GET_FUSE_LOCK()    (ICSP_SEND_FAST(0x58, 0x00, 0x00, 0x00))
#define ICSP_GET_FUSE_LOW()     (ICSP_SEND_FAST(0x50, 0x00, 0x00, 0x00))
#define ICSP_GET_FUSE_HIGH()    (ICSP_SEND_FAST(0x58, 0x08, 0x00, 0x00))
#define ICSP_GET_FUSE_EXT()     (ICSP_SEND_FAST(0x50, 0x08, 0x00, 0x00))

#define ICSP_SET_FUSE_LOCK(f)   (ICSP_SEND_SLOW(0xAC, 0xE0, 0x00, (f)))
#define ICSP_SET_FUSE_LOW(f)    (ICSP_SEND_SLOW(0xAC, 0xA0, 0x00, (f)))
#define ICSP_SET_FUSE_HIGH(f)   (ICSP_SEND_SLOW(0xAC, 0xA8, 0x00, (f)))
#define ICSP_SET_FUSE_EXT(f)    (ICSP_SEND_SLOW(0xAC, 0xA4, 0x00, (f)))
#define ICSP_SET_LOCK_NONE()    (ICSP_SEND_SLOW(0xAC, 0xFF, 0x00, 0x00))
#define ICSP_SET_LOCK_WRITE()   (ICSP_SEND_SLOW(0xAC, 0xFD, 0x00, 0x00))
#define ICSP_SET_LOCK_BOTH()    (ICSP_SEND_SLOW(0xAC, 0xFC, 0x00, 0x00))

#define ICSP_READ_LOW(a)        (ICSP_SEND_FAST(0x20, ((a) >> 8) & 0xFF, (a) & 0xFF, 0))
#define ICSP_READ_HIGH(a)       (ICSP_SEND_FAST(0x28, ((a) >> 8) & 0xFF, (a) & 0xFF, 0))
#define ICSP_WRITE_LOW(a,v)     (ICSP_SEND_FAST(0x40, ((a) >> 8) & 0xFF, (a) & 0xFF, (v)))
#define ICSP_WRITE_HIGH(a,v)    (ICSP_SEND_FAST(0x48, ((a) >> 8) & 0xFF, (a) & 0xFF, (v)))
#define ICSP_READ_EEPROM(a)     (ICSP_SEND_FAST(0xA0, ((a) >> 8) & 0xFF, (a) & 0xFF, 0xFF))
#define ICSP_WRITE_EEPROM(a,v)  (ICSP_SEND_BUSY(0xC0, ((a) >> 8) & 0xFF, (a) & 0xFF, (v)))

#define ICSP_START()            (ICSP_SEND_BUSY(0xAC, 0x53, 0x00, 0x00))
#define ICSP_ERASE()            (ICSP_SEND_BUSY(0xAC, 0x80, 0x00, 0x00))
#define ICSP_COMMIT_PAGE(a)     (ICSP_SEND_BUSY(0x4C, ((a) >> 8) & 0xFF, (a) & 0xFF, 0))
#define ICSP_SET_EXTENDED(a)    (ICSP_SEND_BUSY(0x4D, 0x00, (a), 0x00))

#define ICSP_ENTER_PROG()       ICSP_RESET_HIGH(); ICSP_RESET_START(); ICSP_SPI_START(); ICSP_RESET_HIGH(); _delay_ms(70); ICSP_RESET_LOW(); _delay_ms(30); ICSP_START();
#define ICSP_LEAVE_PROG()       ICSP_SPI_STOP(); ICSP_RESET_HIGH(); ICSP_RESET_STOP();

static inline uint8_t __icsp_transaction(uint8_t a, uint8_t b, uint8_t c, uint8_t d) {
  ICSP_SPI_TRANSFER(a);
  ICSP_SPI_TRANSFER(b);
  ICSP_SPI_TRANSFER(c);
  ICSP_SPI_TRANSFER(d);
  return d;
}

static inline uint8_t __icsp_busy_transaction(uint8_t a, uint8_t b, uint8_t c, uint8_t d) {
  uint8_t result = ICSP_SEND_FAST(a,b,c,d);
  while(ICSP_BUSY()) _delay_ms(0);
  return result;
}

static inline uint8_t __icsp_slow_transaction(uint8_t a, uint8_t b, uint8_t c, uint8_t d) {
  ICSP_SPI_SPEED(ICSP_SPI_SPEED_SLOW);
  uint8_t result = ICSP_SEND_BUSY(a,b,c,d);
  ICSP_SPI_SPEED(ICSP_SPI_SPEED_FAST);
  return result;
}

#endif