/* Copyright (c) 1998 Regents of the University of California.
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. All advertising materials mentioning features or use of this software
 *    must display the following acknowledgment:
 *       This product includes software developed by the Imaging and
 *       Distributed Collaboration Group at Lawrence Berkeley 
 *       National Laboratory.
 * 4. Neither the name of the University nor of the Laboratory may be used
 *    to endorse or promote products derived from this software without
 *    specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS 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 REGENTS 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.
 *
 * @(#) $Header: sonyevid30.cpp,v 0.4.2 98/08/10 10:39:13 mperry Exp $ (LBL)
*/

#include <string.h>
#include <math.h>
#include <stdlib.h>
#include "sonyevid30.h"

short SonyEVID30::cmdcount[MAX_PORTS][MAX_SONY];


SonyEVID30::SonyEVID30(Serialport *port,const char *p,int num,short d,short l)
	: Camera(num, d, l)	
{
	int x;

	if (debugLevel > 0)
	  fprintf(stderr,"INITIALIZING SONY EVI-D30 (address: %d)\n",devnum);
	avail = 0;
	initialized = 0;
	setPort(port);
	int id = portp->getID();
	cmdcount[id][devnum-1] = 0;  

    if (devnum == 1)
	   portp->openPort(p);
	if (portp->getFileDesc() == (HANDLE)-1)
	{
		if (debugLevel > 0)
			 fprintf(stderr,"CAN'T OPEN A PORT FOR THE SONY EVI-D30 (address: %d)\n",devnum);
		return;
	}
	if (devnum == 1)
	{
	   x = portp->setupPort((DWORD)9600, (BYTE)8, (BYTE)0, (BYTE)ONESTOPBIT);
	   if (x < 0)
	   {
	     // Don't exit in case other devices are functional. 
	     if (debugLevel > 0)
	       fprintf(stderr,"CAN'T SET UP PORT FOR THE SONY EVI-D30 (address: %d)\n",devnum);
	     return;
	   }
	   x = init();
	   if (x <= 0)
       {
			initialized = 0;
	        if (debugLevel > 0)
	            fprintf(stderr,"INITIALIZATION FAILED FOR SONY EVI-D30 (address: %d)\n",devnum);
	        return;
  	   }
	}
	initialized = 1;
	avail = 1;
	if (devnum == 1)
	{
	    minpan = -100.0;
	    maxpan = 100.0;
	    mintilt = -25.0;
	    maxtilt = 25.0;	
	    min_pspeed = 1.0;
	    max_pspeed = 24.0;
	    min_tspeed = 1.0;
	    max_tspeed = 20.0; 
	    min_zspeed = 34.0;
        max_zspeed = 55.0;
	    minzoom = 1.0;
	    maxzoom = 12.0;
	}
	if (!lens)
	{ 
	    pcoeff = (float)57.4;
	    pexp = (float)-0.182;
	    tcoeff = (float)40.44;
	    texp = (float)-0.12;
	    zcoeffw = (float)9.65;
	    zcoeffh = (float)7.885;
	    zexpw = (float)-0.213;
	    zexph = (float)-0.196;	
	}
	else 
 	{	
	    if (debugLevel > 0)	
	    {
	      fprintf(stderr, "WARNING: You specified a wide-angle lens ");
	      fprintf(stderr, "for the Sony EVI-D30, and it's calibrated ");
	      fprintf(stderr, "for a regular lens only."); 
	    }	
	}
	pspeed = max_pspeed;
	tspeed = max_tspeed;
	zspeed = max_zspeed;
	strcpy(panspeed, "24");
	strcpy(tiltspeed, "20");
}
	
void SonyEVID30::setPort(Serialport *p)
{
    portp = p;
    assert(portp != NULL); 	
}

int SonyEVID30::init()
{
	int n;
	char cmd1[] = { "88010001FF" };
	char cmd2[] = { "883001FF" }; 

	n = transmit((unsigned char *)cmd1, (int)strlen(cmd1));
	n = transmit((unsigned char *)cmd2, (int)strlen(cmd2));
	//clearInput(portp->getFileDesc());
	return n;
}

void SonyEVID30::setPtSpeedfield(char c)
{
	int x;
	if (c=='p')
	{
	    x = (int)pspeed;
	    intToString(x,2,panspeed);
	}
	else if (c=='t')
	{
	    x = (int)tspeed;
	    intToString(x,2,tiltspeed);
	}
}


int SonyEVID30::doZoomSpeed(float amt)
{
    char buf[15];
    int x = (int)amt;

	sprintf(buf, "8%d", devnum);
    strcat(buf, "010407");	
    intToString(x, 2, &buf[strlen(buf)]);
    strcat(buf, "FF");	
    x = transmit((unsigned char *)buf, (int)strlen(buf));  	
    return x;	
}

int SonyEVID30::move(char *cmd)
{
    struct Cmdline cmds[6];  // store multiple commands
    int i, j, n=0, pt=0, pspd=0, tspd=0, zspd=0;
    int id;

    if (!initialized)
		return -1;
    id = portp->getID();
    if (debugLevel > 2)
    {
       printf("Sony (address %d): cmd: %s\n", devnum, cmd); 	
       printf("Sony (address %d)--move: cmdcount[%d][%d]: %d\n",
			devnum,id,(devnum-1),cmdcount[id][devnum-1]); 	
    }
    i = parseCmd(cmd, cmds, &pt, &pspd, &tspd, &zspd); 
    if (pspd)
		setPtSpeedfield('p');
    if (tspd)
		setPtSpeedfield('t');	
    for (j=0; j<i; j++)
    {
		if (!strcmp(cmds[j].axis, "zoom"))
			n = doZoom();
		else if (!strcmp(cmds[j].axis,"power")||!strcmp(cmds[j].axis,"pow"))
		{
			power = stringToInt(cmds[j].value);
			n = doPower();
		}
		else if (!strcmp(cmds[j].axis, "shutdown") && avail)
			n = shutDown();
		else if (!strcmp(cmds[j].axis, "home"))
			n = goHome();
		else if (pt&&(!strcmp(cmds[j].axis,"pan")||!strcmp(cmds[j].axis,"tilt")))
		{
			n = doPanAndTilt();
			pt = 0;
		}
	}
    if ((n < 0) && (debugLevel>0)) 
		fprintf(stderr, "SONY EVI-D30 (address: %d) NOT RESPONDING...CHECK CONNECTION/POWER\n",devnum);
    return n;	
}

int SonyEVID30::transmit(unsigned char *buf, int len)
{
	int l,x,n=0;
	int id = portp->getID();
	HANDLE fd = portp->getFileDesc();
	unsigned char reply[20]; 
 
    l = asciiToPackedHex(buf,len);
	if (debugLevel > 2)
	   displayString("SONY EVI-D30 CMD", buf, l);
   	if (debugLevel > 2)
    {
	   printf("Sony (address: %d)--transmit: cmdcount[%d][%d] = %d\n",
		   devnum,id,(devnum-1),cmdcount[id][devnum-1]);
    } 

    /* We can write up to 2 commands to each of the Sony EVI-D30's
	   buffers, but if more than 2 commands are sent, the Sony sends
	   a "buffer-full" error message.  So increase the count each
	   time a command is written and decrease the count each time a
	   completion reply is received from the Sony.  Any Sony in
	   a daisy-chain can read the replies to any other Sony in that
	   chain.  So all Sonys on one port have a row of the 2-dimensional
	   array assigned, based on "id".  Each column in the row corresponds
	   to the devnum (internal address).  Any device can decrement the
	   command count.  The device with the specified address looks at
	   at its own command count (if > 1, reads before transmitting)    
	   and increments its own count when it transmits a request.
    */

	while ((cmdcount[id][devnum-1] > 1) && (n<3))
	{
		// readFromSony returns: -1: ReadFile() failed,
		//                        0: nothing to read,
		//		                 >0: num. bytes read
		x = readFromSony(reply);
		++n;
	}
	x = writen(fd, buf, l);

	// Increment command count for all commands except
	// initial broadcasted IF_CLEAR and ADDRESS cmds.
	if ((char)buf[0] != '\x88')
		++cmdcount[id][devnum-1];
	n = 0;
	x = readFromSony(reply);
	if ((x > 0) && (debugLevel > 2))
		displayString("Sony reply", reply, x);
	while ((n<3) && (x==0))
	{
		// If we didn't get a reply, wait 100 millisecs.
		Sleep(100);
		if ((x > 0) && (debugLevel > 2))
			displayString("Sony reply", reply, x);
		++n;
	}
	setAvail(x);
 	return x;
}


int SonyEVID30::readFromSony(unsigned char *buf)
{
   unsigned char c = '\0'; 
   int n, i=0, x=0, cam;		
   HANDLE fd = portp->getFileDesc();
   int id = portp->getID();

    n = readn(fd, &c, 1);

   /* If we read the first byte, we want to keep reading until we
      get the terminator (FFh).  readn() will try to read up to 20
	  times but in the case of a slow device, we may need to keep
	  trying.  However, if something goes wrong (e.g., power goes off
	  or cable gets loose), we don't want to make an infinite number of 
      calls to readn()  */
   if (n)
   {
		buf[i++] = c;
		while (((char)c != '\xff') && (x < 6))
		{
			n = readn(fd, &c, 1);
			if (n)
				buf[i++] = c;
			else if (n==0)     // nothing was read
				x++;
			else		       // error (e.g., sys call failed)
			{
				setAvail(n);
				return n;
			}
		}
   }
   setAvail(n);
   if (n>0)
   {
	   // If we got a completion for any camera, decrement the 
	   // command count for that address (address is devnum).
	   if (((char)buf[1]=='\x51')||((char)buf[1]=='\x52'))
	   {
		   cam = (buf[0] >> 4) - 8;
		   --cmdcount[id][cam-1];
	   }
	   if (((char)buf[1]=='\x61') || ((char)buf[1]=='\x62'))
	   {
			if (debugLevel > 2)
				printf("Sony %d: buffers full\n",devnum);
			return (-1);
	   }
	   return i;
   }
   return n;
}


int SonyEVID30::convert(const char *dof, float limit)
{
    float x;

    if (!strcmp(dof,"pan"))
    {
        x = pposn / limit * (float)880.0;
        if (x > 880.0)
			x = 880.0;
 		else if (x < -880.0)
			x = -880.0;
    }
    else if (!strcmp(dof,"tilt"))
    {
        x = tposn / limit * (float)300.0;
		if (x > 300.0)
			x = 300.0;
		else if (x < -300.0)
			x = -300.0;
    }
    else if (!strcmp(dof,"zoom")) 
    { 
        x = (zposn - (float)1.0) / (float)11.0 * (float)1023.0; 
		if (x < 0.0)
			x = 0.0;
		else if (x > 1023.0)
			x = 1023.0;
    }
    return doubleToInt((double)x);
}


int SonyEVID30::doPanAndTilt()
{
    char buf[40];
    int x;

    sprintf(buf, "8%d", devnum);	
    strcat(buf, "010602");
    strcat(buf, panspeed);
    strcat(buf, tiltspeed);	
    x = convert("pan",maxpan);
 	make0XString(x, &buf[strlen(buf)], 4);
    x = convert("tilt",maxtilt);
	make0XString(x, &buf[strlen(buf)], 4);
    strcat(buf, "FF");
    x = transmit((unsigned char *)buf, (int)strlen(buf));
    return x;
}


int SonyEVID30::doZoom()
{
    char buf[40];
    int x; 	

    sprintf(buf, "8%d", devnum);
    strcat(buf, "010447");	
    x = convert("zoom",maxzoom);
	make0XString(x, &buf[strlen(buf)], 4);
    strcat(buf, "FF");
    x = transmit((unsigned char *)buf, strlen(buf));
    return x; 
}


int SonyEVID30::panTiltPosInq()
{
    char buf[12];
    sprintf(buf, "8%d", devnum);
    strcat(buf, "090612FF");
    transmit((unsigned char *)buf, strlen(buf));
    return 0;	
}

int SonyEVID30::zoomPosInq()
{
    char buf[12];
    sprintf(buf, "8%d", devnum);
    strcat(buf, "090447FF");
    transmit((unsigned char *)buf, (int)strlen(buf));
    return 0;	
}


void SonyEVID30::checkPower()
{
    int x = powerInq();	
    if (x != 1)
    {
		power = 1;
		doPower();
    }
}
 

int SonyEVID30::powerInq()
{
    char buf[12];
    int x=2,y=1;

    sprintf(buf, "8%d", devnum);
    strcat(buf, "090400FF");
    transmit((unsigned char *)buf, (int)strlen(buf));
    while ((x > 1) && (y<4))	
    {
    	x = getPowerInqReply();	
   		y++;
    } 
    return x;	
}


int SonyEVID30::getPowerInqReply()
{
    unsigned char buf[40];

    int x = readFromSony(buf);
    if (!verifyDeviceNumber(buf[0]))
		return 2;
    if (x>0)
    {
		if ((x==4)&&(buf[1]=='\x50')&&(buf[2]=='\x2'))
			return 1;           // power is on 
		else if ((x==4)&&(buf[1]=='\x50')&&(buf[2]=='\x3'))
			return 0;           // power is off 
		else if ((buf[1]=='\x61') || (buf[1]=='\x62'))
			return -1;		// got error message 		
		else
			return 2;	        // got inapplicable message
    }
    return x;
}

int SonyEVID30::verifyDeviceNumber(unsigned char c)
{
    unsigned char buf[3];
    buf[0] = c;
    buf[0] >>=4;
    if ((int)buf[0] == (devnum+8))
		return 1;
    else
		return 0;
} 	

	

int SonyEVID30::doPower()
{
    char buf[13];
    int x;	

    sprintf(buf, "8%d", devnum);
    if (power)	
   		strcat(buf, "01040002FF");
    else
		strcat(buf, "01040003FF");
    x = transmit((unsigned char *)buf, (int)strlen(buf));
    if (avail && power)
        Sleep(3000);
    return x;	
}

int SonyEVID30::shutDown()
{
    int x = 0; 	
    power = 0;
    x = doPower();
    return x; 	
}


int SonyEVID30::goHome()
{
    int x;
    pposn = homepan;
    tposn = hometilt;
    x = doPanAndTilt();
    if (x < 0)
		return x;
    zposn = homezoom;
    x = doZoomSpeed(max_zspeed);
    x = doZoom();
    return x;
}	 


void SonyEVID30::getDesc(char *buf)
{
    char *temp = new char[100];

    strcpy(buf, "\"Sony EVI-D30\" ");
    if (!avail)
    {	
		strcat(buf, "NOT_RESPONDING");
		delete [] temp;
 		return;
    }
    strcat(buf, "power ");
    if (power)
		strcat(buf, "on ");
    else
		strcat(buf, "off ");
    strcat(buf, "off on@pan ");
    sprintf(temp,"%1.2f %1.2f %1.2f %1.2f %1.2f %1.2f",pposn,minpan,maxpan,pspeed,min_pspeed,max_pspeed);
    strcat(buf, temp);
    strcat(buf, "@tilt ");
    sprintf(temp,"%1.2f %1.2f %1.2f %1.2f %1.2f %1.2f",tposn,mintilt,maxtilt,tspeed,min_tspeed,max_tspeed);
    strcat(buf, temp);
    strcat(buf, "@zoom ");
    sprintf(temp, "%1.2f %1.2f %1.2f",zposn,minzoom,maxzoom);   
    strcat(buf, temp);
	delete [] temp;
}

