#include <avr/pgmspace.h>
#include <IRremote.h>
#include <Adafruit_NeoPixel.h>
#include <Servo.h>
#include <Wire.h>
#include <lm75.h>
#include <TSL2581.h>
#include "TRSensors.h"

typedef struct MeModule
{
    int device;
    int port;
    int slot;
    int pin;
    int index;
    float values[3];
} MeModule;

union{
    byte byteVal[4];
    float floatVal;
    long longVal;
}val;

union{
  byte byteVal[8];
  double doubleVal;
}valDouble;

union{
  byte byteVal[2];
  short shortVal;
}valShort;

int analogs[8]={A0,A1,A2,A3,A4,A5,A6,A7};
uint8_t Ports[7][2]={{A0,0},{A1,0},{A2,0},{A3,0},{A4,4},{A5,5},{A6,6}};

String mVersion = "06.01.107";


#define VERSION                0
#define ULTRASONIC_SENSOR      1
#define TEMPERATURE_SENSOR     2
#define LIGHT_SENSOR           3
#define POTENTIONMETER         4
#define JOYSTICK               5
#define GYRO                   6
#define SOUND_SENSOR           7
#define RGBLED                 8
#define SEVSEG                 9
#define MOTOR                  10
#define SERVO                  11
#define ENCODER                12
#define IR                     13
#define IRREMOTE               14
#define PIRMOTION              15
#define INFRARED               16
#define LINEFOLLOWER           17
#define IRREMOTECODE           18
#define SHUTTER                20
#define LIMITSWITCH            21
#define BUTTON                 22
#define HUMITURE               23
#define FLAMESENSOR            24
#define GASSENSOR              25
#define COMPASS                26
#define TEMPERATURE_SENSOR_1   27
#define DIGITAL                30
#define ANALOG                 31
#define PWM                    32
#define LED                    33
#define TONE                   34
#define BUTTON_INNER           35
#define ULTRASONIC_ARDUINO     36
#define PULSEIN                37
#define STEPPER                40
#define LEDMATRIX              41
#define TIMER                  50
#define TOUCH_SENSOR           51
#define JOYSTICK_MOVE          52
#define COMMON_COMMONCMD       60
//Secondary command
#define SET_STARTER_MODE     0x10
#define SET_AURIGA_MODE      0x11
#define SET_MEGAPI_MODE      0x12
#define GET_BATTERY_POWER    0x70
#define GET_AURIGA_MODE      0x71
#define GET_MEGAPI_MODE      0x72
#define ENCODER_BOARD          61
//Read type
#define ENCODER_BOARD_POS    0x01
#define ENCODER_BOARD_SPEED  0x02

#define ENCODER_PID_MOTION     62
//Secondary command
#define ENCODER_BOARD_POS_MOTION         0x01
#define ENCODER_BOARD_SPEED_MOTION       0x02
#define ENCODER_BOARD_PWM_MOTION         0x03
#define ENCODER_BOARD_SET_CUR_POS_ZERO   0x04
#define ENCODER_BOARD_CAR_POS_MOTION     0x05

#define PM25SENSOR     63
//Secondary command
#define GET_PM1_0         0x01
#define GET_PM2_5         0x02
#define GET_PM10          0x03

#define GET 1
#define RUN 2
#define RESET 4
#define START 5

#define PWMA 45
#define AIN2 40
#define AIN1 41
#define BIN1 36
#define BIN2 37
#define PWMB 46
#define BUTTON_PIN 25
#define RGB_PIN 24
#define TONE_PIN 23
#define RECV_PIN 7

#define NUM_SENSORS 5
unsigned int sensorValues[NUM_SENSORS];
TRSensors trs =   TRSensors();

IRrecv irrecv(RECV_PIN);
Adafruit_NeoPixel RGB_OnBoard = Adafruit_NeoPixel(2, RGB_PIN, NEO_GRB + NEO_KHZ800);
Adafruit_NeoPixel RGB = Adafruit_NeoPixel(32, RGB_PIN, NEO_GRB + NEO_KHZ800);
TempI2C_LM75 termo = TempI2C_LM75(0x48,TempI2C_LM75::nine_bits);
TSL2581 tsl = TSL2581();

Servo myservo;
decode_results results;

//decode_results results;
byte cmd[150];
byte Num = 0;
int i;
uint8_t command_index = 0;
char irRead = 0;
char irStatus = 0;
double lastTime = 0.0;
double lastIRTime = 0.0;

void setup()
{
  Serial.begin(115200);
  irrecv.enableIRIn();
  pinMode(PWMA,OUTPUT);
  pinMode(AIN2,OUTPUT);
  pinMode(AIN1,OUTPUT);
  pinMode(BIN1,OUTPUT);
  pinMode(BIN2,OUTPUT);
  pinMode(PWMB,OUTPUT); 
  pinMode(BUTTON_PIN,OUTPUT);
  digitalWrite(BUTTON_PIN,HIGH),
  pinMode(TONE_PIN,OUTPUT);
  RGB_OnBoard.begin();
  RGB_OnBoard.setBrightness(30);
  RGB_OnBoard.show();
  RGB.setBrightness(50);
  tsl.Init();
  myservo.attach(Ports[3][0]);
  myservo.write(90);
  Serial.println("start ...");
}

void loop()
{
  if (irrecv.decode(&results))
  {
    if(results.value%0x100 + (results.value>>8)%0x100 == 0xFF)
    {
      irRead = (results.value>>8)%0x100;
     // Serial.write(irRead);
    }
    lastIRTime = millis();
    irrecv.resume(); // Receive the next value
    irStatus = 1;
  }else{
    if(millis() - lastIRTime >200){
      irRead = 0;
      irStatus = 0;
    }
  }
  while (Serial.available() > 0 && Num <50)
  {
    cmd[Num++] = Serial.read();
    delay(2);
  }
  if (Num > 0)
  {
    if ((cmd[0] == 0xFF) && (cmd[1] == 0x55) && (cmd[2] == Num - 3))
    {
      parseData();
    }
    Num = 0;
  }
}

/*
  ff 55 len idx action device port slot data 
  0  1  2   3   4      5      6    7    8
*/
void parseData() {
  int idx = cmd[3];
  command_index = (uint8_t)idx;
  int action = cmd[4];
  int device = cmd[5];
  switch (action) {
    case GET: {
        if(device != ULTRASONIC_SENSOR){
          Serial.write(0xff);
          Serial.write(0x55);
          Serial.write(idx);
        }
        readSensor(device);
        Serial.println();
      }
      break;
    case RUN: {
        runModule(device);
        reply();
      }
      break;
    case RESET: {
        //reset
        reply();
      }
      break;
    case START: {
        //start
        reply();
      }
      break;
  }
}

/*
ff 55 len idx action device port slot data a
0  1  2   3   4      5      6    7    8
*/
void runModule(int device) {
  int port = cmd[6];
  int pin = port;
  switch (device) {
    case MOTOR: {
        int speed = readShort(7);
        if(pin == 1)
        {
          setSpeed(1,speed);
        }
        else if(pin == 2)
        {
          setSpeed(2,speed);
        }
      }
      break;
    case JOYSTICK: {
        int leftSpeed = readShort(6);
        int rightSpeed = readShort(8);
        setSpeed(1,-leftSpeed);
        setSpeed(2,rightSpeed);
      }
      break;
    case RGBLED: {
        int slot = cmd[7];
        int idx = cmd[8];
        int r = cmd[9];
        int g = cmd[10];
        int b = cmd[11];
        if (pin == 24 ){
          RGB_OnBoard.begin();
          if (idx > 0) {
            RGB_OnBoard.setPixelColor(idx - 1, r, g, b);
          }
          else{
            for (i = 0; i < RGB_OnBoard.numPixels(); i++) {
              RGB_OnBoard.setPixelColor(i, r, g, b);
            }
          }
          RGB_OnBoard.show();
        }
        else
        {
          RGB.setPin(Ports[pin][0]);
          delay(1);
          if (idx > 0) {
            RGB.setPixelColor(idx - 1,r, g, b);
          }
          else{
            for (i = 0; i < RGB.numPixels(); i++) {
              RGB.setPixelColor(i, r, g, b);
            }
          }
          RGB.show();
        }
      }
      break;
    case SERVO: {
        myservo.attach(Ports[pin][0]);
        if(cmd[7] >= 0 && cmd[7] <= 180)
        {
          myservo.write(cmd[7]);
        }
      }
      break;
    case SEVSEG: {
      }
      break;
    case LEDMATRIX: {
      }
      break;
    case LIGHT_SENSOR: {
      }
      break;
    case IR: {
      }
      break;
    case SHUTTER: {
      }
      break;
    case DIGITAL: {
        pinMode(pin, OUTPUT);
        int v = cmd[7];
        digitalWrite(pin, v);
      }
      break;
    case PWM: {
        pinMode(pin, OUTPUT);
        int v = cmd[7];
        analogWrite(pin, v);
      }
      break;
   case LED: {
        pinMode(13, OUTPUT);
        int v = cmd[7];
        digitalWrite(13, v);
      }
      break;
    case TONE: {
         unsigned  int hz = readShort(6);
         unsigned  int tone_time = readShort(8);
         if(hz !=0 && tone_time !=0){
            tone(TONE_PIN,hz);
            delay(tone_time);
            noTone(TONE_PIN);
         }else if(hz !=0){
            tone(TONE_PIN,hz);
         }else{
            noTone(TONE_PIN);
         }
      }
      break;
    case TIMER: {
            lastTime = millis();
      }
      break;
  }
}

void readSensor(int device){
  /**************************************************
      ff    55      len idx action device port slot data 
      0     1       2   3   4      5      6    7    8
      0xff  0x55   0x4 0x3 0x1    0x1    0x1  0xa 
  ***************************************************/
  float value=0.0;
  int port,slot,pin;
  port = cmd[6];
  pin = port;
  switch(device){
   case  ULTRASONIC_SENSOR:{

    int TRIG = A4;
    int ECHO = 4;
    pinMode(ECHO, INPUT);    // Define the ultrasonic echo input pin
    pinMode(TRIG, OUTPUT);   // Define the ultrasonic trigger input pin   
    
    digitalWrite(TRIG, LOW);   // set trig pin low 2μs
    delayMicroseconds(2);
    digitalWrite(TRIG, HIGH);  // set trig pin 10μs , at last 10us 
    delayMicroseconds(10);
    digitalWrite(TRIG, LOW);    // set trig pin low
    noInterrupts();
    float Fdistance = pulseIn(ECHO, HIGH,20000);  // Read echo pin high level time(us)
    interrupts();
    Fdistance= Fdistance/58;       //Y m=（X s*344）/2
    
    Serial.write(0xFF);
    Serial.write(0x55);
    Serial.write(command_index);
    sendFloat(Fdistance);
   }
   break;
   case  TEMPERATURE_SENSOR:{
     value = termo.getTemp();
     sendFloat(value);
   }
   break;
   case  LIGHT_SENSOR: {
    value = tsl.get_light();
    sendFloat(value);
   }
   break;
   case  SOUND_SENSOR:
   case  POTENTIONMETER:{
//     if(generalDevice.getPort()!=port){
//       generalDevice.reset(port);
//       pinMode(generalDevice.pin2(),INPUT);
//     }
//     value = generalDevice.aRead2();
//     sendFloat(value);
   }
   break;
   case  JOYSTICK:{
//     slot = readBuffer(7);
//     if(joystick.getPort() != port){
//       joystick.reset(port);
//     }
//     value = joystick.read(slot);
//     sendFloat(value);
   }
   break;
   case  IR:{
     if(millis()-lastIRTime>200){
      irRead = 0;
      irStatus = 0;
     }
     sendFloat(irRead);
   }
   break;
   case IRREMOTE:{
     char r = cmd[7];

     if(millis()-lastIRTime>200){
      irRead = 0;
      irStatus = 0;
     }
     if(r == 0){
        if(irStatus == 0){
          sendFloat(1);
        }else{
          sendFloat(0);
        }
     }else{
      if(irRead==r){
        irRead = 0xFF;
        sendFloat(1);
      }else{
        sendFloat(0);
      }
     }
   }
   break;
   case IRREMOTECODE:{
//     if(irRead<0xff){
//       sendByte(irRead);
//     }
//     irRead = 0;
//     irIndex = 0;
   }
   break;
   case PIRMOTION:{
//     if(generalDevice.getPort()!=port){
//       generalDevice.reset(port);
//       pinMode(generalDevice.pin2(),INPUT);
//     }
//     value = generalDevice.dRead2();
//     sendFloat(value);
   }
   break;
   case  LINEFOLLOWER:{
//     if(generalDevice.getPort()!=port){
//       generalDevice.reset(port);
//         pinMode(generalDevice.pin1(),INPUT);
//         pinMode(generalDevice.pin2(),INPUT);
//     }
//     value = generalDevice.dRead1()*2+generalDevice.dRead2();
     sendFloat(trs.readLine(sensorValues));
//   }
//   break;
//   case  LINEFOLLOWER:{
//     slot = readBuffer(7);
//     if(generalDevice.getPort()!=port||generalDevice.getSlot()!=slot){
//       generalDevice.reset(port,slot);
//     }
//     if(slot==1){
//       pinMode(generalDevice.pin1(),INPUT_PULLUP);
//       value = !generalDevice.dRead1();
//     }else{
//       pinMode(generalDevice.pin2(),INPUT_PULLUP);
//       value = !generalDevice.dRead2();
//     }
//     sendFloat(value);  
   }
   break;
   case BUTTON_INNER:{
     sendFloat(cmd[7]^(digitalRead(BUTTON_PIN)?0:1));
   }
   break;
   case COMPASS:{
//     if(Compass.getPort()!=port){
//       Compass.reset(port)
//       Compass.setpin(Compass.pin1(),Compass.pin2());
//     }
//     sendFloat(Compass.getAngle());
   }
   break;
   case HUMITURE:{
//     uint8_t index = readBuffer(7);
//     if(humiture.getPort()!=port){
//       humiture.reset(port);
//     }
//     uint8_t HumitureData;
//     humiture.update();
//     HumitureData = humiture.getValue(index);
//     sendByte(HumitureData);
   }
   break;
   case FLAMESENSOR:{
//     if(FlameSensor.getPort()!=port){
//       FlameSensor.reset(port);
//       FlameSensor.setpin(FlameSensor.pin2(),FlameSensor.pin1());
//     }
//     int16_t FlameData; 
//     FlameData = FlameSensor.readAnalog();
//     sendShort(FlameData);
   }
   break;
   case GASSENSOR:{
//     if(GasSensor.getPort()!=port){
//       GasSensor.reset(port);
//       GasSensor.setpin(GasSensor.pin2(),GasSensor.pin1());
//     }
//     int16_t GasData; 
//     GasData = GasSensor.readAnalog();
//     sendShort(GasData);
   }
   break;
   case  GYRO:{
//       int axis = readBuffer(7);
//       gyro.update();
//       value = gyro.getAngle(axis);
//       sendFloat(value);
   }
   break;
   case  VERSION:{
     sendString(mVersion);
   }
   break;
   case  DIGITAL:{
     pinMode(pin,INPUT);
     //digitalWrite(pin,HIGH);
     sendFloat(digitalRead(pin));
   }
   break;
   case  ANALOG:{
     pin = analogs[pin];
     //pin = pgm_read_byte(&analogs[pin]);
     pinMode(pin,INPUT);
     sendFloat(analogRead(pin));
   }
   break;
   case LED:{
     sendFloat(cmd[7]^(!digitalRead(13)?1:0));
   }
   case TIMER:{
     sendFloat(millis()-lastTime);
   }
   break;
   case TOUCH_SENSOR:
   {
//     if(touchSensor.getPort() != port){
//       touchSensor.reset(port);
//     }
//     sendByte(touchSensor.touched());
   }
   break;
   case BUTTON:
   {
//     if(buttonSensor.getPort() != port){
//       buttonSensor.reset(port);
//     }
//     sendByte(keyPressed == readBuffer(7));
   }
   break;
  }
}

void reply()
{
  Serial.write(0xFF);
  Serial.write(0x55);
  Serial.println();
}

//1 byte 2 float 3 short 4 len+string 5 double
void sendByte(char c){
  Serial.write(1);
  Serial.write(c);
}
void sendString(String s){
  int l = s.length();
  Serial.write(4);
  Serial.write(l);
  for(int i=0;i<l;i++){
    Serial.write(s.charAt(i));
  }
}
//1 byte 2 float 3 short 4 len+string 5 double
void sendFloat(float value){ 
     Serial.write(2);
     val.floatVal = value;
     Serial.write(val.byteVal[0]);
     Serial.write(val.byteVal[1]);
     Serial.write(val.byteVal[2]);
     Serial.write(val.byteVal[3]);
}
void sendShort(double value){
     Serial.write(3);
     valShort.shortVal = value;
     Serial.write(valShort.byteVal[0]);
     Serial.write(valShort.byteVal[1]);
     Serial.write(valShort.byteVal[2]);
     Serial.write(valShort.byteVal[3]);
}
void sendDouble(double value){
     Serial.write(5);
     valDouble.doubleVal = value;
     Serial.write(valDouble.byteVal[0]);
     Serial.write(valDouble.byteVal[1]);
     Serial.write(valDouble.byteVal[2]);
     Serial.write(valDouble.byteVal[3]);
     Serial.write(valDouble.byteVal[4]);
     Serial.write(valDouble.byteVal[5]);
     Serial.write(valDouble.byteVal[6]);
     Serial.write(valDouble.byteVal[7]);
}
short readShort(int idx){
  valShort.byteVal[0] = cmd[idx];
  valShort.byteVal[1] = cmd[idx+1];
  return valShort.shortVal; 
}
float readFloat(int idx){
  val.byteVal[0] = cmd[idx];
  val.byteVal[1] = cmd[idx+1];
  val.byteVal[2] = cmd[idx+2];
  val.byteVal[3] = cmd[idx+3];
  return val.floatVal;
}

void setSpeed(uint8_t port, int16_t speed)
{
  int in1,in2,pwm;
  speed  = speed > 255 ? 255 : speed;
  speed = speed < -255 ? -255 : speed;  
  if(port == 2)
  {
    in1 = AIN1;
    in2 = AIN2;
    pwm = PWMA;
  }
  else if(port == 1)
  {
     in1 = BIN1;
     in2 = BIN2;
     pwm = PWMB;
  }
  if(speed >= 0)
  {
    digitalWrite(in1,LOW);
    digitalWrite(in2,HIGH);
    analogWrite(pwm,speed);
  }
  else
  {    
    digitalWrite(in1,HIGH);
    digitalWrite(in2,LOW);
    analogWrite(pwm,- speed);
  }
}






