#include "i2c_master.h"
#include "ch32fun.h"
#include <stdio.h>
#include "ch32common.h"


// use defines to make more meaningful names for our GPIO pins
#define SDA PC1
#define SCK PC2


/****************************

 i2c connections
 
 PC1 SDA
 PC2 SCL

****************************/
void writeDS(uint8_t) ;

#define Dev1307  0xD0
#define Dev8574a 0x70
#define Dev8574  0x40


#define MOUTHBIT1 2
#define MOUTHBIT2 3
#define LEFTEYE   0
#define RIGHTEYE  1
#define FORWARD   5
#define REVERSE   4
#define UP        6
#define DOWN      7

uint8_t buffer[4];

uint8_t digout;

void setTrim(uint32_t trim) {
   uint32_t regtemp;

   regtemp = RCC->CTLR & ~RCC_HSITRIM;
   regtemp |= trim<<3;
   RCC->CTLR = regtemp;
}


void uartInit( uint16_t realBaud ) {


   // Enable GPIOD and UART.
   RCC->APB2PCENR |= RCC_APB2Periph_GPIOD | RCC_APB2Periph_USART1;

   // Push-Pull, 10MHz Output, GPIO D5, with AutoFunction

   funPinMode(PD6, GPIO_Speed_10MHz | GPIO_CNF_OUT_PP_AF); 

   //  8n1.  Note if you don't specify a mode, UART remains off even when UE_Set.
   USART1->CTLR1 = USART_WordLength_8b | USART_Parity_No | USART_Mode_Tx | USART_Mode_Rx;
   USART1->CTLR2 = USART_StopBits_1;
   USART1->CTLR3 = USART_HardwareFlowControl_None;

   USART1->BRR = (((FUNCONF_SYSTEM_CORE_CLOCK) + (realBaud)/2) / (realBaud));
   USART1->CTLR1 |= CTLR1_UE_Set;

   // swap the RX/TX pins so that we use the SWIO pin as RX
//   RCC->APB2PCENR |= RCC_AFIOEN;  
//   AFIO->PCFR1 |= AFIO_PCFR1_USART1_REMAP_1;

  // Make sure USART->CTLR1 has the RE, TE and UE flags set
 
 
}

void putChar(char c) {
   while( !(USART1->STATR & USART_FLAG_TXE)); //USART_FLAG_TC));
   USART1->DATAR = c;
}

int checkRx(void) {
   return ( USART1->STATR & USART_STATR_RXNE);
}

char getChar( void ) {
   while( ( USART1->STATR & USART_STATR_RXNE) == 0 );
   return USART1->DATAR;
}


uint8_t charRx (char c) {

  static uint8_t state = 0;
  static uint8_t ch;
  static uint16_t V;
  
  if ((c == '\r') || (c == '\n')) {
    state = 0;
    return 0;
  }

  switch(state) {

    case 0:        // Channel 
      ch = 0;
      switch(c) {
        case 'D':
          ch++;
        case 'C':
          ch++;
        case 'B':
          ch++;
        case 'A':
          V = 0;
          state = 1;
        break;           
      }
    return 0;
    break;  

    case 1:   // dig1
      if ((c>='0') && (c<='2')) {
        V = (c-'0')*100;
        state++;
      } else state = 1;
    return 0;
    break;

    case 2:   // dig2
      if ((c>='0') && (c<='9')) {
        V += (c-'0')*10;
        state++;
      } else state = 1;
    return 0;
    break;

    case 3:   // dig3
      if ((c>='0') && (c<='9')) {
        V += (c-'0');
        if (V > 255) V = 255;
        buffer[ch]  = V;
      }
      state = 0;
    return 1;
    break;

    default:
      state = 0;
      return 0;
    break;

  }

}



int main() {
  unsigned long timeout;
  SystemInit();
   
  setTrim(15);  // Todays value is 15, 15.

  Delay_Ms( 100); // reprogramming delay

  // Enable GPIOs
  funGpioInitAll();

  funPinMode( SDA,  GPIO_CFGLR_OUT_50Mhz_PP );
  funPinMode( SCL,  GPIO_CFGLR_OUT_50Mhz_PP );
  i2c_init();
  i2c_stop();
  
   uartInit( 9600 );
   buffer[0] = 128;
   buffer[1] = 128;  
   buffer[2] = 128;
   buffer[3] = 128;  
  
   timeout = 0;
   
  while(1)	{
    
  timeout++; 
     if (checkRx()) {
       if (charRx(getChar()) != 0) { // did we get a new value?
         timeout = 0;

         digout = 0;
         
         // drive
         if (0) {
         } else if (buffer[2] > 144) {
            SetBit(FORWARD, digout); 
         } else if (buffer[2] < 112) {
            SetBit(REVERSE, digout); 
         } else {

         }

         // arms
         if (0) {
         } else if (buffer[3] > 144) {
             SetBit(UP, digout); 
         } else if (buffer[3] < 112) {
             SetBit(DOWN, digout); 
         } else {

         }    
         
         // mouth
         if (0) {
         } else if (buffer[1] > 125) {
         } else if (buffer[1] < 112) {
           SetBit(MOUTHBIT1, digout); 
           SetBit(MOUTHBIT2, digout); 
         } else {
           SetBit(MOUTHBIT1, digout); 
         }

         // eyes
         if (0) {
         } else if (buffer[0] > 135) {
           SetBit(LEFTEYE, digout); 
         } else if (buffer[0] < 125) {
           SetBit(RIGHTEYE, digout);
         } else {

         }  
	 
	 writeDS(digout);       

       }
     }          
     
     if (timeout > 2000000) { // signal loss timeout, set to what is needed.
     
       buffer[0] = 128;
       buffer[1] = 128;
       buffer[2] = 128;
       buffer[3] = 128;
       digout = 0x0F;
       
       writeDS(digout);   
     }    
           
   
  }
}


void writeDS(uint8_t data) {
  i2c_start(Dev8574 + I2C_WRITE);      
  i2c_write(data); 
  i2c_stop();
}