/*

Copyright (c) 2013, Silas Parker

All rights reserved.
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
The name of Silas Parker may not be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
//Include the SPI/IIC Library
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
const int SPEEDO_PIN = A0;
const int RPM_PIN = A1;
const int OIL_PRESSURE_PIN = A2;
const int WATER_TEMPERATURE_PIN = A3;
const int OIL_TEMPERATURE_PIN = A4;
const int BRAKE_AIR_PRESSURE_PIN = A5;
const int BRAKE_PIN = A6;
const int FUEL_PIN = A7;
const int WATER_PIN = A8;
const int LEFT_INDICATOR = A9;
const int RIGHT_INDICATOR = A10;
const int PARKING_BREAK = A11;
const int FUEL_WARNING = A12;
const int LOW_BEAM = A13;
const int HIGH_BEAM = A14;
const int BATTERY_VOLTAGE = A15;
const int OIL_PRESSURE = 2;
const int WATER_TEMPERATURE = 3;
const int REVERSE = 4;
const int BRAKE = 5;
const int PARKING_LIGHT = 6;
const int MOTOR_BRAKE = 7;
const int AIR_PRESSURE_WARNING = 8;
const int AIR_PRESSURE_EMERGENCY = 9;
// Servo variables
Servo speedo;
Servo rpm;
Servo fuel;
Servo oil;
Servo water;
Servo oiltemp;
Servo brakeair;
Servo brake;

#define PACKET_SYNC 0xFF

#define PACKET_VER 2
#define SERVO_DIR_NORMAL false
#define SERVO_DIR_INVERT true
int serial_byte;
LiquidCrystal_I2C lcd(0x27,16,2);
void setup()
{
lcd.init();
Serial.begin(115200);
lcd.backlight();
lcd.begin(16, 2);
lcd.print("Cargando");
// Initialise servos
speedo.attach(SPEEDO_PIN);
speedo.write(180);
rpm.attach(RPM_PIN);
rpm.write(180);
fuel.attach(FUEL_PIN);
fuel.write(75);
oil.attach(OIL_PRESSURE_PIN);
oil.write(75);
water.attach(WATER_TEMPERATURE_PIN);
water.write(75);
oiltemp.attach(OIL_TEMPERATURE_PIN);
oiltemp.write(75);
brakeair.attach(BRAKE_AIR_PRESSURE_PIN);
brakeair.write(75);
brake.attach(BRAKE_PIN);
brake.write(75);
// Initialise LEDs
pinMode(LEFT_INDICATOR, OUTPUT);
pinMode(RIGHT_INDICATOR, OUTPUT);
pinMode(PARKING_BREAK, OUTPUT);
pinMode(FUEL_WARNING, OUTPUT);
pinMode(LOW_BEAM, OUTPUT);
pinMode(HIGH_BEAM, OUTPUT);
pinMode(BATTERY_VOLTAGE, OUTPUT);
pinMode(OIL_PRESSURE, OUTPUT);
pinMode(WATER_TEMPERATURE, OUTPUT);
pinMode(REVERSE, OUTPUT);
pinMode(BRAKE, OUTPUT);
pinMode(PARKING_LIGHT, OUTPUT);
pinMode(MOTOR_BRAKE, OUTPUT);
pinMode(AIR_PRESSURE_WARNING, OUTPUT);
pinMode(AIR_PRESSURE_EMERGENCY, OUTPUT);
digitalWrite(LEFT_INDICATOR, 0);
digitalWrite(RIGHT_INDICATOR, 0);
digitalWrite(PARKING_BREAK, 0);
digitalWrite(FUEL_WARNING, 0);
digitalWrite(LOW_BEAM, 0);
digitalWrite(HIGH_BEAM, 0);
digitalWrite(BATTERY_VOLTAGE, 0);
digitalWrite(OIL_PRESSURE, 0);
digitalWrite(WATER_TEMPERATURE, 0);
digitalWrite(REVERSE, 0);
digitalWrite(BRAKE, 0);
digitalWrite(PARKING_LIGHT, 0);
digitalWrite(MOTOR_BRAKE, 0);
digitalWrite(AIR_PRESSURE_WARNING, 0);
digitalWrite(AIR_PRESSURE_EMERGENCY, 0);

delay(500);
speedo.write(0);
rpm.write(0);

digitalWrite(LEFT_INDICATOR, 1);
digitalWrite(RIGHT_INDICATOR, 1);
digitalWrite(PARKING_BREAK, 1);
digitalWrite(FUEL_WARNING, 1);
digitalWrite(LOW_BEAM, 1);
digitalWrite(HIGH_BEAM, 1);
digitalWrite(BATTERY_VOLTAGE, 1);
digitalWrite(OIL_PRESSURE, 1);
digitalWrite(WATER_TEMPERATURE, 1);
digitalWrite(REVERSE, 1);
digitalWrite(BRAKE, 1);
digitalWrite(PARKING_LIGHT, 1);
digitalWrite(MOTOR_BRAKE, 1);
digitalWrite(AIR_PRESSURE_WARNING, 1);
digitalWrite(AIR_PRESSURE_EMERGENCY, 1);

delay(500);
speedo.write(180);
rpm.write(180);

digitalWrite(LEFT_INDICATOR, 0);
digitalWrite(RIGHT_INDICATOR, 0);
digitalWrite(PARKING_BREAK, 0);
digitalWrite(FUEL_WARNING, 0);
digitalWrite(LOW_BEAM, 0);
digitalWrite(HIGH_BEAM, 0);
 digitalWrite(BATTERY_VOLTAGE, 0);
digitalWrite(OIL_PRESSURE, 0);
digitalWrite(WATER_TEMPERATURE, 0);
digitalWrite(REVERSE, 0);
digitalWrite(BRAKE, 0);
digitalWrite(PARKING_LIGHT, 0);
digitalWrite(MOTOR_BRAKE, 0);
digitalWrite(AIR_PRESSURE_WARNING, 0);
digitalWrite(AIR_PRESSURE_EMERGENCY, 0);

lcd.clear();
lcd.print("Wait");
// Wait a second to ensure serial data isn't from re-programming
delay(1000);
lcd.clear();
lcd.print("Segu");
}

void read_serial_byte_set_servo(Servo& servo, bool invert)

{
serial_byte = Serial.read();
serial_byte = (serial_byte < 0) ? 0 : ((serial_byte > 180) ? 180 : serial_byte
);
if (invert)
servo.write(180 - serial_byte);
else
servo.write(serial_byte);
}
void skip_serial_byte()
{
(void)Serial.read();
}
void digitalWriteFromBit(int port, int value, int shift)
{
digitalWrite(port, (value >> shift) & 0x01);
}
void loop()
{
if (Serial.available() < 16)
return;
serial_byte = Serial.read();
if (serial_byte != PACKET_SYNC)
return;
serial_byte = Serial.read();
if (serial_byte != PACKET_VER)
{
lcd.clear();
lcd.print("PROTOCOL VERSION ERROR");
return;
}
 read_serial_byte_set_servo(speedo, SERVO_DIR_INVERT); // Speed
read_serial_byte_set_servo(rpm, SERVO_DIR_INVERT); // RPM
read_serial_byte_set_servo(brakeair, SERVO_DIR_INVERT); // Brake air pressure
read_serial_byte_set_servo(brake, SERVO_DIR_INVERT); // Brake temperature
read_serial_byte_set_servo(fuel, SERVO_DIR_INVERT); // Fuel ratio
read_serial_byte_set_servo(oil, SERVO_DIR_INVERT); // Oil pressure
read_serial_byte_set_servo(oiltemp, SERVO_DIR_INVERT); // Oil temperature
read_serial_byte_set_servo(water, SERVO_DIR_INVERT); // Water temperature
skip_serial_byte(); // Battery voltage

// Truck lights byte

serial_byte = Serial.read();
digitalWriteFromBit(LEFT_INDICATOR, serial_byte, 6);
digitalWriteFromBit(RIGHT_INDICATOR, serial_byte, 5);
digitalWriteFromBit(LOW_BEAM, serial_byte, 4);
digitalWriteFromBit(HIGH_BEAM, serial_byte, 3);
digitalWriteFromBit(REVERSE, serial_byte, 2);
digitalWriteFromBit(BRAKE, serial_byte, 1);
digitalWriteFromBit(PARKING_LIGHT, serial_byte, 0);

// Warning lights bytes

serial_byte = Serial.read();
digitalWriteFromBit(PARKING_BREAK, serial_byte,7 );
digitalWriteFromBit(FUEL_WARNING, serial_byte, 6);
digitalWriteFromBit(BATTERY_VOLTAGE, serial_byte, 5);
digitalWriteFromBit(OIL_PRESSURE, serial_byte, 4);
digitalWriteFromBit(WATER_TEMPERATURE, serial_byte, 3);
digitalWriteFromBit(MOTOR_BRAKE, serial_byte, 2);
digitalWriteFromBit(AIR_PRESSURE_WARNING, serial_byte, 1);
digitalWriteFromBit(AIR_PRESSURE_EMERGENCY, serial_byte, 0);
// Enabled flags
serial_byte = Serial.read();
// Text length
int text_len = Serial.read();
// Followed by text
if (0 < text_len && text_len < 127)
{
lcd.clear();
for (int i = 0; i < text_len; ++i)
{
while (Serial.available() == 0) // Wait for data if slow
{
delay(2);
}
serial_byte = Serial.read();
if (serial_byte < 0 && serial_byte > 127)
return;
if (serial_byte == '\n')
lcd.setCursor(0, 1);
else
lcd.print(char(serial_byte));
// delay(2);
}
 }
}