/*
 * This program operates Dory the robot fish.
 */

/* Include Statement Block */
//#include <NewSoftSerial.h>

#include <PString.h>

#include <inttypes.h>
#include <avr/io.h>
#include <avr/pgmspace.h>
#include <avr/eeprom.h>
#include <avr/interrupt.h>
#include <stdio.h>
#include <util/delay.h>

/* End Include Statement Block */

/* Begin Defines block */
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#define FOSC 16000000 // Clock Speed
#define BAUD 4800
#define MYUBRR FOSC/16/BAUD-1
# define ON 1
# define OFF 0
#define TOGGLE_IO        9  //Arduino pin to toggle in timer ISR
#define TIMER_CLOCK_FREQ 1000000.0 //2MHz for /8 prescale from 16MHz

/* End Defines Block */

/* Begin Global Variable deffinitions */
volatile unsigned char counter = 48;
volatile unsigned char result = 0;
char cFishStatus = ON;
unsigned char ucCommandCnt = 0,
ucCommand_1 = OFF,//jr
ucCommand_2 = OFF,
ucCommand_3 = OFF;
unsigned char ucFrontPositionIndex = 128,
ucRearPositionIndex = 128;
float sensorValue0,
sensorValue1;
char s0[10];
char s1[10];
int sensorpin0 = 0 ;
int sensorpin1 = 1;
// char* current_ascii0= itoa(sensorValue0,s0,10);
// char* current_ascii1= itoa(sensorValue1,s1,10);
int i=0,
j=0;
int inByte=0;
int pos_a = 128,
pos_b = 128,    //variable to store the servo position
pwm1 = 1500,
pwm2 = 1500;
unsigned char dat=0x00;
char cbuffer[40] = "";     // receive buffer
//NewSoftSerial IMU(2, 3);
int stop_char=0,
head_val=0;
char bufarray[20]="";
char comma_counter=0;
int sensorPin = 3;
char iii=0;
//char s[10];
//int sensorPin = 0;    // select the input pin for the potentiometer
//    // select the pin for the LED
//float sensorValue = 0;
/* End Global Variable deffinitions */

PString buffer(cbuffer, sizeof(cbuffer));

void USART_Init( unsigned int ubrr)
{
  sbi(UCSR0A, U2X0);
  sbi(UCSR0B, RXCIE0);
  sbi(UCSR0B, RXEN0);
  sbi(UCSR0B, TXEN0);
  UCSR0C = B00000110;
  //UBRR0H = B0;
  //UBRR0L = B00010000;
  UBRR0H = (unsigned char)(ubrr>>8);
  UBRR0L = (unsigned char)ubrr;
}





void uart_send(unsigned char dat)
{
  while(! (UCSR0A & ( 1 << UDRE0))  );  // until dararegister is NOT empty
  UDR0 = dat;
}

unsigned char uart_read()
{
  while(!  (UCSR0A & (1 << RXC0)) );  // until Receive complete is NOT complete
  return UDR0;
}


ISR(USART_RX_vect)
{
  //   counter++;

  inByte = UDR0;

  //   if (cFishStatus == OFF && inByte ==0x55)
  //    {
  //      UDR0=inByte;
  //      if (++ucCommandCnt == 5)
  //      {
  //         cFishStatus = ON;
  //          uart_send(0x24);
  //      }
  //    return;
  //  }

  if(cFishStatus == ON)
  {


    if(ucCommand_1 == ON)
    {
      ucCommand_1 = OFF;
      ucFrontPositionIndex = inByte;

      uart_send(inByte);


      return;
    }
    if(ucCommand_2 == ON)
    {
      ucCommand_2 = OFF;
      ucRearPositionIndex = inByte;


      uart_send(pwm1);


      return;
    }


    if(ucCommand_1 == OFF && inByte == 0x31)
    {
      ucCommand_1 = ON;




      return;
    }
    if(ucCommand_2 == OFF && inByte == 0x32)
    {
      ucCommand_2 = ON;

      return;
    }
  }




}









unsigned char SetupTimer1(float timeoutFrequency)
{
  unsigned char result; //The value to load into the timer to control the timeout interval.

  //Calculate the timer load value
  result=(int)((257.0-(TIMER_CLOCK_FREQ/timeoutFrequency))+0.5); //the 0.5 is for rounding;
  //The 257 really should be 256 but I get better results with 257, dont know why.


  //  //Timer2 Overflow Interrupt Enable
  TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM11);
  // clear on compare, fast PWM, TOP=ICR1 (WGM13/WGM12 in TCCR1B)
  TCCR1B = _BV(WGM12) | _BV(WGM13) |_BV(CS11); //_BV(CS10) | _BV(CS12); // prescaler 1024
  ICR1 = 40000;//result;
  OCR1A = 1100;//off
  OCR1B = 1100;//off
  DDRB |= _BV(1) | _BV(2); // output on B1 and B2
  //}

  TIMSK1 = 1<<TOIE1;


  return(result);
}





//ISR(ADC_vect)
//{
//  sensorValue = analogRead(sensorPin);

//float sens_tran=sensorValue*.0017+.02;

// char* current_ascii= itoa(sensorValue,s,10);

// uart_send(*current_ascii);
// uart_send(*current_ascii++);
// uart_send(0xA);
//}

//Timer2 overflow interrupt vector handler
ISR(TIMER1_OVF_vect)
{
  pwm1=2*(1900-(int)(3*ucFrontPositionIndex));
  pwm2=2*(1900-(int)(3*ucRearPositionIndex));

  OCR1A=pwm1;
  OCR1B=pwm2;

  //set_servo1(pwm1);}?K;=p0
  //set_servo2(pwm2);
}


// sets "high" time of B1 for pwm*64us
void set_servo1(int pwm1)
{
  OCR1A = pwm1;
}

// sets "high" time of B2 for pwm*64us
void set_servo2(int pwm2)
{
  OCR1B = pwm2;
}



//void IMU_Rx_Tx()
//{
//
//    if (IMU.available())
//    {
//
//      int c = IMU.read();
//
//      if (c==44){
//
//        comma_counter=comma_counter+1;
//                }
//
//   if (comma_counter==2){
//
//     if(c!=44 && c!=10){
//
//     bufarray[iii]=c;
//
//     iii=iii+1;
//
//     }
//     if(c==10)
//     {
//       comma_counter=0;
//
//
//   uart_send(0x21);
//   uart_send(0xA);
//   uart_send(bufarray[0]);
//   uart_send(bufarray[1]);
//   uart_send(bufarray[2]);
//   uart_send(bufarray[3]);
//   uart_send(bufarray[4]);
//   uart_send(bufarray[5]);
//   uart_send(bufarray[6]);
//   uart_send(0xA);
//
//sensorValue = (analogRead(sensorPin)*.009+.01)*100;//.00169+.01)*100;//.77487)*100;
//
//   //float sens_tran=sensorValue*.0017+.02;
//
//  char* current_ascii= itoa(sensorValue,s,10);
//
//   uart_send(0x23);
//   uart_send(0xA);
//   uart_send(s[0]);
//   uart_send(s[1]);
//   uart_send(s[2]);
//   uart_send(s[4]);
//   uart_send(0xA);
//
//iii=0;
//     }
//   }
//}
//  }




//void current_val(void)
//{
//
//   sensorValue = analogRead(sensorPin);
//
//   //float sens_tran=sensorValue*.0017+.02;
//
//  char* current_ascii= itoa(sensorValue,s,10);
//
//   uart_send(*current_ascii);
//   uart_send(*current_ascii++);
//   uart_send(0xA);
//
//}


void setup()
{
  SetupTimer1(50);
  USART_Init(MYUBRR);


  //IMU.begin(9600);

  uart_send(0x21);
  uart_send(0x40);
  uart_send(0x23);

}


void loop()
{
  //sensorValue0 = analogRead(0);

  //uart_send(0x30);
  //uart_send(0xA);
  //Serial.print(sensorValue0);
  // uart_send(0x30);
  //ISR(ADC_vect)
  //{


  // TO ACTIVATE VOLTAGE READINGS, UNCOMMENT FROM HERE-----------

      delay(100);
      sensorValue0 = analogRead(sensorpin0);
      sensorValue1 = analogRead(sensorpin1);
     //float sens_tran=sensorValue*.0017+.02;

     char* current_ascii0= itoa(sensorValue0,s0,10);
     char* current_ascii1= itoa(sensorValue1,s1,10);

     uart_send(0x30);
     uart_send(0xA);
     uart_send(*current_ascii0);
     uart_send(*current_ascii0++);
     uart_send(0xA);

     uart_send(0x31);
     uart_send(0xA);
     uart_send(*current_ascii1);
     uart_send(*current_ascii1++);
     uart_send(0xA);
  //}

  //   TO HERE -------------------------




  // while(1){};
  //  buffer="";
  //
  // if(cFishStatus == ON){
  //    IMU_Rx_Tx();
  //    //delay(50);
  //    //current_val();
  //    //delay(50);
  //}
}