/*
 * Copyright (c) 2017-2019 TOYOTA MOTOR CORPORATION
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#define _GNU_SOURCE
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <net/if.h>
#include <string.h>
#include <sys/ioctl.h>
#include <alloca.h>
#include <math.h>
#include <time.h>
#include <signal.h>

#include "wheel-service.h"
#include "js_signal_event.h"

// Property string
#define VEHICLE_SPEED				"VehicleSpeed"
#define ENGINE_SPEED				"EngineSpeed"
#define ACCELERATOR_PEDAL_POSITION	"AcceleratorPedalPosition"
#define TRANSMISSION_GEAR_INFO		"TransmissionGearInfo"
#define TRANSMISSION_MODE			"TransmissionMode"
#define STEERING_WHEEL_ANGLE		"SteeringWheelAngle"
#define TURN_SIGNAL_STATUS			"TurnSignalStatus"
#define LIGHT_STATUS_BRAKE			"LightStatusBrake"
#define PARKING_STATUS_BRAKE		"ParkingStatusBrake"
#define FUEL_LEVEL					"FuelLevel"

#define MAX_TRANSMISSION_GEAR_INFO 6
#define MIN_TRANSMISSION_GEAR_INFO 0

enum eJsButtonEvent {
	JS_BUTTON_X = 0,
	JS_BUTTON_R = 1,
	JS_BUTTON_O = 2,
	JS_BUTTON_T = 3,
	JS_BUTTON_R1 = 4,
	JS_BUTTON_L1 = 5,
	JS_BUTTON_R2 = 6,
	JS_BUTTON_L2 = 7,
	JS_BUTTON_SHARE = 8,
	JS_BUTTON_OPTION = 9,
	JS_BUTTON_R3 = 10,
	JS_BUTTON_L3 = 11,
	JS_BUTTON_PLUS = 19,
	JS_BUTTON_MINUS = 20,
	JS_BUTTON_SCROLL_R = 21,
	JS_BUTTON_SCROLL_L = 22,
	JS_BUTTON_ENTER = 23,
	JS_BUTTON_PS = 24
};

enum eJsAxisEvent {
	JS_AXIS_STEERING = 0,
	JS_AXIS_CLUTCH = 1,
	JS_AXIS_ACCEL = 2,
	JS_AXIS_BRAKE = 3,
};

enum eTransmissionGearInfo {
	eTransmissionGearInfoLeft = 0,
	eTransmissionGearInfoRight = 1,
};

enum eTurnSignalStatus {
	eTurnSignalStatusOff = 0,
	eTurnSignalStatusRight = 4,
	eTurnSignalStatusLeft = 2,
	eTurnSignalStatusHazard = 1
};

double gearRatio[8] = {
       0.0,    //Neutral
       1.0/4.12,       //First
       1.0/2.84,       //Second
       1.0/2.28,       //Third
       1.0/1.45,       //Fourth
       1.0/1.0,        //Fifth
       1.0/0.69,       //Sixth
       1.0/3.21        //Reverse
};

static int gJsType=JS_TYPE_STEERING;
static int gCurAccPedalPos=0, gAccPedalPos=0;
static int gEngineSpeed=0;
static int gVehicleSpeed=0;
static float gBaseValue = 0.0;

static int gLightStatusBrake = 0;
static int gTransmissionGearInfo = 0;
static int gSteeringWheelAngle = 0;
static int gParkingStatusBrake = 0;
static int gFuelLevel = 0;
static enum eTurnSignalStatus gTurnSignalStatus = eTurnSignalStatusOff;

static void setLightStatusBrake(int val)
{
	if(gLightStatusBrake != val)
	{
		gLightStatusBrake = val;
	}
}

static void setFuelLevel(int val)
{
	if(gFuelLevel != val)
	{
		gFuelLevel = val;
	}
}

static void setTransmissionGearInfo(int val, enum eTransmissionGearInfo eGear)
{
	if(eGear == eTransmissionGearInfoLeft)
	{
		if(val && gTransmissionGearInfo < MAX_TRANSMISSION_GEAR_INFO)
		{
			gTransmissionGearInfo = gTransmissionGearInfo + 1;
		}
	}
	else if(eGear == eTransmissionGearInfoRight)
	{
		if(val && gTransmissionGearInfo > MIN_TRANSMISSION_GEAR_INFO)
		{
			gTransmissionGearInfo = gTransmissionGearInfo - 1;
		}
	}
}

static void setSteeringWheelAngle(int val)
{
	if(gSteeringWheelAngle != val)
	{
		gSteeringWheelAngle = val;
	}
}

static void setTurnSignalStatus(int val, enum eTurnSignalStatus turn)
{
	if(val)
	{
		if(turn == eTurnSignalStatusRight)
		{
			gTurnSignalStatus = eTurnSignalStatusRight;
		}
		else if(turn == eTurnSignalStatusLeft)
		{
			gTurnSignalStatus = eTurnSignalStatusLeft;
		}
		else if(turn == eTurnSignalStatusHazard)
		{
			gTurnSignalStatus = eTurnSignalStatusHazard;
		}
	}
	else
	{
		gTurnSignalStatus = eTurnSignalStatusOff;
	}
}

static void setParkingStatusBrake(int val)
{
	if(gParkingStatusBrake != val)
	{
		gParkingStatusBrake = val;
	}
}

static int calcSteeringWheelAngle()
{
	double steeringWheelAngle  = (((double)gSteeringWheelAngle / (double)32767.0) + (double)1.0) * (double)180.0;

	return (int)steeringWheelAngle;
}

// Method to update property
static void setCanValue(char *prop, int val)
{
	unsigned int nProp = wheel_info->nData;
	for(unsigned int i = 0; i < nProp; i++)
	{
		if(strcmp(prop, wheel_info->property[i].name) == 0)
		{
			if(wheel_info->property[i].curValue.int16_val != val)
			{
				wheel_info->property[i].curValue.int16_val = (int16_t)val;
				//NOTICEMSG("Update property %s", wheel_info->property[i].name);
				notify_property_changed(&wheel_info->property[i]);
			}
		}
	}
}

void updateTransmissionGearInfo()
{
	setCanValue(TRANSMISSION_GEAR_INFO, 5);
}

void updateTransmissionMode()
{
	setCanValue(TRANSMISSION_MODE, 0);
}

static void updateAcceleratorPedalPosition()
{
	setCanValue(ACCELERATOR_PEDAL_POSITION, gAccPedalPos);
	DBG_NOTICE("acceleratorPedalPosition : %d\n", gAccPedalPos);
}

static void updateEngineSpeed()
{
	// Update EngineSpeed
	gEngineSpeed = gBaseValue*3000+1000;
	setCanValue(ENGINE_SPEED, gEngineSpeed);
	DBG_DEBUG("engineSpeed : %d\n", gEngineSpeed);
}

static void updateVehicleSpeed()
{
	// Update VehicleSpeed
	if(gBaseValue < 0)
		gBaseValue = 0;
	gVehicleSpeed = (int)(gBaseValue*9550/100)*100;
	setCanValue(VEHICLE_SPEED, gVehicleSpeed);
	DBG_DEBUG("vehicleSpeed : %d\n", gVehicleSpeed);
}

static void updateLightStatusBrake()
{
	int lightStatusBrake;

	lightStatusBrake = (gLightStatusBrake < 20000);
	setCanValue(LIGHT_STATUS_BRAKE, lightStatusBrake);
}

static void updateFuelLevel()
{
	int fuelLevel = (32511 - gFuelLevel)*256.0/65278.0;
	DBG_DEBUG("fuelLevel: %d, gFuelLevel: %d \n", fuelLevel, gFuelLevel);
	setCanValue(FUEL_LEVEL, fuelLevel);
}

static void updateParkingStatusBrake()
{
	setCanValue(PARKING_STATUS_BRAKE, gParkingStatusBrake);
}

static void updateSteeringWheelAngle()
{
	int steering = calcSteeringWheelAngle();
	setCanValue(STEERING_WHEEL_ANGLE, steering);
}

static void updateTurnSignalStatus()
{
	setCanValue(TURN_SIGNAL_STATUS, gTurnSignalStatus);
}

// Method to handle joy stick event
void newButtonValue(char number, int val)
{
	switch (number)
	{
		case JS_BUTTON_R1:	//Left paddle shifter
			// Set gear position
			setTransmissionGearInfo(val, eTransmissionGearInfoLeft);

			// Update property
			updateTransmissionGearInfo();
			updateTransmissionMode();
			updateVehicleSpeed();
			break;

		case JS_BUTTON_L1:	//Right paddle shifter
			// Set gear position
			setTransmissionGearInfo(val, eTransmissionGearInfoRight);

			// Update property
			updateTransmissionGearInfo();
			updateTransmissionMode();
			updateVehicleSpeed();
			break;

		case JS_BUTTON_R2:	//Right upper wheel button
			// Set value
			setTurnSignalStatus(val, eTurnSignalStatusRight);

			// Update property
			updateTurnSignalStatus();
			break;

		case JS_BUTTON_L2:	//Left upper wheel button
			// Set value
			setTurnSignalStatus(val, eTurnSignalStatusLeft);

			// Update property
			updateTurnSignalStatus();
			break;
		case JS_BUTTON_R3:
			// Set value
			setTurnSignalStatus(val, eTurnSignalStatusHazard);

			// Update property
			updateTurnSignalStatus();
			break;
		case JS_BUTTON_L3:
			// Set value
			setParkingStatusBrake(val);

			// Update property
			updateParkingStatusBrake();
			break;
		default:
			break;
	}
}

void newAxisValue(char number, int val)
{
//	DBG_ERROR("#################number: %d, %d. \n", number, val);
//	DBG_ERROR("#################gJsType: %d. \n", gJsType);
	if ((JS_TYPE_GAME_CTL == gJsType) && (number == JS_AXIS_BRAKE))
	{
		number = JS_AXIS_ACCEL;
	}
	else if ((JS_TYPE_GAME_CTL == gJsType) && (number == JS_AXIS_ACCEL))
	{
		number = JS_AXIS_BRAKE;
	}
	else
	{
		//do nothing
	}

	switch (number)
	{
		case JS_AXIS_STEERING:	//Wheel angle, -32767 - 32767
			// Set value
			setSteeringWheelAngle(val);

			// Update property
			updateSteeringWheelAngle();
			break;

		case JS_AXIS_ACCEL:	//Throttle, -32767 (depressed) - 32767 (undepressed)
			// Set origin value
			if (JS_TYPE_GAME_CTL == gJsType)
			{
				gAccPedalPos = (int)(((double)val/(double)-32767.0)*(double)100.0);
			}
			else
			{
				gAccPedalPos = (int)(((double)(val - 32767)/(double)-65534.0)*(double)100.0);
			}

			// Update property
			updateAcceleratorPedalPosition();
			if (gAccPedalPos > gCurAccPedalPos){
				gCurAccPedalPos = gAccPedalPos;
				gBaseValue = sin((float)gCurAccPedalPos/100.0);

				updateEngineSpeed();
				updateVehicleSpeed();
			}
			break;

		case JS_AXIS_BRAKE:
			// Set value
			setLightStatusBrake(val);

			// Update property
			updateLightStatusBrake();
			break;	//Brake, -32767 (depressed) - 32767 (undepressed)

		case JS_AXIS_CLUTCH:
			setFuelLevel(val);
			updateFuelLevel();
			break;
		default:
			DBG_DEBUG("number: %d, %d. \n", number, val);
			break;
	}
}

void updateTimerHandler(int signum)
{
	if (gCurAccPedalPos == gAccPedalPos){
		gCurAccPedalPos = gCurAccPedalPos + rand()%2;
	}else if (gAccPedalPos < (gCurAccPedalPos - 5)){
		gCurAccPedalPos = gCurAccPedalPos - 5;
	}else{
		gCurAccPedalPos = gAccPedalPos;
	}
	gBaseValue = sin((float)gCurAccPedalPos/100.0);
	updateEngineSpeed();
	updateVehicleSpeed();
}

void setJsType(int jstype)
{
	gJsType = jstype;
}