#define CS_HIGH         PORTB |= (1 << 2)
#define CS_LOW          PORTB &= ~(1 << 2)

uint8_t SPI_SendByte(uint8_t byte);
void AT45DB161_Init();
void AT45DB161_Read_ID(uint8_t *MID, uint8_t *DID);
uint8_t AT45DB161_Read_Status();
void AT45DB161_Read_Data(uint16_t page, uint16_t addr, uint32_t length, uint8_t *out);
void AT45DB161_PageProgram(uint16_t page, uint8_t *dat, uint16_t length);

uint8_t SPI_SendByte(uint8_t byte){
    SPDR = byte;
    while (!(SPSR & (1 << SPIF)));
    return SPDR;
}

void AT45DB161_Init(){
     DDRB |= (1 << 3) | (1 << 5) | (1 << 2);
     SPCR = (1 << SPE) | (1 << MSTR) | (1 << SPR0);
     CS_HIGH;
}

void AT45DB161_Read_ID(uint8_t *MID, uint8_t *DID){
    CS_LOW;
    SPI_SendByte(0x9F);
    *MID = SPI_SendByte(0x00);
    *DID = SPI_SendByte(0x00);
    SPI_SendByte(0x00);
    SPI_SendByte(0x00);
    SPI_SendByte(0x00);
    SPI_SendByte(0x00);
    CS_HIGH;
}

uint8_t AT45DB161_Read_Status(){
    uint8_t temp;
    CS_LOW;
    SPI_SendByte(0xD7);
    temp = SPI_SendByte(0x00);
    CS_HIGH;
    return temp;
}


void AT45DB161_Read_Data(uint16_t page, uint16_t addr, uint32_t length, uint8_t *out){
    uint32_t i;
    uint8_t temp;
    do {
        temp = AT45DB161_Read_Status();
    } while (!(temp & 0x80));
    if (temp & 0x01){   //512
        i = ((page << 9) | (addr & 0x1FF));
    } else {            //528
        i = ((page << 10) | (addr & 0x3FF));
    }
    CS_LOW;
    SPI_SendByte(0x0B);
    SPI_SendByte(i >> 16);
    SPI_SendByte(i >> 8);
    SPI_SendByte(i);
    SPI_SendByte(0x00);
    for (i = 0; i < length; i++){
        out[i] = SPI_SendByte(0xFF);
    }
    CS_HIGH;
}

void AT45DB161_PageProgram(uint16_t page, uint8_t *dat, uint16_t length){
    uint16_t i;
    uint8_t temp;
    temp = AT45DB161_Read_Status();
    CS_LOW;
    SPI_SendByte(0x84);
    SPI_SendByte(0x00);
    SPI_SendByte(0x00);
    SPI_SendByte(0x00);
    for (i = 0; i < length; i++){
        SPI_SendByte(dat[i]);
    }
    CS_HIGH;
    CS_LOW;
    SPI_SendByte(0x83);
    if (temp & 0x01){   //512
        SPI_SendByte((uint8_t)(page >> 7));
        SPI_SendByte((uint8_t)((page & 0x7F) << 1));
        SPI_SendByte(0x00);
    } else {            //528
        SPI_SendByte((uint8_t)(page >> 6));
        SPI_SendByte((uint8_t)((page & 0x3F) << 2));
        SPI_SendByte(0x00);
    }
    CS_HIGH;
    while (!(AT45DB161_Read_Status() & 0x80));
}