#include "amswirelib.hpp"

uint8_t read_portsD()
{
  return PIND;
}

uint8_t read_portsB()
{
  return PINB;
}

uint8_t read_portsC()
{
  return PINC;
}

uint8_t read_portD(uint8_t N)
{
  return (PIND & (1<<N))>>N;
}

uint8_t read_portB(uint8_t N)
{
  return (PINB & (1<<N))>>N;
}

uint8_t read_portC(uint8_t N)
{
  return (PINC & (1<<N))>>N;
}

void write_portsD(uint8_t V)
{
  PORTD = V;
}

void write_portsB(uint8_t V)
{
  PORTB = V;
}

void write_portsC(uint8_t V)
{
  PORTC = V;
}

void write_portD(uint8_t N, uint8_t V)
{
  PORTD = (PORTD & ~(1<<N)) | ((V & 0x01)<<N);
}

void write_portB(uint8_t N, uint8_t V)
{
  PORTB = (PORTB & ~(1<<N)) | ((V & 0x01)<<N);
}

void write_portC(uint8_t N, uint8_t V)
{
  PORTC = (PORTC & ~(1<<N)) | ((V & 0x01)<<N);
}

void set_portsmodeD(uint8_t iomode)
{
  DDRD = iomode;
}
void set_portsmodeB(uint8_t iomode)
{
  DDRB = iomode;
}
void set_portsmodeC(uint8_t iomode)
{
  DDRC = iomode;
}

void set_portmodeD(uint8_t N, uint8_t iomode)
{
  DDRD = (DDRD & ~(1<<N)) | ((iomode & 0x01)<<N);
}

void set_portmodeB(uint8_t N, uint8_t iomode)
{
  DDRB = (DDRB & ~(1<<N)) | ((iomode & 0x01)<<N);
}

void set_portmodeC(uint8_t N, uint8_t iomode)
{
  DDRC = (DDRC & ~(1<<N)) | ((iomode & 0x01)<<N);
}

//Numbered Pin Digital IO
//amsPN mapping for Arduino Nano
//0-7 - PORTD
//8-15 - PORTB
//16-23 - PORTC (Analog Pins)

//1 - output, 0 - input
void set_pinmode(uint8_t amsPN, uint8_t iomode)
{
  uint8_t N;
  if(amsPN>=0&&amsPN<=7)
  {
    N = amsPN-0;
    DDRD = (DDRD & ~(1<<N)) | ((iomode & 0x01)<<N);
  }
  else if(amsPN>=8 && amsPN<=15)
  {
    N = amsPN-8;
    DDRB = (DDRB & ~(1<<N)) | ((iomode & 0x01)<<N);
  }
  else if(amsPN>=16&&amsPN<=23)
  {
    N = amsPN-16;
    DDRC = (DDRC & ~(1<<N)) | ((iomode & 0x01)<<N);
  }
  else
  {
    //nothing;
  }
  return;
}

void write_pin(uint8_t amsPN, uint8_t V)
{
  uint8_t N;
  if(amsPN>=0&&amsPN<=7)
  {
    N = amsPN-0;
    PORTD = (PORTD & ~(1<<N)) | ((V & 0x01)<<N);
  }
  else if(amsPN>=8 && amsPN<=15)
  {
    N = amsPN-8;
    PORTB = (PORTB & ~(1<<N)) | ((V & 0x01)<<N);
  }
  else if(amsPN>=16&&amsPN<=23)
  {
    N = amsPN-16;
    PORTC = (PORTC & ~(1<<N)) | ((V & 0x01)<<N);
  }
  else
  {
    //nothing;
  }
  return;
}

uint8_t read_pin(uint8_t amsPN)
{
  uint8_t ret = 0;
  uint8_t N;
  if(amsPN>=0&&amsPN<=7)
  {
    N = amsPN-0;
    ret = (PIND & (1<<N))>>N;
  }
  else if(amsPN>=8 && amsPN<=15)
  {
    N = amsPN-8;
    ret = (PINB & (1<<N))>>N;
  }
  else if(amsPN>=16&&amsPN<=23)
  {
    N = amsPN-16;
    ret = (PINC & (1<<N))>>N;
  }
  else
  {
    //nothing;
  }
  return ret;;
}