#include <stdlib.h>
#include <string.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/eeprom.h>

#include "interface.h"
#include "spi.h"
#include "tools.h"
#include "extern_vars.h"

//Atmel Includes
#include "command.h"

#include "config.h"

//States used in the receive state machine
#define	ST_START		0xF000
#define	ST_GET_SEQ_NUM	0xF001
#define ST_MSG_SIZE_1	0xF002
#define ST_MSG_SIZE_2	0xF003
#define ST_GET_TOKEN	0xF004
#define ST_GET_DATA		0xF005
#define	ST_GET_CHECK	0xF006
#define	ST_PROCESS		0xF007


void chip_init(void);
unsigned char adc_get(char n);
void process_command(unsigned char seq_num);

//Some global variables
unsigned char msg_buffer[300];
unsigned char clock_speed;
unsigned char reset_polarity;
unsigned char param_controller_init;
unsigned char prgmode;

unsigned long address;

int main(void)
{
  unsigned int i=0;

  unsigned char seq_num=0;
  unsigned int msg_size=0;
  unsigned char checksum=0;

  /// next state
  unsigned int state=ST_START;

  unsigned char t;
  signed int rec_data;

  chip_init();
  interface_init();
  spi_disable();

  clock_speed=eeprom_read_byte(&eeprom_sck_period);
  if (clock_speed==0xFF) clock_speed=SPI_SPEED_125KHZ;
  spi_set_speed(clock_speed);

  sei();

  LED_GN_ON;
  LED_RT_OFF;

  /*
  while(2)
  {
  	rec_data=interface_getc();
  	//if a byte has been received -> process it
  	if(rec_data!=-1)
  	{
  		interface_putc(rec_data);
  		LED_GN_TOG;
  		LED_RT_TOG;
  	}
  }
  */

  while (2)
    {
      rec_data=interface_getc();
      //if a aviliable byte has been received -> process it.
      if (rec_data!=-1)
        {
          t=rec_data&0xFF;

          if (state==ST_START)
            {
              if (t==MESSAGE_START)
                {
                  state = ST_GET_SEQ_NUM;
                  checksum = MESSAGE_START^0;
                }
            }
          else if (state==ST_GET_SEQ_NUM)
            {
              seq_num = t;
              state = ST_MSG_SIZE_1;
              checksum ^= t;
            }
          else if (state==ST_MSG_SIZE_1)
            {
              msg_size = t<<8;
              state = ST_MSG_SIZE_2;
              checksum ^= t;
            }
          else if (state==ST_MSG_SIZE_2)
            {
              msg_size |= t;
              state = ST_GET_TOKEN;
              checksum ^= t;
            }
          else if (state==ST_GET_TOKEN)
            {
              if (t==TOKEN)
                {
                  state = ST_GET_DATA;
                  checksum ^= t;
                  i=0;
                }
              else state = ST_START;
            }
          else if (state==ST_GET_DATA)
            {
              msg_buffer[i++] = t;
              checksum ^= t;
              if (i==msg_size)
                {
                  state = ST_GET_CHECK;
                }
            }
          else if (state==ST_GET_CHECK)
            {
              if (t==checksum)
                {
                  process_command(seq_num);
                }

              state = ST_START;
            }

        }//if(rec_data!=-1)
    }//while(2)

  return 0;

}

/**
	Returns the value of the selected adc channel

	@param n Number of ADC channel

	@return  Value of ADC channel
*/
unsigned char adc_get(char n)
{
  ADMUX&=0xF0;
  ADMUX|=n&0x7;

  while (ADCSRA&0x40);
  ADCSRA|=(1<<ADSC);
  while (ADCSRA&0x40);

  return ADCH;
}

/**
	Processes a command packet received in main loop and located in the global buffer

	@param	seq_num	The sequence number of the command packet
*/
void process_command(unsigned char seq_num)
{
  unsigned char cmd;
  unsigned char tmp=0;
  unsigned char mode;
  unsigned int block_size;
  unsigned int i;
  unsigned int num_bytes=0;
  unsigned long polling_address=0;
  unsigned long start_address = address;

  cmd = msg_buffer[0];

  //////////////////////////////////////
  //CMD_SIGN_ON
  //////////////////////////////////////
  if (cmd==CMD_SIGN_ON)
    {
      num_bytes = 11;
      msg_buffer[0] = CMD_SIGN_ON;
      msg_buffer[1] = STATUS_CMD_OK;
      msg_buffer[2] = 8;
      memcpy(msg_buffer+3,"AVRISP_2",8);
    }
  //////////////////////////////////////
  //CMD_SET_PARAMETER
  //////////////////////////////////////
  else if (cmd==CMD_SET_PARAMETER)
    {
      switch (msg_buffer[1])
        {
        case PARAM_SCK_DURATION:
          clock_speed=SPI_SPEED_2MHZ;
          if ((msg_buffer[2]) >= 1  ) clock_speed=SPI_SPEED_2MHZ;
          if ((msg_buffer[2]) >= 2  ) clock_speed=SPI_SPEED_1MHZ;
          if ((msg_buffer[2]) >= 4  ) clock_speed=SPI_SPEED_500KHZ;
          if ((msg_buffer[2]) >= 8  ) clock_speed=SPI_SPEED_250KHZ;
          if ((msg_buffer[2]) >= 16 ) clock_speed=SPI_SPEED_125KHZ;
          if ((msg_buffer[2]) >= 32 ) clock_speed=SPI_SPEED_62KHZ;
          if (eeprom_read_byte(&eeprom_sck_period)!=clock_speed) eeprom_write_byte(&eeprom_sck_period,clock_speed);
          spi_set_speed(clock_speed);
          break;
        case PARAM_RESET_POLARITY:
          reset_polarity = msg_buffer[2];
          break;
        case PARAM_CONTROLLER_INIT:
          param_controller_init = msg_buffer[2];
          break;
        }

      num_bytes = 2;
      msg_buffer[0] = CMD_SET_PARAMETER;
      msg_buffer[1] = STATUS_CMD_OK;
    }
  //////////////////////////////////////
  //CMD_GET_PARAMETER
  //////////////////////////////////////
  else if (cmd==CMD_GET_PARAMETER)
    {
      switch (msg_buffer[1])
        {
        case PARAM_BUILD_NUMBER_LOW:
          tmp = CONFIG_PARAM_BUILD_NUMBER_LOW;
          break;
        case PARAM_BUILD_NUMBER_HIGH:
          tmp = CONFIG_PARAM_BUILD_NUMBER_HIGH;
          break;
        case PARAM_HW_VER:
          tmp = CONFIG_PARAM_HW_VER;
          break;
        case PARAM_SW_MAJOR:
          tmp = CONFIG_PARAM_SW_MAJOR;
          break;
        case PARAM_SW_MINOR:
          tmp = CONFIG_PARAM_SW_MINOR;
          break;
        case PARAM_SCK_DURATION:
          tmp = clock_speed;
          break;
        case PARAM_RESET_POLARITY:
          tmp = reset_polarity;
          break;
        case PARAM_CONTROLLER_INIT:
          tmp = param_controller_init;
          break;
        }

      num_bytes = 3;
      msg_buffer[0] = CMD_GET_PARAMETER;
      msg_buffer[1] = STATUS_CMD_OK;
      msg_buffer[2] = tmp;
    }
  //////////////////////////////////////
  //CMD_LOAD_ADDRESS
  //////////////////////////////////////
  else if (cmd==CMD_LOAD_ADDRESS)
    {
      address =  ((unsigned long)msg_buffer[1])<<24;
      address |= ((unsigned long)msg_buffer[2])<<16;
      address |= ((unsigned long)msg_buffer[3])<<8;
      address |= ((unsigned long)msg_buffer[4]);

      num_bytes = 2;
      msg_buffer[0] = CMD_LOAD_ADDRESS;
      msg_buffer[1] = STATUS_CMD_OK;
    }
  //////////////////////////////////////
  //CMD_ENTER_PROGMODE_ISP
  //////////////////////////////////////
  else if (cmd==CMD_ENTER_PROGMODE_ISP)
    {
      //msg_buffer[1] //timeout		//Command time-out (in ms)
      //msg_buffer[2] //stabDelay		//Delay (in ms) used for pin stabilization
      //msg_buffer[3] //cmdexeDelay	//Delay (in ms) in connection with the EnterProgMode command execution
      //msg_buffer[4] //synchLoops	//Number of synchronization loops
      //msg_buffer[5] //byteDelay		//Delay (in ms) between each byte in the EnterProgMode command.
      //msg_buffer[6] //pollValue		//Poll value: 0x53 for AVR, 0x69 for AT89xx
      //msg_buffer[7] //pollIndex		//Start address, received byte: 0 = no polling, 3 = AVR, 4 = AT89xx
      //msg_buffer[8] //cmd1			//Command Byte # 1 to be transmitted
      //msg_buffer[9] //cmd2			//Command Byte # 2 to be transmitted
      //msg_buffer[10]//cmd3			//Command Byte # 3 to be transmitted
      //msg_buffer[11]//cmd4			//Command Byte # 4 to be transmitted

      prgmode=1;

      spi_enable();

      wait_ms(msg_buffer[2]);

      LED_GN_ON;
      LED_RT_ON;

      //Try to get connection with the target chip
      for (i=0;i<msg_buffer[4];i++)
        {
          //spi_transfer_16(0xAC53);
          spi_transfer_8(msg_buffer[8]);
          wait_ms(msg_buffer[5]);
          spi_transfer_8(msg_buffer[9]);
          wait_ms(msg_buffer[5]);

          if (msg_buffer[7]==3)
            {
              tmp=spi_transfer_8(msg_buffer[10]);
              wait_ms(msg_buffer[5]);
              spi_transfer_8(msg_buffer[11]);
              wait_ms(msg_buffer[5]);
            }
          else
            {
              spi_transfer_8(msg_buffer[10]);
              wait_ms(msg_buffer[5]);
              tmp=spi_transfer_8(msg_buffer[11]);
              wait_ms(msg_buffer[5]);
            }

          if ((tmp==msg_buffer[6])||(msg_buffer[7]==0))
            {
              LED_GN_OFF;
              LED_RT_ON;
              i=0xFF;
              break;
            }

          spi_clock_pulse();
        }

      num_bytes = 2;
      msg_buffer[0] = CMD_ENTER_PROGMODE_ISP;

      if (i==0xFF)
        {
          msg_buffer[1] = STATUS_CMD_OK;
        }
      else
        {
          msg_buffer[1] = STATUS_CMD_FAILED;
        }
    }
  //////////////////////////////////////
  //CMD_LEAVE_PROGMODE_ISP
  //////////////////////////////////////
  else if (cmd==CMD_LEAVE_PROGMODE_ISP)
    {
      prgmode=0;

      spi_disable();

      LED_RT_OFF;
      LED_GN_ON;

      num_bytes = 2;
      msg_buffer[0] = CMD_LEAVE_PROGMODE_ISP;
      msg_buffer[1] = STATUS_CMD_OK;
    }
  //////////////////////////////////////
  //CMD_CHIP_ERASE_ISP
  //////////////////////////////////////
  else if (cmd==CMD_CHIP_ERASE_ISP)
    {
      spi_transfer_8(msg_buffer[3]);
      spi_transfer_8(msg_buffer[4]);
      spi_transfer_8(msg_buffer[5]);
      spi_transfer_8(msg_buffer[6]);

      //Newer AVR's seems to have a busy bit
      //cant test this because I don't have any of these new chips
      if (msg_buffer[2]==0)
        {
          wait_ms(msg_buffer[1]);
        }
      else //if(msg_buffer[2]==1)
        {
          while (spi_transfer_32(0xF0000000)&1);
        }

      num_bytes = 2;
      msg_buffer[0] = CMD_CHIP_ERASE_ISP;
      msg_buffer[1] = STATUS_CMD_OK;
    }
  //////////////////////////////////////
  //CMD_PROGRAM_FLASH_ISP
  //////////////////////////////////////
  else if (cmd==CMD_PROGRAM_FLASH_ISP)
    {
      block_size = ((unsigned int)msg_buffer[1])<<8;
      block_size |= msg_buffer[2];
      mode = msg_buffer[3];

      //Word Mode
      if ((mode&1) == 0)
        {
          for (i=0;i<block_size;i++)
            {
              //If we have an uneven byte programm the
              //high byte
              if (i&1)
                {
                  spi_transfer_8(msg_buffer[5]|(1<<3));
                }
              else
                {
                  spi_transfer_8(msg_buffer[5]);
                }

              spi_transfer_16(address&0xFFFF);
              spi_transfer_8(msg_buffer[i+10]);

              //Check if we can do polling
              if (msg_buffer[8]!=msg_buffer[i+10])
                {
                  polling_address = address&0xFFFF;
                }
              //If not switch the mode hard waiting
              else
                {
                  mode = (mode&(~0x0E)) | 0x02;
                }

              //Different polling methods
              //Hard waiting
              if ((mode&0x0E) == 0x02)
                {
                  wait_ms(msg_buffer[4]);
                }
              //Data polling
              else if ((mode&0x0E) == 0x04)
                {
                  do
                    {
                      //If we have an uneven byte read the
                      //high byte
                      if (i&1)
                        {
                          spi_transfer_8(msg_buffer[7]|(1<<3));
                        }
                      else
                        {
                          spi_transfer_8(msg_buffer[7]);
                        }
                      spi_transfer_16(polling_address);
                      tmp=spi_transfer_8(0x00);
                    }
                  while (tmp==msg_buffer[8]);
                }
              //RDY/BSY polling
              else if ((mode&0x0E) == 0x08)
                {
                  while (spi_transfer_32(0xF0000000)&1);
                }
              //If something was not correct with the given mode do
              //hard waiting. Should never reach this point
              else
                {
                  wait_ms(msg_buffer[4]);
                }

              //increment (word) address only when we have an uneven (byte) address
              if (i&1) address++;
            }
        }
      //Page Mode
      else
        {
          for (i=0;i<block_size;i++)
            {
              //If we have an uneven byte programm the
              //high byte
              if (i&1)
                {